Implement the nav_rth_home_altitude setting

The aircraft will go to set altitude after reaching home

docs/Cli.md

@@ -168,6 +168,7 @@ Re-apply any new defaults as desired.
 |  nav_rth_climb_ignore_emerg  | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. |
 |  nav_rth_alt_mode  | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details |
 |  nav_rth_altitude  | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) |
+|  nav_rth_home_altitude  | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] |
 |  nav_rth_abort_threshold  | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 |  nav_mc_bank_angle  | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
 |  nav_mc_hover_thr  | 1500 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. |

src/main/fc/settings.yaml

@@ -1281,6 +1281,9 @@ groups:
       - name: nav_rth_altitude
         field: general.rth_altitude
         max: 65000
+      - name: nav_rth_home_altitude
+        field: general.rth_home_altitude
+        max: 65000
 
       - name: nav_mc_bank_angle
         field: mc.max_bank_angle

src/main/navigation/navigation.c

@@ -108,6 +108,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .emerg_descent_rate = 500,    // 5 m/s
         .min_rth_distance = 500,      // If closer than 5m - land immediately
         .rth_altitude = 1000,         // 10m
+        .rth_home_altitude = 0,
         .rth_abort_threshold = 50000, // 500m - should be safe for all aircraft
         .max_terrain_follow_altitude = 100,     // max 1m altitude in terrain following mode
     },
@@ -1182,6 +1183,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
 
     // If position ok OR within valid timeout - continue
     if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
+
+        // set altitude to go to when landing is not allowed
+        if (navConfig()->general.rth_home_altitude && !navigationRTHAllowsLanding()) {
+            posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude;
+            setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
+        }
+
         // Wait until target heading is reached (with 15 deg margin for error)
         if (STATE(FIXED_WING)) {
             resetLandingDetector();
@@ -1201,8 +1209,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
                 return NAV_FSM_EVENT_NONE;
             }
         }
-    }
-    else {
+
+    } else {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 }

src/main/navigation/navigation.h

@@ -140,6 +140,7 @@ typedef struct navConfig_s {
         uint16_t land_slowdown_maxalt;          // Altitude to start lowering descent rate during RTH descend
         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 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