Cascaded Controller

#include <branes/reflex/control/cascade.hpp>

Grade: C, under the building block standard. Its closed-loop evidence is design verification on a nominal plant, not validation. The V&V plan has the assessment and the path to A.

This is the developer guide: the loops, the schedule, anti-windup, the default gains, and the API. The explainer shows the loops at work, with widgets that run the cascade in your browser, and the deployment guide covers the configuration and wiring it into a flight stack.

branes::reflex::control::Cascade<T> is the multirotor flight controller behind racing-drone’s B0 baseline. It turns a vehicle state and a setpoint into a body wrench (collective thrust and torque) for the mixer. The interface is plain state in and wrench out, so the same cascade runs in SITL, in hardware-in-the-loop, and on the flight controller. It is templated on the scalar type, allocates nothing, and has no data-dependent loops.

PositionController ──(q_sp, T)──▶ AttitudeController ──(ω_sp)──▶ RateController ──(τ)──▶ Mixer
   position.hpp                      attitude.hpp                    rate.hpp

Each loop is its own header and can be used alone. The conventions are the quadrotor model’s: world frame z up, body frame x forward, y left, z up, attitude as a body-to-world unit quaternion.

Usage

using namespace branes::reflex;
const model::QuadrotorParams<double> vehicle{};             // RP-1
const control::Mixer<double> mixer(control::mixer_params_from(vehicle));
const control::CascadeConfig<double> cfg = control::cascade_config_from(vehicle);
if (!control::cascade_config_valid(cfg)) {
    return;  // the vehicle cannot hover inside the derived limits: do not build a Cascade
}
control::Cascade<double> cascade(cfg);

control::PositionSetpoint<double> sp;
sp.position = vec3(0.0, 0.0, 2.0);
// Every 1 ms:
const control::Wrench<double> w = cascade.update(state, sp, 1e-3);
const control::MixerOutput<double> out = mixer.mix(w);
cascade.feedback(out);  // the integrators see the mixer's saturation next tick
const model::RotorCommand<double> u = mixer.to_rotor_command(out.thrusts);

Schedule and modes

update() runs once per rate-loop tick, with dt the rate-loop period. The attitude loop runs on every attitude_divider-th tick and the position loop on every position_divider-th, each holding its output in between: by default 1 kHz, 250 Hz, and 50 Hz. The position divider is a multiple of the attitude divider, so a new position-loop target is flown on the same tick. The position loop’s PIDs integrate over dt · position_divider.

There are two modes, one update() overload each:

Position mode takes a PositionSetpoint: position, velocity and acceleration feed-forward, and yaw. All four loops run.
Attitude mode takes an AttitudeTarget: an attitude and a collective thrust. The position loop does not run. Use it for a pilot’s angle mode or for an outer loop of your own.

Switching from attitude to position mode resets the position loop and realigns the schedule, so both outer loops run on that tick and no attitude-mode target is held. A dt ≤ 0 is a timing fault in the caller: update() returns the last wrench and changes nothing.

The loops

Rate (rate.hpp). One PID per body axis outputs angular acceleration, and the torque is τ = J·α + ω × Jω. Because the gains act on acceleration, they do not depend on the vehicle’s inertia (k_p in 1/s, k_i in 1/s², k_d dimensionless), so one loop shape carries across vehicle sizes; the ω × Jω term cancels the plant’s gyroscopic coupling. Each axis’s output limit is the angular acceleration the rotors can give that axis.

Attitude (attitude.hpp). Tilt-prioritized control after Brescianini and D’Andrea. The error q_e = q⁻¹ ⊗ q_sp is split into a tilt q_red, which moves the thrust axis, followed by a yaw q_yaw about it. Roll and pitch rates come from the tilt alone and the yaw rate from the yaw alone, so the thrust axis converges at its own gain whatever the heading does. The error magnitude 2·sin(θ/2) grows up to 180°, q and −q give the same output, and no Euler angles appear. The attitude loop is stateless.

Does a 90° turn change how the vehicle tilts? 1× real time

World: z up · Body: x forward, y left, z up
Rotors (PX4 Quad X): 0 FR, 1 RL spin CCW · 2 FL, 3 RR spin CW
Translucent: the vehicle that only tilts

Thrust arrows: 1 N = 4 cm · Grid: 0.5 m · Body axes: x red, y green, z blue

Tilt error [rad] tilt and turn tilt only

Heading still to turn [rad] tilt and turn

t = 0.000 s / 1.500 s Speed

Keys (with the player focused): Space play/pause · ← → one sample (Shift: ten) · Home/End · [ ] speed

Two RP-1 vehicles in attitude mode are given the same 30° roll at 0.1 s; the solid one is also turned 90° about its thrust axis. Their tilt errors fall together, within 2° of the target 2 ms apart, and the vehicles stay within 0.1 m of each other. The turn follows on its own, with the yaw-rate setpoint at its 4 rad/s limit until about 0.4 s. Pass criterion: the turning vehicle's tilt is within 2° no later than the other's, to one attitude-loop period (4 ms).

target: roll 30°; the solid vehicle also turns 90°

✓ passBB-I2 tilt_settle_delay_from_yaw = 0.002 s (<= 0.004), computed by the scenario harness

Position (position.hpp). A position P loop makes a velocity setpoint (the position gain times the error, plus the velocity feed-forward, limited horizontally and vertically). Velocity PIDs make an acceleration, plus the acceleration feed-forward. The specific force f = a + g·e₃ is saturated vertical first: the vertical component is clamped to the thrust range, then the horizontal part to the tilt limit and the remaining thrust. The attitude target puts the body z axis along f with the commanded heading, and the collective thrust is f projected on the current body z axis, so altitude holds while the tilt converges.

Anti-windup

Each PID freezes its integral while its own output is saturated and the error pushes further in. Two outer reports extend that:

The mixer’s Saturation report (saturation_from()): roll and pitch when the mixer scaled them down, yaw when it gave yaw up, and the collective when it shifted the thrust. feedback() stores it, and the next tick passes it to the rate PIDs (roll, pitch, yaw) and, on a position tick, the collective to the vertical velocity PID, as Pid’s external_saturation (+1 cut high, −1 cut low).
The position loop’s force limits: a velocity integrator freezes while its axis was cut on the previous position tick. The capped climb shows it.

Without feedback(), the rate integrators wind up against the mixer while it cuts, and the vehicle overshoots once the cut ends (the PID’s external saturation).

Default gains

cascade_config_from(params) designs the loops from the vehicle by bandwidth separation, starting from the motor lag τ (ω_n = 1/τ):

Loop	Gain	Why
Rate	k_p = 1/τ, k_d = 0.4, k_i = k_p·ω_n/10, derivative filter τ/10	PD on the measurement places the closed loop at ω_n with ζ = 0.7; the integral zero sits a decade below
Attitude	ω_n/5 roll and pitch, ω_n/10 yaw	Keeps overshoot under 25 % with motors twice as slow, 30 % more inertia, and 4 ms of delay
Velocity	k_v = ω_n/14, k_i = k_v²/10, integral ≤ g/2	The integral bound covers a 33 % hover-thrust error
Position	ω_n/32	With the velocity loop, ζ ≈ 0.76

The limits come from the vehicle too: angular acceleration from the full differential thrust, collective thrust from 10 % of hover to 80 % of 4·T_max (the rest stays free for torque), tilt 40°. The header derives each choice, and the deployment guide lists RP-1’s values.

The derived config is valid only for a vehicle whose thrust-to-weight ratio exceeds 1.25, so that it can hover inside the collective limit. For a weaker vehicle cascade_config_from() returns a config that fails cascade_config_valid(). Check it before constructing a Cascade: the constructor only asserts validity, and release builds compile the assertion out. To use other gains, fill a CascadeConfig directly and check it the same way.

API

Member	Contract
`VehicleState<T>`	Position and velocity (world, m and m/s), attitude (body → world unit quaternion), body rates (rad/s): an estimator’s output, or truth in SITL
`PositionSetpoint<T>`	Position [m], velocity and acceleration feed-forward [m/s, m/s²], yaw ψ [rad]
`AttitudeTarget<T>`	Attitude (body → world) and collective thrust [N]
`CascadeConfig<T>`	The three loops’ configs and the two dividers. A plain aggregate
`cascade_config_valid(c)`	Every loop’s config valid, `attitude_divider` ≥ 1, `position_divider` a positive multiple of it (I1)
`cascade_config_from(p)`	The default gains and limits for vehicle p. Pre: `quadrotor_params_valid(p)`. Valid iff 0.8·4·T_max > m·g
`Cascade(cfg)`	Pre: `cascade_config_valid(cfg)`. Every loop reset, tick 0
`update(x, sp, dt)`	Position mode. Pre: x finite, unit attitude; sp finite; dt > 0, else returns the last wrench. Post: the wrench’s thrust is `target().thrust`; every `rate_setpoint()` component within ±ω_max
`update(x, target, dt)`	Attitude mode. Pre: as above; target attitude unit, thrust finite. Post: as above
`feedback(out)`	Pre: out is `mix(wrench())`. Post: the next tick’s integrators see its report. `noexcept`
`reset()`	Back to the state after construction. `noexcept`
`config()`, `position()`, `attitude()`, `rate()`	The config and each loop, for inspection (`position().cut()`, `position().axis(i).integral()`, …)
`target()`, `rate_setpoint()`, `wrench()`, `saturation()`	The held outputs of each loop, the last wrench, and the stored mixer report

Cascade holds four invariants: a valid config (I1), the tick counter inside the position period (I2), rate setpoints within their limits and a unit attitude target (I3), and the wrench’s thrust equal to the target’s (I4). Debug builds assert them after every tick.

Not covered

Throttle-PID attenuation, I-term relax, gain scheduling, INDI, drag compensation, setpoint differentiation, and sensor noise are outside the cascade. The deployment guide lists them with the rest of the contract.