diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
index a5e5be3e7c..de0ba91b05 100644
--- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
+++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
@@ -56,9 +56,6 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const
 {
 	Quatf qd = _attitude_setpoint_q;
 
-	// ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
-	qd.normalize();
-
 	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
 	const Vector3f e_z = q.dcm_z();
 	const Vector3f e_z_d = qd.dcm_z();
diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
index 9b55e9e8b9..70980e2b35 100644
--- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
+++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
@@ -75,7 +75,7 @@ public:
 	 * @param qd desired vehicle attitude setpoint
 	 * @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame
 	 */
-	void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _yawspeed_setpoint = yawspeed_setpoint; }
+	void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _attitude_setpoint_q.normalize(); _yawspeed_setpoint = yawspeed_setpoint; }
 
 	/**
 	 * Adjust last known attitude setpoint by a delta rotation