AttitudeControl: only normalize attitude setpoint once on update

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();

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