From c2b38ba6ce2d1bf7a90d77f65a2f55130b44357c Mon Sep 17 00:00:00 2001 From: theArchLadder Date: Wed, 4 May 2016 11:01:47 +0200 Subject: [PATCH] Added settings for nav_land_slowdown_minalt and maxalt --- src/main/config/config.c | 2 ++ src/main/flight/navigation_rewrite.c | 10 +++++++--- src/main/flight/navigation_rewrite.h | 2 ++ src/main/io/serial_cli.c | 2 ++ 4 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0d4abc11d3..7ed37d30ce 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 476325a316..1fecb68c79 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -951,9 +951,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati updateAltitudeTargetFromClimbRate(-0.15f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } else { - // Ramp down decend velocity from 100% at 2000cm altitude to 25% from 500cm to 0cm. - // TODO: make the number 500 and 2000 settings called nav_land_slowdown_minalt and nav_land_slowdown_maxalt. - float decendVelScaling = (posControl.actualState.pos.V.Z - posControl.homePosition.pos.V.Z - 500.0f) / (2000.0f - 500.0f) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt + // 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); } diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index 215732d740..46453df2a6 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 786c6410f9..e6b028294e 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 },