FlightTaskAuto: check yawspeed saturation before constraining

to avoid possible numerical float equality issues.
2020-06-15 09:55:15 +02:00 · 2020-06-15 09:55:15 +02:00 · 1efc8c27eb
parent 12449c23c5
commit 1efc8c27eb
1 changed files with 2 additions and 2 deletions
--- 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);
 	}
 }