PID control

Lesson 3 told the drone how it is tilted. This lesson decides what to do about it. The question sounds simple: if the drone is 10 degrees off, how hard should the motors push to fix it? Push too gently and it drifts. Push too hard and it overshoots, then overcorrects the other way, and soon it is shaking itself apart. The PID controller is the classic answer, and it runs inside almost every drone ever built.

Error and output

The controller compares what you want (the setpoint) with what the sensors say (the measurement):

error = setpoint - measurement

Its output goes straight into the mixer from lesson 1 as the roll (or pitch, or yaw) command, normalised from -1 to 1. If the drone should be level (setpoint 0 deg) and it is tilted by 10 deg, the error is -10 deg and the output pushes the other way.

A PID has three terms, each looking at the error from a different angle: the present, the past and the future.

P, I and D

P (proportional) reacts to the present error: P = Kp * error. With Kp = 0.02 per degree and an error of 10 deg, P = 0.02 * 10 = 0.2. P acts like a spring: the farther from target, the harder it pulls.

I (integral) reacts to the accumulated past: I = Ki * (sum of error * dt). With Ki = 0.005 per degree-second, an error of 2 deg that stays for 4 s adds up to 2 * 4 = 8 deg*s, so I = 0.005 * 8 = 0.04. I is a slow learner. It removes a steady error that P alone cannot, such as a battery mounted slightly off-centre or a constant wind.

D (derivative) reacts to how fast things are changing, which predicts the near future: D = -Kd * (rate of the measurement). If the drone is closing on the target at 20 deg/s and Kd = 0.006 s, then D = -0.006 * 20 = -0.12. It brakes in advance, like a shock absorber.

Add them up for one example moment: 0.2 + 0.04 - 0.12 = 0.12. The motors get a modest push because the drone is already moving in the right direction.

Why a drone needs D

An airframe is mostly inertia. A command does not set the angle, it sets the angular acceleration. Suppose a unit of output gives G = 1000 deg/s^2. With P only, the closed loop is an undamped spring: it oscillates forever. Adding D supplies the damping. For a P+D loop on this plant:

• natural frequency: wn = sqrt(Kp * G) = sqrt(0.02 * 1000) = 4.47 rad/s
• damping ratio: zeta = Kd * G / (2 * wn) = 6 / 8.94 = 0.67
• overshoot to a step: exp(-pi * zeta / sqrt(1 - zeta^2)) = exp(-2.84) = 5.9 percent

So zeta near 0.7 is the sweet spot: quick, with only a little overshoot. Too low and it rings, too high and it feels sluggish.

Tuning by hand

Real tuning goes in a fixed order, changing one gain at a time with I and D at zero to start.

1. Raise P until the drone reacts crisply, then until it starts to oscillate. Back off to about 60 percent of that value.
2. Raise D until the oscillation is damped and the response looks like a firm stop instead of a bounce. If the motors get hot or you hear a high buzz, D is amplifying noise: lower it or filter harder.
3. Raise I last, only enough to hold the attitude in wind or after a throttle change. Too much I gives slow wobbles.

Tune on a tether, a test stand, or in very short low hovers in a large open area, never near people. Change gains by about 20 percent per step.

Kp1.20

Ki0.40

Kd0.30

Move the sliders and read the response. Kp alone gives a fast but ringing curve. Add Kd and the ringing dies. Add Ki and a steady offset slowly disappears, at the price of some overshoot.

Windup and derivative kick

Two traps catch every beginner.

Integral windup. Imagine the drone sits on the ground with the pilot holding a 10 deg tilt command. The error cannot be corrected because the props are not producing lift, but the integral keeps growing: after 10 s it holds 10 * 10 = 100 deg*s, and I = 0.005 * 100 = 0.5, half of the whole output range. At take-off it will violently flip the drone. The fixes are to clamp the integral term to a small limit, and to reset or freeze it when the drone is disarmed or on the ground.

Derivative kick. If the pilot snaps the setpoint from 0 to 10 deg in one 4 ms control cycle, the derivative of the error is 10 / 0.004 = 2500 deg/s. With Kd = 0.006 the D term is 15, fifteen times the entire output range, for a single cycle. The cause: error = setpoint - measurement, so d(error)/dt = d(setpoint)/dt - d(measurement)/dt. The setpoint part is a spike. The cure is to take the derivative of the measurement only. The measurement is continuous, so there is no kick, and the gyro already gives you its rate directly.

Gyro noise is also amplified by differentiation, so real firmware passes the D term through a low-pass filter.

Discrete-time code

The controller runs once per loop iteration with a fixed dt. This version keeps the integral as an already-scaled term so you can change Ki in flight without a jump, clamps it, and uses the derivative of the measurement.

struct PID {
  float kp, ki, kd;
  float iTerm = 0;          // integral, already multiplied by ki
  float iLimit = 0.3;       // anti-windup clamp on the I contribution
  float prevMeas = 0;
  float dFilt = 0;          // low-pass filtered derivative
  float dAlpha = 0.2;       // 1 = no filtering, smaller = smoother

  PID(float p, float i, float d) : kp(p), ki(i), kd(d) {}

  float update(float setpoint, float measurement, float dt, bool onGround) {
    float error = setpoint - measurement;

    if (onGround) {
      iTerm = 0;                                    // freeze and clear on the ground
    } else {
      iTerm += ki * error * dt;
      iTerm = constrain(iTerm, -iLimit, iLimit);    // clamp: no windup
    }

    float dMeas = (measurement - prevMeas) / dt;    // derivative of measurement
    prevMeas = measurement;
    dFilt += dAlpha * (dMeas - dFilt);

    float out = kp * error + iTerm - kd * dFilt;
    return constrain(out, -1.0, 1.0);
  }
};

PID rollPID(0.02, 0.005, 0.006);

void setup() {}

void loop() {
  // roll comes from lesson 3, rollSetpoint from the pilot, in degrees
  // float cmd = rollPID.update(rollSetpoint, roll, 0.004, false);
  // then feed cmd into mix() from lesson 1
}

To see the numbers, here is the same controller in Python driving a simulated airframe with G = 1000 deg/s^2 per unit output, tested with a 10 deg step:

class PID:
    def __init__(self, kp, ki, kd, i_limit=0.3):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.i_term, self.i_limit = 0.0, i_limit
        self.prev, self.d_filt, self.d_alpha = 0.0, 0.0, 0.2

    def update(self, setpoint, meas, dt):
        error = setpoint - meas
        self.i_term += self.ki * error * dt
        self.i_term = max(-self.i_limit, min(self.i_limit, self.i_term))
        d_meas = (meas - self.prev) / dt
        self.prev = meas
        self.d_filt += self.d_alpha * (d_meas - self.d_filt)
        out = self.kp * error + self.i_term - self.kd * self.d_filt
        return max(-1.0, min(1.0, out))

for ki in (0.0, 0.005):
    dt, angle, rate, peak = 0.004, 0.0, 0.0, 0.0
    pid = PID(0.02, ki, 0.006)
    for _ in range(int(3 / dt)):               # simulate 3 seconds
        u = pid.update(10.0, angle, dt)
        rate += 1000.0 * u * dt                # output sets angular acceleration
        angle += rate * dt
        peak = max(peak, angle)
    print(f"ki={ki}: overshoot {(peak - 10) / 10 * 100:.1f} percent")
# ki=0.0 gives about 4 percent, ki=0.005 about 12 percent

The overshoot with Ki = 0 is close to the 5.9 percent predicted by the formula. The extra overshoot with Ki > 0 is the price you pay for removing steady error.