From e370b3f4b8e145d64c5a8171783e56de0ded0954 Mon Sep 17 00:00:00 2001 From: ttechnick Date: Tue, 13 Jan 2026 15:28:31 +0100 Subject: [PATCH] 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 | 1 + msg/versioned/VehicleEulerRatesSetpoint.msg | 5 + .../FixedwingAttitudeControl.cpp | 96 +++++++++++++++++-- .../FixedwingAttitudeControl.hpp | 14 ++- src/modules/logger/logged_topics.cpp | 1 + 5 files changed, 109 insertions(+), 8 deletions(-) create mode 100644 msg/versioned/VehicleEulerRatesSetpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 66a9f7e599..a2ad4a4a1b 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/versioned/VehicleEulerRatesSetpoint.msg b/msg/versioned/VehicleEulerRatesSetpoint.msg new file mode 100644 index 0000000000..802b025989 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 22d42c791e..08497b83d0 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/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(), - _yaw_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()); + + _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); } } } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index e2caac588b..2b459e9c53 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -65,6 +65,7 @@ #include #include #include +#include #include using matrix::Eulerf; @@ -115,12 +116,14 @@ private: uORB::Publication _attitude_sp_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _euler_rates_sp_pub{ORB_ID(vehicle_euler_rates_setpoint)}; uORB::Publication _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(); -}; +}; \ No newline at end of file diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 0810dfc9a1..41e2cd163a 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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");