Files
Silvan Fuhrer b4134d75f2 feat(tecs): don't trade altitude for speed above trim airspeed (#29009)
* fix(tecs): don't trade altitude for speed above trim airspeed

With an airspeed setpoint above trim and the throttle at `FW_THR_MAX`, TECS pitches down to
keep chasing the setpoint and settles below the altitude setpoint.
Accelerating above trim should be the throttle's job; losing altitude at high speed is not wanted.

The pitch loop now sees the airspeed setpoint capped at max(trim, current airspeed);
the throttle loop still tracks the full setpoint. Below trim, pitch still trades height for speed, up to trim.
Overspeed above the setpoint is still corrected with pitch.
The cap fades out as the effective `FW_T_SPDWEIGHT` goes from 1 to 2, so gliders (weight 2),
fast descend and underspeed recovery keep using pitch to reach their airspeed.
The behaviour is documented in the TECS docs and the `FW_T_SPDWEIGHT` description.

Signed-off-by: Silvan <silvan@auterion.com>

* fix(tecs): use tas and not tas_sp for the pitch loop

Signed-off-by: Silvan <silvan@auterion.com>

* fix(tecs): don't counter throttle acceleration with pitch above trim

Capping the pitch loop's airspeed setpoint at max(trim, airspeed) keeps
pitch from diving to gain speed, but between trim and the setpoint the
throttle's acceleration still shows up in the kinetic energy rate
estimate. Against a kinetic energy rate setpoint of zero, the pitch loop
then pitches up and turns the throttle's energy into height. At a speed
weight of 1 this halves the acceleration and lifts the vehicle above the
altitude setpoint while accelerating.

Remove the kinetic term, setpoint and estimate, from the pitch loop
between trim and the setpoint, so pitch controls height only there. As
for the cap, this fades out with speed priority.

Point-mass simulation with the TECS defaults, speed step from 15 m/s:
- to 20 m/s: 95% of the step in 21 s instead of 37 s, 1.2 m instead of
  2.4 m above the altitude setpoint
- to 28 m/s, above the 24.7 m/s the vehicle can reach: 2.2 m instead of
  5.2 m above the altitude setpoint, still no altitude loss
- deceleration, climbs, a speed weight of 2 and cruise at trim are
  unchanged; in turbulent cruise above trim the pitch rate RMS is 6%
  higher, as the kinetic term switches when the airspeed crosses the
  setpoint

The remaining rise comes from the lift gained with speed at constant
pitch, which only altitude feedback corrects.

Assisted-by: Claude:claude-opus-5-5

* fix(tecs): fade the pitch loop's kinetic setpoint only once

Between trim airspeed and the airspeed setpoint, the capped pitch loop
setpoint already fades the kinetic energy rate setpoint in with the
speed weight between 1 and 2. Scaling it by the same fade factor again
faded the setpoint quadratically but the estimate only linearly, so at a
speed weight of 1.5 the pitch loop countered acceleration it should only
half ignore. Scale only the estimate.

Speed weights up to 1 and of 2 are unaffected. In a point-mass
simulation at a speed weight of 1.5, a step from 15 to 20 m/s now takes
30 s instead of 37 s and rises 1.6 m instead of 2.4 m, between the
results at weights 1 and 2 instead of worse than both.

Assisted-by: Claude:claude-opus-5-5

* docs(tecs): show the pitch and throttle speed roles in a diagram

The paragraph on how pitch and throttle share speed control above trim
was hard to follow. Show the three regions of measured airspeed (below
trim, between trim and the setpoint, above the setpoint) with what the
pitch and throttle loops do in each, and describe each region in its own
bullet. Also state that there is no middle region with a setpoint at or
below trim.

Assisted-by: Claude:claude-opus-5-5

---------

Signed-off-by: Silvan <silvan@auterion.com>
2026-10-15 08:33:02 +02:00

24 KiB
Raw Permalink Blame History

Controller Diagrams

This section contains diagrams for the main PX4 controllers.

The diagrams use the standard PX4 notation (and each have an annotated legend).

Multicopter Control Architecture

MC Controller Diagram

  • This is a standard cascaded control architecture.
  • The controllers are a mix of P and PID controllers.
  • Estimates come from EKF2.
  • Depending on the mode, the outer (position) loop is bypassed (shown as a multiplexer after the outer loop). The position loop is only used when holding position or when the requested velocity in an axis is null.

Multicopter Angular Rate Controller

MC Rate Control Diagram

  • K-PID controller. See Rate Controller for more information.

  • The integral authority is limited to prevent wind up.

  • The outputs are limited (in the control allocation module), usually at -1 and 1.

  • A Low Pass Filter (LPF) is used on the derivative path to reduce noise (the gyro driver provides a filtered derivative to the controller).

    ::: info The IMU pipeline is: gyro data > apply calibration parameters > remove estimated bias > notch filter (IMU_GYRO_NF0_BW and IMU_GYRO_NF0_FRQ) > low-pass filter (IMU_GYRO_CUTOFF) > vehicleangular_velocity (_filtered angular rate used by the P and I controllers) > derivative -> low-pass filter (IMU_DGYRO_CUTOFF) > vehicleangular_acceleration (_filtered angular acceleration used by the D controller)

    IMU pipeline :::

Multicopter Attitude Controller

MC Angle Control Diagram

  • The attitude controller makes use of quaternions.
  • The controller is implemented from this article.
  • The attitude setpoint is first smoothed by a 2nd-order critically-damped reference model (MC_REF_W_N); the P-law tracks its reference attitude, and the model's reference body rate is fed forward to the rate setpoint, removing the pure-P law's steady-state tracking lag.
  • The feedforward is scaled by MC_REF_FF (0 disables it), clipped per axis by MC_REF_FF_MAX, and suppressed during autotuning.
  • Higher MC_REF_W_N or MC_REF_FF tracks more aggressively but demands more peak rate; the P gain remains the main tuning parameter.
  • The rate command is saturated.

Multicopter Acceleration to Thrust and Attitude Setpoint Conversion

  • The acceleration setpoints generated by the velocity controller will be converted to thrust and attitude setpoints.
  • Converted acceleration setpoints will be saturated and prioritized in vertical and horizontal thrust.
  • Thrust saturation is done after computing the corresponding thrust:
    1. Compute required vertical thrust (thrust_z)
    2. Saturate thrust_z with MPC_THR_MAX
    3. Saturate thrust_xy with (MPC_THR_MAX^2 - thrust_z^2)^0.5

Implementation details can be found in PositionControl.cpp and ControlMath.cpp.

Multicopter Velocity Controller

MC Velocity Control Diagram

  • PID controller to stabilise velocity. Commands an acceleration.
  • The integrator includes an anti-reset windup (ARW) using a clamping method.
  • The commanded acceleration is NOT saturated - a saturation will be applied to the converted thrust setpoints in combination with the maximum tilt angle.
  • Horizontal gains set via parameter MPC_XY_VEL_P_ACC, MPC_XY_VEL_I_ACC and MPC_XY_VEL_D_ACC.
  • Vertical gains set via parameter MPC_Z_VEL_P_ACC, MPC_Z_VEL_I_ACC and MPC_Z_VEL_D_ACC.

Multicopter Position Controller

MC Position Control Diagram

  • Simple P controller that commands a velocity.
  • The commanded velocity is saturated to keep the velocity in certain limits. See parameter MPC_XY_VEL_MAX. This parameter sets the maximum possible horizontal velocity. This differs from the maximum desired speed MPC_XY_CRUISE (autonomous modes) and MPC_VEL_MANUAL (manual position control mode).
  • Horizontal P gain set via parameter MPC_XY_P.
  • Vertical P gain set via parameter MPC_Z_P.

Combined Position and Velocity Controller Diagram

MC Position Controller Diagram

  • Mode dependent feedforwards (ff) - e.g. Mission mode trajectory generator (jerk-limited trajectory) computes position, velocity and acceleration setpoints.
  • Acceleration setpoints (inertial frame) will be transformed (with yaw setpoint) into attitude setpoints (quaternion) and collective thrust setpoint.

Fixed-Wing Position Controller

Total Energy Control System (TECS)

The PX4 implementation of the Total Energy Control System (TECS) enables simultaneous control of true airspeed and altitude of a fixed-wing aircraft. The code is implemented as a library which is used in the fixed-wing position control module.

TECS

As seen in the diagram above, TECS receives as inputs airspeed and altitude setpoints and outputs a throttle and pitch angle setpoint. These two outputs are sent to the fixed-wing attitude controller which implements the attitude control solution. However, the throttle setpoint is passed through if it is finite and if no engine failure was detected. It's therefore important to understand that the performance of TECS is directly affected by the performance of the pitch control loop. A poor tracking of airspeed and altitude is often caused by a poor tracking of the aircraft pitch angle.

::: info Make sure to tune the attitude controller before attempting to tune TECS. :::

Simultaneous control of true airspeed and height is not a trivial task. Increasing aircraft pitch angle will cause an increase in height but also a decrease in airspeed. Increasing the throttle will increase airspeed but also height will increase due to the increase in lift. Therefore, we have two inputs (pitch angle and throttle) which both affect the two outputs (airspeed and altitude) which makes the control problem challenging.

TECS offers a solution by representing the problem in terms of energies rather than the original setpoints. The total energy of an aircraft is the sum of kinetic and potential energy. Thrust (via throttle control) increases the total energy state of the aircraft. A given total energy state can be achieved by arbitrary combinations of potential and kinetic energies. In other words, flying at a high altitude but at a slow speed can be equivalent to flying at a low altitude but at a faster airspeed in a total energy sense. We refer to this as the specific energy balance and it is calculated from the current altitude and true airspeed setpoint. The specific energy balance is controlled via the aircraft pitch angle. An increase in pitch angle transfers kinetic to potential energy and a negative pitch angle vice versa. The control problem was therefore decoupled by transforming the initial setpoints into energy quantities which can be controlled independently. We use thrust to regulate the specific total energy of the vehicle and pitch maintain a specific balance between potential (height) and kinetic (speed) energy.

Total energy control loop

Energy loop

Total energy balance control loop

Energy balance loop

The total energy of an aircraft is the sum of kinetic and potential energy:

E_T = \frac{1}{2} m V_T^2 + m g h

Taking the derivative with respect to time leads to the total energy rate:

\dot{E_T} = m V_T \dot{V_T} + m g \dot{h}

Dividing by the mass gives the specific (per unit mass) total energy rate, in m²/s³:

\dot{E_s} = \frac{\dot{E_T}}{m} = V_T \dot{V_T} + g \dot{h}

Dividing it further by g V_T{} gives the normalised energy rate, which is dimensionless:

\dot{E} = \frac{\dot{E_s}}{g V_T} = \frac{\dot{V_T}}{g} + \frac{\dot{h}}{V_T} = \frac{\dot{V_T}}{g} + sin(\gamma)

where \gamma{} is the flight path angle. For small \gamma{} we can approximate this as:

\dot{E} \approx \frac{\dot{V_T}}{g} + \gamma

From the dynamic equations of an aircraft we get the following relation:

T - D = mg(\frac{\dot{V_T}}{g} + sin(\gamma)) \approx mg(\frac{\dot{V_T}}{g} + \gamma)

where T and D are the thrust and drag forces. In level flight, initial thrust is trimmed against the drag and a change in thrust results thus in:

\Delta T = mg(\frac{\dot{V_T}}{g} + \gamma)

As can be seen, \Delta T{} is proportional to \dot{E}{}, and thus the thrust setpoint should be used for total energy control.

Elevator control on the other hand is energy conservative, and is thus used for exchanging potential energy for kinetic energy and vice versa. To this end, the specific energy balance rate is defined as:

\dot{B_s} = g \dot{h} - V_T \dot{V_T}

and its normalised form as:

\dot{B} = \frac{\dot{B_s}}{g V_T} \approx \gamma - \frac{\dot{V_T}}{g}

TECS computes the energy rates in their specific form (m²/s³). It only converts the energy balance rate to its normalised form, an equivalent flight path angle, when forming the pitch setpoint.

Speed and Altitude Weighting

FW_T_SPDWEIGHT sets how the pitch loop weights speed errors against height errors in the energy balance. A value of 0 controls height only, which is also used when flying without an airspeed sensor. A value of 2 controls speed only, which is intended for gliders. The default of 1 weights both equally.

Pitch only trades altitude for speed up to the trim airspeed (FW_AIRSPD_TRIM): accelerating beyond trim is the throttle's job. With an airspeed setpoint above trim, what the pitch loop does therefore depends on the measured airspeed:

Pitch and throttle roles along the measured airspeed: below trim, pitch trades height for speed aiming for trim; between trim and the airspeed setpoint, pitch holds height only and the throttle accelerates; above the setpoint, pitch trades speed for height and the throttle is reduced

  • Below trim, pitch trades height for speed, but only aims for trim airspeed.
  • Between trim and the setpoint, pitch ignores airspeed and holds height only. It neither descends to gain speed nor pitches up to turn the throttle's acceleration into height. If the throttle saturates at FW_THR_MAX, the vehicle holds its altitude and settles at a lower airspeed than commanded.
  • Above the setpoint, pitch trades speed for height to correct the overspeed.

With an airspeed setpoint at or below trim there is no middle region, and pitch and throttle share speed control as weighted by FW_T_SPDWEIGHT.

These regions apply up to a speed weight of 1 and are faded out as the speed weight goes from 1 to 2, so with a weight of 2 pitch alone tracks the full airspeed setpoint. Gliders, which have no throttle to accelerate with, should therefore use FW_T_SPDWEIGHT = 2. The speed weight is also raised towards 2 automatically during underspeed recovery and fast descend, which therefore still use pitch to reach their airspeed setpoint.

Fixed-Wing Attitude Controller

Setpoint modificaiton

Most fixed-wing aircraft cannot generate a sustained yaw rate using the rudder alone. As a result, the yaw component of the quaternion attitude error should be removed before computing the control action.

This is achieved by premultiplying the setpoint quaternion with a rotation about the global down axis. The additional rotation cancels the yaw component of the attitude error while preserving the roll and pitch components.

The yaw offset is


\psi =-2\frac{\hat{q}_0 q_3 - \hat{q}_1 q_2 + \hat{q}_2 q_1 -\hat{q}_3 q_0}
{\hat{q}_0 q_0 - \hat{q}_1 q_1 - \hat{q}_2 q_2 + \hat{q}_3 q_3}

The quaternion representing the yaw offset is


ℚ_{\text{yaw}} =
\operatorname{normalize}
\left(
\begin{bmatrix}
1 \
0 \
0 \
\frac{\psi}{2}
\end{bmatrix}
\right)

The corrected setpoint quaternion is then obtained by applying the rotation


ℚ_{\text{sp, corrected}} = ℚ_{\text{yaw}} \otimes ℚ_{sp}

Quaternion based attitude controller

FW Attitude Controller Diagram

The attitude controller works using a cascaded loop method. The outer loop computes the error between the attitude setpoint and the estimated attitude that, multiplied by a gain (P controller), generates a rate setpoint. The inner loop then computes the error in rates and uses a PI (proportional + integral) controller to generate the desired angular acceleration.

The angular position of the control effectors (ailerons, elevators, rudders, ...) is then computed using this desired angular acceleration and a priori knowledge of the system through control allocation (also known as mixing). Furthermore, since the control surfaces are more effective at high speed and less effective at low speed, the controller - tuned for cruise speed - is scaled using the airspeed measurements (if such a sensor is used).

::: info If no airspeed sensor is used then gain scheduling for the FW attitude controller is disabled (it's open loop); no correction is/can be made in TECS using airspeed feedback. :::

The feedforward gain is used to compensate for aerodynamic damping. Basically, the two main components of body-axis moments on an aircraft are produced by the control surfaces (ailerons, elevators, rudders, - producing the motion) and the aerodynamic damping (proportional to the body rates - counteracting the motion). In order to keep a constant rate, this damping can be compensated using feedforward in the rate loop.

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.

r_{sp} = \frac{2g}{V_T}\left(q_0 q_1 + q_2 q_3\right)

This also helps to counteract adverse yaw effects and to damp the Dutch roll mode 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.

q_{sp}^{ff} = \frac{4g(q_0 q_1 + q_2 q_3)^2}{V(1 - 2q_1^2 - 2q_2^2)}

VTOL Flight Controller

VTOL Attitude Controller Diagram

This section gives a short overview on the control structure of Vertical Take-off and Landing (VTOL) aircraft. The VTOL flight controller consists of both the multicopter and fixed-wing controllers, either running separately in the corresponding VTOL modes, or together during transitions. The diagram above presents a simplified control diagram. Note the VTOL attitude controller block, which mainly facilitates the necessary switching and blending logic for the different VTOL modes, as well as VTOL-type-specific control actions during transitions (e.g. ramping up the pusher motor of a standard VTOL during forward transition). The inputs into this block are called "virtual" as, depending on the current VTOL mode, some are ignored by the controller.

For a standard and tilt-rotor VTOL, during transition the fixed-wing attitude controller produces the rate setpoints, which are then fed into the separate rate controllers, resulting in torque commands for the multicopter and fixed-wing actuators. For tailsitters, during transition the multicopter attitude controller is running.

The outputs of the VTOL attitude block are separate torque and force commands for the multicopter and fixed-wing actuators (two instances for vehicle_torque_setpoint and vehicle_thrust_setpoint). These are handled in an airframe-specific control allocation class.

For more information on the tuning of the transition logic inside the VTOL block, see VTOL Configuration.

Airspeed Scaling

The objective of this section is to explain with the help of equations why and how the output of the rate PI and feedforward (FF) controllers can be scaled with airspeed to improve the control performance. We will first present the simplified linear dimensional moment equation on the roll axis, then show the influence of airspeed on the direct moment generation and finally, the influence of airspeed during a constant roll.

As shown in the fixed-wing attitude controller above, the rate controllers produce angular acceleration setpoints for the control allocator (here named "mixer"). In order to generate these desired angular accelerations, the mixer produces torques using available aerodynamic control surfaces (e.g.: a standard airplane typically has two ailerons, two elevators and a rudder). The torques generated by those control surfaces is highly influenced by the relative airspeed and the air density, or more precisely, by the dynamic pressure. If no airspeed scaling is made, a controller tightly tuned for a certain cruise airspeed will make the aircraft oscillate at higher airspeed or will give bad tracking performance at low airspeed.

The reader should be aware of the difference between the true airspeed (TAS) and the indicated airspeed (IAS) as their values are significantly different when not flying at sea level.

The definition of the dynamic pressure is

\bar{q} = \frac{1}{2} \rho V_T^2

where \rho{} is the air density and V_T{} the true airspeed (TAS).

Taking the roll axis for the rest of this section as an example, the dimensional roll moment can be written

\ell = \frac{1}{2}\rho V_T^2 S b C_\ell = \bar{q} S b C_\ell

where \ell{} is the roll moment, b{} the wing span and S{} the reference surface.

The nondimensional roll moment derivative C_\ell{} can be modeled using the aileron effectiveness derivative C_{\ell_{\delta_a}}{}, the roll damping derivative C_{\ell_p}{} and the dihedral derivative C_{\ell_\beta}{}

C_\ell = C_{\ell_0} + C_{\ell_\beta}\:\beta + C_{\ell_p}\:\frac{b}{2V_T}\:p + C_{\ell_{\delta_a}} \:\delta_a

where \beta{} is the sideslip angle, p{} the body roll rate and \delta_a{} the aileron deflection.

Assuming a symmetric (C_{\ell_0} = 0{}) and coordinated (\beta = 0{}) aircraft, the equation can be simplified using only the rollrate damping and the roll moment produced by the ailerons

\ell = \frac{1}{2}\rho V_T^2 S b \left [C_{\ell_{\delta_a}} \:\delta_a + C_{\ell_p}\:\frac{b}{2V_T} \: p \right ]

This final equation is then taken as a baseline for the two next subsections to determine the airspeed scaling expression required for the PI and the FF controllers.

Static torque (PI) scaling

At a zero rates condition (p = 0{}), the damping term vanishes and a constant - instantaneous - torque can be generated using:

\ell = \frac{1}{2}\rho V_T^2 S b \: C_{\ell_{\delta_a}} \:\delta_a = \bar{q} S b \: C_{\ell_{\delta_a}} \:\delta_a

Extracting \delta_a{} gives

\delta_a = \frac{2bS}{C_{\ell_{\delta_a}}} \frac{1}{\rho V_T^2} \ell = \frac{bS}{C_{\ell_{\delta_a}}} \frac{1}{\bar{q}} \ell

where the first fraction is constant and the second one depends on the air density and the true airspeed squared.

Furthermore, instead of scaling with the air density and the TAS, it can be shown that the indicated airspeed (IAS, V_I{}) is inherently adjusted by the air density since at low altitude and speed, IAS can be converted to TAS using a simple density error factor

V_T = V_I \sqrt{\frac{\rho_0}{\rho}}

, where \rho_o{} is the air density as sea level, 15°C.

Squaring, rearranging and adding a 1/2 factor to both sides makes the dynamic pressure \bar{q}{} expression appear

\bar{q} = \frac{1}{2} \rho V_T^2 = \frac{1}{2} V_I^2 \rho_0

We can now easily see that the dynamic pressure is proportional to the IAS squared:

\bar{q} \propto V_I^2

The scaler previously containing TAS and the air density can finally be written using IAS only

\delta_a = \frac{2bS}{C_{\ell_{\delta_a}}\rho_0} \frac{1}{V_I^2} \ell

Rate (FF) scaling

The main use of the feedforward of the rate controller is to compensate for the natural rate damping. Starting again from the baseline dimensional equation but this time, during a roll at constant speed, the torque produced by the ailerons should exactly compensate for the damping such as

- C_{\ell_{\delta_a}} \:\delta_a = C_{\ell_p} \frac{b}{2 V_T} \: p

Rearranging to extract the ideal ailerons deflection gives

\delta_a = -\frac{b \: C_{\ell_p}}{2 \: C_{\ell_{\delta_a}}} \frac{1}{V_T} \: p

The first fraction gives the value of the ideal feedforward and we can see that the scaling is linear to the TAS. Note that the negative sign is then absorbed by the roll damping derivative which is also negative.

Conclusion

The output of the rate PI controller has to be scaled with the indicated airspeed (IAS) squared and the output of the rate feedforward (FF) has to be scaled with the true airspeed (TAS)

\delta_{a} = \frac{V_{I_0}^2}{V_I^2} \delta_{a_{PI}} + \frac{V_{T_0}}{V_T} \delta_{a_{FF}}

where V_{I_0}{} and V_{T_0}{} are the IAS and TAS at trim conditions.

Finally, since the actuator outputs are normalized and that the mixer and the servo blocks are assumed to be linear, we can rewrite this last equation as follows:

\dot{\mathbf{\omega}}_{sp}^b = \frac{V_{I_0}^2}{V_I^2} \dot{\mathbf{\omega}}_{sp_{PI}}^b + \frac{V_{T_0}}{V_T} \dot{\mathbf{\omega}}_{sp_{FF}}^b

and implement it directly in the rollrate, pitchrate and yawrate controllers.

In the case of airframes with controls performance that is not dependent directly on airspeed e.g. a rotorcraft like autogyro. There is possibility to disable airspeed scaling feature by FW_ARSP_SCALE_EN parameter.

Tuning recommendations

The beauty of this airspeed scaling algorithm is that it does not require any specific tuning. However, the quality of the airspeed measurements directly influences its performance.

Furthermore, to get the largest stable flight envelope, one should tune the attitude controllers at an airspeed value centered between the stall speed and the maximum airspeed of the vehicle (e.g.: an airplane that can fly between 15 and 25m/s should be tuned at 20m/s). This "tuning" airspeed should be set in the FW_AIRSPD_TRIM parameter.