Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Merge branch 'smooth-rth-decend' of https://github.com/theArchLadder/inav into theArchLadder-smooth-rth-decend

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-05-04 19:45:00 +10:00
commit bd7ee5ae96
4 changed files with 16 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/config/config.c
View file

@ -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

21
src/main/flight/navigation_rewrite.c
View file

@ -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;

2
src/main/flight/navigation_rewrite.h
View file

@ -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

2
src/main/io/serial_cli.c
View file

@ -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 },