Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

feat(fw q att control): improve pitchrate tracking using turn coordination constraint

Browse Source 
refactor(fw_att_control): clean-up & optimization
This commit is contained in:
bresch
2026-03-11 12:30:53 +01:00
committed by Nick
parent 00f952ed93
commit c71e196dcf
5 changed files with 16 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/Quaternion.hpp
-42
View File
@@ -518,48 +518,6 @@ public:
return Vector3<Type>(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<Type> dcm_x() const
{
const Quaternion &q = *this;
Vector3<Type> 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<Type> dcm_y() const
{
const Quaternion &q = *this;
Vector3<Type> 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
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+15 -13
View File
@@ -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()));
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+1 -3
View File
@@ -34,8 +34,8 @@
#pragma once
#include <drivers/drv_hrt.h>
#include "fw_wheel_controller.h"
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@@ -157,13 +157,11 @@ private:
(ParamFloat<px4::params::FW_WR_IMAX>) _param_fw_wr_imax,
(ParamFloat<px4::params::FW_WR_P>) _param_fw_wr_p,
(ParamFloat<px4::params::FW_YAW_WEIGHT>) _param_fw_yaw_weight,
(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax,
(ParamFloat<px4::params::FW_MAN_YR_MAX>) _param_man_yr_max
)
matrix::Vector3f _proportional_gain;
float _yaw_w{1.0f};
WheelController _wheel_ctrl;
void parameters_update();
src/modules/fw_att_control/fw_att_control_params.c
-15
View File
@@ -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)
*
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
-2
View File
@@ -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) {