feat(fw_att_control): Add Initial Quat Controller + Debug topic

This commit introduces a new control mode for fixed-wing aircraft that
utilizes Euler angles for attitude control.
Additionally, a new logged topic has been added to facilitate debugging and
monitoring of the Euler rates setpoints during flight.

msg/CMakeLists.txt

@@ -276,6 +276,7 @@ set(msg_files
 	versioned/VehicleLocalPosition.msg
 	versioned/VehicleOdometry.msg
 	versioned/VehicleRatesSetpoint.msg
+	versioned/VehicleEulerRatesSetpoint.msg
 	versioned/VehicleStatus.msg
 	versioned/VtolVehicleStatus.msg
 	versioned/Wind.msg

msg/versioned/VehicleEulerRatesSetpoint.msg

@@ -0,0 +1,5 @@
+uint64 timestamp		# time since system start (microseconds)
+
+float32 roll_rate		# body frame roll rate setpoint (rad/s)
+float32 pitch_rate		# body frame pitch rate setpoint (rad/s)
+float32 yaw_rate		# body frame yaw rate setpoint (rad/s)

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -72,6 +72,10 @@ FixedwingAttitudeControl::init()
 void
 FixedwingAttitudeControl::parameters_update()
 {
+	_proportional_gain = matrix::Vector3f(1.0f / _param_fw_p_tc.get(),
+					      1.0f / _param_fw_p_tc.get(),
+					      0.0f);
+
 	_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
 	_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
 
@@ -81,6 +85,12 @@ FixedwingAttitudeControl::parameters_update()
 
 	_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
 
+
+	_max_roll_rate = radians(_param_fw_r_rmax.get());
+	_max_pitch_rate_pos = radians(_param_fw_p_rmax_pos.get());
+	_max_pitch_rate_neg = radians(_param_fw_p_rmax_neg.get());
+	_max_yaw_rate = radians(_param_fw_y_rmax.get());
+
 	_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
 	_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
 	_wheel_ctrl.set_k_ff(_param_fw_wr_ff.get());
@@ -90,8 +100,10 @@ FixedwingAttitudeControl::parameters_update()
 }
 
 void
-FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
+FixedwingAttitudeControl::vehicle_manual_poll(matrix::Quatf R)
 {
+	const matrix::Eulerf euler_angles(R);
+	float yaw_body = euler_angles.psi();
 	if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
 
 		// Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values
@@ -207,7 +219,7 @@ void FixedwingAttitudeControl::Run()
 			dt = math::constrain((att.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX);
 			_last_run = att.timestamp_sample;
 
-			// get current rotation matrix and euler angles from control state quaternions
+			// get current rotation matrix from control state quaternions
 			_R = matrix::Quatf(att.q);
 		}
 
@@ -254,10 +266,9 @@ void FixedwingAttitudeControl::Run()
 			/* fill in new attitude data */
 			_R = R_adapted;
 		}
-
 		const matrix::Eulerf euler_angles(_R);
 
-		vehicle_manual_poll(euler_angles.psi());
+		vehicle_manual_poll(_R);
 
 		vehicle_attitude_setpoint_poll();
 
@@ -299,6 +310,70 @@ void FixedwingAttitudeControl::Run()
 				const Quatf q_sp(_att_sp.q_d);
 
 				if (q_sp.isAllFinite()) {
+
+
+					// Current attitude
+					const Quatf q_current(att.q);
+
+					// Full desired attitude (setpoint)
+					const Quatf qd_full = q_sp;
+
+					// --- Scheduling: 0 = normal FW, 1 = near-vertical/full-3D ---
+					const float pitch = Eulerf(q_current).theta(); // scheduling only
+					const float s = math::constrain((fabsf(pitch) - radians(60.f)) / radians(20.f), 0.f, 1.f); // 0..1
+
+					// --- Build reduced desired attitude qd_red: align "tilt" (body Z axis) but don't chase yaw ---
+					// Note: assumes Quatf::dcm_z() returns body Z axis expressed in world/NED (same convention as PX4 MC).
+					const Vector3f ez_current = q_current.dcm_z(); // current body Z in world
+					const Vector3f ez_desired = qd_full.dcm_z();   // desired body Z in world
+
+					// Quaternion rotating ez_current -> ez_desired (tilt correction only)
+					Quatf q_tilt_err(ez_current, ez_desired);
+
+					// Convert that tilt error into an absolute reduced desired attitude.
+					// This matches the PX4 multicopter pattern (right multiplication).
+					Quatf qd_red;
+
+					if (fabsf(q_tilt_err(1)) > (1.f - 1e-5f) || fabsf(q_tilt_err(2)) > (1.f - 1e-5f)) {
+						// Corner case: vectors almost exactly opposite (180 deg). Tilt-only separation is ill-defined.
+						// Fall back to full desired attitude; it's safe/stable.
+						qd_red = qd_full;
+
+					} else {
+						// Apply tilt correction to current attitude to get a reduced desired attitude
+						qd_red = q_tilt_err * q_current;
+					}
+
+					// --- Compute rate setpoint A: tilt-only attitude tracking (normal FW) ---
+
+					// qe_tilt is rotation from current to reduced desired
+					const Quatf qe_tilt = (q_current.inversed() * qd_red).canonical();
+
+					// Use bounded quaternion vector part as error (stable & well-tuned pattern)
+					const Vector3f e_tilt = 2.f * qe_tilt.imag();
+
+					Vector3f rates_tilt = e_tilt.emult(_proportional_gain);
+
+					// In normal FW flight, yaw is NOT attitude-tracked; use existing yaw/turn coordination
+					const float yaw_rate_tc = _yaw_ctrl.get_body_rate_setpoint();
+					rates_tilt(2) = yaw_rate_tc;
+
+					// --- Compute rate setpoint B: full 3D attitude tracking (near-vertical) ---
+					const Quatf qe_full = (q_current.inversed() * qd_full).canonical();
+					const Vector3f e_full = 2.f * qe_full.imag();
+					Vector3f rates_full = e_full.emult(_proportional_gain);
+
+					// --- Blend A and B ---
+					Vector3f body_rates_setpoint = (1.f - s) * rates_tilt + s * rates_full;
+
+
+					// limit rates (kept identical)
+					body_rates_setpoint(0) = constrain(body_rates_setpoint(0), -_max_roll_rate, _max_roll_rate);
+					body_rates_setpoint(1) = constrain(body_rates_setpoint(1), -_max_pitch_rate_neg, _max_pitch_rate_pos);
+					body_rates_setpoint(2) = constrain(body_rates_setpoint(2), -_max_yaw_rate, _max_yaw_rate);
+
+
+					///////////////////////////////////
 					const Eulerf euler_sp(q_sp);
 					const float roll_sp = euler_sp.phi();
 					const float pitch_sp = euler_sp.theta();
@@ -311,8 +386,16 @@ void FixedwingAttitudeControl::Run()
 							      euler_angles.theta(), get_airspeed_constrained());
 
 					/* Update input data for rate controllers */
-					Vector3f body_rates_setpoint = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(),
+					Vector3f _euler_body_rates_sp = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(),
-										_yaw_ctrl.get_body_rate_setpoint());
+									_yaw_ctrl.get_body_rate_setpoint());
+
+					_euler_rates_sp.timestamp = hrt_absolute_time();
+					_euler_rates_sp.roll_rate = _euler_body_rates_sp(0);
+					_euler_rates_sp.pitch_rate = _euler_body_rates_sp(1);
+					_euler_rates_sp.yaw_rate = _euler_body_rates_sp(2);
+					///////////////////////////////////
+
+
 
 					autotune_attitude_control_status_s pid_autotune;
 					matrix::Vector3f bodyrate_autotune_ff;
@@ -351,6 +434,7 @@ void FixedwingAttitudeControl::Run()
 					_rates_sp.timestamp = hrt_absolute_time();
 
 					_rate_sp_pub.publish(_rates_sp);
+					_euler_rates_sp_pub.publish(_euler_rates_sp);
 				}
 			}
 		}

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -65,6 +65,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_euler_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 
 using matrix::Eulerf;
@@ -115,12 +116,14 @@ private:
 
 	uORB::Publication<vehicle_attitude_setpoint_s>	_attitude_sp_pub;
 	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Publication<vehicle_euler_rates_setpoint_s> _euler_rates_sp_pub{ORB_ID(vehicle_euler_rates_setpoint)};
 	uORB::Publication<landing_gear_wheel_s>		_landing_gear_wheel_pub{ORB_ID(landing_gear_wheel)};
 
 	manual_control_setpoint_s		_manual_control_setpoint{};
 	vehicle_attitude_setpoint_s		_att_sp{};
 	vehicle_control_mode_s			_vcontrol_mode{};
 	vehicle_rates_setpoint_s		_rates_sp{};
+	vehicle_euler_rates_setpoint_s		_euler_rates_sp{};
 	vehicle_status_s			_vehicle_status{};
 	landing_gear_wheel_s			_landing_gear_wheel{};
 
@@ -164,14 +167,21 @@ private:
 
 	)
 
+	matrix::Vector3f _proportional_gain;
 	RollController _roll_ctrl;
 	PitchController _pitch_ctrl;
 	YawController _yaw_ctrl;
+
+	float _max_roll_rate;
+	float _max_pitch_rate_pos;
+	float _max_pitch_rate_neg;
+	float _max_yaw_rate;
+
 	WheelController _wheel_ctrl;
 
 	void parameters_update();
-	void vehicle_manual_poll(const float yaw_body);
+	void vehicle_manual_poll(matrix::Quatf R);
 	void vehicle_attitude_setpoint_poll();
 	void vehicle_land_detected_poll();
 	float get_airspeed_constrained();
-};
+};

src/modules/logger/logged_topics.cpp

@@ -148,6 +148,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_local_position_setpoint", 100);
 	add_topic("vehicle_magnetometer", 200);
 	add_topic("vehicle_rates_setpoint", 20);
+	add_topic("vehicle_euler_rates_setpoint", 20);
 	add_topic("vehicle_roi", 1000);
 	add_topic("vehicle_status");
 	add_topic("vtx");