diff --git a/src/main/config/config.c b/src/main/config/config.c index b6ad7d8140..b02224226c 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -231,6 +231,8 @@ void resetNavConfig(navConfig_t * navConfig) navConfig->max_manual_speed = 500; navConfig->max_manual_climb_rate = 200; navConfig->land_descent_rate = 200; // 2 m/s + navConfig->land_slowdown_minalt = 500; // 5 meters of altitude + navConfig->land_slowdown_maxalt = 2000; // 20 meters of altitude navConfig->emerg_descent_rate = 500; // 5 m/s navConfig->min_rth_distance = 500; // If closer than 5m - land immediately navConfig->rth_altitude = 1000; // 10m diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index c4c136bd8c..1fecb68c79 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -689,7 +689,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState) { UNUSED(previousState); - + // If we enable terrain mode and surface offset is not set yet - do it if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) { setDesiredSurfaceOffset(posControl.actualState.surface); @@ -951,16 +951,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati updateAltitudeTargetFromClimbRate(-0.15f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } else { - // Gradually reduce descent speed depending on actual altitude. - if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 1500.0f)) { - updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); - } - else if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 500.0f)) { - updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); - } - else { - updateAltitudeTargetFromClimbRate(-0.25f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); - } + // Ramp down decend velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. + int minAlt = posControl.navConfig->land_slowdown_minalt; + int maxAlt = posControl.navConfig->land_slowdown_maxalt; + + if (minAlt > (maxAlt - 100)) minAlt = maxAlt - 100; // Make sure minAlt is not more than maxAlt, maxAlt cannot be set lower than 500. + + float decendVelScaling = (posControl.actualState.pos.V.Z - posControl.homePosition.pos.V.Z - minAlt) / (maxAlt - minAlt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt + decendVelScaling = constrainf(decendVelScaling, 0.25f, 1.0f); + updateAltitudeTargetFromClimbRate(-decendVelScaling * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } return NAV_FSM_EVENT_NONE; diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index 64e361b564..e02937a234 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/src/main/flight/navigation_rewrite.h @@ -107,6 +107,8 @@ typedef struct navConfig_s { 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_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_style) (cm) uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTL in cm, otherwise it will just autoland diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b1a32c0cc6..cc0385e3d0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -596,6 +596,8 @@ const clivalue_t valueTable[] = { { "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_land_slowdown_minalt", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.land_slowdown_minalt, .config.minmax = { 50, 1000 }, 0 }, + { "nav_land_slowdown_maxalt", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.land_slowdown_maxalt, .config.minmax = { 500, 4000 }, 0 }, { "nav_emerg_landing_speed", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.emerg_descent_rate, .config.minmax = { 100, 2000 }, 0 }, { "nav_min_rth_distance", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.min_rth_distance, .config.minmax = { 0, 5000 }, 0 }, { "nav_rth_tail_first", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.rth_tail_first, .config.lookup = { TABLE_OFF_ON }, 0 },