From c71e196dcfbaad1a0b433624442cf1e5ad7bb466 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 11 Mar 2026 12:30:53 +0100 Subject: [PATCH] feat(fw q att control): improve pitchrate tracking using turn coordination constraint refactor(fw_att_control): clean-up & optimization --- src/lib/matrix/matrix/Quaternion.hpp | 42 ------------------- .../FixedwingAttitudeControl.cpp | 28 +++++++------ .../FixedwingAttitudeControl.hpp | 4 +- .../fw_att_control/fw_att_control_params.c | 15 ------- .../FwLateralLongitudinalControl.cpp | 2 - 5 files changed, 16 insertions(+), 75 deletions(-) diff --git a/src/lib/matrix/matrix/Quaternion.hpp b/src/lib/matrix/matrix/Quaternion.hpp index 4ac6243bc6..a2637606fd 100644 --- a/src/lib/matrix/matrix/Quaternion.hpp +++ b/src/lib/matrix/matrix/Quaternion.hpp @@ -518,48 +518,6 @@ public: return Vector3(q(1), q(2), q(3)); } - /** - * Corresponding body x-axis to an attitude quaternion / - * first orthogonal unit basis vector - * - * == first column of the equivalent rotation matrix - * but calculated more efficiently than a full conversion - */ - Vector3 dcm_x() const - { - const Quaternion &q = *this; - Vector3 R_x; - const Type a = q(0); - const Type b = q(1); - const Type c = q(2); - const Type d = q(3); - R_x(0) = a * a + b * b - c * c - d * d; - R_x(1) = 2 * (b * c + a * d); - R_x(2) = 2 * (b * d - a * c); - return R_x; - } - - /** - * Corresponding body y-axis to an attitude quaternion / - * second orthogonal unit basis vector - * - * == second column of the equivalent rotation matrix - * but calculated more efficiently than a full conversion - */ - Vector3 dcm_y() const - { - const Quaternion &q = *this; - Vector3 R_y; - const Type a = q(0); - const Type b = q(1); - const Type c = q(2); - const Type d = q(3); - R_y(0) = 2 * (b * c - a * d); - R_y(1) = a * a - b * b + c * c - d * d; - R_y(2) = 2 * (c * d + a * b); - return R_y; - } - /** * Corresponding body z-axis to an attitude quaternion / * last orthogonal unit basis vector diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index b8611d9ff2..6ed96842d5 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -74,8 +74,7 @@ FixedwingAttitudeControl::parameters_update() { _proportional_gain = matrix::Vector3f(1.0f / _param_fw_r_tc.get(), 1.0f / _param_fw_p_tc.get(), - 0.0f); - _yaw_w = _param_fw_yaw_weight.get(); + 1.0f); _wheel_ctrl.set_k_p(_param_fw_wr_p.get()); _wheel_ctrl.set_k_i(_param_fw_wr_i.get()); @@ -104,9 +103,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) pitch_body = constrain(pitch_body, -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - float yaw_sp = yaw_body; - - const Quatf q(Eulerf(roll_body, pitch_body, yaw_sp)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; @@ -297,20 +294,25 @@ void FixedwingAttitudeControl::Run() const Quatf q_sp(_att_sp.q_d); if (q_sp.isAllFinite()) { - Quatf q_current(att.q); - Quatf q_err = (q_current.inversed() * q_sp).canonical(); - Vector3f e = 2.f * q_err.imag(); + const Quatf q_current(att.q); + // We rotate the setpoint, to prevent yaw errors that we do not have authority to control. + const float yaw_offset = -2.f * (q_current(0) * q_sp(3) - q_current(1) * q_sp(2) + q_current(2) * q_sp(1) - q_current(3) * q_sp(0)) / + (q_current(0) * q_sp(0) - q_current(1) * q_sp(1) - q_current(2) * q_sp(2) + q_current(3) * q_sp(3)); + const Quatf q_yaw_offset = Quatf(1.f, 0.f, 0.f, yaw_offset / 2.f).normalized(); + const Quatf q_err = (q_current.inversed() * q_yaw_offset * q_sp).canonical(); // See mc_att_control for details + const Vector3f q_err_imag = 2.f * q_err.imag(); Vector3f body_rates_setpoint; - body_rates_setpoint(0) = _proportional_gain(0) * e(0); - body_rates_setpoint(1) = _proportional_gain(1) * e(1) - 6.f * _yaw_w * e(2) * tanf(euler_angles.phi()); - body_rates_setpoint(2) = 0.f; // yaw handled through turn coordination and manual yaw input + body_rates_setpoint = _proportional_gain.emult(q_err_imag); // Turn coordination const float V = math::max(get_airspeed_constrained(), 0.1f); - const float r_tc_ff = 9.81f * 2.0f * (q_current(0) * q_current(1) + q_current(2) * q_current(3)) / V; + const float q1 = 2.f * (q_current(0) * q_current(1) + q_current(2) * q_current(3)); + const float r_tc_ff = CONSTANTS_ONE_G * q1 / V; + const float p_tc_ff = q1 * r_tc_ff / (1.f - 2.f * q_current(1) * q_current(1) - 2.f * q_current(2) * q_current(2)); - body_rates_setpoint(2) = r_tc_ff; + body_rates_setpoint(1) += p_tc_ff; + body_rates_setpoint(2) += r_tc_ff; body_rates_setpoint(0) = math::constrain(body_rates_setpoint(0), -radians(_param_fw_r_rmax.get()), radians(_param_fw_r_rmax.get())); body_rates_setpoint(1) = math::constrain(body_rates_setpoint(1), -radians(_param_fw_p_rmax_neg.get()), radians(_param_fw_p_rmax_pos.get())); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index e0c4e10eac..da8b4d4b97 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -34,8 +34,8 @@ #pragma once #include - #include "fw_wheel_controller.h" +#include #include #include #include @@ -157,13 +157,11 @@ private: (ParamFloat) _param_fw_wr_imax, (ParamFloat) _param_fw_wr_p, - (ParamFloat) _param_fw_yaw_weight, (ParamFloat) _param_fw_y_rmax, (ParamFloat) _param_man_yr_max ) matrix::Vector3f _proportional_gain; - float _yaw_w{1.0f}; WheelController _wheel_ctrl; void parameters_update(); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index bf018a3776..554eac360c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -197,21 +197,6 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f); */ PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); -/** - * Yaw weight - * - * Scales yaw-error correction through the pitch-rate setpoint in non-linear attitude control. - * - * For yaw control tuning use FW_Y_RMAX and rate controller tuning. This ratio has no impact on yaw gain. - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.1 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_YAW_WEIGHT, 1.0f); - /** * Pitch setpoint offset (pitch at level flight) * diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index e6cbedfea2..9a8e5e3e9a 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -298,9 +298,7 @@ void FwLateralLongitudinalControl::Run() // additional is_finite checks that should not be necessary, but are kept for safety float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f; float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f; - float yaw_body = _yaw; - const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f; if (_control_mode_sub.get().flag_control_manual_enabled) {