From c9779c2dee475b3627d8cfa469963e2e27a5ea1c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 14 May 2024 11:42:13 +0100
Subject: [PATCH] rth trackback tweaks

---
 src/main/navigation/navigation.c    |  8 ++------
 src/main/navigation/rth_trackback.c | 15 ++++++++-------
 src/main/navigation/rth_trackback.h |  2 +-
 3 files changed, 11 insertions(+), 14 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index c776ce13f54..cc4e269ef4b 100644
--- a/src/main/navigation/navigation.c
+++ b/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;
     }
 
diff --git a/src/main/navigation/rth_trackback.c b/src/main/navigation/rth_trackback.c
index 37467d9b385..dfdcf801f7c 100644
--- a/src/main/navigation/rth_trackback.c
+++ b/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;
 }
\ No newline at end of file
diff --git a/src/main/navigation/rth_trackback.h b/src/main/navigation/rth_trackback.h
index e3c71d0d45e..f3337bb4b79 100644
--- a/src/main/navigation/rth_trackback.h
+++ b/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);