diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 6ba97158de..1c9f634084 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -133,6 +133,9 @@ void PositionControl::update(const float dt)
 
 	_positionControl();
 	_velocityControl(dt);
+
+	_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
+	_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
 }
 
 bool PositionControl::_interfaceMapping()