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

msg/VehicleAttitudeSetpoint.msg

@@ -1,9 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
 
-float32 roll_body		# body angle in NED frame (can be NaN for FW)
-float32 pitch_body		# body angle in NED frame (can be NaN for FW)
-float32 yaw_body		# body angle in NED frame (can be NaN for FW)
-
 float32 yaw_sp_move_rate	# rad/s (commanded by user)
 
 # For quaternion-based attitude control

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -96,17 +96,16 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 
 				// STABILIZED mode generate the attitude setpoint from manual user inputs
 
-				_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
+				const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
 
-				_att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
+				float pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
-						     + radians(_param_fw_psp_off.get());
+						   + radians(_param_fw_psp_off.get());
-				_att_sp.pitch_body = constrain(_att_sp.pitch_body,
+				pitch_body = constrain(pitch_body,
-							       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
+						       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
 
-				_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
 				_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
 
-				Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
+				const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
 				q.copyTo(_att_sp.q_d);
 
 				_att_sp.reset_integral = false;
@@ -325,16 +324,22 @@ void FixedwingAttitudeControl::Run()
 			/* Run attitude controllers */
 
 			if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
-				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
+				const Eulerf setpoint(Quatf(_att_sp.q_d));
-					_roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
+				const float roll_body = setpoint.phi();
+				const float pitch_body = setpoint.theta();
+
+				if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) {
+
+					_roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
 								euler_angles.theta());
-					_pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
+					_pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
 								  euler_angles.theta());
-					_yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
+					_yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
 							      euler_angles.theta(), get_airspeed_constrained());
 
 					if (wheel_control) {
-						_wheel_ctrl.control_attitude(_att_sp.yaw_body, euler_angles.psi());
+						Eulerf attitude_setpoint(Quatf(_att_sp.q_d));
+						_wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi());
 
 					} else {
 						_wheel_ctrl.reset_integrator();

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -451,9 +451,6 @@ FixedwingPositionControl::status_publish()
 {
 	position_controller_status_s pos_ctrl_status = {};
 
-	pos_ctrl_status.nav_roll = _att_sp.roll_body;
-	pos_ctrl_status.nav_pitch = _att_sp.pitch_body;
-
 	npfg_status_s npfg_status = {};
 
 	npfg_status.wind_est_valid = _wind_valid;
@@ -791,8 +788,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 			/* reset setpoints from other modes (auto) otherwise we won't
 			 * level out without new manual input */
-			_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
+			float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
-			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			const Eulerf current_setpoint(Quatf(_att_sp.q_d));
+			const Quatf setpoint(Eulerf(roll_body, current_setpoint.theta(), yaw_body));
+			setpoint.copyTo(_att_sp.q_d);
 		}
 
 		_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
@@ -877,11 +877,16 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 	}
 
 	switch (position_sp_type) {
-	case position_setpoint_s::SETPOINT_TYPE_IDLE:
+	case position_setpoint_s::SETPOINT_TYPE_IDLE: {
-		_att_sp.thrust_body[0] = 0.0f;
+			_att_sp.thrust_body[0] = 0.0f;
-		_att_sp.roll_body = 0.0f;
+			const float roll_body = 0.0f;
-		_att_sp.pitch_body = radians(_param_fw_psp_off.get());
+			const float pitch_body = radians(_param_fw_psp_off.get());
-		break;
+			const float yaw_body = 0.0f;
+
+			const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+			setpoint.copyTo(_att_sp.q_d);
+			break;
+		}
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
 		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
@@ -927,9 +932,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
 	}
 
-	/* Copy thrust and pitch values from tecs */
-	_att_sp.pitch_body = get_tecs_pitch();
-
 	if (!_vehicle_status.in_transition_to_fw) {
 		publishLocalPositionSetpoint(current_sp);
 	}
@@ -950,8 +952,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get());
 
-	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
+	const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
-	_att_sp.yaw_body = 0.f;
+	const float yaw_body = 0.f;
 
 	if (_landed) {
 		_att_sp.thrust_body[0] = _param_fw_thr_min.get();
@@ -960,7 +962,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 		_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
 	}
 
-	_att_sp.pitch_body = get_tecs_pitch();
+	const float pitch_body = get_tecs_pitch();
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 
 }
 
@@ -983,11 +987,13 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 				   false,
 				   descend_rate);
 
-	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
+	const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
-	_att_sp.yaw_body = 0.f;
+	const float yaw_body = 0.f;
 
 	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
-	_att_sp.pitch_body = get_tecs_pitch();
+	const float pitch_body = get_tecs_pitch();
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 uint8_t
@@ -1109,10 +1115,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 		navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 	}
 
-	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+	float roll_body = getCorrectedNpfgRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
@@ -1123,6 +1129,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get());
+	const float pitch_body = get_tecs_pitch();
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 void
@@ -1158,10 +1167,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 	navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
-	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+	float roll_body = getCorrectedNpfgRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
-	_att_sp.yaw_body = _yaw;
+	float yaw_body = _yaw;
 
 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
@@ -1174,6 +1183,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 				   _param_climbrate_target.get(),
 				   false,
 				   pos_sp_curr.vz);
+	const float pitch_body = get_tecs_pitch();
+
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 void
@@ -1253,10 +1266,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
 		       ground_speed,
 		       _wind_vel);
-	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+	float roll_body = getCorrectedNpfgRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	float alt_sp = pos_sp_curr.alt;
 
@@ -1267,7 +1280,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 		} else {
 			// continue straight until vehicle has sufficient altitude
-			_att_sp.roll_body = 0.0f;
+			roll_body = 0.0f;
 		}
 
 		_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
@@ -1282,6 +1295,11 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get());
+
+	const float pitch_body = get_tecs_pitch();
+
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 #ifdef CONFIG_FIGURE_OF_EIGHT
@@ -1306,7 +1324,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 
 	// Apply control
 	_figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed);
-	_att_sp.roll_body = _figure_eight.getRollSetpoint();
+	float roll_body = _figure_eight.getRollSetpoint();
 	target_airspeed = _figure_eight.getAirspeedSetpoint();
 	_target_bearing = _figure_eight.getTargetBearing();
 	_closest_point_on_path = _figure_eight.getClosestPoint();
@@ -1337,7 +1355,11 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 				   _param_climbrate_target.get());
 
 	// Yaw
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	const float pitch_body = get_tecs_pitch();
+
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp)
@@ -1392,10 +1414,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 				0.0f;
 	navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature);
 
-	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+	float roll_body = getCorrectedNpfgRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	tecs_update_pitch_throttle(control_interval,
 				   pos_sp_curr.alt,
@@ -1408,7 +1430,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 				   _param_climbrate_target.get());
 
 	_att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max);
-	_att_sp.pitch_body = get_tecs_pitch();
+	const float pitch_body = get_tecs_pitch();
+
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 void
@@ -1494,7 +1519,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 		navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
 
-		_att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint());
+		float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint());
 
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
@@ -1502,7 +1527,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		const float bearing = _npfg.getBearing();
 
 		// heading hold mode will override this bearing setpoint
-		_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
+		float yaw_body = _runway_takeoff.getYaw(bearing);
 
 		// update tecs
 		const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
@@ -1529,9 +1554,16 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 		_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
 
-		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
+		const float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
 		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
 
+		roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt);
+
+		const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+		attitude_setpoint.copyTo(_att_sp.q_d);
+
+		_flaps_setpoint = _param_fw_flaps_to_scl.get();
+
 		// retract ladning gear once passed the climbout state
 		if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) {
 			_new_landing_gear_position = landing_gear_s::GEAR_UP;
@@ -1588,7 +1620,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 			navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
-			_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+			float roll_body = getCorrectedNpfgRollSetpoint();
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 
@@ -1613,17 +1645,27 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 				_att_sp.thrust_body[0] = get_tecs_thrust();
 			}
 
-			_att_sp.pitch_body = get_tecs_pitch();
+			float pitch_body = get_tecs_pitch();
-			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+
+			roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt);
+
+			Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+			attitude_setpoint.copyTo(_att_sp.q_d);
 
 		} else {
 			/* Tell the attitude controller to stop integrating while we are waiting for the launch */
 			_att_sp.reset_integral = true;
 
 			/* Set default roll and pitch setpoints during detection phase */
-			_att_sp.roll_body = 0.0f;
+			float roll_body = 0.0f;
+			float yaw_body = _yaw;
 			_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
-			_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
+			float pitch_body = radians(_takeoff_pitch_min.get());
+			roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt);
+			Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+			attitude_setpoint.copyTo(_att_sp.q_d);
+
 		}
 
 		launch_detection_status_s launch_detection_status;
@@ -1633,7 +1675,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 	}
 
 	_flaps_setpoint = _param_fw_flaps_to_scl.get();
-	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt);
 
 	if (!_vehicle_status.in_transition_to_fw) {
 		publishLocalPositionSetpoint(pos_sp_curr);
@@ -1738,10 +1779,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 		navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-		_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+		float roll_body = getCorrectedNpfgRollSetpoint();
 
 		// use npfg's bearing to commanded course, controlled via yaw angle while on runway
-		_att_sp.yaw_body = _npfg.getBearing();
+		float yaw_body = _npfg.getBearing();
 
 		/* longitudinal guidance */
 
@@ -1787,7 +1828,13 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		/* set the attitude and throttle commands */
 
 		// TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle
-		_att_sp.pitch_body = get_tecs_pitch();
+		float pitch_body = get_tecs_pitch();
+
+		roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
+
+
+		const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+		attitude_setpoint.copyTo(_att_sp.q_d);
 
 		// enable direct yaw control using rudder/wheel
 		_att_sp.fw_control_yaw_wheel = true;
@@ -1817,7 +1864,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 		navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-		_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+		float roll_body = getCorrectedNpfgRollSetpoint();
 
 		/* longitudinal guidance */
 
@@ -1839,10 +1886,15 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 
 		/* set the attitude and throttle commands */
 
-		_att_sp.pitch_body = get_tecs_pitch();
+		float pitch_body = get_tecs_pitch();
+
+		roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
 
 		// yaw is not controlled in nominal flight
-		_att_sp.yaw_body = _yaw;
+		float yaw_body = _yaw;
+
+		const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+		attitude_setpoint.copyTo(_att_sp.q_d);
 
 		// enable direct yaw control using rudder/wheel
 		_att_sp.fw_control_yaw_wheel = false;
@@ -1852,8 +1904,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 
 	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
 
-	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
-
 	_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
 	_spoilers_setpoint = _param_fw_spoilers_lnd.get();
 
@@ -1911,6 +1961,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 	}
 
 	// the terrain estimate (if enabled) is always used to determine the flaring altitude
+	float roll_body;
+	float yaw_body;
+	float pitch_body;
+
 	if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) {
 		// flare and land with minimal speed
 
@@ -1943,9 +1997,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 			       pos_sp_curr.loiter_direction_counter_clockwise,
 			       ground_speed, _wind_vel);
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-		_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+		roll_body = getCorrectedNpfgRollSetpoint();
 
-		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+		yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 		/* longitudinal guidance */
 
@@ -1990,7 +2044,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 		/* set the attitude and throttle commands */
 
 		// TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle
-		_att_sp.pitch_body = get_tecs_pitch();
+		pitch_body = get_tecs_pitch();
 
 		// enable direct yaw control using rudder/wheel
 		_att_sp.fw_control_yaw_wheel = true;
@@ -2020,7 +2074,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 			       pos_sp_curr.loiter_direction_counter_clockwise,
 			       ground_speed, _wind_vel);
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-		_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+		roll_body = getCorrectedNpfgRollSetpoint();
 
 		/* longitudinal guidance */
 
@@ -2044,10 +2098,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 
 		/* set the attitude and throttle commands */
 
-		_att_sp.pitch_body = get_tecs_pitch();
+		pitch_body = get_tecs_pitch();
 
 		// yaw is not controlled in nominal flight
-		_att_sp.yaw_body = _yaw;
+		yaw_body = _yaw;
 
 		// enable direct yaw control using rudder/wheel
 		_att_sp.fw_control_yaw_wheel = false;
@@ -2057,7 +2111,11 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 
 	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
 
-	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
+	roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
+
+	Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
+
 
 	_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
 	_spoilers_setpoint = _param_fw_spoilers_lnd.get();
@@ -2104,11 +2162,14 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 				   false,
 				   height_rate_sp);
 
-	_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
+	float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
-	_att_sp.pitch_body = get_tecs_pitch();
+	const float pitch_body = get_tecs_pitch();
+
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 void
@@ -2137,6 +2198,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		_time_last_xy_reset = _local_pos.timestamp;
 	}
 
+	Eulerf current_setpoint(Quatf(_att_sp.q_d));
+	float yaw_body = current_setpoint.psi();
+	float roll_body = current_setpoint.phi();
+	float pitch_body = current_setpoint.theta();
+
 	/* heading control */
 	// TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here)
 	if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH &&
@@ -2177,10 +2243,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 			_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
 			_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 			navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel);
-			_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+			roll_body = getCorrectedNpfgRollSetpoint();
 			calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
 
-			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 		}
 	}
 
@@ -2202,12 +2268,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		_hdg_hold_enabled = false;
 		_yaw_lock_engaged = false;
 
-		_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
+		roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
-		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+		yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 	}
 
 	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
-	_att_sp.pitch_body = get_tecs_pitch();
+
+	pitch_body = get_tecs_pitch();
+
+	Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 
 void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
@@ -2230,10 +2300,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
 
 	navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 
-	_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
+	float roll_body = getCorrectedNpfgRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	tecs_update_pitch_throttle(control_interval,
 				   pos_sp_curr.alt,
@@ -2246,7 +2316,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
 				   _param_climbrate_target.get());
 
 	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
-	_att_sp.pitch_body = get_tecs_pitch();
+	const float pitch_body = get_tecs_pitch();
+
+	const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
+	attitude_setpoint.copyTo(_att_sp.q_d);
 }
 float
 FixedwingPositionControl::get_tecs_pitch()
@@ -2556,12 +2629,16 @@ FixedwingPositionControl::Run()
 
 
 		if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
+			Eulerf attitude_setpoint(Quatf(_att_sp.q_d));
+			float roll_body = attitude_setpoint.phi();
+			float pitch_body = attitude_setpoint.theta();
+			float yaw_body = attitude_setpoint.psi();
 
 			if (_control_mode.flag_control_manual_enabled) {
-				_att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()),
+				roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()),
-							      radians(_param_fw_r_lim.get()));
+						      radians(_param_fw_r_lim.get()));
-				_att_sp.pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()),
+				pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()),
-							       radians(_param_fw_p_lim_max.get()));
+						       radians(_param_fw_p_lim_max.get()));
 			}
 
 			if (_control_mode.flag_control_position_enabled ||
@@ -2571,9 +2648,9 @@ FixedwingPositionControl::Run()
 			    _control_mode.flag_control_climb_rate_enabled) {
 
 				// roll slew rate
-				_att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval);
+				roll_body = _roll_slew_rate.update(roll_body, control_interval);
 
-				const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
+				const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
 				q.copyTo(_att_sp.q_d);
 
 				_att_sp.timestamp = hrt_absolute_time();
@@ -3162,8 +3239,10 @@ float FixedwingPositionControl::getLoadFactor()
 {
 	float load_factor_from_bank_angle = 1.0f;
 
-	if (PX4_ISFINITE(_att_sp.roll_body)) {
+	float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi();
-		load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON);
+
+	if (PX4_ISFINITE(roll_body)) {
+		load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON);
 	}
 
 	return load_factor_from_bank_angle;

src/modules/mavlink/mavlink_receiver.cpp

@@ -1626,11 +1626,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 			const matrix::Quatf q{attitude_target.q};
 			q.copyTo(attitude_setpoint.q_d);
 
-			matrix::Eulerf euler{q};
-			attitude_setpoint.roll_body = euler.phi();
-			attitude_setpoint.pitch_body = euler.theta();
-			attitude_setpoint.yaw_body = euler.psi();
-
 			// TODO: review use case
 			attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ?
 							     (float)NAN : attitude_target.body_yaw_rate;

src/modules/mavlink/streams/HIGH_LATENCY2.hpp

@@ -264,7 +264,9 @@ private:
 		vehicle_attitude_setpoint_s attitude_sp;
 
 		if (_attitude_sp_sub.update(&attitude_sp)) {
-			msg->target_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
+
+			msg->target_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(matrix::Eulerf(matrix::Quatf(
+						      attitude_sp.q_d)).psi())) * 0.5f);
 			return true;
 		}
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -182,12 +182,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
 
 	q_sp.copyTo(attitude_setpoint.q_d);
 
-	// Transform to euler angles for logging only
-	const Eulerf euler_sp(q_sp);
-	attitude_setpoint.roll_body = euler_sp(0);
-	attitude_setpoint.pitch_body = euler_sp(1);
-	attitude_setpoint.yaw_body = euler_sp(2);
-
 	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle);
 
 	attitude_setpoint.timestamp = hrt_absolute_time();

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	// copy quaternion setpoint to attitude setpoint topic
 	const Quatf q_sp{R_sp};
 	q_sp.copyTo(att_sp.q_d);
-
-	// calculate euler angles, for logging only, must not be used for control
-	const Eulerf euler{R_sp};
-	att_sp.roll_body = euler.phi();
-	att_sp.pitch_body = euler.theta();
-	att_sp.yaw_body = euler.psi();
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -108,18 +108,20 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	float yaw = 0.f;
 	vehicle_attitude_setpoint_s att{};
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(att.q_d[0], 1.f);
-	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(att.q_d[1], 0.f);
-	EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
+	EXPECT_FLOAT_EQ(att.q_d[2], 0.f);
+	EXPECT_FLOAT_EQ(att.q_d[3], 0.f);
 	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
 	yaw = M_PI_2_F;
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
+	Eulerf euler_att(Quatf(att.q_d));
-	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
-	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
+	EXPECT_FLOAT_EQ(euler_att.psi(), M_PI_2_F);
 	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
 	/* expected: same as before but roll 180
@@ -127,9 +129,10 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	 * order is: 1. roll, 2. pitch, 3. yaw */
 	thr = Vector3f(0.f, 0.f, 1.f);
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
+	Eulerf euler_att2(Quatf(att.q_d));
-	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(std::abs(euler_att2.phi()), std::abs(M_PI_F));
-	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(euler_att2.theta(), 0.f);
+	EXPECT_FLOAT_EQ(euler_att2.psi(), M_PI_2_F);
 	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 }
 

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -56,9 +56,10 @@ TEST(PositionControlTest, EmptySetpoint)
 
 	vehicle_attitude_setpoint_s attitude{};
 	position_control.getAttitudeSetpoint(attitude);
-	EXPECT_FLOAT_EQ(attitude.roll_body, 0.f);
+	Eulerf euler_att(Quatf(attitude.q_d));
-	EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
-	EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f);
+	EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
+	EXPECT_FLOAT_EQ(euler_att.psi(), 0.f);
 	EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
 	EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
 	EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
@@ -184,7 +185,8 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
 	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST);
 
 	// Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust
-	EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
+	Eulerf euler_att(Quatf(_attitude.q_d));
+	EXPECT_FLOAT_EQ(euler_att.phi(), asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
 	// TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore
 	// EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
 }
@@ -198,9 +200,10 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
 	EXPECT_FLOAT_EQ(thrust.length(), 0.1f);
 
 	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f);
+	Eulerf euler_att(Quatf(_attitude.q_d));
 
-	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
-	EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f);
+	EXPECT_FLOAT_EQ(euler_att.theta(), -1.f);
 }
 
 TEST_F(PositionControlBasicTest, FailsafeInput)
@@ -377,6 +380,7 @@ TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
 	EXPECT_TRUE(runController());
 
 	// THEN: the integral did not wind up and produce unexpected deviation
-	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
+	Eulerf euler_att(Quatf(_attitude.q_d));
-	EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(euler_att.phi(), 0.f);
+	EXPECT_FLOAT_EQ(euler_att.theta(), 0.f);
 }

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -122,8 +122,8 @@ RoverPositionControl::manual_control_setpoint_poll()
 
 				if (_control_mode.flag_control_attitude_enabled) {
 					// STABILIZED mode generate the attitude setpoint from manual user inputs
-					_att_sp.roll_body = 0.0;
+					float roll_body = 0.0;
-					_att_sp.pitch_body = 0.0;
+					float pitch_body = 0.0;
 
 					/* reset yaw setpoint to current position if needed */
 					if (_reset_yaw_sp) {
@@ -137,10 +137,10 @@ RoverPositionControl::manual_control_setpoint_poll()
 						_manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt);
 					}
 
-					_att_sp.yaw_body = _manual_yaw_sp;
+					float yaw_body = _manual_yaw_sp;
 					_att_sp.thrust_body[0] = _manual_control_setpoint.throttle;
 
-					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
+					const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
 					q.copyTo(_att_sp.q_d);
 
 					_att_sp.timestamp = hrt_absolute_time();

src/modules/uuv_att_control/uuv_att_control.cpp

@@ -138,9 +138,10 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
 	 */
 	Eulerf euler_angles(matrix::Quatf(attitude.q));
 
-	float roll_body = attitude_setpoint.roll_body;
+	const Eulerf setpoint_euler_angles(matrix::Quatf(attitude_setpoint.q_d));
-	float pitch_body = attitude_setpoint.pitch_body;
+	const float roll_body = setpoint_euler_angles(0);
-	float yaw_body = attitude_setpoint.yaw_body;
+	const float pitch_body = setpoint_euler_angles(1);
+	const float yaw_body = setpoint_euler_angles(2);
 
 	float roll_rate_desired = rates_setpoint.roll;
 	float pitch_rate_desired = rates_setpoint.pitch;
@@ -227,9 +228,8 @@ void UUVAttitudeControl::Run()
 			_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
 
 			if (input_mode == 1) { // process manual data
-				_attitude_setpoint.roll_body = _param_direct_roll.get();
+				Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get()));
-				_attitude_setpoint.pitch_body = _param_direct_pitch.get();
+				attitude_setpoint.copyTo(_attitude_setpoint.q_d);
-				_attitude_setpoint.yaw_body = _param_direct_yaw.get();
 				_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
 				_attitude_setpoint.thrust_body[1] = 0.f;
 				_attitude_setpoint.thrust_body[2] = 0.f;

src/modules/uuv_pos_control/uuv_pos_control.cpp

@@ -97,9 +97,8 @@ void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float
 	vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {};
 	vehicle_attitude_setpoint.timestamp = hrt_absolute_time();
 
-	vehicle_attitude_setpoint.roll_body = roll_des;
+	const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des));
-	vehicle_attitude_setpoint.pitch_body = pitch_des;
+	attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d);
-	vehicle_attitude_setpoint.yaw_body = yaw_des;
 
 	vehicle_attitude_setpoint.thrust_body[0] = thrust_x;
 	vehicle_attitude_setpoint.thrust_body[1] = thrust_y;

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(roll_body, pitch_body, yaw_body));
-		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->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));
+		const float pitch_body = atan2f(tilt_new(0), tilt_new(2));
-		_v_att_sp->roll_body = -asinf(tilt_new(1));
+		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);
 	}