Embedded sensor fusion, 9-state navigation filter, and binary telemetry
on the Teensy 4.0 airborne flight computer
This paper describes the firmware architecture of the airborne flight computer in the MicroRobo Systems GPS beacon tracking system. A Teensy 4.0 integrates four sensors — an ICM-20948 IMU via SPI1 using its on-chip Digital Motion Processor, a u-blox NEO-M9N GPS receiver, a Bosch BMP581 barometer, and a QMC5883L magnetometer — and fuses them in a 9-state Extended Kalman Filter operating in the NED coordinate frame. Every 100 ms, a 34-byte CRC-16 protected binary telemetry frame is transmitted to the ground station over XBee 802.15.4. The firmware simultaneously operates as a 3-axis rate and heading-hold gyroscope stabiliser for the carrier fixed-wing aircraft, reading S.Bus RC inputs and driving six PWM servo outputs. Key design decisions are discussed, including DMP mode selection, SPI1 bus isolation, dual I2C bus assignment, GPS fix validation, and the known limitation of the absent iTOW field in the telemetry frame.
The airborne firmware serves two simultaneous roles: as a GPS radio beacon for the optical tracking system on the ground, and as a gyroscope stabilizer for the fixed-wing aircraft that carries it. These roles share the same sensor suite — the same IMU quaternion that drives the servo PID loops also contributes attitude data to the telemetry frame — but operate on different timing paths. The gyroscope functions run at the IMU DMP output rate (~112 Hz); telemetry transmission is decimated to 10 Hz to match the GPS fix rate.
The platform is a PJRC Teensy 4.0, chosen for its 600 MHz ARM Cortex-M7 core, hardware floating-point unit, and availability of multiple SPI and I2C buses on a compact form factor. The firmware is written in C++ under the Arduino framework using Teensyduino 1.59, which provides access to Teensy-specific peripherals while retaining the broad library ecosystem.
| Sensor / Device | Interface | Pins | Output Rate |
|---|---|---|---|
| ICM-20948 IMU | SPI1 w/ RE1C002UNTCL level shifters | MOSI=26, MISO=1, SCK=27, CS=0 | 112 Hz (DMP Quat6) |
| u-blox NEO-M9N GPS | UART Serial5 | TX=20, RX=21, 38400 baud | 10 Hz (UBX NAV-PVT) |
| Bosch BMP581 | I2C Wire2 | SDA=25, SCL=24, 2.2 kΩ pull-up | Polled ~10 Hz |
| QMC5883L magnetometer | I2C Wire0 | SDA=18, SCL=19, on GPS module | 50 Hz (continuous) |
| XBee 802.15.4 radio | UART Serial3 | TX=14, RX=15, 230400 baud | 10 Hz TX (telemetry) |
| S.Bus receiver | UART Serial4 (inverted) | RX=16, 100000 baud 8E2 | ~70 Hz frame rate |
The ICM-20948 is placed on SPI1 rather than I2C for two reasons. First, SPI supports the higher clock rates needed to drain the DMP FIFO within a 112 Hz budget without stalling the main loop. Second, the DMP generates burst traffic that can cause NAKs on a shared I2C bus. The RE1C002UNTCL MOSFET level shifters on all four SPI1 lines (MOSI, MISO, SCK, CS) provide clean logic level translation and protect against latch-up on power cycling.
The BMP581 and QMC5883L are placed on separate I2C buses (Wire2 and Wire0 respectively) to allow independent bring-up and eliminate inter-device bus contention. The QMC5883L resides physically on the NEO-M9N carrier module and its pull-ups are provided by that module.
The ICM-20948's on-chip Digital Motion Processor is used in preference to raw accelerometer/gyroscope fusion on the Teensy. The DMP runs TDK's factory-calibrated sensor fusion algorithm, producing a Quat6 (6-axis, accelerometer + gyroscope, no magnetometer) quaternion at 112 Hz. This offloads attitude computation entirely from the main processor and eliminates the need for a separately maintained AHRS. The tradeoffs are reduced introspection into the fusion internals and a dependency on SparkFun's ICM-20948 library maintaining compatibility with the silicon revision.
Quat6 (magnetometer-excluded) is used rather than Quat9 because the QMC5883L heading is consumed separately for compass display and calibration; fusing it through the DMP would couple the compass calibration state to the quaternion output in a way that complicates the gimbal geometry calculations.
The SparkFun ICM-20948 library ships with DMP support disabled. Line 29 of ICM_20948_C.h must be uncommented to read #define ICM_20948_USE_DMP before the sketch will compile. This edit must be reapplied after any library update.
The ICM-20948 is mounted such that its physical axes do not align with the airframe NED convention. The following remap is applied after reading the DMP FIFO:
| Output | Source | Notes |
|---|---|---|
| qw | +q0 | Scalar component; forced positive |
| qx | −q1 | Physical Q1 negated |
| qy | +q2 | Physical Q2 unchanged |
| qz | −q3 | Physical Q3 negated |
The same axis swap is applied to the raw accelerometer data from the DMP FIFO before it is passed to the EKF predict step, ensuring that the body-to-NED rotation matrix constructed from the quaternion correctly transforms accelerometer measurements into the NED frame.
The DMP FIFO provides raw accelerometer counts in addition to the quaternion. These are read at 112 Hz and passed directly to the EKF predict step without scaling. The EKF applies the ±4g FSR scale factor (8192 LSB/g) internally. Passing raw counts rather than scaled values reduces fixed-point precision loss on the Teensy's single-precision FPU.
The NEO-M9N outputs UBX binary protocol rather than NMEA. UBX NAV-PVT provides a 92-byte payload (M9-generation silicon) containing GPS time-of-week, position (lat/lon/height in 1e-7 degree and mm units), velocity NED (mm/s), and accuracy estimates (hAcc, vAcc in mm). All values are in fixed-point integer format — no floating-point conversion is needed before packing the telemetry frame, which reduces the risk of rounding artifacts.
A custom non-blocking UBX parser state machine drains Serial5 on every main loop iteration, accumulating bytes into a 256-byte payload buffer and validating the Fletcher-8 checksum before dispatching each message. NAV-VELNED is enabled in addition to NAV-PVT and provides an independent velocity measurement at 10 Hz; when VELNED is active it takes priority over the velocity fields in NAV-PVT.
The firmware sends a CFG-PRT command to switch the NEO-M9N to UBX-only output at 38,400 baud, immediately followed by CFG-CFG to save that configuration to the module's flash. This ordering is critical: the CFG-GNSS multi-constellation enable command that follows triggers an internal tracking channel reconfiguration on the M9N that reloads port configuration from flash. If CFG-PRT is RAM-only at that point the module reverts to NMEA output. The correct sequence is:
| Step | Command | Purpose |
|---|---|---|
| 1 | CFG-PRT | Switch to UBX-only, 38400 baud (RAM) |
| 2 | CFG-CFG | Save to flash immediately |
| 3 | CFG-RATE | Set 100 ms measurement interval (10 Hz) |
| 4 | CFG-GNSS | Enable GPS + GLONASS + Galileo + BeiDou |
| 5 | CFG-MSG ×4 | Enable NAV-PVT, NAV-VELNED, NAV-POSLLH, NAV-STATUS |
| 6 | CFG-CFG | Final flash save of all settings |
GPS fix validity requires both fixType ≥ 3 AND the gnssFixOK flag (NAV-PVT flags byte bit 0). The u-blox documentation recommends this dual check because fixType can report 3 while the internal accuracy filters are not satisfied — a condition that produces large position errors without any indication from the fix type alone. A 15-second watchdog re-transmits the full configuration sequence if no valid NAV-PVT has been received, handling cold-start and link-recovery scenarios.
The BMP581 provides absolute pressure at approximately 10 Hz via SparkFun's BMP581 Arduino library. A static offset (PRESSURE_OFFSET_PA) calibrated against a reference pressure source corrects for sensor bias. The hypsometric formula converts calibrated pressure to altitude above the launch-point reference pressure:
h = 44330 × (1 − (P / P₀)1/5.255)
where P₀ is the reference pressure recorded at power-up by EKF_baro_init(). The result is altitude above ground level relative to the launch site, not MSL altitude. The EKF uses this as a Down-state measurement; the ground station receives GPS MSL altitude in the telemetry frame for absolute reference.
The BMP581 is packaged in a 10-pad LGA with no visible solder joints after reflow. Intermittent I2C failures on Wire2 are the most likely symptom of a marginal solder joint on this device. If beginI2C() fails repeatedly, inspect with X-ray or re-reflow before assuming device failure. Additionally, the CSB pin must be pulled high to 3.3 V to latch I2C mode at power-up — a floating CSB initialises the device in SPI mode.
The airborne EKF maintains a 9-element state vector in the NED (North-East-Down) frame:
| States | Symbol | Units | Description |
|---|---|---|---|
| 0–2 | pN, pE, pD | m | Position NED, origin at first GPS fix |
| 3–5 | vN, vE, vD | m/s | Velocity NED |
| 6–8 | baN, baE, baD | m/s² | Accelerometer bias NED |
The process model propagates position from velocity and velocity from bias-corrected IMU acceleration. The body-frame accelerometer reading is rotated to NED using the quaternion from the DMP, gravity is removed from the Down channel, and the estimated bias is subtracted. Bias states propagate as a random walk driven by process noise Q_BIAS.
GPS latitude and longitude are converted to NED offsets from the first-fix reference point using a flat-earth approximation valid to 0.01% error within 10 km. The 6-element measurement vector (3 position + 3 velocity) is applied in a single update step using the Joseph form of the covariance update for numerical stability. Horizontal position measurement noise is set adaptively from the NAV-PVT hAcc estimate when available; the vertical noise is scaled at 4× horizontal to reflect the typically worse GPS vertical accuracy.
Barometric altitude enters as a scalar measurement on the Down position state. The update is a rank-1 operation — only the Down column of the Kalman gain and the Down state are affected — making it computationally inexpensive. The baro update runs even before the first GPS fix, providing altitude tracking during the fix acquisition period.
After each update step, all diagonal covariance elements are floored at 1e-8 to prevent numerical underflow from driving P indefinitely small. The matrix is symmetrised by averaging off-diagonal pairs after each predict step. No outlier gate is applied to barometric measurements; the GPS position gate rejects fixes implying more than 15 m of travel in a single 100 ms epoch.
The 36-byte telemetry frame (34-byte payload + 2-byte CRC) is transmitted at up to 112 Hz. Frames marked gps_fresh = 0 carry attitude-only data and tell the ground station EKF not to apply a GPS measurement update. Frames marked gps_fresh = 1 (every 25th IMU tick, ~4.5 Hz) carry a new GPS fix and trigger the ground station position update.
| Offset | Size | Type | Field | Description |
|---|---|---|---|---|
| 0–1 | 2 | uint8[2] | sync | 0xAA 0x55 — not covered by CRC |
| 2–3 | 2 | uint16 | frame_id | Rolling counter, wraps at 65535 |
| 4–7 | 4 | uint32 | timestamp_ms | millis() at frame assembly |
| 8–11 | 4 | int32 | lat_deg7 | WGS-84 latitude × 1e7 |
| 12–15 | 4 | int32 | lon_deg7 | WGS-84 longitude × 1e7 |
| 16–17 | 2 | int16 | alt_cm | MSL altitude, cm (range ±327 m) |
| 18–19 | 2 | int16 | vel_n_cms | NED North velocity, cm/s |
| 20–21 | 2 | int16 | vel_e_cms | NED East velocity, cm/s |
| 22–23 | 2 | int16 | vel_d_cms | NED Down velocity, cm/s |
| 24–25 | 2 | int16 | quat_x_sc | Quaternion X × 32767 |
| 26–27 | 2 | int16 | quat_y_sc | Quaternion Y × 32767 |
| 28–29 | 2 | int16 | quat_z_sc | Quaternion Z × 32767 |
| 30 | 1 | uint8 | gps_fix | bits[7:4]=fix_type, bits[3:0]=HDOP×2 |
| 31 | 1 | uint8 | gps_fresh | 1 = new GPS fix included |
| 32–33 | 2 | uint16 | crc16 | CRC-16/CCITT-FALSE over bytes [2..31] |
All multi-byte fields are little-endian. Quaternion W is omitted from the frame; the ground station reconstructs it as qw = sqrt(max(0, 1 − qx² − qy² − qz²)). The firmware enforces qw ≥ 0, making the positive-root reconstruction unambiguous. The CRC is CRC-16/CCITT-FALSE (polynomial 0x1021, initial value 0xFFFF, no bit reflection) and covers the 30 payload bytes that precede the CRC field. A self-test in binary_telemetry_setup() verifies the algorithm against the standard test vector (0x29B1 for "123456789") at boot and halts with a LED flash pattern on failure.
The firmware runs a 3-axis rate and heading-hold PID controller in parallel with the telemetry pipeline. The controller reads pilot inputs from an S.Bus receiver at up to 70 Hz and outputs stabilised commands to six PWM servo outputs: SERVO1/2 for aileron, SERVO3/4 for elevator, SERVO5/6 for rudder. Control mode is selected by S.Bus Ch8.
| Ch8 Range | Mode | Description |
|---|---|---|
| < 600 µs | Rate | Gyro-rate tracking; stick commands angular rate up to 240 °/s (roll/pitch) or 120 °/s (yaw) |
| 600–1200 µs | Gyro off | PID bypassed; stick inputs passed directly to servo outputs |
| > 1200 µs | Heading hold | Attitude stabilisation; stick commands angle setpoint ±90°; yaw remains in rate mode |
Kp, Ki, and Kd are tunable in flight via S.Bus pot channels (Ch13, Ch9, Ch14) without reflashing. The same gain set applies to all three axes. The gyroscope stabilization and telemetry functions share the ICM-20948 quaternion and gyroscope rate outputs and impose no additional sensor read overhead.
The NEO-M9N NAV-PVT message includes a 4-byte millisecond-accurate GPS time-of-week (iTOW) that uniquely identifies each fix. This field is not included in the 34-byte AirTelemetry_t frame. The ground station EKF cannot therefore distinguish a freshly received GPS fix from a delayed or retransmitted frame, and fuses each received frame as if the GPS measurement occurred at the moment the serial byte arrived. Under low-latency, low-loss link conditions this introduces a timing error of approximately 5–10 ms per fix. Under poor link conditions the error can reach one full GPS epoch (100 ms). Adding iTOW as a uint32 field to the frame is the highest-priority firmware revision item. See Issue #3 in the repository.
The NEO-M9N is an L1-only multi-constellation receiver. Under open-sky conditions horizontal accuracy is approximately 2–3 m CEP. At 100 m slant range this represents a ±1.1° angular uncertainty — comparable in magnitude to the measured ground station gimbal steady-state error. Upgrading to an L1/L5 dual-frequency receiver would reduce position uncertainty to sub-meter CEP and is the clearest path to improved end-to-end tracking accuracy.
The IMU-to-body-frame alignment is assumed to be identity (IMU mounted nominally aligned with airframe axes). Any physical mounting offset introduces a constant bias in the NED acceleration estimates and a corresponding systematic position error in the EKF. A one-time extrinsic calibration routine that estimates the mounting rotation would eliminate this bias.
The attitude display and heading-hold PID controller derive Euler angles from the DMP quaternion using the standard ZYX convention. The pitch singularity at ±90° causes gimbal lock in the Euler representation. This is not an operational issue for fixed-wing aircraft with small pitch excursions but would become problematic for aerobatic or high-angle-of-attack flight. The quaternion itself is singularity-free; all control and telemetry paths that matter operate on the quaternion directly.
The airborne firmware demonstrates that a compact, low-power platform is sufficient for simultaneous GPS beacon operation and 3-axis flight stabilization. The DMP-based attitude solution eliminates the Teensy-side AHRS computation entirely, leaving the main processor free for EKF predict steps, UBX parsing, S.Bus decoding, and PID output — all within a 4 ms main loop budget. The 34-byte binary telemetry format is compact enough to transmit at the IMU rate without saturating the XBee link, while the gps_fresh flag allows the ground station to differentiate attitude-only frames from GPS-update frames without a separate channel.
The principal outstanding limitation — the absent iTOW field — is a one-field addition to the wire struct that also requires a coordinated update to the ground station parser. It is the only known firmware change with material impact on tracking accuracy under degraded link conditions.
Source code is available at github.com/lcumbridge/mrs-airborne-tracker. Commercial licensing inquiries should be directed to microrobosys.com.