main.cpp

#include <Arduino.h>
#include "flight_computer.h"

// The PD loops assume a fixed cadence - their derivative term is only meaningful if dt is
// constant. 20 ms is the 50 Hz they are written for; the period lives here and nowhere else.
static const uint32_t CONTROL_PERIOD_MS = 20;

static uint32_t nextCycleMs = 0;

void setup() {
    Serial.begin(9600);
    initFlightComputer();
    nextCycleMs = millis();
}

void loop() {
    const uint32_t now = millis();

    // millis() wraps at 2^32 ms, about every 49 days. Subtracting first and reading the
    // result as signed keeps the deadline comparison correct across that wrap, which is why
    // both sides are pinned to 32 bits rather than left as int/long.
    if ((int32_t)(now - nextCycleMs) < 0) {
        return;
    }
    nextCycleMs += CONTROL_PERIOD_MS;

    // A cycle that overran by a full period or more would otherwise be followed by a burst
    // of back-to-back catch-up cycles. Give up the lost time instead. The comparison is
    // >= rather than > because an overrun of exactly one period leaves the new deadline
    // sitting on the current millisecond, which is a catch-up cycle with no wait in it.
    if ((int32_t)(now - nextCycleMs) >= 0) {
        nextCycleMs = now + CONTROL_PERIOD_MS;
    }

    updateFlightComputer();
}