The Challenge
Most hobbyist flight controllers rely on pre-built hardware and standard libraries that obscure low-level dynamics. The challenge with TeensyPilot was to go the opposite direction: design a custom hardware carrier board from scratch, model the system dynamics mathematically, and implement the entire control and sensor fusion stack in bare-metal C++ on the Teensy 4.0.
Technical Deep Dive
Quadcopter Dynamic Modeling & Simulation (MATLAB/Simulink)
Before writing any firmware, I modeled the quadrotor’s physical dynamics (moments of inertia, gyroscopic torques, motor-propeller thrust curves, and drag coefficients) in MATLAB and Simulink. Using this model, I designed the nested cascaded PID control loops (angular rate as the inner loop and attitude angle as the outer loop). I simulated the closed-loop system response to step inputs and tuned the PID controller gains to optimize settling time and prevent overshoot under simulated wind gusts, providing a robust theoretical baseline for the physical platform.
Custom Hardware & PCB Carrier (KiCad)
I designed a compact KiCad carrier PCB to act as the structural and electrical backbone of the flight controller, integrating the Teensy 4.0, sensor suite, and power regulation. To maximize signal integrity:
- Dual-Bus Topology: I routed the ICM-20948 IMU on a dedicated high-speed 7 MHz SPI bus, while placing the DPS310 barometer and MLX90393 magnetometer on a separate 400 kHz I2C bus to prevent slow peripherals from blocking the time-critical 1 kHz IMU path.
- Ground Isolation: I separated the digital and analog ground planes, routing them through a single-point star ground to isolate the sensitive IMU from the high-frequency switching noise generated by the drone’s ESC lines.
Firmware & Control Loop Implementation
I co-developed the bare-metal C++ firmware on the Teensy 4.0. The code integrates the quaternion EKF state estimator and the simulated cascaded PID loop running at 1 kHz with zero-blocking timing. The IMU’s data-ready pin triggers hardware interrupts to run the control loop instantly upon sensor refresh.
Results
- Simulink-to-Flight Alignment: The PID gains derived from the Simulink simulation translated to physical flight with minimal tuning, validating the fidelity of the mathematical model.
- Vibration-Isolated Sensor Path: The dual-bus topology and isolated PCB ground planes delivered clean sensor readings, ensuring stable and reliable EKF state estimation.
- Lean, High-Frequency Loop: The bare-metal control loop consistently executes in under 250 µs, leaving ample overhead in the 1 ms time step.