Tag: M5Stack

Building a Custom Drone Controller from Scratch — Part 3: Firmware Architecture (HAL Pattern, Non-Blocking Loop, and the Flight State Machine)

In Part 1 we talked about why this project exists and what hardware it runs on, and in Part 2 we reverse-engineered the UDP protocol that lets an M5Stack AtomS3 talk to a cheap WiFi toy drone. Before going further into flight control and gamepad support, it’s worth stopping to look at how the firmware itself is put together — because the same skeleton ends up driving two completely different drones (Maritaca Force 1, the black E58-protocol drone, and Dr.One, the grey FLOW-WIFI drone) and two completely different input methods (accelerometer tilt and a Bluetooth gamepad) without duplicating the flight logic.

The full source is on GitHub: github.com/popolony2k/maritaca-e88-controller.

A quick tour of the project structure

The firmware is built with PlatformIO, using plain .cpp/.h files (no .ino) so everything can be unit-structured and reasoned about like normal C++. The layout looks like this:

src/
├── main.cpp                  # wiring + non-blocking main loop
├── hal/
│   ├── hal.h                  # BoardHal, DisplayHal, ImuHal, ButtonHal
│   ├── m5atoms3.h
│   └── m5atoms3.cpp           # only file that includes M5Unified.h
├── imu/
│   └── accelerometer.h/.cpp   # ImuData struct, filtering
├── comm/
│   ├── drone_protocol_base.h  # abstract DroneProtocolBase interface
│   ├── drone_protocol.h/.cpp  # WIFI_8K_ black drone (E58 8-byte)
│   ├── flow_wifi_protocol.h/.cpp # FLOW-WIFI grey drone (88-byte)
│   └── wifi_manager.h/.cpp    # WiFi station + auto-detect scan
├── bt/
│   ├── gamepad_axes.h         # normalized axes, no BLE deps
│   └── ble_gamepad.h/.cpp     # BLE HID host (iPega, 8BitDo)
├── control/
│   ├── accel_controller.h/.cpp   # tilt → roll/pitch/yaw/throttle
│   ├── gamepad_controller.h/.cpp # gamepad axes → DroneState
│   ├── flight_controller.h/.cpp  # the state machine
│   └── operation_mode.h
└── ui/
    └── display.h/.cpp         # pure renderer

Three ideas repeat throughout this tree, and they’re the subject of the rest of this article:

  1. A hardware abstraction layer (HAL) that keeps the M5Stack SDK out of almost every file.
  2. A non-blocking main loop built entirely on millis() — no delay(), anywhere.
  3. A flight state machine that’s shared by both drones and both input modes.

The HAL: one file gets to know about M5Unified

M5Unified is a great library — it gives you the display, the IMU, the button and the power management of the AtomS3 behind one consistent API. The problem is that once #include <M5Unified.h> shows up in a file, that file is now tied to this specific board. For a project that already juggles two drone protocols and two control schemes, we didn’t want a third axis of “which file is allowed to touch the hardware.”

So the rule is simple: only src/hal/m5atoms3.cpp includes M5Unified.h. Everything else — the flight controller, the display renderer, the accelerometer filter — talks to a set of plain interfaces defined in hal.h:

struct BoardHal {
    void (*begin)           ();
    void (*update)          ();
    int  (*getBatteryLevel) ();   // 0/25/50/75/100 in %
    bool (*isCharging)      ();   // always false on this hardware
};

struct ButtonHal {
    bool (*wasPressed)  ();
    bool (*wasReleased) ();
    bool (*pressedFor)  (uint32_t ms);
};

These are structs of function pointers, not abstract classes with virtual methods. The implementation, in m5atoms3.cpp, fills them in with non-capturing lambdas:

const BoardHal kBoard {
    .begin           = [] { auto cfg = M5.config(); M5.begin(cfg); },
    .update          = [] { M5.update(); },
    .getBatteryLevel = [] { return batteryLevel(); },
    .isCharging      = [] { return false; },
};

const ButtonHal kButton {
    .wasPressed  = []()            -> bool { return M5.BtnA.wasPressed(); },
    .wasReleased = []()            -> bool { return M5.BtnA.wasReleased(); },
    .pressedFor  = [](uint32_t ms) -> bool { return (bool)M5.BtnA.pressedFor(ms); },
};

A non-capturing lambda — one that doesn’t reference any outside variables — has no state of its own, so it decays to a plain function pointer at compile time. There’s no closure object, no heap allocation, no vtable lookup. kBoard.update() compiles down to exactly the same code as calling M5.update() directly, but every other file in the project now depends on hal.h (a tiny, dependency-free header) instead of the entire M5Unified SDK.

This pays off in a very concrete way: the battery-level logic — reading GPIO8/ADC2 through a voltage divider, averaging samples, applying hysteresis around the 25/50/75% boundaries — lives entirely inside m5atoms3.cpp as a couple of small static functions. The flight controller and the display just call kBoard.getBatteryLevel() and get back a number from 0–100. When the battery calibration changed (we covered that story in an earlier debugging session), not a single line outside m5atoms3.cpp needed to change.

A loop with no delay()

The AtomS3 in this project is doing a lot at once: polling a button, running an IMU filter, talking to a BLE gamepad, maintaining a WiFi connection, sending UDP control packets at a fixed rate, and redrawing a small LCD — all from a single loop(). If any one of those used delay(), everything else would stall for that duration. A drone waiting for its next 40 ms control packet doesn’t care that the display “only” wanted to sleep for 10 ms.

So the project has a hard rule: no delay() in loop(), ever. Every periodic task instead remembers the last time it ran and checks millis() on every pass:

static uint32_t _lastDisplayMs = 0;
static constexpr uint32_t DISPLAY_INTERVAL_MS = 100; // 10 Hz

void loop() {
    kBoard.update();
    wifi.update();
    imu.update(kImu);

    // ... gamepad + flight controller update, every iteration ...

    uint32_t now = millis();
    if (now - _lastDisplayMs >= DISPLAY_INTERVAL_MS) {
        _lastDisplayMs = now;
        display.update(/* ... */);
    }
}

The same pattern shows up at every layer, each with its own cadence chosen for a reason:

  • Drone control packets go out at ~25 Hz (every ~40 ms) — fast enough for responsive flight, matching what the original Android app does.
  • The keepalive packet (AA 80 80 00 80 00 80 55) fires every ~790 ms when the sticks are idle, so the drone doesn’t think the link has died.
  • The display redraws at 10 Hz — plenty for human eyes, and it keeps SPI traffic from competing with the more time-critical UDP and BLE work.
  • BLE scanning uses 5-second timed windows, restarted every 6 seconds while no gamepad is connected.

Because every one of these is just “a timestamp and a constant,” loop() can run thousands of times a second, doing almost nothing on most iterations and exactly the right thing the moment any of those windows elapses. Nothing ever blocks anything else.

One subtlety we ran into directly because of this design: WiFiUDP::begin() needs WiFi.mode() to have been called at least once to initialize the ESP32’s network stack — even on builds where WiFi is never actually connected. Skip it, and the drone protocol driver crashes during setup(), producing a silent boot loop. It’s a good reminder that “non-blocking” and “stateless” aren’t the same thing — some subsystems still have a required initialization order, even if nothing about them looks synchronous.

The flight state machine

All of the above exists to support the actual brain of the firmware: FlightController. It’s a small state machine with six states:

enum class FlightState {
    Idle,        // disarmed, sending keepalive packets
    Calibrating, // sending CaliGyro for 1.5 s before arming
    Arming,      // Unlock, then TakeOff
    Flying,      // active flight, 25 Hz control packets
    Landing,     // sending Land for 2 s, then back to Idle
    Emergency,   // EmergStop, then immediately back to Idle
};

The normal lifecycle is exactly what you’d expect:

Idle ──(double-click + WiFi ok)──▶ Calibrating ──▶ Arming ──▶ Flying
Flying ──(double-click)──▶ Landing ──▶ Idle
any state ──(triple-click)──▶ Emergency ──▶ Idle

Every transition goes through one function, enterState(), which resets all the per-state bookkeeping (button click counters, one-shot command timers, the “is this the first frame in this state” flag) and logs the transition to Serial. runState() then does whatever that state needs to do every frame: send the right command bytes, check elapsed time, and decide whether it’s time to move on.

Gestures, not menus

Remember — the only physical input in ACCEL mode is a single screen button (BtnA). Every gesture has to be encoded in click count and timing:

  • Single click in Flying toggles yaw on/off.
  • Double click in Idle arms and takes off (if WiFi is connected); double click in Flying starts landing.
  • Triple click, from any state, is an immediate emergency stop.
  • Press-and-hold (ACCEL mode, while Flying) drives the throttle — first hold = climb, click-then-hold = descend.

All of this is decided by a small amount of state — _clickCount, _lastReleaseMs, _buttonDown — updated once per frame in handleButton(). A click only “counts” once the double-click window (DOUBLE_CLICK_MS = 1000) has elapsed without a follow-up press, which is what lets a double-click and a hold-after-click (the throttle-down gesture) coexist on the same physical button.

In BLUETOOTH mode, the same state machine is driven by handleGamepadButtons() instead — rising edges on the gamepad’s button bitmask map directly to the same transitions (A = arm, B = land, X = emergency, etc.), so runState() itself doesn’t need to know or care which input method is active.

One state machine, two drones

This is the part that ties back to Part 2. Maritaca Force 1 (the WIFI_8K_ / E58-protocol drone) needs the full Calibrating → Arming(Unlock → TakeOff) → Flying sequence. Dr.One (the FLOW-WIFI drone) auto-arms the moment it receives a single TakeOff toggle — sending it through a multi-second calibration dance would just be wrong for that hardware.

Rather than branch the state machine on “which drone is this,” the abstract DroneProtocolBase interface exposes a single capability flag:

// FlightController::handleDoubleClick()
enterState(_deps.drone.supportsArmSequence()
           ? FlightState::Calibrating   // Maritaca Force 1
           : FlightState::Flying);      // Dr.One — auto-arms on TakeOff

and enterState() fires the one-shot TakeOff command itself when that flag is false:

if (s == FlightState::Flying) {
    _accel.begin();
    _gamepad.begin();
    if (!_deps.drone.supportsArmSequence()) {
        _oneShotCmd   = DroneCmd::TakeOff;
        _oneShotUntil = millis() + 1000;
    }
}

Everything else — gesture handling, throttle hold, the altitude-hold throttle logic, the landing and emergency paths — is shared. Swapping drones at boot is just a matter of which concrete DroneProtocolBase implementation gets injected into FlightController‘s constructor; WifiManager::scanForFirst() figures out which SSID is visible and the rest follows automatically.

Safety net: WiFi loss means emergency stop

Every call to runState() starts with one check, before the big switch statement:

if (!wifiOk && _state != FlightState::Idle && _state != FlightState::Emergency) {
    Serial.println("[Flight] WiFi lost — emergency stop");
    enterState(FlightState::Emergency);
    return;
}

If the AtomS3 ever drops its connection to the drone’s access point while armed, calibrating, flying, or landing, the very next frame forces an emergency stop — regardless of what gesture or gamepad input is happening. The same idea applies to losing the Bluetooth gamepad mid-flight in BLUETOOTH mode. Given that this whole project is one step away from “small spinning blades a meter from your face,” this single guard clause is arguably the most important seven lines in the firmware.

What’s next

With the skeleton in place — HAL, non-blocking loop, and a drone-agnostic state machine — the next two articles get to the fun part: turning physical input into flight commands. Part 4 covers the accelerometer/tilt control path (and the altitude-hold throttle gesture that took some real debugging to get right), and Part 5 covers building a BLE HID host from scratch to support a Bluetooth gamepad.

As always, the full source is on GitHub: github.com/popolony2k/maritaca-e88-controller. You can also see Maritaca Force 1 in action on the project’s YouTube playlist.

Enjoy
[]’s
PopolonY2k