Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Configurable descent speed for RTH landing and EMERG landing

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-11 16:27:41 +10:00
parent 1ff38c5dbe
commit 5c045ba898
5 changed files with 13 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -133,7 +133,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 108; static const uint8_t EEPROM_CONF_VERSION = 109;
static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain) static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
{ {
@ -249,6 +249,8 @@ void resetNavConfig(navConfig_t * navConfig)
navConfig->max_speed = 250; navConfig->max_speed = 250;
navConfig->max_manual_speed = 500; navConfig->max_manual_speed = 500;
navConfig->max_manual_climb_rate = 200; navConfig->max_manual_climb_rate = 200;
navConfig->land_descent_rate = 200;
navConfig->emerg_descent_rate = 500; // 5 m/s
navConfig->min_rth_distance = 500; // If closer than 5m - land immediately navConfig->min_rth_distance = 500; // If closer than 5m - land immediately
navConfig->rth_altitude = 1000; // 10m navConfig->rth_altitude = 1000; // 10m
navConfig->pos_hold_deadband = 20; navConfig->pos_hold_deadband = 20;

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

@ -896,13 +896,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
else { else {
// Gradually reduce descent speed depending on actual altitude. // Gradually reduce descent speed depending on actual altitude.
if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 1000.0f)) { if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 1000.0f)) {
updateAltitudeTargetFromClimbRate(-200.0f); updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->land_descent_rate);
} }
else if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 250.0f)) { else if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 250.0f)) {
updateAltitudeTargetFromClimbRate(-100.0f); updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->land_descent_rate);
} }
else { else {
updateAltitudeTargetFromClimbRate(-50.0f); updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate);
} }
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
@ -912,7 +912,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
updateAltitudeTargetFromClimbRate(-50.0f); // FIXME updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate); // FIXME
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
@ -920,7 +920,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigat
{ {
// Stay in this state // Stay in this state
UNUSED(previousState); UNUSED(previousState);
updateAltitudeTargetFromClimbRate(-50.0f); // FIXME updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate); // FIXME
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }

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

@ -100,6 +100,8 @@ typedef struct navConfig_s {
uint16_t max_speed; // autonomous navigation speed cm/sec uint16_t max_speed; // autonomous navigation speed cm/sec
uint16_t max_manual_speed; // manual velocity control max horizontal speed 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 max_manual_climb_rate; // manual velocity control max vertical speed
uint16_t land_descent_rate; // normal RTH landing descent rate
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 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 uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTL in cm, otherwise it will just autoland
uint8_t dterm_cut_hz; // Low pass filter cut frequency for D-term calculation (default 5Hz) uint8_t dterm_cut_hz; // Low pass filter cut frequency for D-term calculation (default 5Hz)

2
src/main/flight/navigation_rewrite_multicopter.c
View file

@ -444,7 +444,7 @@ void applyMulticopterEmergencyLandingController(uint32_t currentTime)
// Check if last correction was too log ago - ignore this update // Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateAltitudeTargetFromClimbRate(-100.0f); updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->emerg_descent_rate);
updateAltitudeVelocityController_MC(); updateAltitudeVelocityController_MC();
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
} }

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

@ -565,6 +565,8 @@ const clivalue_t valueTable[] = {
{ "nav_max_speed", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_speed, .config.minmax = { 10, 2000 }, 0 }, { "nav_max_speed", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_speed, .config.minmax = { 10, 2000 }, 0 },
{ "nav_manual_speed", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_manual_speed, .config.minmax = { 10, 2000 }, 0 }, { "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_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_emerg_landing_speed", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.emerg_descent_rate, .config.minmax = { 100, 2000 }, 0 },
{ "nav_pos_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.pos_hold_deadband, .config.minmax = { 10, 250 }, 0 }, { "nav_pos_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.pos_hold_deadband, .config.minmax = { 10, 250 }, 0 },
{ "nav_alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.alt_hold_deadband, .config.minmax = { 10, 250 }, 0 }, { "nav_alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.alt_hold_deadband, .config.minmax = { 10, 250 }, 0 },
{ "nav_min_rth_distance", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.min_rth_distance, .config.minmax = { 0, 5000 }, 0 }, { "nav_min_rth_distance", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.min_rth_distance, .config.minmax = { 0, 5000 }, 0 },