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

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

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

* FC_MSP: add land_minalt_vspd to RTH_AND_LAND_CONFIG

* Update CLI docs

* Fix settings description

* Cosmetic fix

* Homogenize landing settings prefix
This commit is contained in:
Michel Pastor 2021-04-19 23:42:46 +02:00 • committed by GitHub
parent 036e86ac10
commit a008a81a38
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 33 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

7
docs/Settings.md
View file

@ -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_maxalt_vspd | 200 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] |
| nav_land_minalt_vspd | 50 | Vertical descent velocity under nav_land_slowdown_minalt 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_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm] |
| nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] |
| 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 |

3
src/main/cms/cms_menu_navigation.c
View file

@ -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_LAND_MINALT_VSPD),
OSD_SETTING_ENTRY("LAND MAXALT VSPD", SETTING_NAV_LAND_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),

6
src/main/fc/fc_msp.c
View file

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

16
src/main/fc/settings.yaml
View file

@ -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_land_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_land_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_land_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_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm]"
default_value: 2000
field: general.land_slowdown_maxalt
min: 500

21
src/main/navigation/navigation.c
View file

@ -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_LAND_MINALT_VSPD_DEFAULT, // centimeters/s
.land_maxalt_vspd = SETTING_NAV_LAND_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

3
src/main/navigation/navigation.h
View file

@ -208,7 +208,8 @@ 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_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_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