telemetry.cpp

// Frames go down the same LoRa radio that carries the uplink commands. The
// airframe has one radio, and the serial port is a debug log, not a downlink.
//
// Frame format - one line per packet, comma delimited. The packet boundary ends
// it, so there is no terminator:
//
//   $V,<t_ms>,<mode>,<armed>,<alt_cm>,<vs_cm_s>,<hdg_ddeg>,<gs_cm_s>,
//      <lat_e7>,<lon_e7>,<m0>,<m1>,<m2>,<m3>,<s0>,<s1>,<s2>,<s3>
//
//   t_ms      milliseconds since boot
//   mode      FlightMode ordinal: 0 IDLE, 1 VERTICAL, 2 TRANSITION, 3 HORIZONTAL
//   armed     0 or 1
//   alt_cm    altitude above the launch point, centimetres, positive up
//   vs_cm_s   vertical speed, cm/s, positive up
//   hdg_ddeg  heading, tenths of a degree, 0..3599
//   gs_cm_s   GPS ground speed, cm/s
//   lat_e7    latitude in decimal degrees x 1e7
//   lon_e7    longitude in decimal degrees x 1e7
//   m0..m3    lift motor commands, per mille of full throttle, 0..1000
//   s0..s3    servo angles, tenths of a degree, 0..1800
//
// Every field is a scaled integer on purpose: the frame stays short, the units
// are unambiguous on the ground, and nothing here depends on whether the
// toolchain's printf was built with float support. There is no checksum in the
// frame itself - initGroundStation() turns the modem's payload CRC on, so the
// bad packets never reach the ground station's parser, and a second check would
// only cost air time.
//
// The 1e7 position scaling is the conventional one, but do not read metres into
// the last two digits: FlightData carries position as a 32-bit float, which is
// worth about a metre at these magnitudes and no more.
//
// Values are clamped to the ranges above. A NaN reads as the field's lower
// limit, which is throttle-off for the motors and an obviously impossible
// -1000.00 m for altitude - wrong in a way the ground station will notice.

#include "telemetry.h"

#include "flight_computer.h"
#include "ground_station.h"

#include <Arduino.h>
#include <math.h>
#include <stdio.h>

// The control loop runs at 50 Hz; the radio cannot. LoRa.begin() never writes
// RegModemConfig1/2, so the SX1276 reset values stand: SF7, 125 kHz, CR 4/5,
// 8-symbol preamble. The Semtech time-on-air formula puts the 106-byte
// worst-case frame above at about 180 ms, and the radio is half duplex, so that
// is 180 ms per frame with the uplink not being listened to.
//
// What sets this interval is that deaf window landing on the heartbeat, not the
// duty cycle. The ground station is asked for a PING every 500 ms and the
// failsafe disarms after 2000 ms of silence, so the question is whether the
// aircraft can talk over its own heartbeat for two seconds together. Once a
// second it cannot: two pings arrive per second, 500 ms apart, and the window
// is 200 ms wide, so one of the pair always lands outside it. Twice a second it
// can - both periods would be 500 ms, two free-running crystals drift past each
// other over hours rather than seconds, and a ground station whose phase
// happened to sit inside the window would watch the aircraft disarm itself on a
// link that never actually failed.
//
// The air time is calculated, not measured. Nobody here has had this radio on a
// bench.
static const unsigned long TELEMETRY_INTERVAL_MS = 1000;

// Worst case above is a little over a hundred characters; this leaves room.
static const int FRAME_MAX_LEN = 160;

static unsigned long lastSendMs = 0;

// Scales a float into the fixed-point integer the frame carries. The clamp is
// what keeps a runaway control output or a NaN from stretching the frame past
// the buffer. The arithmetic is done in double so the 1e7 position scaling does
// not add rounding of its own on top of the float it started from.
static long fixedPoint(float value, double scale, long low, long high) {
    if (!isfinite(value)) {
        return low;
    }
    double scaled = (double)value * scale;
    if (scaled < (double)low) {
        return low;
    }
    if (scaled > (double)high) {
        return high;
    }
    return lround(scaled);
}

void initTelemetry() {
    // The radio itself is brought up by initGroundStation(). Holding the timer
    // here just staggers the first frame by one interval so it does not land in
    // the middle of the boot chatter.
    lastSendMs = millis();
}

void sendTelemetry(const FlightData &data) {
    unsigned long now = millis();
    if (now - lastSendMs < TELEMETRY_INTERVAL_MS) {
        return;
    }
    lastSendMs = now;

    char frame[FRAME_MAX_LEN];
    snprintf(frame, sizeof(frame),
             "$V,%lu,%d,%d,%ld,%ld,%ld,%ld,%ld,%ld,%ld,%ld,%ld,%ld,%ld,%ld,%ld,%ld",
             now,
             (int)data.mode,
             data.armed ? 1 : 0,
             fixedPoint(data.altitude, 100.0, -100000L, 100000L),
             fixedPoint(data.verticalSpeed, 100.0, -10000L, 10000L),
             fixedPoint(data.heading, 10.0, 0L, 3599L),
             fixedPoint(data.speed, 100.0, 0L, 20000L),
             fixedPoint(data.latitude, 1e7, -900000000L, 900000000L),
             fixedPoint(data.longitude, 1e7, -1800000000L, 1800000000L),
             fixedPoint(data.motorPower[0], 1000.0, 0L, 1000L),
             fixedPoint(data.motorPower[1], 1000.0, 0L, 1000L),
             fixedPoint(data.motorPower[2], 1000.0, 0L, 1000L),
             fixedPoint(data.motorPower[3], 1000.0, 0L, 1000L),
             fixedPoint(data.servoAngles[0], 10.0, 0L, 1800L),
             fixedPoint(data.servoAngles[1], 10.0, 0L, 1800L),
             fixedPoint(data.servoAngles[2], 10.0, 0L, 1800L),
             fixedPoint(data.servoAngles[3], 10.0, 0L, 1800L));

    groundStationSend(frame);
}