ground_station.cpp

// Uplink wire format
// ------------------
// One command per LoRa packet, plain ASCII. The packet boundary terminates the
// command, so no newline is needed. Parsing is case-insensitive and leading and
// trailing whitespace is ignored.
//
//   ARM                arm the aircraft
//   DISARM             disarm and cut the motors
//   MODE VERTICAL      hover / altitude hold
//   MODE HORIZONTAL    begin the transition to cruise
//   ALT <metres>       altitude setpoint above the launch point, 0..120
//   HDG <degrees>      heading setpoint, 0..360
//   PING               heartbeat: carries no order, only refreshes the link timer
//
// Anything else is rejected and logged over the debug serial port. The ground
// station should send PING at about 2 Hz whenever it has nothing else to say -
// without it the aircraft cannot tell a quiet pilot from a dead radio, and it
// will drop into the link-loss failsafe two seconds after the last command.
// Two per second is a floor rather than a suggestion: the radio is half duplex,
// so the aircraft is deaf for as long as its own telemetry frame takes to go
// out, and the downlink interval in telemetry.cpp is picked so that a 2 Hz
// heartbeat cannot be swallowed whole by that window.
//
// The downlink shares this radio. Its frame format is documented in telemetry.cpp.

#include "ground_station.h"

#include <Arduino.h>
#include <LoRa.h>
#include <ctype.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

// Wiring.
#define LORA_SS   10
#define LORA_RST  9
#define LORA_DIO0 2

static const long LORA_FREQUENCY_HZ = 915E6;   // 915 MHz ISM, North America

// Four missed heartbeats at the 2 Hz ground-station cadence. Long enough that a
// single dropped packet or a brief antenna null does not trip the failsafe,
// short enough that the aircraft is not flying uncommanded for more than a
// couple of seconds.
static const unsigned long LINK_TIMEOUT_MS = 2000;

// "MODE HORIZONTAL" is the longest thing we accept, plus slack for whitespace.
// The cap matters: an accumulator with no limit lets a noisy channel grow a
// buffer until the heap gives out.
static const unsigned int CMD_MAX_LEN = 32;

// Above 400 ft AGL you are out of Part 107 airspace, so there is no legitimate
// setpoint up there and a garbled one should not be honoured.
static const float ALT_MIN_M = 0.0f;
static const float ALT_MAX_M = 120.0f;

// The downlink is handed to the modem and left to finish on its own, so
// something has to know when the radio is free again. LoRa.h keeps
// isTransmitting() private and publishes no "done" flag, which leaves the
// clock: stand off for the air time of the longest frame telemetry.cpp emits.
//
// At the rate LoRa.begin() leaves configured - SF7, 125 kHz, CR 4/5, 8-symbol
// preamble, payload CRC on - the Semtech time-on-air formula gives 12.5 ms of
// preamble plus 163 symbols of 1.024 ms for the 106-byte worst case, so about
// 180 ms. Rounded up, because ending the standoff early is the expensive
// mistake: parsePacket() writes RegOpMode without asking whether the modem is
// busy, and would drag it out of TX mid-packet. Standing off too long only
// costs a few milliseconds of extra deafness on a link that was deaf anyway.
static const unsigned long TX_STANDOFF_MS = 200;

static bool radioUp = false;
static bool everHeard = false;
static unsigned long lastPacketMs = 0;

static bool txInFlight = false;
static unsigned long txStartedMs = 0;

static GroundStationCommand pending = { CMD_NONE, 0.0f };
static bool pendingValid = false;

// True while the last downlink frame is still going out.
static bool transmitting() {
    if (!txInFlight) {
        return false;
    }
    if ((millis() - txStartedMs) >= TX_STANDOFF_MS) {
        txInFlight = false;
        return false;
    }
    return true;
}

static bool parseFloatStrict(const char *text, float *out) {
    if (text == NULL || *text == '\0') {
        return false;
    }
    char *tail = NULL;
    double value = strtod(text, &tail);
    if (tail == text || *tail != '\0') {
        return false;   // trailing junk: "115abc" is not a number
    }
    if (!isfinite(value)) {
        return false;   // strtod happily parses "nan" and "inf"
    }
    *out = (float)value;
    return true;
}

// Parses one packet's worth of text in place. Returns false if it is not
// something we understand, in which case nothing is acted on and the link timer
// is left alone. On success *out may still be CMD_NONE - PING is a recognised
// packet that carries no order.
static bool parseCommand(char *text, GroundStationCommand *out) {
    out->type = CMD_NONE;
    out->value = 0.0f;

    char *start = text;
    while (*start == ' ' || *start == '\t' || *start == '\r' || *start == '\n') {
        start++;
    }
    char *end = start + strlen(start);
    while (end > start && (end[-1] == ' ' || end[-1] == '\t' ||
                           end[-1] == '\r' || end[-1] == '\n')) {
        *--end = '\0';
    }
    for (char *p = start; *p != '\0'; p++) {
        *p = (char)toupper((unsigned char)*p);
    }
    if (*start == '\0') {
        return false;
    }

    // Split the keyword from its argument at the first space.
    char *arg = strchr(start, ' ');
    if (arg != NULL) {
        *arg++ = '\0';
        while (*arg == ' ') {
            arg++;
        }
    }

    if (strcmp(start, "PING") == 0) {
        return true;
    }
    if (strcmp(start, "ARM") == 0) {
        out->type = CMD_ARM;
        return true;
    }
    if (strcmp(start, "DISARM") == 0) {
        out->type = CMD_DISARM;
        return true;
    }

    if (strcmp(start, "MODE") == 0) {
        if (arg != NULL && strcmp(arg, "VERTICAL") == 0) {
            out->type = CMD_MODE_VERTICAL;
            return true;
        }
        if (arg != NULL && strcmp(arg, "HORIZONTAL") == 0) {
            out->type = CMD_MODE_HORIZONTAL;
            return true;
        }
        Serial.println("Uplink: MODE needs VERTICAL or HORIZONTAL");
        return false;
    }

    if (strcmp(start, "ALT") == 0) {
        float metres;
        if (!parseFloatStrict(arg, &metres)) {
            Serial.println("Uplink: ALT needs a number");
            return false;
        }
        if (metres < ALT_MIN_M || metres > ALT_MAX_M) {
            Serial.print("Uplink: ALT out of range: ");
            Serial.println(metres);
            return false;
        }
        out->type = CMD_SET_ALTITUDE;
        out->value = metres;
        return true;
    }

    if (strcmp(start, "HDG") == 0) {
        float degrees;
        if (!parseFloatStrict(arg, &degrees)) {
            Serial.println("Uplink: HDG needs a number");
            return false;
        }
        if (degrees < 0.0f || degrees > 360.0f) {
            Serial.print("Uplink: HDG out of range: ");
            Serial.println(degrees);
            return false;
        }
        out->type = CMD_SET_HEADING;
        out->value = degrees;
        return true;
    }

    Serial.print("Uplink: unknown command: ");
    Serial.println(start);
    return false;
}

bool initGroundStation() {
    LoRa.setPins(LORA_SS, LORA_RST, LORA_DIO0);

    radioUp = LoRa.begin(LORA_FREQUENCY_HZ);
    if (!radioUp) {
        // Return rather than retry or block: the caller needs to know the radio
        // is dead so it can refuse to arm.
        Serial.println("LoRa init failed - no uplink and no downlink");
        return false;
    }

    // LoRa.begin() leaves RxPayloadCrcOn at the SX1276 reset default, which is
    // off: it sets the frequency, the FIFO base addresses, the LNA, the AGC and
    // the TX power and nothing else.
    //
    // Worth being clear about which direction it buys, because the intuitive
    // answer is the wrong one. The radio runs in explicit header mode, which
    // parsePacket() re-asserts on every call, and there RxPayloadCrcOn only
    // governs CRC generation on transmit. So what this hardens is the downlink:
    // our frames go out with a CRC and a header flag saying so, and the ground
    // station's modem throws away the telemetry that arrived damaged.
    //
    // Inbound, the modem takes CRC presence from the received packet's own
    // header. So whether a mangled PING can reach parseCommand() - and the
    // keywords are short enough that a few bit flips in one could land on
    // DISARM - comes down to whether the ground station enabled CRC at its end,
    // and nothing here can make it. Same shelf as the default sync word further
    // down: half of this link's integrity is somebody else's setting.
    LoRa.enableCrc();

    everHeard = false;
    txInFlight = false;
    pendingValid = false;
    pending.type = CMD_NONE;
    pending.value = 0.0f;

    Serial.println("LoRa up at 915 MHz");
    return true;
}

bool groundStationCommandReceived() {
    if (!radioUp) {
        return false;
    }
    if (transmitting()) {
        // parsePacket() would put the modem into receive on the spot and cut
        // our own frame off mid-air. Nothing is lost by waiting that was not
        // already lost - a half-duplex radio cannot hear while it talks.
        return pendingValid;
    }

    if (LoRa.parsePacket() > 0) {
        char buffer[CMD_MAX_LEN + 1];
        unsigned int length = 0;
        bool oversized = false;

        while (LoRa.available()) {
            char c = (char)LoRa.read();
            if (length < CMD_MAX_LEN) {
                buffer[length++] = c;
            } else {
                oversized = true;   // keep draining, but stop storing
            }
        }
        buffer[length] = '\0';

        GroundStationCommand received = { CMD_NONE, 0.0f };
        if (oversized) {
            Serial.println("Uplink: oversized packet dropped");
        } else if (parseCommand(buffer, &received)) {
            // Only a packet that parsed counts as contact. A CRC drops the
            // corrupted ones if the sender asked for one, and the sync word
            // filters most of the band, but neither says the packet came from
            // our ground station, and the failsafe is only worth having if it
            // means that.
            // The sync word is the library default (0x12), so anything else on
            // 915 MHz using it can talk to this aircraft - there is no
            // authentication on the uplink.
            lastPacketMs = millis();
            everHeard = true;

            if (received.type != CMD_NONE) {
                pending = received;
                pendingValid = true;
            }
        }
    }

    return pendingValid;
}

GroundStationCommand getGroundStationCommand() {
    GroundStationCommand command = pending;
    pending.type = CMD_NONE;
    pending.value = 0.0f;
    pendingValid = false;
    return command;
}

bool groundStationLinkAlive() {
    if (!radioUp || !everHeard) {
        return false;
    }
    return (millis() - lastPacketMs) < LINK_TIMEOUT_MS;
}

bool groundStationSend(const char *line) {
    if (!radioUp || line == NULL) {
        return false;
    }
    if (transmitting()) {
        return false;
    }
    if (LoRa.beginPacket() != 1) {
        return false;
    }
    LoRa.print(line);

    // Asynchronous deliberately. endPacket() in its blocking form spins on the
    // TX-done flag for the whole air time, and this is reached from inside a
    // 50 Hz control cycle: about 180 ms with the lift rotors held on their last
    // command, every time a frame goes down. So true here means the modem took
    // the packet, not that it left the antenna.
    if (LoRa.endPacket(true) != 1) {
        return false;
    }
    txStartedMs = millis();
    txInFlight = true;
    return true;
}