PX4 Source Dive #4: 4-Layer Cascade PID — From Position Setpoint to Motor PWM

Part 4 of the PX4 Source Code Deep Dive series. Originally published by 码农实验室 (CoderLab) on WeChat.

In the first three installments, we dissected PX4’s data flow, EKF2, and uORB messaging. But one critical question remained unanswered: after EKF2 figures out where the drone is and which way it’s facing, who decides how to fly?

The answer is a four-layer nested control chain that runs from outer to inner:

Position Setpoint → Position Controller → Velocity Setpoint → Velocity Controller → Thrust + Attitude Setpoint
→ Attitude Controller → Rate Setpoint → Rate Controller → Torque → Mixer → Motor PWM

Each layer’s output feeds the next layer as input. Understand this chain and you understand why a PX4 drone can hover, hold position, and fly waypoints. Don’t understand it, and PID tuning is just blind guessing. At Aomway, we break down every layer with actual source code.

Why Four Layers?

The intuitive approach is tempting: the drone needs to reach position X, so just compute a force vector and push it there. This fails because position-to-force involves double integration (force → acceleration → velocity → position). Two integrators means 180° phase lag — pure proportional control will always oscillate.

Cascade PID’s insight: don’t let the outer loop control the physical quantity directly. Let it control an intermediate variable, and let the inner loop stabilize that variable.

• Position Controller: Outputs target velocity (1st derivative of position — easier to control)
• Velocity Controller: Outputs thrust + target attitude (tilt to move horizontally)
• Attitude Controller: Outputs target angular rate (1st derivative of attitude — faster)
• Rate Controller: Outputs torque command (innermost, fastest response)

Each layer handles exactly one derivative relationship. Outer layers handle integration, inner layers handle proportional response. Bandwidths can be independently designed — outer slow, inner fast — for layered stability.

Layer 1: Position Controller (50 Hz)

The outermost and slowest layer. Its job: given target position and current position, compute the target velocity.

Inputs & Outputs

Signal	Source	uORB Topic
Current Position (NED)	EKF2	vehicle_local_position
Current Velocity (NED)	EKF2	vehicle_local_position
Position Setpoint	Navigator	trajectory_setpoint

Output: Velocity setpoint via trajectory_setpoint (same topic — not ideal design, but PX4 uses timing to prevent self-reading).

Algorithm: P-Only

// src/modules/mc_pos_control/MulticopterPositionControl.cpp
Vector3f pos_error = _pos_sp - _pos;
_vel_sp(0) = _params.pos_p(0) * pos_error(0); // North
_vel_sp(1) = _params.pos_p(1) * pos_error(1); // East
_vel_sp(2) = _params.pos_p(2) * pos_error(2); // Down

P-only — no I, no D. Here’s why:

• No I: Position integration causes overshoot and oscillation. Wind-induced steady-state error is compensated by the velocity controller’s I term downstream.
• No D: The derivative of position is velocity, which has independent sensor measurement via EKF2 — no numerical differentiation needed.

Parameters: MPC_XY_P (default 0.95), MPC_Z_P (default 1.0). Higher = more aggressive position tracking. Too high = oscillation.

Safety: Velocity outputs are clamped (MPC_XY_VEL_MAX, MPC_Z_VEL_MAX_UP/DN) — not for control, for safety.

Layer 2: Velocity Controller (100 Hz)

This is the most complex layer — the bridge between position and attitude. Its output spans two coupled quantities: thrust (scalar) and attitude (3D rotation).

The Physics: Thrust Always Along Body Z

Multirotor thrust is always along the body Z-axis (upward). To move horizontally, the drone must tilt — redirecting thrust to produce a horizontal component. This coupling means thrust and attitude can’t be computed independently.

// Velocity PID with gravity compensation
// Full code: see control_velocity() in MulticopterPositionControl.cpp

Key physics in these few lines:

• Gravity compensation: At hover, acceleration is zero but thrust must counteract gravity. In NED (Z down), gravity is +9.81, thrust is along body Z (up), so we add 9.81 to the Z component.
• Thrust direction = target attitude: To move north, the body Z-axis must tilt northward — the tilt angle is determined by the required horizontal acceleration.
• Thrust and attitude are coupled outputs: The combined force vector is decomposed into magnitude (thrust) and direction (attitude).

Full PID with separate horizontal/vertical parameters: MPC_XY_VEL_P/I/D for horizontal, MPC_Z_VEL_P/I/D for vertical — because horizontal relies on tilting while vertical uses direct thrust.

Layer 3: Attitude Controller (250 Hz)

Receives target quaternion, outputs target angular rate. P-only again, because angular rate is the derivative of attitude.

The Quaternion Problem

Unlike positions or velocities, you can’t simply subtract quaternions — quaternion space is nonlinear (on a unit sphere). Direct subtraction takes the “straight line” rather than the “arc.”

PX4’s solution: convert the quaternion error to axis-angle, then map to rate via P gain.

// Error quaternion: q_err = qd * q^(-1) = rotation from current to target
Quatf qe = qd * q.inversed();

// Extract axis-angle from error quaternion
// q = [cos(θ/2), sin(θ/2)*axis]
// Small-angle approximation: axis-angle ≈ 2 * q_imaginary
Vector3f angle_error;
if (qe_norm_imag > 1e-4f) {
    float angle = 2.0f * atan2f(qe_norm_imag, qe(0));
    angle_error = angle * qe_imag / qe_norm_imag;
} else {
    angle_error = 2.0f * qe_imag; // small-angle approx
}

// P-only: rate_setpoint = P * angle_error
_rates_sp = _params.att_p.emult(angle_error);

Small-angle approximation: When error is small (normal flight), sin(θ/2) ≈ θ/2 and cos(θ/2) ≈ 1, so 2 × imaginary_part ≈ θ × axis. Error < 1% within ±30° — sufficient for flight control.

Why P-only? No I (rate controller’s I compensates constant disturbance torque). No D (gyroscope directly measures angular rate). Parameters: MC_ROLL_P, MC_PITCH_P (typically 2.5–4.0), MC_YAW_P (typically 1.5–2.5, yaw response is naturally slower).

Layer 4: Rate Controller (250 Hz)

The innermost and fastest layer. Gyroscope measurements at 250 Hz+ feed directly into this controller. Output is torque commands — which the mixer converts to motor speed differentials.

Full PID with anti-windup and feedforward:

// Rate PID with anti-windup and feedforward
// Full code: see control_rates() in MulticopterRateControl.cpp

Three Critical Design Details

1. Anti-Windup: The rate I-term is the most dangerous. If the drone is on the ground (motors off), the rate controller is already integrating errors from gyro noise. On arming, the accumulated integral outputs a massive torque — causing a violent flip. PX4 freezes integration when thrust < 0.01, and clamps it to ~33% of max torque.

2. D-term Implementation: D is based on angular acceleration (rate derivative), not error derivative. Gyro noise amplifies through differentiation, so D is very small: roll D=0.004, pitch D=0.004, yaw D=0.0. Yaw needs no D because counter-torque provides natural damping.

3. Feedforward: Target rate × feedforward gain = I × α (moment of inertia × angular acceleration). Usually set to 0 unless you know your inertia. Getting it wrong is worse than not having it.

Sample gains: 250mm racer: P=0.15, I=0.2, D=0.004. 450mm+ frame: P=0.08, I=0.15, D=0.002.

Mixer: From Torque to Motor PWM

Four PID layers produce a thrust scalar (0–1) and three torque components (roll/pitch/yaw, −1 to 1). But motors speak PWM (0–100% throttle). The mixer translates between them.

X-Quadrotor Mixer Matrix

Four motors (top view): motors 1 and 3 spin CCW, motors 2 and 4 spin CW.

Motor	Thrust	Roll	Pitch	Yaw
1	+1	−1	+1	−1
2	+1	−1	−1	+1
3	+1	+1	+1	+1
4	+1	+1	−1	−1

The forward matrix maps motor commands to forces. The mixer computes the inverse: given desired T, τroll, τpitch, τyaw, solve for M1–M4.

[M1]   [1 -1 +1 -1] [T     ]
[M2] = [1 -1 -1 +1] [τroll ] × 0.25
[M3]   [1 +1 +1 +1] [τpitch]
[M4]   [1 +1 -1 -1] [τyaw  ]

Saturation: The Mixer’s Biggest Problem

All four motors are bounded to [0, 1]. PID doesn’t know this — it may request motor values > 1.0. Saturation creates two problems:

• Priority conflict: Thrust and torque compete. Full-throttle climb (T≈1) leaves zero headroom for torque → loss of attitude control.
• Integral windup: Rate I-term accumulates while mixer is saturated. When thrust drops, the accumulated integral causes massive torque overshoot.

PX4’s solution: Prioritize attitude over thrust. If any motor saturates (>1.0), scale all motors down proportionally — thrust decreases but torque ratios are preserved. If any motor goes negative, shift all motors up. The rationale: losing attitude control is catastrophic (drone flips, thrust vector points wrong direction). Losing altitude gives time to recover.

Timing: Who Runs When

Layer	Frequency	Priority
Rate Controller	250 Hz	Highest (FIFO real-time)
Attitude Controller	250 Hz	High
Velocity Controller	100 Hz	Medium
Position Controller	50 Hz	Low

Attitude + Rate run in the same module (mc_rate_control), serialized per cycle. Position + Velocity run in mc_pos_control. The two modules connect via uORB — mc_pos_control publishes vehicle_attitude_setpoint, mc_rate_control subscribes. At 250 vs 100 Hz, the rate controller reads the same attitude setpoint for ~5 consecutive cycles — not a problem, it just maintains the current rate.

Tuning Guide: Inside-Out

Iron rule: tune from inside out. Inner layers unstable = outer layers useless.

Step 1: Rate Loop

Secure the drone on a desk (props off). Perturb by hand.

• P: Start 0.05, increment 0.01 until response is fast but not oscillating.
• I: Start 0.05. Apply constant torque (finger push), verify I eliminates steady-state error. Too high = oscillation.
• D: Usually leave at default. If high-frequency oscillation, decrease P first, don’t increase D.

Step 2: Attitude Loop

Props on, low hover.

• P: Start 2.0. Move roll/pitch stick, observe tracking. Slow = increase, oscillating = decrease.
• Yaw P typically smaller than roll/pitch.

Step 3: Velocity Loop

Position hold. Push the drone. P from 0.5 — does it return fast? I from 0.1 — does it hold in wind? D typically 0.

Step 4: Position Loop

Fly waypoints. P from 0.95 (default is good). Only P. If drifting, the problem is likely velocity I, not position P.

DShot: The Last Mile

The mixer outputs normalized values [0–1]. The final conversion depends on protocol:

Protocol	Type	Update Rate	Calibration
PWM	Analog	400–490 Hz	Required
DShot	Digital (11-bit + CRC)	1 kHz+	None

DShot advantages: no calibration (0–2047 directly maps to 0–100%), higher update rate, bidirectional telemetry (RPM feedback), and CRC-protected against interference. If your ESCs support DShot, use it.

PX4 vs ArduPilot: PID Implementation Differences

Layer	PX4	ArduPilot
Position	P only	P + I (AC_PosControl)
Velocity	PID	PID
Attitude	P only	P + D (angle error derivative)
Rate	PID + FF	PID
Mixer	Matrix inversion + saturation	Matrix inversion + priority strategy
Update Freq	Uniform 250 Hz	Configurable (typically 400 Hz for attitude/rate)

The most notable difference: ArduPilot’s position controller includes an I term, PX4’s doesn’t. ArduPilot argues that wind-induced position drift needs integral compensation. PX4 counters that position integration increases overshoot, and velocity I already handles wind. Both fly well — PX4 is more conservative (harder to create dangerous parameter combos), ArduPilot is more flexible (but higher tuning bar).

Summary: The Complete Control Chain

The four-layer cascade PID compressed into one principle:

Outer layers compute setpoints, inner layers track setpoints. Each layer handles exactly one derivative relationship, progressing layer by layer.

Layer	Type	Input	Output	Key Insight
Position	P only	Pos error	Vel setpoint	No I (velocity I compensates wind); No D (velocity is sensor-measured)
Velocity	PID	Vel error	Thrust + Attitude	Gravity compensation in NED: +9.81 on Z; coupled output via force vector decomposition
Attitude	P only	Quaternion error	Rate setpoint	Axis-angle from q error; small-angle approximation; rate D replaces attitude D
Rate	PID+FF	Rate error	Torque	Anti-windup freezes I when throttle=0; D on angular acceleration; FF = I×α
Mixer	Matrix	T + 3τ	4× PWM	X-quad 4×4 inverse; saturation: preserve torque ratios, sacrifice thrust

Frequently Asked Questions

---

Any questions pls contact: [email protected]