From 1efc8c27ebfc8c8414153746c99ea7e118c4e9ca Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 15 Jun 2020 09:55:15 +0200
Subject: [PATCH] FlightTaskAuto: check yawspeed saturation before constraining
to avoid possible numerical float equality issues.
---
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
index 56f23989a1..7b3f026d1a 100644
--- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
@@ -117,10 +117,10 @@ void FlightTaskAuto::_limitYawRate()
}
if (PX4_ISFINITE(_yawspeed_setpoint)) {
- _yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
-
// The yaw setpoint is aligned when its rate is not saturated
_yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max);
+
+ _yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
}
}