diff --git a/src/main/config/config.c b/src/main/config/config.c index ed48069a65..5aaa90abbf 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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 diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index b88f5fea0f..64d6ed5ede 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/src/main/flight/navigation_rewrite.h @@ -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 diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index 2b0f5ffa31..bef93e0a1d 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/src/main/flight/navigation_rewrite_multicopter.c @@ -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); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 61fb1b25ce..775d53e9b9 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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 },