Improved Code

Cleaned up code removing Climb First flag. Tested in flight, works as expected.

src/main/navigation/navigation.c

@@ -229,7 +229,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void);
 void initializeRTHSanityChecker(const fpVector3_t * pos);
 bool validateRTHSanityChecker(void);
 
-static void rthAltControlStickOverrideCheck(unsigned axis);
+static bool rthAltControlStickOverrideCheck(unsigned axis);
 
 /*************************************************************************************************/
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
@@ -1093,9 +1093,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
 {
     navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
 
-    // reset flag to override RTH climb first
-    posControl.flags.rthClimbFirstOverride = false;
-
     if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) {
         // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
         // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
@@ -1163,7 +1160,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
     UNUSED(previousState);
 
     rthAltControlStickOverrideCheck(PITCH);
-    rthAltControlStickOverrideCheck(ROLL);
 
     if ((posControl.flags.estHeadingStatus == EST_NONE)) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
@@ -1180,7 +1176,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
         const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
 
         // If we reached desired initial RTH altitude or we don't want to climb first
-        if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || posControl.flags.rthClimbFirstOverride) {
+        if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || rthAltControlStickOverrideCheck(ROLL)) {
 
             // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
             if (STATE(FIXED_WING_LEGACY)) {
@@ -2517,12 +2513,12 @@ void updateHomePosition(void)
 
 /* -----------------------------------------------------------
  * Override RTH preset altitude and Climb First option
- * Set using Pitch/Roll stick held for > 2 seconds
+ * using Pitch/Roll stick held for > 2 seconds
  *-----------------------------------------------------------*/
-static void rthAltControlStickOverrideCheck(unsigned axis)
+static bool rthAltControlStickOverrideCheck(unsigned axis)
 {
     if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated) {
-        return;
+        return false;
     }
 
     static timeMs_t rthOverrideStickHoldStartTime[2];
@@ -2534,16 +2530,19 @@ static void rthAltControlStickOverrideCheck(unsigned axis)
         if (!rthOverrideStickHoldStartTime[axis]) {
             rthOverrideStickHoldStartTime[axis] = millis();
         } else if (ABS(2500 - holdTime) < 500) {
-            if (axis == PITCH) {    // pitch down to override preset altitude reset to current altitude
+            if (axis == PITCH) {    // PITCH down to override preset altitude reset to current altitude
                 posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
                 posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;                
-            } else {                // roll right to override climb first
+                return true;
-                posControl.flags.rthClimbFirstOverride = true;
+            } else if (axis == ROLL) {     // ROLL right to override climb first
+                return true;
             }
         }
     } else {
         rthOverrideStickHoldStartTime[axis] = 0;
     }
+
+    return false;
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation.h

@@ -181,7 +181,7 @@ typedef struct navConfig_s {
             uint8_t disarm_on_landing;          //
             uint8_t rth_allow_landing;          // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e.
             uint8_t rth_climb_ignore_emerg;     // Option to ignore GPS loss on initial climb stage of RTH
-            uint8_t rth_alt_control_override;   // Set RTH preset climb altitude to Current altutude during climb using full down pitch stick
+            uint8_t rth_alt_control_override;   // Override RTH Altitude and Climb First settings using Pitch and Roll stick
             uint8_t nav_overrides_motor_stop;   // Autonomous modes override motor_stop setting and user command to stop motor
         } flags;
 

src/main/navigation/navigation_private.h

@@ -87,7 +87,6 @@ typedef struct navigationFlags_s {
     bool isTerrainFollowEnabled;            // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings)
 
     bool forcedRTHActivated;
-    bool rthClimbFirstOverride;             // RTH Climb First override using Roll stick
 } navigationFlags_t;
 
 typedef enum {