From 888155b2caad9b56c6dd9d674834bbf8b39de87e Mon Sep 17 00:00:00 2001 From: theArchLadder Date: Tue, 3 May 2016 13:29:32 +0200 Subject: [PATCH 1/4] RTH: Reduce decend vel with smooth ramp instead of steps --- src/main/flight/navigation_rewrite.c | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index c4c136bd8c..21d48d8b1d 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,10 @@ 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 1500cm altitude to 25% at 500cm altitude. 25% from 500cm to 0cm as safety margin for baro drift. + float decendVelScaling = (posControl.actualState.pos.V.Z - (posControl.homePosition.pos.V.Z + 500.0f)) / 1000.0f; + decendVelScaling = constrainf(decendVelScaling, 0.25f, 1.0f); + updateAltitudeTargetFromClimbRate(-decendVelScaling * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } return NAV_FSM_EVENT_NONE; From 5fd414660ec6234dd8312e8d32d3f3f7dd34180b Mon Sep 17 00:00:00 2001 From: theArchLadder Date: Tue, 3 May 2016 14:11:59 +0200 Subject: [PATCH 2/4] RTH: Fixed and simplified decend vel ramp --- src/main/flight/navigation_rewrite.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 21d48d8b1d..e31ac458c5 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -951,8 +951,9 @@ 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 1500cm altitude to 25% at 500cm altitude. 25% from 500cm to 0cm as safety margin for baro drift. - float decendVelScaling = (posControl.actualState.pos.V.Z - (posControl.homePosition.pos.V.Z + 500.0f)) / 1000.0f; + // Ramp down decend velocity from 100% at 2000cm altitude to 25% from 500cm to 0cm. + // TODO: make the number 2000.0f a setting, call it something like rth_decendvel_rampheight. + float decendVelScaling = (posControl.actualState.pos.V.Z - posControl.homePosition.pos.V.Z) / 2000.0f; decendVelScaling = constrainf(decendVelScaling, 0.25f, 1.0f); updateAltitudeTargetFromClimbRate(-decendVelScaling * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } From 9c59b5193f301fc7c83cfffd87050854d9419f88 Mon Sep 17 00:00:00 2001 From: theArchLadder Date: Wed, 4 May 2016 10:23:01 +0200 Subject: [PATCH 3/4] Improved scaling formula for RTH descend vel ramp --- src/main/flight/navigation_rewrite.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index e31ac458c5..476325a316 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -952,8 +952,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati } else { // Ramp down decend velocity from 100% at 2000cm altitude to 25% from 500cm to 0cm. - // TODO: make the number 2000.0f a setting, call it something like rth_decendvel_rampheight. - float decendVelScaling = (posControl.actualState.pos.V.Z - posControl.homePosition.pos.V.Z) / 2000.0f; + // 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 decendVelScaling = constrainf(decendVelScaling, 0.25f, 1.0f); updateAltitudeTargetFromClimbRate(-decendVelScaling * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } From c2b38ba6ce2d1bf7a90d77f65a2f55130b44357c Mon Sep 17 00:00:00 2001 From: theArchLadder Date: Wed, 4 May 2016 11:01:47 +0200 Subject: [PATCH 4/4] 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 },