Remove euler angles from attitude setpoint (#23482)

* Remove euler angles from attitude setpoint message

* Remove usage of euler angles in attitude setpoint messages

This commit removes the usage of euler angles in the vehicle_attitude_setpoint messages

* Fix standard vtol

src/modules/vtol_att_control/standard.cpp

@@ -172,6 +172,11 @@ void Standard::update_transition_state()
 
 	VtolType::update_transition_state();
 
+	const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
+	float roll_body = attitude_setpoint_euler.phi();
+	float pitch_body = attitude_setpoint_euler.theta();
+	float yaw_body = attitude_setpoint_euler.psi();
+
 	// we get attitude setpoint from a multirotor flighttask if climbrate is controlled.
 	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
 	if (_v_control_mode->flag_control_climb_rate_enabled) {
@@ -181,7 +186,7 @@ void Standard::update_transition_state()
 		}
 
 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
-		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+		roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
 
 	} else {
 		// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
@@ -227,21 +232,19 @@ void Standard::update_transition_state()
 		}
 
 		// ramp up FW_PSP_OFF
-		_v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);
-
+		pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);
 		_v_att_sp->thrust_body[0] = _pusher_throttle;
-
-		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 
 	} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
 			// control backtransition deceleration using pitch.
-			_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
+			pitch_body = update_and_get_backtransition_pitch_sp();
 		}
 
-		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+		const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 
 		_pusher_throttle = 0.0f;

src/modules/vtol_att_control/tailsitter.cpp

@@ -179,7 +179,8 @@ void Tailsitter::update_transition_state()
 			// the yaw setpoint and zero roll since we want wings level transition.
 			// If for some reason the fw attitude setpoint is not recent then don't use it and assume 0 pitch
 			if (_fw_virtual_att_sp->timestamp > (now - 1_s)) {
-				_q_trans_start = Eulerf(0.f, _fw_virtual_att_sp->pitch_body, yaw_sp);
+				const float pitch_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).theta();
+				_q_trans_start = Eulerf(0.f, pitch_body, yaw_sp);
 
 			} else {
 				_q_trans_start = Eulerf(0.f, 0.f, yaw_sp);
@@ -191,7 +192,8 @@ void Tailsitter::update_transition_state()
 
 		} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 			// initial attitude setpoint for the transition should be with wings level
-			_q_trans_start = Eulerf(0.f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
+			const Eulerf setpoint_euler(Quatf(_mc_virtual_att_sp->q_d));
+			_q_trans_start = Eulerf(0.f, setpoint_euler.theta(), setpoint_euler.psi());
 			Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1.f, 0.f, 0.f);
 			_trans_rot_axis = -x.cross(Vector3f(0.f, 0.f, -1.f));
 		}
@@ -238,10 +240,6 @@ void Tailsitter::update_transition_state()
 	_v_att_sp->timestamp = hrt_absolute_time();
 
 	const Eulerf euler_sp(_q_trans_sp);
-	_v_att_sp->roll_body = euler_sp.phi();
-	_v_att_sp->pitch_body = euler_sp.theta();
-	_v_att_sp->yaw_body = euler_sp.psi();
-
 	_q_trans_sp.copyTo(_v_att_sp->q_d);
 }
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -203,6 +203,11 @@ void Tiltrotor::update_transition_state()
 
 	const hrt_abstime now = hrt_absolute_time();
 
+	const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
+	float roll_body = attitude_setpoint_euler.phi();
+	float pitch_body = attitude_setpoint_euler.theta();
+	float yaw_body = attitude_setpoint_euler.psi();
+
 	// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
 	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
 	if (_v_control_mode->flag_control_climb_rate_enabled) {
@@ -212,7 +217,7 @@ void Tiltrotor::update_transition_state()
 		}
 
 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
-		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+		roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else {
@@ -290,7 +295,7 @@ void Tiltrotor::update_transition_state()
 
 		// control backtransition deceleration using pitch.
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
-			_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
+			pitch_body = update_and_get_backtransition_pitch_sp();
 		}
 
 		if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
@@ -321,7 +326,7 @@ void Tiltrotor::update_transition_state()
 
 	_v_att_sp->thrust_body[2] = -_thrust_transition;
 
-	const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+	const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
 	q_sp.copyTo(_v_att_sp->q_d);
 
 	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);

src/modules/vtol_att_control/vtol_type.cpp

@@ -563,10 +563,10 @@ float VtolType::pusher_assist()
 		tilt_new = R_yaw_correction * tilt_new;
 
 		// now extract roll and pitch setpoints
-		_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
-		_v_att_sp->roll_body = -asinf(tilt_new(1));
+		const float pitch_body = atan2f(tilt_new(0), tilt_new(2));
+		const float roll_body = -asinf(tilt_new(1));
 
-		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
+		const Quatf q_sp(Eulerf(roll_body, pitch_body, euler_sp(2)));
 		q_sp.copyTo(_v_att_sp->q_d);
 	}