Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-03-23 18:13:49 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

feat(fw_att_control): limit turn coordination to normal flight

Browse Source
This commit is contained in:
ttechnick
2026-03-17 15:06:49 +01:00
committed by Nick
parent ca4ae7cf41
commit 3601a01594
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@@ -307,13 +307,21 @@ void FixedwingAttitudeControl::Run()
// Turn coordination
const float V = math::max(get_airspeed_constrained(), 0.1f);
const float q1 = 2.f * (q_current(0) * q_current(1) + q_current(2) * q_current(3));
const float q1 = 2.f * (q_current(0) * q_current(1) + q_current(2) * q_current(3)); // equivalent to 2.f * sin(roll) * cos(pitch)
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(1) += p_tc_ff;
body_rates_setpoint(2) += r_tc_ff;
// Limit turn coordination to normal flight envelope
const float cos_tilt = 1.f - 2.f * (q_current(1) * q_current(1) + q_current(2) * q_current(2));
const float tilt = acosf(math::constrain((cos_tilt), -1.f, 1.f));
const float ff_scale = math::interpolate(tilt, radians(70.f), radians(75.f), 1.f, 0.f);
body_rates_setpoint(1) += ff_scale * p_tc_ff;
body_rates_setpoint(2) += ff_scale * 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()));
body_rates_setpoint(2) = math::constrain(body_rates_setpoint(2), -radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));