From 280ad784947f118aa3424bedab65dc926518e0c2 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 21 Jan 2019 22:53:00 +0100 Subject: [PATCH 1/2] [NAV] Restrict MC RoC/RoD rate-limiting to the case where we actually accelerate --- src/main/navigation/navigation_multicopter.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 266782426e..168c516052 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -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 = (targetVel > 0 && posControl.desiredState.vel.z > 0) || (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); } From 3f46bd43239297d9c84b88abfd3386a3a70044ae Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Wed, 23 Jan 2019 19:40:37 +0100 Subject: [PATCH 2/2] Use signbit() --- src/main/navigation/navigation_multicopter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 168c516052..7c2ef07f35 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -76,7 +76,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) // 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) - const bool isSameDirection = (targetVel > 0 && posControl.desiredState.vel.z > 0) || (targetVel < 0 && posControl.desiredState.vel.z < 0); + 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);