TeensyPilot — Custom Flight Controller Firmware

The Challenge

Most hobbyist flight controllers lean on existing HALs or the Arduino framework to handle timing and hardware access. The challenge with TeensyPilot was to go the opposite direction: design a custom hardware carrier board and build the entire firmware stack from scratch in bare-metal C++ on the Teensy 4.0.

Technical Deep Dive

Custom Hardware & PCB Design

• Custom PCB Carrier: I designed a dedicated carrier board using KiCad, integrating the IMU, barometer, and magnetometer alongside RC and ESC connections into a single, compact flight-ready module. This formed the robust hardware backbone of the drone.
• Hardware-Level Sensor Topology: I split communication across a dedicated 7 MHz SPI bus for the ICM-20948 IMU and a separate 400 kHz I2C bus for the DPS310 barometer and MLX90393 magnetometer to prevent slower peripherals from blocking the time-critical 1 kHz IMU path.
• Interrupt-Driven Sampling: I utilized a hardware interrupt on the IMU’s data-ready pin to trigger sensor reads, keeping timing jitter to a minimum on the Cortex-M7 architecture.

Firmware Integration

• Quaternion EKF: I built a custom Extended Kalman Filter fusing gyroscope, accelerometer, barometric, and magnetometer data into a single attitude estimate.
• Cascaded PID Control: I structured attitude control as nested rate and angle PID loops, feeding a Quad-X motor mixer that drives four ESCs at 400 Hz PWM.
• Zero-Blocking Design: I eliminated every delay() and blocking I/O call from the post-boot flight loop, relying entirely on interrupts and non-blocking peripheral access.

Results

• True Multi-Sensor Fusion: The custom PCB and hardware architecture enabled three independent sensors to be sampled and fused asynchronously.
• Sub-Millisecond Latency: The tightly coupled hardware and bare-metal firmware ensures the full pipeline completes well within the 888 µs inter-sample window.
• Lean Codebase: I achieved sustained 1000 Hz IMU sampling with zero missed samples, packaged in a dependency-free 3,500 line C++ codebase.