Configurable descent speed for RTH landing and EMERG landing

src/main/config/config.c

@@ -133,7 +133,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 108;
+static const uint8_t EEPROM_CONF_VERSION = 109;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
 {
@@ -249,6 +249,8 @@ void resetNavConfig(navConfig_t * navConfig)
     navConfig->max_speed = 250;
     navConfig->max_manual_speed = 500;
     navConfig->max_manual_climb_rate = 200;
+    navConfig->land_descent_rate = 200;
+    navConfig->emerg_descent_rate = 500;    // 5 m/s
     navConfig->min_rth_distance = 500;   // If closer than 5m - land immediately
     navConfig->rth_altitude = 1000;      // 10m
     navConfig->pos_hold_deadband = 20;

src/main/flight/navigation_rewrite.c

@@ -896,13 +896,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
     else {
         // Gradually reduce descent speed depending on actual altitude.
         if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 1000.0f)) {
-            updateAltitudeTargetFromClimbRate(-200.0f);
+            updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->land_descent_rate);
         }
         else if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 250.0f)) {
-            updateAltitudeTargetFromClimbRate(-100.0f);
+            updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->land_descent_rate);
         }
         else {
-            updateAltitudeTargetFromClimbRate(-50.0f);
+            updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate);
         }
 
         return NAV_FSM_EVENT_NONE;
@@ -912,7 +912,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
-    updateAltitudeTargetFromClimbRate(-50.0f);  // FIXME
+    updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate);  // FIXME
     return NAV_FSM_EVENT_SUCCESS;
 }
 
@@ -920,7 +920,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigat
 {
     // Stay in this state
     UNUSED(previousState);
-    updateAltitudeTargetFromClimbRate(-50.0f);  // FIXME
+    updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate);  // FIXME
     return NAV_FSM_EVENT_NONE;
 }
 

src/main/flight/navigation_rewrite.h

@@ -100,6 +100,8 @@ typedef struct navConfig_s {
     uint16_t max_speed;                     // autonomous navigation speed cm/sec
     uint16_t max_manual_speed;              // manual velocity control max horizontal speed
     uint16_t max_manual_climb_rate;         // manual velocity control max vertical speed
+    uint16_t land_descent_rate;             // normal RTH landing descent rate
+    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_style) (cm)
     uint16_t min_rth_distance;              // 0 Disables. Minimal distance for RTL in cm, otherwise it will just autoland
     uint8_t  dterm_cut_hz;                  // Low pass filter cut frequency for D-term calculation (default 5Hz)

src/main/flight/navigation_rewrite_multicopter.c

@@ -444,7 +444,7 @@ void applyMulticopterEmergencyLandingController(uint32_t currentTime)
 
             // Check if last correction was too log ago - ignore this update
             if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
-                updateAltitudeTargetFromClimbRate(-100.0f);
+                updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->emerg_descent_rate);
                 updateAltitudeVelocityController_MC();
                 updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
             }

src/main/io/serial_cli.c

@@ -565,6 +565,8 @@ const clivalue_t valueTable[] = {
     { "nav_max_speed",              VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_speed, .config.minmax = { 10,  2000 }, 0 },
     { "nav_manual_speed",           VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_manual_speed, .config.minmax = { 10,  2000 }, 0 },
     { "nav_manual_climb_rate",      VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_manual_climb_rate, .config.minmax = { 10,  2000 }, 0 },
+    { "nav_landing_speed",          VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.land_descent_rate, .config.minmax = { 100,  2000 }, 0 },
+    { "nav_emerg_landing_speed",    VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.emerg_descent_rate, .config.minmax = { 100,  2000 }, 0 },
     { "nav_pos_hold_deadband",      VAR_UINT8  | MASTER_VALUE, &masterConfig.navConfig.pos_hold_deadband, .config.minmax = { 10,  250 }, 0 },
     { "nav_alt_hold_deadband",      VAR_UINT8  | MASTER_VALUE, &masterConfig.navConfig.alt_hold_deadband, .config.minmax = { 10,  250 }, 0 },
     { "nav_min_rth_distance",       VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.min_rth_distance, .config.minmax = { 0,  5000 }, 0 },