Merge pull request #10042 from breadoven/abo_trackback_logic_fix

RTH trackback code cleanup

Author: breadoven

src/main/navigation/navigation.c

@@ -301,10 +301,6 @@ bool validateRTHSanityChecker(void);
 void updateHomePosition(void);
 bool abortLaunchAllowed(void);
 
-// static bool rthAltControlStickOverrideCheck(unsigned axis);
-// static void updateRthTrackback(bool forceSaveTrackPoint);
-// static fpVector3_t * rthGetTrackbackPos(void);
-
 #ifdef USE_FW_AUTOLAND
 static float getLandAltitude(void);
 static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
@@ -1449,7 +1445,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
         }
         else {
             // Switch to RTH trackback
-            if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
+            if (rthTrackBackCanBeActivated() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
                 rthTrackBackUpdate(true);  // save final trackpoint for altitude and max trackback distance reference
                 posControl.flags.rthTrackbackActive = true;
                 calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
@@ -1568,7 +1564,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
-    if (rthTrackBackSetNewPosition()) {
+    if (!rthTrackBackSetNewPosition()) {
         return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
     }
 

src/main/navigation/rth_trackback.c

@@ -37,9 +37,10 @@
 
 rth_trackback_t rth_trackback;
 
-bool rthTrackBackIsActive(void) 
+bool rthTrackBackCanBeActivated(void)
 {
-    return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||  (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
+    return posControl.flags.estPosStatus >= EST_USABLE &&
+           (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated));
 }
 
 void rthTrackBackUpdate(bool forceSaveTrackPoint)
@@ -127,7 +128,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint)
 bool rthTrackBackSetNewPosition(void)
 {
     if (posControl.flags.estPosStatus == EST_NONE) {
-        return false;
+        return false;   // will fall back to RTH initialize allowing full RTH to handle position loss correctly
     }
 
     const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex]));
@@ -142,7 +143,7 @@ bool rthTrackBackSetNewPosition(void)
     if (rth_trackback.activePointIndex < 0 || cancelTrackback) {
         rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
         posControl.flags.rthTrackbackActive = false;
-        return true;    // Procede to home after final trackback point
+        return false;    // No more trackback points to set, procede to home
     }
 
     if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
@@ -161,7 +162,7 @@ bool rthTrackBackSetNewPosition(void)
         setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
     }
 
-    return false;
+    return true;
 }
 
 fpVector3_t *getRthTrackBackPosition(void)
@@ -174,9 +175,9 @@ fpVector3_t *getRthTrackBackPosition(void)
     return &rth_trackback.pointsList[rth_trackback.activePointIndex];
 }
 
-void resetRthTrackBack(void) 
+void resetRthTrackBack(void)
 {
     rth_trackback.activePointIndex = -1;
     posControl.flags.rthTrackbackActive = false;
-    rth_trackback.WrapAroundCounter = -1;   
+    rth_trackback.WrapAroundCounter = -1;
 }

src/main/navigation/rth_trackback.h

@@ -35,7 +35,7 @@ typedef struct
 
 extern rth_trackback_t rth_trackback;
 
-bool rthTrackBackIsActive(void);
+bool rthTrackBackCanBeActivated(void);
 bool rthTrackBackSetNewPosition(void);
 void rthTrackBackUpdate(bool forceSaveTrackPoint);
 fpVector3_t *getRthTrackBackPosition(void);