TeensyPilot — Custom Flight Controller Firmware

The Challenge

Most hobbyist flight controllers lean on existing HALs, RTOS schedulers, or the Arduino framework to handle timing and hardware access. The challenge with TeensyPilot was to go the opposite direction: build the entire stack — drivers, sensor fusion, and control loops — from scratch in bare-metal C++ on the Teensy 4.0, with full ownership over every register write and every clock cycle in the flight loop.

Technical Deep Dive

Hardware Architecture

• Dual-Bus Sensor Topology: 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, ensuring the time-critical 1 kHz IMU path is never blocked by slower peripherals.
• Interrupt-Driven Sampling: Used a hardware interrupt on the IMU’s data-ready pin to trigger sensor reads, rather than polling, keeping timing jitter to a minimum on the 600 MHz Cortex-M7.
• Custom PCB Carrier: Designed a dedicated carrier board integrating the IMU, barometer, and magnetometer alongside RC and ESC connections into a single flight-ready module.

Firmware & Control

• Multi-Rate Processing: Implemented four independent processing rates (1000 Hz rate control, 500 Hz attitude control, 50 Hz altitude hold, and background sensor fusion) inside a single loop(), using static dividers and timestamp checks instead of an RTOS scheduler.
• Quaternion EKF: Built a custom quaternion-based Extended Kalman Filter fusing gyroscope, accelerometer, barometric, and magnetometer data into a single attitude and altitude estimate.
• Cascaded PID Control: Structured attitude control as nested rate and angle PID loops, feeding a Quad-X motor mixer that drives four ESCs at 400 Hz PWM.
• Multi-Protocol RC Input: Abstracted RC input behind a common interface supporting SBUS, CRSF, IBUS, and PPM receivers, so the flight loop stays agnostic to the radio hardware in use.
• Zero-Blocking Design: Eliminated every delay() and blocking I/O call from the post-boot flight loop, relying entirely on interrupts and non-blocking, timestamp-gated peripheral access.

Results

• Sustained 1000 Hz IMU Sampling: Validated over 60-minute bench tests via hardware interrupt with zero missed samples.
• Sub-Millisecond Pipeline Latency: Full SPI read → EKF update → PID → motor output pipeline completes well within the 888 µs inter-sample window.
• True Multi-Sensor Fusion: Three independent sensors sampled at different rates (IMU, baro, magnetometer) fused asynchronously into one real-time state estimate.
• Lean, Dependency-Free Codebase: ~3,500 lines of hand-written C++ across 26 source files, with zero external sensor libraries.
• Active Development: Currently in tethered hover validation, with full EKF covariance estimation and DShot600 ESC protocol next on the roadmap.