mc_att_control: adapt setpoint on estimation reset

There was a gap where the attitude controller already used the estimate
with a new reference but the last known attitude setpoint was still based
on the old reference. This leads to a big glitch on reset because until
the attitude setpoint gets updated the error is wrong and as large as the
attitude delta of the reset.

src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp

@@ -77,6 +77,13 @@ public:
 	 */
 	void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _yawspeed_setpoint = yawspeed_setpoint; }
 
+	/**
+	 * Adjust last known attitude setpoint by a delta rotation
+	 * Optional use to avoid glitches when attitude estimate reference e.g. heading changes.
+	 * @param q_delta delta rotation to apply
+	 */
+	void adaptAttitudeSetpoint(const matrix::Quatf &q_delta) { _attitude_setpoint_q = q_delta * _attitude_setpoint_q; }
+
 	/**
 	 * Run one control loop cycle calculation
 	 * @param q estimation of the current vehicle attitude unit quaternion