ESP32-S3 Quadcopter Flight Controller

A custom quadcopter flight control system built around an ESP32-S3, sensor fusion, cascaded PID stabilization, DShot ESC control, wireless command/telemetry, and a separate remote control/display interface.

This project was built to explore the core systems behind a multirotor flight controller from the ground up: IMU sampling, attitude estimation, PID stabilization, motor mixing, ESC signalling, telemetry, remote tuning, and sensor integration.

Table of Contents

• Introduction
• System Overview
• Flight Controller Architecture
  • FreeRTOS Task Layout
  • Sensor Update Rates
• Sensor Fusion and Filtering
  • MPU-6500 IMU
  • Mahony Attitude Filter
  • Magnetometer, Barometer, Optical Flow, and Lidar
• PID Stabilization
  • Cascaded Angle and Rate Control
  • Altitude Hold
  • Optical Flow Velocity Hold
• Motor Output and ESC Control
  • DShot Motor Signalling
  • ESC Passthrough and Firmware Configuration
• Remote Control and Telemetry
  • Control Packet Structure
  • Telemetry Packet Structure
  • Remote PID Tuning
• Remote Display Interface
• Hardware and 3D Design
• Safety Features
• Repository Structure
• Current Status and Future Improvements

---

Introduction

This project started as a custom ESP32 quadcopter flight controller and evolved into a larger embedded control system with a dedicated transmitter, telemetry display, and modular sensor stack.

The flight controller runs directly on an ESP32-S3 rather than using an off the shelf autopilot board. The goal was to implement the main flight control pipeline manually:

1. Read IMU data.
2. Estimate roll, pitch, and yaw using sensor fusion.
3. Convert pilot commands into desired angles and rates.
4. Stabilize the quadcopter using cascaded PID loops.
5. Mix PID outputs into four motor commands.
6. Send commands to the ESC using DShot.
7. Receive remote input and return telemetry over a wireless link.

The system also includes slower sensors for stability and feedback, including a barometer, optical flow/lidar module, magnetometer, GPS module, battery voltage sensing, and a separate LCD/LVGL display for telemetry and menu interaction.

---

System Overview

At a high level, the project is split into three main embedded systems:

Subsystem	Purpose
ESP32-S3/	Main quadcopter flight controller. Runs the IMU loop, sensor fusion, PID control, motor mixing, telemetry, DShot output, and safety logic.
remote_control/	Earlier transmitter version using a PS5 controller, wireless command link, LCD output, and PID tuning menu.
remote_control_V2/	Newer remote control architecture split into a USB gamepad transmitter and a separate LVGL display module.

The main control path is:

Gamepad / Controller
        ↓
Remote ESP32-S3 transmitter
        ↓
2.4 GHz wireless command packet
        ↓
ESP32-S3 flight controller
        ↓
Sensor fusion + PID control
        ↓
Motor mixer
        ↓
DShot ESC commands
        ↓
ESC
        ↓
Motors

Telemetry follows the reverse path, allowing the remote controller/display to show attitude, altitude, throttle, battery voltage, and flight state.

---

Flight Controller Architecture

The flight controller is organized around separate FreeRTOS tasks so that the high rate flight loop is isolated from slower I/O, telemetry, Wi-Fi, OTA, and debug printing.

FreeRTOS Task Layout

The main sketch creates multiple pinned tasks:

Task	Core	Purpose
SensorTask	Core 1	Highest priority real time loop. Runs the filter and motor mixer at a fixed IMU period.
IOTask	Core 0	Handles slower sensors, battery monitoring, LED state indication, and remote control input.
CommsTask	Core 0	Handles telnet/debug output, remote commands, telemetry logging, and command processing.
OTATask	Core 0	Handles Arduino OTA firmware updates.

The most timing critical part of the project is SensorTask. It uses a fixed microsecond period, calls updateFilter(), runs mixMotor(), and records execution time for debugging.

updateFilter(start);
mixMotor(start);
g_execTimeUS = esp_timer_get_time() - start;

This structure keeps the stabilizing loop separate from slower tasks such as GPS reads, lidar parsing, debug output, and OTA handling.

Sensor Update Rates

Different sensors are read at different rates depending on how quickly their data changes and how expensive they are to read.

Sensor / Function	Approximate Rate	Purpose
IMU / attitude filter	1 kHz target	Roll/pitch/yaw stabilization
Optical flow + lidar	100 Hz	Low altitude velocity and height feedback
Barometer	25 Hz	Altitude estimation support
GPS	5 Hz	Position data hook for future navigation
LED / battery update	20 Hz	State indication and voltage monitoring
Telemetry request	20 Hz	Remote display updates

The faster IMU loop handles stabilization, while slower sensors are used for flight modes such as altitude hold, velocity hold, and telemetry.

---

Sensor Fusion and Filtering

MPU-6500 IMU

The main attitude sensor is an MPU-6500 connected over SPI. SPI was chosen for the IMU because it provides faster and more deterministic access than I2C, which is helpful for a high rate flight loop.

The IMU read function collects accelerometer and gyro data in one burst read starting from ACCEL_XOUT_H:

spiRead(MPU_REG_ACCEL_XOUT_H, buf, 14);

The raw accelerometer and gyroscope values are corrected using stored calibration offsets and scaling values. This improves the accuracy of roll/pitch estimation and reduces drift caused by sensor bias.

Mahony Attitude Filter

The project uses a Mahony filter for attitude estimation. The filter integrates gyroscope data and uses the accelerometer as a gravity reference when the measured acceleration is trustworthy.

One important detail is that accelerometer correction is reduced or disabled during aggressive motion. This prevents the filter from treating real linear acceleration as a false gravity vector.

The filter logic uses:

• accelerometer norm checking
• gyro rate weighting
• bias correction for roll and pitch gyro drift
• quaternion normalization
• conversion from quaternion to Euler angles

This allows the flight controller to estimate roll, pitch, and yaw without relying on slow angle calculations directly from the accelerometer.

Magnetometer, Barometer, Optical Flow, and Lidar

The sensor stack is designed to be modular:

Sensor	Use
QMC5883P magnetometer	Heading/yaw reference
BMP280/BME280 barometer	Slower altitude reference
Optical flow + lidar module	Low altitude velocity and height feedback
GPS module	Position data hook for outdoor navigation

The optical flow/lidar module sends range and flow data over UART. The code parses packets using a small state machine and verifies the checksum before extracting distance, flow velocity, and quality values.

The lidar height is converted from millimetres to metres, while the flow velocity is scaled by height:

height_lidar = d.distance_mm * 0.001f;
vx = d.flow_vel_x * height_lidar;
vy = d.flow_vel_y * height_lidar;

This makes the optical flow velocity estimate height aware, which is important because the apparent pixel motion from the sensor depends on distance from the ground.

---

PID Stabilization

The quadcopter uses cascaded PID loops. Instead of sending stick inputs directly to the motors, the controller converts pilot commands into desired attitude/rate targets, then uses feedback loops to stabilize.

Cascaded Angle and Rate Control

The roll and pitch control path uses two PID stages:

1. Angle loop: compares desired angle against measured angle.
2. Rate loop: compares desired angular rate against measured gyro rate.

A simplified version of the control flow is:

Pilot stick input
        ↓
Desired roll/pitch angle
        ↓
Angle PID
        ↓
Desired roll/pitch rate
        ↓
Rate PID
        ↓
Motor mixer correction

Yaw is handled as a rate control loop, where stick input maps to a desired yaw rate.

The PID implementation includes:

• integral limiting
• derivative filtering
• output limiting
• reset behaviour for inactive/disarmed states

This helps prevent integral windup and reduces noise sensitivity in the derivative term.

Altitude Hold

Altitude hold is designed around the lidar/barometer height estimate. When altitude hold is enabled, the current height is captured as the target height. The altitude PID then adjusts throttle to hold that target.

This allows the pilot to stabilize the quadcopter vertically without manually maintaining a constant throttle value.

Optical Flow Velocity Hold

The optical flow sensor is used to estimate horizontal motion close to the ground. This can be used for velocity hold or position hold at low altitude.

The general control idea is:

Optical-flow velocity estimate
        ↓
Velocity PID
        ↓
Desired corrective roll/pitch angle
        ↓
Angle + rate stabilization loops

Because optical flow data can become unreliable over poor surfaces, the code tracks flow quality and applies filtering/spike rejection before using the measurement.

---

Motor Output and ESC Control

DShot Motor Signalling

The motor output system uses DShot rather than traditional PWM. DShot is a digital ESC protocol, so it avoids the calibration issues associated with analog PWM throttle signals.

Each motor command is converted into a DShot throttle value, combined with a telemetry request bit, and protected with a checksum nibble before transmission.

A simplified DShot packet layout is:

[11 bit throttle/command][1 bit telemetry request][4 bit CRC]

The motor mixer combines throttle with roll, pitch, and yaw corrections, then sends four synchronized motor commands to the ESC.

ESC Passthrough and Firmware Configuration

The project includes ESC passthrough support so the ESP32-S3 can act as an interface between a PC configuration tool and the 4 in 1 ESC.

When passthrough mode is enabled at compile time, normal flight tasks are skipped. This is important because the ESC configuration tool needs direct, uninterrupted access to the ESC signal lines.

#if ESC_PASSTHROUGH_MODE
    esc4wayRun();
    return;
#endif

This was used for Bluejay ESC configuration and firmware flashing. Keeping passthrough mode separate from normal flight mode reduces the chance of the motors being driven unexpectedly during ESC setup.

---

Remote Control and Telemetry

The remote control system sends packed control packets to the flight controller and receives packed telemetry packets in return.

The project has gone through multiple transmitter versions:

1. Bluetooth PS5 controller transmitter using an ESP32 and LCD.
2. ESP32-S3 USB gamepad transmitter using USB host mode.
3. Separate LVGL display module for a larger telemetry GUI.

Control Packet Structure

The control packet is packed to keep the radio payload small and deterministic. It contains:

• left and right stick positions
• analog trigger values
• button bitmask
• flags for telemetry/tuning commands
• tuning parameter ID
• tuning value
• CRC byte

The CRC is used to reject corrupted packets before they affect the motors.

Telemetry Packet Structure

Telemetry packets return useful flight information to the remote:

• roll
• pitch
• altitude
• velocity estimates
• state flags
• throttle
• receiver battery voltage
• CRC byte

The transmitter periodically requests telemetry by setting a flag in the outgoing control packet, briefly switches the radio into receive mode, and listens for the response packet.

Remote PID Tuning

The remote control menu can send PID tuning commands over the same wireless link. This allows gains to be adjusted without recompiling and reflashing the flight controller for every tuning change.

Supported tuning categories include:

• roll/pitch rate PID
• yaw rate PID
• roll/pitch angle PID
• velocity PID
• altitude PID
• individual motor scale offsets

This is useful during early flight testing because PID values often need many small adjustments before the quadcopter feels stable.

---

Remote Display Interface

The newer remote display is built around an ESP32 display board running LVGL. The gamepad/transmitter sends display packets to the screen over I2C.

The display protocol uses a packed DisplayPacket structure with two packet types:

Packet Type	Contents
PACKET_TELEM	Roll, pitch, altitude, throttle, TX battery, RX battery, and flight state
PACKET_TEXT	Four text lines for menu/status output

The LVGL interface shows flight data such as:

• roll and pitch
• altitude
• throttle percentage
• receiver battery level
• transmitter battery level
• armed / altitude hold / throttle lock state

This makes the remote controller more useful during testing because key flight information is visible without needing a serial monitor.

---

Hardware and 3D Design

The physical quadcopter frame uses a design by @ProgrammaDan called Leopard, slightly modified for 10 inch propellers.

Major hardware elements include:

Component	Role
ESP32-S3	Main flight controller MCU
MPU-6500	IMU
QMC5883P	Magnetometer
BMP280/BME280	Barometer
Optical-flow/lidar module	Low altitude velocity and height sensing
GPS module	Position data for future navigation features
4-in-1 ESC	Drives the four brushless motors
A2122 920KV motors + propellers	Quadcopter propulsion
Remote ESP32/ESP32-S3	Wireless transmitter and gamepad interface
ESP32 display module	LVGL telemetry/menu display

---

Safety Features

Because this project directly controls brushless motors, several safety behaviours are included in the firmware:

• motors stop when the system is disarmed
• emergency motor stop is called during OTA start
• shutdown handler stops motors before reset
• stack overflow hook stops motors and restarts the ESP32
• watchdog ISR handler stops motors
• CRC validation rejects corrupted control and telemetry packets
• ESC passthrough mode disables normal flight tasks
• LED colour indicates armed and flight mode state

The flight controller also includes dedicated arming/disarming controls and flight mode toggles for features such as altitude hold and throttle lock.

---

Repository Structure

Flight-Controller/
├── ESP32-S2/
│   └── Earlier ESP32/ESP32-S2 flight controller code
│
├── ESP32-S3/
│   ├── ESP32-S3.ino        # Main flight controller task setup
│   ├── sensors.cpp/.h      # IMU, magnetometer, filtering, calibration
│   ├── mahony.cpp/.h       # Quaternion attitude filter
│   ├── motors.cpp/.h       # Motor mixing, DShot output, telemetry packets
│   ├── pid.cpp/.h          # PID controller implementation
│   ├── optical_flow.cpp/.h # Lidar/optical-flow packet parsing
│   ├── baro.cpp/.h         # Barometer support
│   ├── gps.cpp/.h          # GPS support
│   ├── comms.cpp/.h        # Wi-Fi/telnet/debug commands
│   ├── esc_4way.cpp/.h     # ESC passthrough support
│   └── config.h            # Pins, gains, sensor constants, calibration values
│
├── remote_control/
│   └── Earlier PS5 controller transmitter with LCD telemetry/menu support
│
├── remote_control_V2/
│   ├── gamepad/            # ESP32-S3 USB gamepad transmitter
│   └── display/            # ESP32 LVGL telemetry display
│
└── PID_Tuning_GUI/
    └── PC tuning interface / tuning experiments

---

Current Status and Future Improvements

Current implemented features include:

• ESP32-S3 flight controller
• SPI IMU
• Mahony attitude estimation
• cascaded PID stabilization
• DShot motor output
• wireless command and telemetry packets
• remote PID tuning commands
• optical flow/lidar parsing
• battery voltage monitoring
• OTA support
• ESC passthrough mode
• LVGL telemetry display

Future improvements planned or under development:

• improved bidirectional DShot RPM telemetry
• RPM based filtering/notch tuning
• more complete altitude hold tuning
• more reliable optical flow velocity hold
• improved GPS integration
• refined remote GUI pages
• better failsafe behaviour for radio loss

---