Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/master' into release_7.1.1

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-05-07 14:49:55 +02:00
commit cd6c51ddfe
182 changed files with 5488 additions and 2180 deletions
Download patch file Download diff file Expand all files Collapse all files

245
src/main/navigation/navigation.c
View file

@ -56,6 +56,7 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "navigation/rth_trackback.h"
#include "rx/rx.h"
@ -155,9 +156,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
.min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s)
.max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
.max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
.max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
.land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters
.land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters
.land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s
@ -176,13 +175,15 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
},
// MC-specific
.mc = {
.max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees
.max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT,
#ifdef USE_MR_BRAKING_MODE
.braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s
@ -203,43 +204,44 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// Fixed wing
.fw = {
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
.minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
.loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
//Fixed wing landing
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
// Fixed wing launch
.launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
.launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
.launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
.launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us
.launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
.launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_wiggle_wake_idle = SETTING_NAV_FW_LAUNCH_WIGGLE_TO_WAKE_IDLE_DEFAULT, // uint8_t
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to greaually increase throttle from idle to launch
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
.launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT, // OFF
.launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT, // pitch angle mode deadband when Saoring mode enabled
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
}
);
@ -284,11 +286,9 @@ static void resetJumpCounter(void);
static void clearJumpCounters(void);
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
bool isWaypointAltitudeReached(void);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
static navigationFSMEvent_t nextForNonGeoStates(void);
@ -300,10 +300,9 @@ bool validateRTHSanityChecker(void);
void updateHomePosition(void);
bool abortLaunchAllowed(void);
static bool rthAltControlStickOverrideCheck(unsigned axis);
static void updateRthTrackback(bool forceSaveTrackPoint);
static fpVector3_t * rthGetTrackbackPos(void);
// static bool rthAltControlStickOverrideCheck(unsigned axis);
// static void updateRthTrackback(bool forceSaveTrackPoint);
// static fpVector3_t * rthGetTrackbackPos(void);
#ifdef USE_FW_AUTOLAND
static float getLandAltitude(void);
@ -1453,9 +1452,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
(navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference
rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference
posControl.flags.rthTrackbackActive = true;
calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK;
}
@ -1571,36 +1570,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (posControl.flags.estPosStatus >= EST_USABLE) {
const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
#ifdef USE_MULTI_FUNCTIONS
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
#else
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
#endif
const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
(overrideTrackback && !posControl.flags.forcedRTHActivated);
if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false;
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point
}
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
posControl.activeRthTBPointIndex--;
if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
posControl.activeRthTBPointIndex = NAV_RTH_TRACKBACK_POINTS - 1;
}
calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
if (posControl.activeRthTBPointIndex - posControl.rthTBWrapAroundCounter == 0) {
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
}
} else {
setDesiredPosition(rthGetTrackbackPos(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
if (rthTrackBackSetNewPosition()) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
}
return NAV_FSM_EVENT_NONE;
@ -2865,7 +2836,7 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
* Check if waypoint is/was reached.
* 'waypointBearing' stores initial bearing to waypoint.
*-----------------------------------------------------------*/
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing)
{
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
@ -2987,8 +2958,19 @@ bool validateRTHSanityChecker(void)
return true;
}
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
posControl.rthSanityChecker.minimalDistanceToHome = 1e10f;
//schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true;
}
#endif
// Check at 10Hz rate
if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
@ -3212,12 +3194,13 @@ void updateHomePosition(void)
* Climb First override limited to Fixed Wing only
* Roll also cancels RTH trackback on Fixed Wing and Multirotor
*-----------------------------------------------------------*/
static bool rthAltControlStickOverrideCheck(unsigned axis)
bool rthAltControlStickOverrideCheck(uint8_t axis)
{
if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated ||
(axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) {
return false;
}
static timeMs_t rthOverrideStickHoldStartTime[2];
if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
@ -3256,110 +3239,6 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
return false;
}
/* --------------------------------------------------------------------------------
* == RTH Trackback ==
* Saves track during flight which is used during RTH to back track
* along arrival route rather than immediately heading directly toward home.
* Max desired trackback distance set by user or limited by number of available points.
* Reverts to normal RTH heading direct to home when end of track reached.
* Trackpoints logged with precedence for course/altitude changes. Distance based changes
* only logged if no course/altitude changes logged over an extended distance.
* Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
* --------------------------------------------------------------------------------- */
static void updateRthTrackback(bool forceSaveTrackPoint)
{
static bool suspendTracking = false;
bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
if (!fwLoiterIsActive && suspendTracking) {
suspendTracking = false;
}
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND) || !ARMING_FLAG(ARMED) || suspendTracking) {
return;
}
// Record trackback points based on significant change in course/altitude until
// points limit reached. Overwrite older points from then on.
if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) {
static int32_t previousTBTripDist; // cm
static int16_t previousTBCourse; // degrees
static int16_t previousTBAltitude; // meters
static uint8_t distanceCounter = 0;
bool saveTrackpoint = forceSaveTrackPoint;
bool GPSCourseIsValid = isGPSHeadingValid();
// start recording when some distance from home, 50m seems reasonable.
if (posControl.activeRthTBPointIndex < 0) {
saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
previousTBTripDist = posControl.totalTripDistance;
} else {
// Minimum distance increment between course change track points when GPS course valid - set to 10m
const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(10);
// Altitude change
if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > 10) { // meters
saveTrackpoint = true;
} else if (distanceIncrement && GPSCourseIsValid) {
// Course change - set to 45 degrees
if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) {
saveTrackpoint = true;
} else if (distanceCounter >= 9) {
// Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change
// and deviation from projected course path > 20m
float distToPrevPoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]);
fpVector3_t virtualCoursePoint;
virtualCoursePoint.x = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse));
virtualCoursePoint.y = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse));
saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(20);
}
distanceCounter++;
previousTBTripDist = posControl.totalTripDistance;
} else if (!GPSCourseIsValid) {
// if no reliable course revert to basic distance logging based on direct distance from last point - set to 20m
saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20);
previousTBTripDist = posControl.totalTripDistance;
}
// Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
if (distanceCounter && fwLoiterIsActive) {
saveTrackpoint = suspendTracking = true;
}
}
// when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position
if (saveTrackpoint) {
if (posControl.activeRthTBPointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) { // wraparound to start
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = 0;
} else {
posControl.activeRthTBPointIndex++;
if (posControl.rthTBWrapAroundCounter > -1) { // track wraparound overwrite position after store first filled
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex;
}
}
posControl.rthTBPointsList[posControl.activeRthTBPointIndex] = posControl.actualState.abs.pos;
posControl.rthTBLastSavedIndex = posControl.activeRthTBPointIndex;
previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z);
previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse;
distanceCounter = 0;
}
}
}
static fpVector3_t * rthGetTrackbackPos(void)
{
// ensure trackback altitude never lower than altitude of start point
if (posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z < posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z) {
posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z = posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z;
}
return &posControl.rthTBPointsList[posControl.activeRthTBPointIndex];
}
/*-----------------------------------------------------------
* Update flight statistics
*-----------------------------------------------------------*/
@ -4035,7 +3914,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
}
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos)
{
// Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints)
if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
@ -4135,7 +4014,7 @@ bool isWaypointNavTrackingActive(void)
return FLIGHT_MODE(NAV_WP_MODE)
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|| (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
|| (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex);
}
/*-----------------------------------------------------------
@ -4175,9 +4054,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3);
#if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif
// naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag
if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
// Reset all navigation requests - NAV controllers will set them if necessary
@ -4192,9 +4069,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
// ensure WP missions always restart from first waypoint after disarm
posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback
posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1;
resetRthTrackBack();
return;
}
@ -4646,7 +4521,11 @@ void updateWpMissionPlanner(void)
{
static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
);
posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
@ -4734,7 +4613,7 @@ void updateWaypointsAndNavigationMode(void)
updateWpMissionPlanner();
// Update RTH trackback
updateRthTrackback(false);
rthTrackBackUpdate(false);
//Update Blackbox data
navCurrentState = (int16_t)posControl.navPersistentId;