Allow setting an altitude limit for climb first

This feature adds two CLI commands:
- nav_rth_climb_first_stage_mode
- nav_rth_climb_first_stage_altitude

nav_rth_climb_first_stage_altitude sets an altitude to transition from climb first to turn first. 0 is disabled.

nav_rth_climb_first_stage_mode determines how nav_rth_climb_first_stage_altitude is used. Currently either as an at least altitude or adding to the current altitude.

Initial coding.

src/main/fc/settings.yaml

@@ -63,6 +63,8 @@ tables:
     values: ["ATTI", "CRUISE"]
   - name: nav_rth_alt_mode
     values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"]
+  - name: nav_rth_climb_first_stage_modes
+    values: ["AT_LEAST", "EXTRA"]
   - name: osd_unit
     values: ["IMPERIAL", "METRIC", "METRIC_MPH", "UK"]
     enum: osd_unit_e
@@ -2542,6 +2544,16 @@ groups:
         default_value: "ON"
         field: general.flags.rth_climb_first
         table: nav_rth_climb_first
+      - name: nav_rth_climb_first_stage_mode
+        description: "This determines how rth_climb_first_stage_altitude is used. Default is AT_LEAST."
+        default_value: "AT_LEAST"
+        field: general.flags.rth_climb_first_stage_mode
+        table: nav_rth_climb_first_stage_modes
+      - name: nav_rth_climb_first_stage_altitude
+        description: "The altitude [cm] at which climb first will transition to turn first. How the altitude is used is determined byt nav_rth_climb_first_stage_mode. Default=0; feature disabled."
+        default_value: 0
+        field: general.rth_climb_first_stage_altitude
+        max: 65000
       - name: nav_rth_climb_ignore_emerg
         description: "If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status."
         default_value: OFF

src/main/navigation/navigation.c

@@ -107,6 +107,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
             .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT,
             .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT,
             .rth_climb_first = SETTING_NAV_RTH_CLIMB_FIRST_DEFAULT,                     // Climb first, turn after reaching safe altitude
+            .rth_climb_first_stage_mode = SETTING_NAV_CLIMB_FIRST_STAGE_TYPE_DEFAULT,   // To determine how rth_climb_first_stage_altitude is used
             .rth_climb_ignore_emerg = SETTING_NAV_RTH_CLIMB_IGNORE_EMERG_DEFAULT,       // Ignore GPS loss on initial climb
             .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT,
             .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT,
@@ -133,6 +134,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT,                               // centimeters, if closer than this land immediately
         .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT,                                       // altitude in centimeters
         .rth_home_altitude = SETTING_NAV_RTH_HOME_ALTITUDE_DEFAULT,                             // altitude in centimeters
+        .rth_climb_first_stage_altitude = SETTING_NAV_RTH_CLIMB_FIRST_STAGE_ALTITUDE_DEFAULT    // altitude in centimetres, 0= off
         .rth_abort_threshold = SETTING_NAV_RTH_ABORT_THRESHOLD_DEFAULT,                         // centimeters - 500m should be safe for all aircraft
         .max_terrain_follow_altitude = SETTING_NAV_MAX_TERRAIN_FOLLOW_ALT_DEFAULT,              // max altitude in centimeters in terrain following mode
         .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT,                         // Max distance that a safehome is from the arming point
@@ -1193,7 +1195,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 == OFF) || rthAltControlStickOverrideCheck(ROLL)) {
+        if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL) || rthClimbStageActiveAndComplete()) {
 
             // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
             if (STATE(FIXED_WING_LEGACY)) {
@@ -2452,6 +2454,30 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
     return false;
 }
 
+/* ---------------------------------------------------
+ * If climb stage is being used, see if it is time to 
+ * transiton in to turn.
+ * Limited to fixed wing only.
+ * --------------------------------------------------- */
+ bool rthClimbStageActiveAndComplete() {
+    if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (general.rth_climb_first_stage_altitude > 0)) {
+        switch (navConfig()->general.flags.rth_climb_first_stage_mode) {
+            case NAV_RTH_CLIMB_STAGE_AT_LEAST:
+                if (posControl.actualState.abs.pos.z >= general.rth_climb_first_stage_altitude) {
+                    return true;
+                }
+                break;
+            case NAV_RTH_CLIMB_STAGE_EXTRA:
+                if (posControl.actualState.abs.pos.z >= (posControl.rthState.rthInitialAltitude + general.rth_climb_first_stage_altitude)) {
+                    return true;
+                }
+                break; 
+        }
+    }
+
+    return false;
+ }
+
 /*-----------------------------------------------------------
  * Update flight statistics
  *-----------------------------------------------------------*/

src/main/navigation/navigation.h

@@ -100,6 +100,11 @@ enum {
                                               // descend linearly to reach home at predefined altitude if above it
 };
 
+enum {
+    NAV_RTH_CLIMB_STAGE_AT_LEAST        = 0, // Will climb to at least the specified altitude before turning
+    NAV_RTH_CLIMB_STAGE_EXTRA           = 1, // Will climb the specified altitude before turning
+};
+
 enum {
     NAV_HEADING_CONTROL_NONE = 0,
     NAV_HEADING_CONTROL_AUTO,
@@ -192,6 +197,7 @@ typedef struct navConfig_s {
             uint8_t user_control_mode;          // NAV_GPS_ATTI or NAV_GPS_CRUISE
             uint8_t rth_alt_control_mode;       // Controls the logic for choosing the RTH altitude
             uint8_t rth_climb_first;            // Controls the logic for initial RTH climbout
+            uint8_t rth_climb_first_stage_mode  // To determine how rth_climb_first_stage_altitude is used
             uint8_t rth_tail_first;             // Return to home tail first
             uint8_t disarm_on_landing;          //
             uint8_t rth_allow_landing;          // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e.
@@ -216,6 +222,7 @@ typedef struct navConfig_s {
         uint16_t emerg_descent_rate;            // emergency landing descent rate
         uint16_t rth_altitude;                  // altitude to maintain when RTH is active (depends on rth_alt_control_mode) (cm)
         uint16_t rth_home_altitude;             // altitude to go to during RTH after the craft reached home (cm)
+        uint16_t rth_climb_first_stage_altitude // Altitude to reach before transitioning from climb first to turn first    
         uint16_t min_rth_distance;              // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland
         uint16_t rth_abort_threshold;           // Initiate emergency landing if during RTH we get this much [cm] away from home
         uint16_t max_terrain_follow_altitude;   // Max altitude to be used in SURFACE TRACKING mode
@@ -528,6 +535,8 @@ bool navigationIsControllingAltitude(void);
 bool navigationRTHAllowsLanding(void);
 bool isWaypointMissionRTHActive(void);
 
+bool rthClimbStageActiveAndComplete(void);
+
 bool isNavLaunchEnabled(void);
 bool isFixedWingLaunchDetected(void);
 bool isFixedWingLaunchFinishedOrAborted(void);