200Hz cascaded PID flight controller on ESP32, FreeRTOS, MPU-6050 + complementary filter — real-hardware port of my PID-controller simulation.
More on my website: rssndr.com
A quadcopter flight controller written from scratch for the ESP32, built incrementally on real hardware. No flight-controller framework, no sensor libraries — every layer (I2C chip driver, sensor fusion, control stack, real-time scheduling) is implemented directly to understand how a flight controller actually works from first principles.
The end goal is a drone that estimates its own attitude and holds stable control on real hardware, as the foundation for later autonomous, GPS-denied navigation.
The full pipeline runs on hardware at a deterministic 200 Hz, and the control loop is closed: the IMU is read, calibrated, and fused into a live attitude estimate; that estimate feeds the cascaded PID controller; and the controller's output drives the mixer to per-motor thrust commands. Tilting the frame produces the correct restoring response on all axes — the low side is commanded up.
What's left before flight is actuation and sensing breadth: PWM output to real ESCs (props off first), and the GY-87 upgrade for absolute heading and altitude. The controller thinks correctly; it does not yet drive motors or hold altitude.
Sensor driver (mpu6050.c) — a from-scratch I2C driver for the MPU-6050 (GY-521). Device probe via WHO_AM_I, wake from sleep, gyro full-scale range configuration (±1000°/s), a single-transaction 14-byte burst read of the accelerometer and gyroscope, and scaling from raw 16-bit counts to physical units (m/s², rad/s). A sensor-to-body mounting transform (currently negating the Y and Z axes on both accel and gyro — a 180° rotation about X) maps the breadboard-mounted chip into the NED body frame; this lives in one place in the read path and is the only orientation-dependent code. I2C register read/write primitives are built directly on the ESP-IDF command-link API; the bus is owned by main, not the driver, so additional I2C devices can share it later.
Bias calibration — a zero-rate startup calibration averages a second of samples while the board is stationary and stores per-axis accelerometer and gyroscope bias, subtracted from every subsequent reading. This corrects the substantial factory bias typical of cheap GY-521 boards (the raw chip read gravity ~18% high before calibration). Gyro bias is temperature-sensitive, so calibration is done after the board has settled.
Attitude estimation (imu.c) — a complementary filter fuses accelerometer and gyroscope into roll, pitch, and yaw, natively in the NED body frame the control stack uses. The accelerometer provides an absolute, drift-free gravity reference; the gyroscope provides clean short-term rates. The two are blended so the gyro is trusted on fast motion and the accelerometer slowly corrects long-term drift. The accelerometer-derived angle formulas were derived from the rotation matrices rather than copied. Yaw integrates gyro only (no magnetometer yet) and drifts slowly — acceptable because the controller uses a yaw rate setpoint, not absolute heading.
Real-time loop (control_loop.c) — a FreeRTOS architecture gives a deterministic 200 Hz control tick. A hardware esp_timer fires every 5 ms and does nothing but signal a high-priority control task via a task notification; the task wakes, runs the full read → fuse → control → mix → log pipeline, and blocks again. This keeps blocking I2C out of the timer context and guarantees a fixed, jitter-free dt for integration. main.c is reduced to I2C bus initialization and starting the control loop.
Control stack (pid.c, mixer.c, control.c) — a cascaded PID controller: altitude, roll/pitch angle-and-rate cascades, and a yaw-rate loop, with conditional-integration anti-windup and derivative-on-measurement. A mixer maps the four control outputs to per-motor thrusts for an X-configuration quad. Unit-agnostic, NED coordinate convention. Ported from the PID-controller simulation and now running on live sensor data. Gains live in tuning.h, kept separate from the cascade wiring. Tuned for realistic FPV-class parameters (~0.25 kg, 4:1 thrust-to-weight); real-hardware retuning comes with prop-on testing.
main.c I2C bus init, starts the control loop
|
v
control_loop.c FreeRTOS control task + 200 Hz timer, full pipeline glue
|
v
mpu6050.c I2C chip driver: raw registers -> calibrated NED-body physical units
|
v
imu.c complementary filter: accel + gyro -> roll / pitch / yaw (NED)
|
v
control.c cascaded PID: attitude + setpoints -> control commands
|
v
mixer.c control commands -> per-motor thrusts
Each layer has a single responsibility and a clean boundary. 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. Gains are isolated in tuning.h. The one place sensor orientation is handled is the mounting transform in the read path — everything above it works in a consistent NED frame.
- MCU: ESP32 DevKit V1
- IMU: GY-521 (MPU-6050), I2C — SDA on GPIO 21, SCL on GPIO 22, 400 kHz, powered at 3.3 V
- Planned: GY-87 (MPU-6050 + QMC5883L magnetometer + BMP180 barometer) for absolute heading and altitude
ESP-IDF v5.3.
idf.py build
idf.py -p /dev/ttyUSB0 flash monitor
- Yaw drifts. With no magnetometer, integrated yaw has no absolute reference and slowly drifts. Accepted for now because control is yaw-rate-based, not heading-based. The planned GY-87 magnetometer fixes it.
- No altitude sensor yet. The altitude PID has no real input; on the bench, thrust is held at a fixed baseline so the differential (roll/pitch/yaw) corrections are visible. The GY-87 barometer will feed the altitude loop.
- Mounting transform is breadboard-specific. The Y/Z negation matches the current breadboard orientation. On the real airframe the sensor will be mounted to match the body frame directly, and this transform gets revisited in one place.
- PWM motor output (props off) — map mixer thrusts to ESC signals on four channels, verify motors respond correctly to attitude before mounting props.
- Add the GY-87 for absolute heading (magnetometer) and altitude (barometer), feeding the yaw and altitude loops.
- Prop-on testing — tethered, incremental throttle, retune gains against the real plant.
- Build toward autonomous, GPS-denied navigation and target tracking.
This is a learning project built from first principles — derive, implement, verify on hardware, one layer at a time. The commit history follows that path.