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

Configurable limitation of max climb/descent rate (#257)

Browse source 
* ALTHOLD: Configurable limitation of max climb/descent rate
* ALTHOLD: Ignore the vertical acceleration limit when stopping (abs target vel < abs actual vel)
This commit is contained in:
Konstantin Sharlaimov 2016-07-23 13:25:04 +03:00 • committed by GitHub
parent 19921a0a3c
commit bbf032cd0e
4 changed files with 14 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -269,6 +269,7 @@ void resetNavConfig(navConfig_t * navConfig)
navConfig->pos_failure_timeout = 5; // 5 sec
navConfig->waypoint_radius = 100; // 2m diameter
navConfig->max_speed = 300; // 3 m/s = 10.8 km/h
navConfig->max_climb_rate = 500; // 5 m/s
navConfig->max_manual_speed = 500;
navConfig->max_manual_climb_rate = 200;
navConfig->land_descent_rate = 200; // 2 m/s

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

@ -104,6 +104,7 @@ typedef struct navConfig_s {
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)
uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint16_t max_speed; // autonomous navigation speed cm/sec
uint16_t max_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

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

@ -87,12 +87,18 @@ static void updateAltitudeVelocityController_MC(uint32_t deltaMicros)
float altitudeError = posControl.desiredState.pos.V.Z - posControl.actualState.pos.V.Z;
float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;
// hard limit desired target velocity to +/- 20 m/s
targetVel = constrainf(targetVel, -2000.0f, 2000.0f);
// hard limit desired target velocity to max_climb_rate
targetVel = constrainf(targetVel, -posControl.navConfig->max_climb_rate, posControl.navConfig->max_climb_rate);
// limit max vertical acceleration 250 cm/s/s - reach the max 20 m/s target in 80 seconds
float maxVelDifference = US2S(deltaMicros) * 250.0f;
posControl.desiredState.vel.V.Z = constrainf(targetVel, posControl.desiredState.vel.V.Z - maxVelDifference, posControl.desiredState.vel.V.Z + maxVelDifference);
// limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
// if we are decelerating - don't limit (allow better recovery from falling)
if (ABS(targetVel) > ABS(posControl.desiredState.vel.V.Z)) {
float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS / 5.0f);
posControl.desiredState.vel.V.Z = constrainf(targetVel, posControl.desiredState.vel.V.Z - maxVelDifference, posControl.desiredState.vel.V.Z + maxVelDifference);
}
else {
posControl.desiredState.vel.V.Z = targetVel;
}
#if defined(NAV_BLACKBOX)
navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.V.Z), -32678, 32767);

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

@ -621,6 +621,7 @@ const clivalue_t valueTable[] = {
{ "nav_position_timeout", VAR_UINT8 | MASTER_VALUE, &masterConfig.navConfig.pos_failure_timeout, .config.minmax = { 0, 10 }, 0 },
{ "nav_wp_radius", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.waypoint_radius, .config.minmax = { 10, 10000 }, 0 },
{ "nav_max_speed", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_speed, .config.minmax = { 10, 2000 }, 0 },
{ "nav_max_climb_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.max_climb_rate, .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_landing_speed", VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.land_descent_rate, .config.minmax = { 100, 2000 }, 0 },