fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

docs(fw_att_ctrl): update quat turn coordination

Browse Source 
fix(fw_att_control): use correct formula
This commit is contained in:
ttechnick 2026-03-12 18:05:23 +01:00 committed by Nick
parent a42e7ebb2a
commit 82bf75df0f
3 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
docs/en/flight_stack/controller_diagrams.md
View File

@ -219,15 +219,20 @@ In order to keep a constant rate, this damping can be compensated using feedforw
### Turn coordination
The yaw rate setpoint is generated using the turn coordination constraint in order to minimize lateral acceleration, generated when the aircraft is slipping.
The yaw rate setpoint is generated using the turn coordination constraint in order to minimize lateral acceleration, generated when the aircraft is slipping.
$$\dot{\Psi}_{sp} = \frac{g}{V_T} \tan{\phi_{sp}} \cdot 0.6$$
$$\dot{\Psi}_{sp} = \frac{2g}{V_T}\left(q_0 q_1 + q_2 q_3\right)$$
This also helps to counteract [adverse yaw effects](https://youtu.be/sNV_SDDxuWk) and to damp the [Dutch roll mode](https://en.wikipedia.org/wiki/Dutch_roll) by providing extra directional damping.
To compensate for the non-zero pitch rate that naturally occurs during coordinated turns, a geometry-based feedforward term is added to the pitch-rate command.
This feedforward term accounts for the aircraft's current attitude and airspeed so that the controller does not need to generate this motion purely through feedback.
$$\dot{\theta}_{sp}^{ff} = \frac{4g(q_0 q_1 + q_2 q_3)^2}{V(1 - 2q_1^2 - 2q_2^2)}$$
To compensate for the non-zero pitch rate that naturally occurs during coordinated turns, a geometry-based feedforward term is added to the pitch-rate command.
This feedforward term accounts for the aircraft's current attitude and airspeed so that the controller does not need to generate this motion purely through feedback.
$$\dot{\theta}_{sp}^{ff} = \frac{2g(q_0 q_1 + q_2 q_3)^2}{V(1 - 2q_1^2 - 2q_2^2)}$$
## VTOL Flight Controller

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

@ -32,6 +32,8 @@
****************************************************************************/
#include "FixedwingAttitudeControl.hpp"
#include <lib/geo/geo.h>
using namespace time_literals;
using namespace matrix;
@ -309,7 +311,7 @@ void FixedwingAttitudeControl::Run()
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 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));
const float p_tc_ff = 2.f * 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;

1
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -35,7 +35,6 @@
#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>