Copter: add new setting for final landing vspeed instead of being hardcoded to 25% of set landing vspeed

docs/Settings.md

@@ -318,9 +318,10 @@
 | nav_fw_pos_z_i | 5 | I gain of altitude PID controller (Fixedwing) |
 | nav_fw_pos_z_p | 40 | P gain of altitude PID controller (Fixedwing) |
 | nav_fw_yaw_deadband | 0 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course |
-| nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm] |
-| nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm] |
-| nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] |
+| nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from `nav_landing_maxalt_vspd` to `nav_landing_minalt_vspd`. [cm] |
+| nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be `nav_landing_minalt_vspd` [cm] |
+| nav_landing_maxalt_vspd | 200 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] |
+| nav_landing_minalt_vspd | 50 | Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] |
 | nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] |
 | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
 | nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode |

src/main/cms/cms_menu_navigation.c

@@ -75,7 +75,8 @@ static const CMS_Menu cmsx_menuNavSettings = {
     OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST),
     OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST),
     OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING),
-    OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED),
+    OSD_SETTING_ENTRY("LAND MINALT VSPD", SETTING_NAV_LANDING_MINALT_VSPD),
+    OSD_SETTING_ENTRY("LAND MAXALT VSPD", SETTING_NAV_LANDING_MAXALT_VSPD),
     OSD_SETTING_ENTRY("LAND SPEED MIN AT", SETTING_NAV_LAND_SLOWDOWN_MINALT),
     OSD_SETTING_ENTRY("LAND SPEED SLOW AT", SETTING_NAV_LAND_SLOWDOWN_MAXALT),
     OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE),

src/main/fc/fc_msp.c

@@ -1334,7 +1334,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, navConfig()->general.flags.rth_alt_control_mode);
         sbufWriteU16(dst, navConfig()->general.rth_abort_threshold);
         sbufWriteU16(dst, navConfig()->general.rth_altitude);
-        sbufWriteU16(dst, navConfig()->general.land_descent_rate);
+        sbufWriteU16(dst, navConfig()->general.land_minalt_vspd);
+        sbufWriteU16(dst, navConfig()->general.land_maxalt_vspd);
         sbufWriteU16(dst, navConfig()->general.land_slowdown_minalt);
         sbufWriteU16(dst, navConfig()->general.land_slowdown_maxalt);
         sbufWriteU16(dst, navConfig()->general.emerg_descent_rate);
@@ -2307,7 +2308,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             navConfigMutable()->general.flags.rth_alt_control_mode = sbufReadU8(src);
             navConfigMutable()->general.rth_abort_threshold = sbufReadU16(src);
             navConfigMutable()->general.rth_altitude = sbufReadU16(src);
-            navConfigMutable()->general.land_descent_rate = sbufReadU16(src);
+            navConfigMutable()->general.land_minalt_vspd = sbufReadU16(src);
+            navConfigMutable()->general.land_maxalt_vspd = sbufReadU16(src);
             navConfigMutable()->general.land_slowdown_minalt = sbufReadU16(src);
             navConfigMutable()->general.land_slowdown_maxalt = sbufReadU16(src);
             navConfigMutable()->general.emerg_descent_rate = sbufReadU16(src);

src/main/fc/settings.yaml

@@ -2278,20 +2278,26 @@ groups:
         field: general.max_manual_climb_rate
         min: 10
         max: 2000
-      - name: nav_landing_speed
-        description: "Vertical descent velocity during the RTH landing phase. [cm/s]"
+      - name: nav_landing_minalt_vspd
+        description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]"
+        default_value: 50
+        field: general.land_minalt_vspd
+        min: 50
+        max: 500
+      - name: nav_landing_maxalt_vspd
+        description: "Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s]"
         default_value: 200
-        field: general.land_descent_rate
+        field: general.land_maxalt_vspd
         min: 100
         max: 2000
       - name: nav_land_slowdown_minalt
-        description: "Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm]"
+        description: "Defines at what altitude the descent velocity should start to be `nav_landing_minalt_vspd` [cm]"
         default_value: 500
         field: general.land_slowdown_minalt
         min: 50
         max: 1000
       - name: nav_land_slowdown_maxalt
-        description: "Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm]"
+        description: "Defines at what altitude the descent velocity should start to ramp down from `nav_landing_maxalt_vspd` to `nav_landing_minalt_vspd`. [cm]"
         default_value: 2000
         field: general.land_slowdown_maxalt
         min: 500

src/main/navigation/navigation.c

@@ -96,7 +96,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 11);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -124,9 +124,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT,                   // 5 m/s
         .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
         .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
-        .land_descent_rate = SETTING_NAV_LANDING_SPEED_DEFAULT,                       // centimeters/s
         .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT,             // altitude in centimeters
         .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT,             // altitude in meters
+        .land_minalt_vspd = SETTING_NAV_LANDING_MINALT_VSPD_DEFAULT,                  // centimeters/s
+        .land_maxalt_vspd = SETTING_NAV_LANDING_MAXALT_VSPD_DEFAULT,                  // centimeters/s
         .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT,                // centimeters/s
         .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
@@ -1413,22 +1414,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
         if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
             // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
             // Do not allow descent velocity slower than -30cm/s so the landing detector works.
-            descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
+            descentVelLimited = navConfig()->general.land_minalt_vspd;
         }
         else {
-            fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND);
 
             // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
-            float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt)
-                / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f;  // Yield 1.0 at 2000 alt and 0.25 at 500 alt
+            float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
+                                                    navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
+                                                    navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
 
-            descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
+            descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
 
-            // Do not allow descent velocity slower than -50cm/s so the landing detector works.
-            descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
         }
 
-        updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
+        updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
 
         return NAV_FSM_EVENT_NONE;
     }
@@ -1452,7 +1451,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
     UNUSED(previousState);
 
     if (STATE(ALTITUDE_CONTROL)) {
-        updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL);  // FIXME
+        updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL);  // FIXME
     }
 
     // Prevent I-terms growing when already landed

src/main/navigation/navigation.h

@@ -208,8 +208,9 @@ typedef struct navConfig_s {
         uint16_t max_auto_climb_rate;           // max vertical speed limitation 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 land_slowdown_minalt;          // Altitude to stop lowering descent rate during RTH descend
+        uint16_t land_minalt_vspd;              // Final RTH landing descent rate under minalt
+        uint16_t land_maxalt_vspd;              // RTH landing descent rate target at maxalt
         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)