Handle invalid yaw rate setpoints

PX4 · Mar 13, 2021 · 6b4ccaa · 6b4ccaa
1 parent 06e3d38
commit 6b4ccaa
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
@@ -97,7 +97,9 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
 	// and multiply it by the yaw setpoint rate (yawspeed_setpoint).
 	// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
 	// such that it can be added to the rates setpoint.
-	rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
+	if (is_finite(_yawspeed_setpoint)) {
+		rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
+	}
 
 	// limit rates
 	for (int i = 0; i < 3; i++) {