VTOL Flight Computer

Flight-control firmware for a fixed-wing VTOL (vertical take-off and landing) aircraft, running on a Teensy microcontroller. It manages the full flight envelope — vertical hover, transition, and horizontal cruise — from a barometer, a GPS and a magnetometer read on a fixed 50 Hz cycle, and talks to a ground station over a long-range LoRa radio link.

Not built, not flashed, not flown. There is no Teensy toolchain on the machine I wrote this on, so pio run has never been executed against this tree, and no aircraft has ever carried it. What does run is the logic. Five host test suites compile every one of the 13 sources at the repo root, main.cpp included, and exercise the real control loops, mode machine, command parser and 50 Hz scheduler on a desktop:

./run_tests.sh

The comms, flight-computer and scheduler suites name each check as they run; control_math and drivers report a count for the whole suite. That is close to four hundred lines, ending:

-- millis() rollover --
        ticked at 4294967265 4294967285 9 29
  ok    the wrap neither drops a cycle nor adds one
  ok    every gap is 20 ms, including the one straddling 2^32
  ok    and cycles land on both sides of it
  ok    a second spanning the wrap is 50 cycles at 20 ms from all 20 alignments

20 checks, 0 failures
scheduler: PASS

== warning sweep (informative, cannot fail the run)
warning sweep: 13 files, clean

5 suites, all passed

That is the whole of what has been proven. Tests says what the suites cover and what they cannot, and Status says what is left.

Flight modes

The vehicle runs a small mode state machine and only actuates once armed from the ground:

stateDiagram-v2
    state "IDLE — disarmed, all four ESCs parked at 1000 µs" as IDLE
    state "VERTICAL — hover, the lift rotors hold altitude" as VERTICAL
    state "TRANSITION — rotors tilting forward at 15 deg/s" as FWD
    state "TRANSITION — rotors tilting back to hover" as BACK
    state "HORIZONTAL — cruise, holds heading and the hand-off speed" as CRUISE

    [*] --> IDLE
    IDLE --> Armed: ARM — refused unless the radio came up and sensorsHealthy()

    state Armed {
        [*] --> VERTICAL
        VERTICAL --> FWD: MODE HORIZONTAL — refused unless sensorsHealthy()
        FWD --> CRUISE: ramp done, GPS fix, ground speed ≥ 12 m/s
        FWD --> BACK: ramp done, gate fails — no flying speed confirmed
        FWD --> BACK: MODE VERTICAL — abort mid-ramp
        CRUISE --> BACK: MODE VERTICAL
        BACK --> VERTICAL: ramp done
        BACK --> FWD: MODE HORIZONTAL — turn the abort round, same health gate
    }

    Armed --> IDLE: DISARM, or 2 s with no packet — all four motors stopped

Loading

Mode	Behavior
IDLE	Disarmed; waiting for the ground-station ARM command over LoRa
VERTICAL	Hover / vertical control — holds a target altitude (barometer-driven)
TRANSITION	Open-loop ramp of the tilt servos from hover to cruise at a fixed 15 deg/s, gated on GPS ground speed
HORIZONTAL	Cruise — holds a target heading, and the ground speed it was handed over at. No altitude authority

TRANSITION walks the servos at that fixed rate and trusts them to keep up; nothing measures where they actually got to. Aborting one second in therefore costs about one second to undo. The lift rotors keep holding altitude the whole way, and the hand-off to cruise only happens if the GPS has a fix and reports enough speed for the wing to be flying. Otherwise it walks the rotors back up. A MODE VERTICAL command aborts the ramp mid-way.

Cruise holds heading and speed and nothing else. There is no vertical loop running in HORIZONTAL and the airframe carries no pitch surface, so the aircraft holds whatever altitude the wing and the cruise thrust happen to give it and will drift. Coming back off the wing, the vertical loop re-seats on wherever the aircraft ended up rather than resuming the altitude it left with — climbing back to a stale setpoint would spend the whole six-second ramp at high throttle with the rotors still tilted forward, which is thrust in the wrong direction. Ask for the altitude again once it is back in VERTICAL.

Hardware

A Teensy 4.1 on the Arduino / PlatformIO framework, a LoRa module at 915 MHz (sandeepmistry/LoRa) carrying the commands up and the telemetry down, and three sensors: a barometer for altitude, a GPS for position and ground speed, and a magnetometer for heading. There is no IMU on this airframe — see What's not done yet. Four lift rotors on hobby ESCs carry the aircraft in hover, and the tilt servo pair swings them forward to become the cruise thrust; the other two servo channels are spare.

Every pin below is either set by a file in this repo or is a Teensy 4.1 core default the firmware relies on without setting. The last column says which, so you can go and read it:

Signal	Teensy 4.1 pin	Where the number comes from
Lift ESCs, motors 0-3	3, 4, 5, 6	motors.cpp
Tilt servos, left and right	20, 21	servos.cpp for the pins; flight_computer.cpp for which two channels are the tilt pair
Spare servo channels 2 and 3	22, 23	servos.cpp — attached and centred at boot, never flown
LoRa NSS / RST / DIO0	10 / 9 / 2	ground_station.cpp
LoRa SPI — MOSI / MISO / SCK	11 / 12 / 13	Teensy core default, not set here. motors.cpp reserves them so nothing else lands on them
GPS, 9600 baud	Serial1 — RX 0, TX 1	gps.cpp opens Serial1; the pin pair is the Teensy core default
Barometer and magnetometer, one shared I2C bus	SDA 18, SCL 19	Either driver's begin() brings the bus up — sensors.cpp runs the magnetometer's first; the pin pair is the Teensy core default
Debug log, 9600 baud	USB serial	main.cpp

Pins 7 and 8 are Serial2 and are deliberately left free for a wired link. Nothing in this repo uses them — the downlink goes over the same LoRa radio as the uplink.

Ground station link

The uplink is plain ASCII, one command per LoRa packet, case-insensitive. Whitespace around the command is trimmed, including a trailing \r\n, so a ground station that writes lines to a serial port does not have to strip them first:

ARM                DISARM
MODE VERTICAL      MODE HORIZONTAL
ALT <metres>       0..120
HDG <degrees>      0..360
PING               heartbeat, carries no order

ALT acts in VERTICAL and TRANSITION, the modes where the lift rotors are flying the aircraft. In cruise it is refused, with a line on the debug port saying so — there is no altitude loop running there for it to reach. Sent before ARM it is accepted but does not survive arming, which re-seats the loop on the current altitude so that arming can never command a climb by itself.

HDG is the mirror of that, and is refused the same way outside HORIZONTAL. In hover all four lift rotors are commanded identically, so there is no yaw authority and no heading loop for the setpoint to act on — and the hand-off to cruise captures whatever heading the aircraft is on at the time, so a number sent earlier would be overwritten rather than obeyed. Ask again once it is on the wing. It is refused in cruise as well if the magnetometer has stopped answering, and says which of the two reasons it was: with no measured heading left the yaw term has already stood down, so there is nothing for a setpoint to mean anything against.

MODE HORIZONTAL has a refusal of its own: it is turned down while sensorsHealthy() is false, because cruise holds no altitude and the way back out of it hands the aircraft to a vertical loop closing on a barometer that has stopped answering. MODE VERTICAL is never refused for that reason — hover on a frozen altitude is bad, and it is still better than the wing.

The ground station has to send PING at about 2 Hz whenever it has nothing else to say — the downlink rate below is chosen around this one. There is no other way for the aircraft to tell a quiet pilot from a dead radio, and two seconds after the last packet that parsed it treats the link as lost, disarms and stops all four motors. Only a packet the parser accepted counts as contact, so noise on the band that decodes into something the parser rejects — up to and including a packet of nothing but whitespace — does nothing to hold the failsafe off. Full wire format at the top of ground_station.cpp.

There is no authentication on the uplink and the sync word is the library default, so anything on 915 MHz using it can send this aircraft a DISARM.

Telemetry does not come out of the serial port — it goes down the same LoRa radio that carries the uplink, as a comma-delimited frame once a second. The radio is half duplex, so the aircraft is deaf for the ~180 ms a frame takes to go out, and the rate is picked so that a 2 Hz heartbeat cannot be talked over for long enough to trip the link-loss failsafe. That makes this number and the ground station's ping rate above a matched pair — change one and you have to redo the arithmetic on the other. The frame format is documented at the top of telemetry.cpp. The serial port carries debug lines only.

Architecture

Each subsystem is a self-contained module (.cpp/.h pair):

• flight_computer.* — the brain: arming, the mode state machine, the setpoints, the link-loss failsafe, and the order everything gets called in each cycle. Collects a single FlightData state for the downlink
• flight_control_vertical.* / flight_control_horizontal.* — the two PD control loops
• control_math.* — PD, clamping, heading wrap. No Arduino headers, on purpose, so it builds and runs on a desktop
• sensors.* — one facade over the sensor drivers, handing the controllers unit-correct state (metres, m/s, degrees) instead of raw sensor readings
• barometer.*, gps.*, magnetometer.* — sensor drivers
• motors.*, servos.* — actuator outputs
• ground_station.* — the LoRa link: init, command parsing, link health
• telemetry.* — packs FlightData for the LoRa downlink
• main.cpp — the Arduino entrypoint and nothing else: start the flight computer, then tick it at a fixed 50 Hz

Tests

Five host test suites. They compile the real source files and exercise the actual logic on a desktop — no Teensy, no aircraft. Four of them link against stub Arduino and sensor-library headers in test/stubs/; the fifth, test_control_math.cpp, needs no stubs at all, because control_math.* deliberately includes no Arduino headers. One command from the repo root runs the lot:

./run_tests.sh

It builds into a temp directory it deletes on the way out, runs each suite, and finishes by re-compiling all 13 root translation units at -Wall -Wextra -Wshadow -Wconversion as a warning sweep. The sweep prints what it finds but cannot fail the run: it is a stricter bar than the suites build at, and it runs under whatever compiler the machine happens to have, so it reports rather than blocks.

./run_tests.sh --sanitize rebuilds the same five under AddressSanitizer and UndefinedBehaviorSanitizer, with -fno-sanitize-recover=all so the first diagnostic ends the process instead of scrolling past. It is the pass to reach for after touching arithmetic or a buffer: this firmware turns floats into microsecond pulse widths and fixed-point telemetry fields throughout, and the uplink parser fills a fixed char array straight off the radio — none of which an assertion on a return value can see. (It skips the warning sweep, which is syntax-only and would report exactly what it reports in the ordinary run.) It comes back clean on the two compilers it has been run on here — Apple clang 21 on arm64 macOS, and GCC 13.3 on x86-64 Ubuntu — which is the whole of what that proves.

A GitHub Actions workflow runs both of those on every push, one job each, with no compiler override in either — so what CI does and what this section tells you to run cannot drift apart. It asks the repository for read-only permissions and nothing more. Nobody has watched that workflow run yet, though: it is written and unobserved, which is also why there is no badge at the top of this file.

To build one suite on its own while working on it:

g++ -std=c++17 -Wall -Wextra -o /tmp/tcm test/test_control_math.cpp control_math.cpp && /tmp/tcm

g++ -std=c++17 -Wall -Wextra -Itest/stubs -o /tmp/tdrv test/test_drivers.cpp \
    barometer.cpp gps.cpp magnetometer.cpp motors.cpp servos.cpp && /tmp/tdrv

g++ -std=c++17 -Wall -Wextra -Itest/stubs -o /tmp/tcomms test/test_comms.cpp \
    ground_station.cpp sensors.cpp telemetry.cpp && /tmp/tcomms

g++ -std=c++17 -Wall -Wextra -Itest/stubs -o /tmp/tfc test/test_flight_computer.cpp \
    flight_computer.cpp flight_control_vertical.cpp flight_control_horizontal.cpp \
    control_math.cpp && /tmp/tfc

g++ -std=c++17 -Wall -Wextra -Itest/stubs -o /tmp/tsched test/test_scheduler.cpp main.cpp \
    && /tmp/tsched

The flight-computer suite is the one that covers the transition — the thing this aircraft could not do at all before, and the phase of a VTOL flight that most deserves a test. Its plant is a toy: altitude is whatever the test says it is, and nothing responds to thrust. It checks the mode machine, the gates and the units, and says nothing about the airframe.

The scheduler suite links the real main.cpp against a clock the test drives, which is the only way to see the three things in that file that cannot be checked by eye: that the deadline stays on a fixed 20 ms grid even when loop() is polled off it, rather than re-anchoring on the late call and walking forward a millisecond a cycle; that an overrun costs one cycle rather than a burst of catch-ups; and that the cadence survives the millis() wrap 49 days in — cycles land at 4294967265, 4294967285, then 9, with every gap still 20 ms across it.

What the suites prove is that the maths, the unit conversions, the clamping, the command parser, the link-health rules, the mode machine and the control cadence do what they say. What they cannot prove is that any of it builds for a Teensy, or that a stub behaves the way the real library does. The stubs were written from the libraries' source, and where one of them is wrong the tests will go green anyway.

pio test is not wired up: these are plain main() binaries, not Unity tests, and the test/stubs/ tree shadows the real headers.

Build & flash

Built with PlatformIO:

pio run              # build
pio run -t upload    # flash to the Teensy
pio device monitor   # debug log, 9600 baud

None of those three has ever been run against this tree — there is no Teensy toolchain here, so the firmware image is unproven. See Status.

The sources live at the repo root rather than under src/, so platformio.ini sets src_dir = . and narrows the build filter to the top-level .cpp files.

Safety

The aircraft boots disarmed. The four ESCs are parked at 1000 µs from initMotors() and nothing raises them until the ground station sends ARM, so the props cannot spin up on the bench by accident. The servos are a different story: Servo::attach() starts pulsing a channel the moment it is attached, so all four servo channels are live from boot — initServos() centres them and the flight computer immediately drives the two tilt servos to the hover position, while disarmed. setMotorPower() and setServoAngle() also refuse a command that is not a number — the motor drops to idle, the servo keeps pulsing the last angle it was given and waits for a finite one — which the uplink parser already catches upstream, but the layer nearest the wire is where that guarantee belongs.

This is real flight-controls code — treat any live-vehicle testing with the usual RC/UAV safety practices (props off for bench tests, open field, spotter).

Status

The firmware has never been through a Teensy toolchain. The build config is written against the PlatformIO docs, every library pin is checked against the registry, and src_dir = . is reasoned from the documentation rather than observed — but the compile itself is unproven, and a first pio run on a real toolchain is the next thing that needs doing.

What's not done yet

• Attitude stabilisation. There is no IMU on this airframe — no gyro, no accelerometer, nothing to close a pitch or roll loop around. The vertical loop holds altitude and that is all it does. An IMU is the next hardware item; until it is on the aircraft, staying upright is the airframe's job, not the firmware's.
• The transition. TRANSITION is an open-loop servo ramp at a fixed rate. It has never been run on the bench, let alone in the air, and it is the phase of a VTOL flight that most deserves to be tested before it is trusted.
• Every gain. KP_ALTITUDE, KD_ALTITUDE, HOVER_THROTTLE, CRUISE_THROTTLE and the 12 m/s transition speed are guesses at numbers that have to come off a thrust stand and a real airframe.
• The magnetometer. No hard/soft-iron calibration and no declination correction, and the axis convention in magnetometer.cpp has never been checked against the real vehicle. Get that wrong and the cruise heading loop runs as positive feedback, so a bench spin is a gating check, not a nicety. One that quits in the air no longer takes the aircraft with it: headingValid() goes false, cruise stands its yaw term down and flies straight on the heading it had rather than rolling into a turn it can never finish, says so once on the debug port, and refuses any later HDG. That is as far as it goes, though — there is still no second heading source on this airframe, so what the aircraft ends up holding is a heading nothing is measuring.
• Nothing changes what the motors are doing when a sensor dies in flight. The aircraft does notice now: sensorsHealthy() going false while armed says so once on the debug port and refuses MODE HORIZONTAL — the command, not a ramp already under way — because cruise has no altitude authority and the way back out of it hands the aircraft to a vertical loop closing on a frozen number, which is the one decision here that is clearly wrong. But what an aircraft already in the air does with its motors is unchanged: it goes on hovering against the last altitude the barometer gave it. Deciding what it should actually do — hold, descend, cut — is a flight-safety call that wants a bench and an airframe behind it, not a guess written here.
• The failsafe stops the motors. Correct on the bench, wrong in the air, where it should be flying a controlled descent.
• Barometric altitude never re-baselines, so it drifts with the weather — several metres over an hour as a front moves through. A SET_QNH uplink command would be the honest fix.

License

MIT — see LICENSE.