diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
index de0ba91b05..859b86316f 100644
--- 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++) {