flight-controller

A quadcopter flight controller on ESP32, ported from my PID-controller simulation to real hardware: an MPU-6050 over I2C, a complementary filter for attitude, and the same cascaded PID stack running in a deterministic 200 Hz loop under FreeRTOS.

GitHub repo: flight-controller
Last update: 2026/08/23

From simulation to hardware

The PID controller simulation is done: cascaded PIDs tuned, closed-loop flight tested, 3D visualizer rendering. It proved the control logic on idealized physics with perfect state estimation. But a simulation never encounters real-world problems like gyro drift, I2C latency, timer jitter, or sensor noise. Running the flight controller on real hardware means all these issues need to be addressed. The ESP32 flight controller takes the exact same control stack and runs it on an ESP32 under FreeRTOS with a real MPU-6050, a complementary filter, and a deterministic 200 Hz loop. Where the simulation computed QuadState from forces, this one measures it from an accelerometer instead. The architecture transfers, but the numbers don’t.

The simulation left behind four modules that needed no changes: pid.c, control.c, mixer.c, and the tests. Same cascade wiring, same mixer signs, same NED frame, same 23 unit tests passing. The PID design decisions (derivative on measurement, not on error; conditional-integration anti-windup; explicit dt) transfer because they’re about control structure, not the specific hardware they run on.

But, the hardware needed three additions the simulation didn’t need: an I2C chip driver (mpu6050.c, in my case), a complementary filter (imu.c) that fuses accelerometer and gyroscope into attitude, and a real-time loop — a FreeRTOS task woken by a 5 ms hardware timer, not a for loop in main.c.

So, in short, the key difference is the sensor layer: while the sim computes state from forces, this one reads it from sensor registers. Everything above the sensor layer stays identical.

The real-time loop

The sim loop is a for loop in main.c with dt = 0.005, one iteration per tick, no scheduling. That works because the loop body finishes in microseconds on a laptop. That’s not the case on a microcontroller.

On an ESP32 running ESP-IDF (FreeRTOS is the default SDK, not a choice) I started with FreeRTOS’ vTaskDelay: sleep for 5 ms, tick, repeat. The problem is that vTaskDelay measures from the moment you call it. If the control code takes 2 ms, the next tick fires at 7 ms, not 5, so the effective rate drops, and the error compounds each cycle.

I fixed it with the hardware timer first; building tinyRTOS later is what made the reason precise. A delay is a relative minimum — the next delay is measured from the end of the last iteration, so scheduling latency and body time accumulate and the loop drifts. A hardware timer fires on a fixed grid every 5 ms, on its own clock, regardless of what the scheduler is doing. The task blocks on ulTaskNotifyTake and wakes on every edge. Nothing accumulates.

One iteration, start to finish

Every tick starts with a single 14-byte I2C read of all six MPU-6050 registers we need (3 accel, 3 gyro), so they’re sampled at the same instant. The raw values are scaled to physical units and bias gets subtracted. Then the complementary filter (imu.c) derives roll and pitch from the accelerometer’s gravity vector using atan2, integrates the gyro rates, and blends them: 98% gyro trust, 2% accelerometer correction. Yaw is pure gyro integration, no absolute reference yet. The filtered angles and raw body rates then go into the control stack: six PID controllers run in control_update. Altitude is a single-loop PID acting on vehicle_state_t.z (in NED, altitude). Roll and pitch are two-stage cascades: an outer PID converts angle error into a desired body rate, an inner PID converts rate error into a torque command. Yaw is rate-only. The output is four differential commands that are sent through the mixer: for now, a fixed hover thrust of 6.0 N split equally, plus ±roll ±pitch ±yaw applied as differential offsets to each motor. Outputs clamp to [0, 2.5 N]. The four motor thrusts print to the serial console. Then the task blocks on ulTaskNotifyTake and waits for the next tick.

The convention bug

While the board was sitting flat on the desk, pitch was holding steady, but roll was jumping between +179° and −179°. I thought it was gyro bias first: the MPU-6050 has factory offsets, so I added a calibration period at the start, averaging 200 samples over one second of stationary readings and then subtracting them. Gravity was about 18% high, gyro zero-rate offsets were 0.1–0.3 rad/s, so calibration was necessary. But it didn’t fix the roll problem.

The asymmetry was the clue. If it were a bias, both axes would drift. Only roll jumped. The root cause is the atan2 discontinuity. On a flat board, the scaled accelerometer reads az ≈ −9.81. The roll formula atan2(ay, az) becomes atan2(±noise, −9.81), which lands exactly at ±π. Tiny noise on ay flips the output between +180° and −180° every tick. Pitch is immune because its denominator √(ay²+az²) is always positive, so atan2(0, positive) stays at zero.

The actual fix had two parts. First, corrected the atan2 formulas in imu.c to match the sign convention that the acceleration derivation assumes. Second, the mounting transform in mpu6050_read: the board was mounted component-side up with silkscreen +X forward, so the sensor’s +Y pointed left and +Z pointed up. NED requires +Y right, +Z down. The shortcut was to negate only Y and patch the atan2 signs. But a single-axis negation is a reflection (det −1): it flips pseudovectors (gyro) differently from vectors (accel). The gyro would point the wrong way relative to the accelerometer, and the complementary filter’s blend would be wrong. The two-axis negation (negate both Y and Z) is a 180° rotation about X (proper rotation, det +1). Both vectors and pseudovectors transform consistently. Calibration’s gravity term also flips sign to match: bias_az = sum/n + 9.81.

Then I verified direction. Tilt the board forward, pitch goes up, front motors slow, nose dips. Tilt left, roll goes up, right motor speeds, board rights itself. Four directions: forward, backward, left, right. If any one commands the wrong way, something in the sign chain is broken.

The sensor

The driver is four register writes and one burst read. Probe WHO_AM_I, expect 0x68. Wake the chip by clearing the SLEEP bit in PWR_MGMT_1 (it boots asleep, oscillator off, stale zeros in every register). Set the gyro range to ±1000°/s (default is ±250°/s, which saturates when you rotate the board fast). Then read: write 0x3B (the starting register), repeated start, read 14 bytes. The MPU-6050 auto-increments the pointer. Six values from a single sampling, all from the same instant.

The bias was the surprise. Datasheet says gyro bias is typically ±0.25°/s, about 0.004 rad/s; my board showed 0.1–0.3 rad/s, nearly two orders of magnitude. Cheap GY-521s often have sloppy factory calibration. Accelerometer Z reads ~11.6 m/s² when flat instead of 9.81. What’s reported in the datasheet can’t be trusted with cheap components: calibration is fundamental. I averaged 200 samples over one second, board stationary. Subtract the mean. Add 9.81 to the Z offset to remove gravity. One second of averaging is more than enough for bench testing.

The filter

Complementary filter is the simplest thing that works. Gyro is clean but drifts, while accel is absolute but noisy. The filter solves this by trusting the gyro for short timescales, the accel for long ones. One parameter: α = 0.98. At 200 Hz that means the accelerometer takes about 0.25 seconds to pull the estimate halfway toward truth. Roll and pitch from the filter are good enough for attitude-hold. Yaw is pure gyro integration: it drifts, but the controller uses yaw rate setpoints, so absolute heading doesn’t matter.

A Kalman filter comes later. The complementary filter doesn’t estimate bias as a state but assumes constant offset. Under temperature changes or sustained flight, that breaks. The ESKF in my eskf-vio project estimates position, velocity, orientation, and IMU biases jointly, fusing IMU with camera. GPS-denied navigation is the target. The complementary filter is the scaffold.

The current state

The board tilts, the motors respond: four directions, four restoring responses, verified at 200 Hz. The gains are sim values, so they’ll change on the real frame. What’s not done yet: PWM to ESCs, arming, failsafe and actual flight. The mixer outputs are currently logging to console, real motors aren’t connected yet, as I’m waiting on the missing hardware. The software is done up to the motor outputs.

What the sim never taught me

The sim had clean inputs: perfect angles, perfect rates, no noise, no sign ambiguities, no mounting transforms, no scaling steps. The hardware introduces layers between the math and the sensor, such as the I2C register map, the bias calibration, the axis negation for mounting orientation, the atan2 formulas. That’s where things often break.

In fact, the biggest bug was an assumption about which way Z points. A flat board, atan2(0, −9.81), roll flickers between +180° and −180° because noise crosses the ±π discontinuity. The fix was to derive the atan2 formulas from the rotation matrices, verify the sign chain end-to-end, calibrate empirically.