project

ESP32 Flight Controller

200Hz cascaded PID flight controller on ESP32, FreeRTOS, MPU-6050 + complementary filter — real-hardware port of my PID-controller sim

GitHub repo: esp32-controller
Last update: 2026/06/18

The premise

The PID controller sim is closed — all six cascaded PIDs tuned, the full flight scenario flying in NED, the 3D visualizer rendering. The simulation is a solved problem.

The ESP32 controller is the hardware evolution. The control stack — pid.c, mixer.c, control.c — is ported from the simulation and runs under ESP-IDF on bare metal with a real IMU. Same cascade, same anti-windup logic, same derivative-on-measurement, same mixer sign conventions. The simulation proved the control logic works on idealized physics with perfect state estimation. The hardware project asks: does it still work when the state estimate is noisy, the gyro drifts, the I2C bus has latency, and the timer jitter is real?

The projects are separate repositories but share the control stack design. What’s new here is everything below it: the I2C chip driver, the sensor fusion, and the real-time scheduling — the three layers that turn a mathematical controller into a physical one.

What works today

The full sensing and timing pipeline runs on hardware at a deterministic 200 Hz. The IMU is read, calibrated, and fused into a live attitude estimate on a fixed real-time loop. The control stack is ported and tested but is not yet wired to the live sensor data — that integration is the next milestone.

To put it plainly: the drone knows its orientation in real time. It doesn’t yet act on it.

Why from scratch

Flight controller firmware is a solved problem. Betaflight, INAV, ArduPilot — mature stacks that fly thousands of drones. This project isn’t trying to compete with them. It’s trying to understand them deeply enough to eventually do things they weren’t designed for.

When you use someone else’s sensor driver, you inherit their assumptions about timing, their error-handling philosophy, their calibration defaults. When you write your own I2C driver from the register map, you learn that the MPU-6050 boots in sleep mode and won’t produce data until you clear a bit in a power management register — a detail you’d never encounter if mpu6050_init() just worked. When you implement the complementary filter by deriving the accelerometer angle formulas from the rotation matrices instead of copying them, you understand why the filter breaks during sustained acceleration and why a gyro-only dead-reckoning mode matters.

Every layer is a first-principles understanding. That understanding is the deliverable, not the firmware.

Architecture

Six modules with a clean pipeline. Each layer owns one concern and has a narrow, well-defined interface:

main.c        I2C bus init, FreeRTOS task + 200 Hz timer, pipeline glue
  ↓
mpu6050.c     I2C chip driver: raw registers → calibrated physical units
  ↓
imu.c         Complementary filter: accel + gyro → roll / pitch / yaw
  ↓
control.c     Cascaded PID: attitude + setpoints → control commands   [not yet wired]
  ↓
mixer.c       Control commands → per-motor thrusts

The chip driver knows nothing about the drone. The filter knows nothing about the specific chip. The control stack is independent of how state is measured. Each boundary is a plain struct — no globals, no cross-layer coupling, no hidden state.

This is the same architecture as the simulation project, but with one crucial difference: quad.c — the physics module — is replaced by the real world. The simulation computed QuadState from forces and torques. The hardware measures it from an IMU. The control stack above doesn’t care which one provides the state.

The I2C driver

The MPU-6050 driver (mpu6050.c) talks directly to the chip over I2C using the ESP-IDF command-link API. Nothing buffered, no DMA, no abstraction layer — just register reads and writes.

The bus is owned by main.c, not by the driver. The mpu6050_t struct stores only port and addr. This is a deliberate choice: the planned GY-87 upgrade adds a magnetometer and barometer on the same I2C bus, and they’ll share I2C_NUM_0 without either driver needing to know about the other.

The startup sequence is four steps, each with a clear failure path:

mpu6050_init(&imu, I2C_NUM_0, 0x68);     // store port/addr, zero bias fields
mpu6050_probe(&imu);                     // read WHO_AM_I, expect 0x68
mpu6050_wake(&imu);                      // clear sleep bit in PWR_MGMT_1
mpu6050_calibrate(&imu, 200);            // 1-second bias average

If the probe fails (wrong address, dead chip, missing pull-ups), the task deletes itself with a visible failure message. No silent degradation.

The data read is a single I2C transaction: write the starting register address (0x3B), issue a repeated start, read 14 bytes in one i2c_master_read call. This eliminates inter-register timing skew — all six axes come from the same sampling instant. Reading one register at a time would mix measurements from different cycles, producing phantom cross-axis coupling that would confuse the filter.

Scaling from raw 16-bit counts to physical units:

• Accelerometer: ±2g range → 16,384 LSB/g → multiply by 9.81 / 16384.0 to get m/s²
• Gyroscope: ±250°/s range → 131 LSB/(°/s) → multiply by (π/180) / 131.0 to get rad/s

The gyro range of ±250°/s is the chip default and saturates under fast hand rotation. Widening it is a one-register change in GYRO_CONFIG when the control loop needs it.

Bias calibration

Cheap GY-521 boards have substantial factory bias. My specific unit read gravity at ~11.6 m/s² before calibration — about 18% high. The gyro axes showed ~0.1–0.3 rad/s of zero-rate offset. Neither is subtle enough to ignore.

The calibration averages 200 samples over one second while the board is stationary:

m->bias_ax = sum_ax / n_samples;
m->bias_ay = sum_ay / n_samples;
m->bias_az = sum_az / n_samples - 9.81f;  // remove gravity
m->bias_gx = sum_gx / n_samples;
m->bias_gy = sum_gy / n_samples;
m->bias_gz = sum_gz / n_samples;

The - 9.81f on the Z accelerometer axis is necessary: when the board is flat and stationary, the accelerometer reads +g on Z (gravity pointing down in the sensor frame). Subtracting it from the bias means a calibrated stationary board reads (0, 0, 0), not (0, 0, -g). Every subsequent mpu6050_read subtracts these biases from the raw scaled values, so the filter sees net acceleration, not the gravity vector plus bias.

This is a one-point calibration: one temperature, one orientation. It’s enough for bench testing at room temperature with the board flat. A production flight controller would store bias vs. temperature curves and calibrate across orientations. For proving the pipeline works, one second of averaging gets the job done.

The complementary filter

imu.c implements a complementary filter that fuses the accelerometer and gyroscope into roll, pitch, and yaw. The core idea is simple: the accelerometer provides an absolute, drift-free reference from gravity, but it’s noisy and useless during sustained acceleration. The gyroscope provides clean, responsive short-term rates, but integrated over time it accumulates unbounded drift. The filter trusts the gyro on fast timescales and the accelerometer on slow ones.

The filter structure:

typedef struct {
    float roll;   // rad
    float pitch;  // rad
    float yaw;    // rad
    float alpha;  // gyro trust weight (0–1), typically 0.98
} imu_t;

Each update step:

1. Integrate the gyro rates: angle += gyro_rate * dt — this responds instantly to motion
2. Derive absolute angles from the accelerometer: roll_acc = atan2(ay, az), pitch_acc = atan2(-ax, sqrt(ay² + az²)) — a noisy but drift-free gravity reference
3. Blend: angle = alpha * gyro_angle + (1 - alpha) * acc_angle

With alpha = 0.98, each 5 ms tick the filter takes 98% of the gyro’s change and 2% of the accelerometer correction. This means the accelerometer takes about 0.25 seconds to pull the estimate halfway toward truth, but over 2–3 seconds it fully corrects any gyro drift. The gyro dominates during fast motion; the accelerometer slowly steers it back during steady flight.

The accelerometer angle formulas are derived from the rotation matrices, not copied:

The accelerometer measures (ax, ay, az) in the body frame. When the board is stationary, this is the reaction to gravity — it points opposite to the gravity vector in the inertial frame. For a board with roll $φ$, pitch $θ$, yaw $ψ$, the gravity vector in body coordinates is:

$$a = R^T \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix}$$

Expanding the ZYX rotation matrix and solving for $φ$ and $θ$ gives the formulas above. Yaw — rotation around the gravity vector — is invisible to the accelerometer. That’s why yaw has no absolute reference without a magnetometer.

The filter has known limits. During sustained horizontal acceleration (forward flight, coordinated turns), the accelerometer no longer measures pure gravity — it measures gravity plus the acceleration vector, and the derived angles become wrong. The α=0.98 blend partially masks this (the gyro dominates for about 2.5 seconds), but sustained maneuvers will pull the estimate off. Adding a magnetometer for absolute heading and an extended Kalman or Madgwick filter for all-attitude estimation is the next fusion upgrade.

The real-time loop

The timing architecture is the part I’m most satisfied with. Getting a deterministic 200 Hz control loop on FreeRTOS requires solving a specific problem: I2C is blocking — mpu6050_read sits in a busy-wait for the bus transaction to complete — and you cannot run blocking code inside an interrupt service routine.

The solution is a timer-and-task pattern borrowed from real-time control systems:

// Timer ISR — does nothing but notify the task
static void timer_callback(void *arg) {
    BaseType_t higher_priority_task_woken = pdFALSE;
    vTaskNotifyGiveFromISR(control_task_handle, &higher_priority_task_woken);
    portYIELD_FROM_ISR(higher_priority_task_woken);
}

// Control task — blocks until notified, then runs the pipeline
while (1) {
    ulTaskNotifyTake(pdTRUE, portMAX_DELAY);  // block until timer ticks
    mpu6050_read(&imu, &sample);              // blocking I2C, safe in task context
    imu_update(&attitude, &sample, DT);        // sensor fusion
    // printf every 40th tick (5 Hz logging)
}

The timer (esp_timer) fires every 5,000 µs. The ISR is four lines: notify the task, yield if needed, return. The task wakes, runs the read→fuse→log pipeline, and blocks again. The ISR takes microseconds; the I2C read takes maybe a hundred. The loop runs at exactly 200 Hz because the task always finishes before the next tick — if it didn’t, the FreeRTOS notification count would accumulate and the task would process backlogged ticks at full speed until caught up.

dt is hardcoded at 0.005 seconds. On a real-time loop with guaranteed scheduling, measuring dt adds noise without adding accuracy. Hardcoding it makes the integration exact for the nominal case and slightly wrong if a tick is missed — but if the loop is missing ticks, the problem is the scheduler, not the dt value.

The task is pinned to core 1 at priority 10, leaving core 0 free for the Wi-Fi stack and other ESP-IDF housekeeping. Stack is 4096 bytes — verified against the watermark in early testing, with plenty of headroom.

The control stack (ported, not yet wired)

pid.c, mixer.c, and control.c are ported from the PID Controller simulation and compile cleanly under ESP-IDF. They are unit-agnostic, NED-native, and identical in logic to the simulation versions. The cascade is the same: altitude PID with single-loop position control, roll/pitch angle→rate cascades, and a yaw rate loop with anti-windup on every axis.

The control stack compiles but control_update() is never called. The control_task loop currently stops at imu_update and prints roll/pitch/yaw. It needs to become:

mpu6050_read → imu_update → control_update → mixer_compute → (PWM)

That single connection — sensor data into the control stack — is the next commit. Everything on both sides of it is built and verified. The gap is one function call.

Where this is going

The immediate next steps, in order:

1. Widen the gyro range. The ±250°/s default saturates under fast rotation. One register write in GYRO_CONFIG fixes it.
2. Wire the control loop. control_update() receives live imu_t data instead of simulated state. For the first closed-loop test, setpoints are hardcoded hover: altitude = 1.0, roll = pitch = 0, yaw_rate = 0. Props off. Move the board by hand and watch the motor commands respond.
3. PWM motor output. Four ledc channels at 50–400 Hz, verified on an oscilloscope or logic analyzer before ever mounting props. Motor direction and magnitude must match the mixer expectations.
4. GY-87 upgrade. Add the QMC5883L magnetometer (absolute heading, fixes yaw drift) and BMP180 barometer (absolute altitude, feeds the altitude loop that currently has no real input).
5. Prop-on testing. Tether, incremental throttle, retune gains against the real plant.

Beyond that: GPS-denied position hold via optical flow or visual odometry, object detection on a Jetson feeding commands into the flight controller, autonomous waypoint navigation. Those are the projects that ladder into the career target — a Ukrainian drone company building autonomous systems. But they all depend on step 2 above. Close the loop first.

What transfers from simulation

The control stack design — the cascade wiring, the anti-windup logic, the derivative-on-measurement decision, the mixer sign conventions, the NED coordinate frame — transfers directly. These are architecture decisions, not implementation details, and they’re valid regardless of whether the state data comes from quad_step() or mpu6050_read().

What doesn’t transfer:

• The gains. Tuned against a simulated plant with idealized motors, perfect state estimation, and no sensor noise. The real drone has different inertia, motor response curves, ESC latency, and battery sag. Gains will be re-tuned from scratch on the physical airframe.
• The state estimation. The simulation had perfect QuadState. The hardware has a noisy IMU and a complementary filter that drifts on yaw and breaks during sustained acceleration. The gap between perfect and real state is the central challenge of the hardware phase.
• The physics model. quad.c disappears entirely. The real world provides ground truth.

The architecture transfers; the numbers don’t. This is the right shape.

Project structure

main/
├── main.c              FreeRTOS task + timer, I2C init, pipeline glue
├── mpu6050.h / .c      I2C chip driver: probe → wake → calibrate → read
├── imu.h / .c          Complementary filter: accel + gyro → roll/pitch/yaw
├── control.h / .c      Cascaded PID: attitude + setpoints → control commands
├── pid.h / .c          Single-axis PID with anti-windup
├── mixer.h / .c        X-frame mixer: control commands → per-motor thrusts
└── CMakeLists.txt      Registers all source files

ESP-IDF v5.3. Build: idf.py build, flash: idf.py -p /dev/ttyUSB0 flash monitor.

Hardware

• MCU: ESP32 DevKit V1
• IMU: GY-521 (MPU-6050), I2C at 400 kHz — SDA on GPIO 21, SCL on GPIO 22, powered at 3.3 V
• Planned: GY-87 (MPU-6050 + QMC5883L magnetometer + BMP180 barometer)

The GY-521 costs about €3 on AliExpress. The chip’s register map is well-documented, and the ESP32’s I2C peripheral is mature and reliable. The hardware is deliberately cheap — the point is the software, and expensive components wouldn’t teach anything the cheap ones don’t.

Known limits

Yaw drifts. With no magnetometer, the integrated gyro yaw has no absolute reference and slowly accumulates error — about 1° per second at rest, more under motion. This is accepted for now because the controller uses yaw rate setpoints, not absolute heading — the drone doesn’t need to know which way is north, only how fast it’s turning. The magnetometer fixes this when the GY-87 upgrade arrives.

Gyro saturation. The ±250°/s default range clips during fast hand rotation. Widening to ±500 or ±1000°/s is a one-register write, deferred until the control loop is closed and real motion profiles demand it.

Control loop is open. The architecture is built, tested, and ported — but the final wire from imu_update to control_update hasn’t been connected. That’s a single function call away, and it’s the next commit.