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 pull request #4245 from iNavFlight/de_fix_roc_rod_rate_limiting

Browse source 
[NAV] Restrict MC RoC/RoD rate-limiting to the case where we actually accelerate
This commit is contained in:
Konstantin Sharlaimov 2019-01-25 13:22:46 +01:00 • committed by GitHub
commit da94e5efbe
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 3 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/navigation/navigation_multicopter.c
View file

@ -74,9 +74,10 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
posControl.pids.pos[Z].output_constrained = targetVel;
// limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
// limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing RoC or RoD (only if vel is of the same sign)
// if we are decelerating - don't limit (allow better recovery from falling)
if (fabsf(targetVel) > fabsf(posControl.desiredState.vel.z)) {
const bool isSameDirection = (signbit(targetVel) == signbit(posControl.desiredState.vel.z)) && (targetVel != 0) && (posControl.desiredState.vel.z != 0);
if (isSameDirection && (fabsf(targetVel) > fabsf(posControl.desiredState.vel.z))) {
const float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS / 5.0f);
posControl.desiredState.vel.z = constrainf(targetVel, posControl.desiredState.vel.z - maxVelDifference, posControl.desiredState.vel.z + maxVelDifference);
}