sensors.cpp

#include "sensors.h"

#include "barometer.h"
#include "gps.h"
#include "magnetometer.h"

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

// Barometric altitude, standard atmosphere:
//
//     h = 44330 * (1 - (p / p0) ^ (1 / 5.255))
//
// p0 is the pressure averaged on the pad during initSensors(), so h is height
// above the launch point and reads about zero before takeoff. The constants are
// the ISA ones - 288.15 K at sea level, 6.5 K/km lapse rate. We do not
// temperature-compensate and we never re-baseline in flight, so the reading
// drifts with the weather: a front moving through is worth several metres over
// an hour. Good enough to hold a hover, not a survey instrument.
static const float BARO_ALT_SCALE_M = 44330.0f;
static const float BARO_ALT_EXPONENT = 0.1902949f;   // 1 / 5.255

// Barometric altitude is noisy at the tens-of-centimetres level, and
// differentiating that at 50 Hz produces tens of m/s of nonsense. A first-order
// low pass with this time constant costs roughly half a second of lag on the
// climb-rate signal, which the altitude PD loop can live with.
static const float VSPEED_FILTER_TAU_S = 0.5f;

// Reads averaged at init to set the ground reference. The DPS310 is quiet
// enough that a handful is plenty; this costs about a third of a second of
// boot time.
static const int REF_PRESSURE_SAMPLES = 16;
static const unsigned long REF_PRESSURE_SETTLE_MS = 20;

// barometer.cpp configures the DPS310 to convert at 32 Hz and it is polled at
// 50, so a cycle that finds no new sample is ordinary and means nothing. Half a
// second of them is not ordinary - the part has missed sixteen conversions by
// then and is not coming back. That is the point at which holding a frozen
// altitude while still telling the flight computer the sensor is fine becomes
// the worse of the two failures.
static const int BARO_FAIL_LIMIT = 25;   // consecutive failed reads: 0.5 s at 50 Hz

// The TLV493D is configured for master-controlled mode, so it converts on demand
// and every poll should answer. There is no equivalent of the barometer's
// "nothing new this cycle", and the limit is here only so that one dropped I2C
// transaction does not condemn a live part.
static const int MAG_FAIL_LIMIT = 10;    // consecutive failed reads: 0.2 s at 50 Hz

static bool baroUp = false;
static bool magUp = false;
static float refPressure_hPa = 0.0f;

static float altitude_m = 0.0f;
static float verticalSpeed_mps = 0.0f;
static float heading_deg = 0.0f;
static float groundSpeed_mps = 0.0f;
static float latitude_deg = 0.0f;
static float longitude_deg = 0.0f;
static bool gpsFix = false;

static float lastAltitude_m = 0.0f;
static unsigned long lastAltitudeMs = 0;
static bool altitudePrimed = false;
static int baroFailStreak = 0;
static int magFailStreak = 0;

void initSensors() {
    // Clear the cache first. Nothing here should ever survive a re-init and be
    // mistaken for a fresh reading.
    altitude_m = 0.0f;
    verticalSpeed_mps = 0.0f;
    heading_deg = 0.0f;
    groundSpeed_mps = 0.0f;
    latitude_deg = 0.0f;
    longitude_deg = 0.0f;
    gpsFix = false;
    altitudePrimed = false;
    baroFailStreak = 0;
    magFailStreak = 0;

    initGPS();
    magUp = initMagnetometer();
    baroUp = initBarometer();

    if (baroUp) {
        float sum = 0.0f;
        int samples = 0;
        for (int i = 0; i < REF_PRESSURE_SAMPLES; i++) {
            float pressure_hPa;
            if (readBarometer(&pressure_hPa)) {
                sum += pressure_hPa;
                samples++;
            }
            delay(REF_PRESSURE_SETTLE_MS);
        }

        if (samples > 0) {
            refPressure_hPa = sum / (float)samples;
        } else {
            // The chip answered at init but will not give us a sample. Report
            // it unhealthy rather than flying on an invented reference.
            baroUp = false;
        }
    }

    if (!baroUp) {
        Serial.println("Barometer down - no altitude, the aircraft will not arm");
    }
    if (!magUp) {
        Serial.println("Magnetometer down - heading unavailable");
    }
}

void updateSensors() {
    updateGPS();

    gpsFix = gpsHasFix();
    if (gpsFix) {
        latitude_deg = gpsLatitude();
        longitude_deg = gpsLongitude();
        groundSpeed_mps = gpsGroundSpeed();
    } else {
        // Position and speed are treated differently on a fix loss, on purpose.
        // The last known position is kept rather than snapped to 0,0: stale
        // coordinates are more use to the ground station than the Gulf of
        // Guinea, and hasGpsFix() tells the caller not to trust them. Speed is
        // a rate, and a rate held over from a fix the aircraft lost a minute
        // ago is not stale data, it is wrong data - the transition-to-cruise
        // gate in flight_computer.cpp reads this number to decide whether the
        // wing is flying, so it goes to zero and the gate fails closed.
        groundSpeed_mps = 0.0f;
    }

    if (magUp) {
        float measured_deg;
        if (readHeading(&measured_deg)) {
            heading_deg = measured_deg;
            magFailStreak = 0;
        } else if (magFailStreak < MAG_FAIL_LIMIT) {
            // Same treatment as the barometer below. The cruise loop reads
            // getHeading() every cycle, and one that has stopped updating gives
            // it a constant error, so it would hold a yaw differential into a
            // turn that never converges. There is no substitute heading source
            // on this airframe, so headingValid() goes false here and the loop
            // stops steering rather than steering on a frozen number.
            magFailStreak++;
            if (magFailStreak == MAG_FAIL_LIMIT) {
                magUp = false;
                Serial.println("Magnetometer stopped answering - heading is frozen at its last reading");
            }
        }
    }

    if (!baroUp) {
        return;
    }

    float pressure_hPa;
    if (!readBarometer(&pressure_hPa)) {
        // Hold the previous altitude for this cycle. Keep holding it and the
        // reading is no longer a measurement, so sensorsHealthy() has to stop
        // claiming otherwise - that flag is what the arming gate is built on.
        // There is no way back from here on purpose: a barometer that went
        // quiet for half a second in flight has not earned the benefit of the
        // doubt by starting to answer again.
        if (baroFailStreak < BARO_FAIL_LIMIT) {
            baroFailStreak++;
            if (baroFailStreak == BARO_FAIL_LIMIT) {
                baroUp = false;
                Serial.println("Barometer stopped answering - altitude will not update again");
            }
        }
        return;
    }
    baroFailStreak = 0;

    altitude_m = BARO_ALT_SCALE_M *
                 (1.0f - powf(pressure_hPa / refPressure_hPa, BARO_ALT_EXPONENT));

    unsigned long now = millis();
    if (!altitudePrimed) {
        lastAltitude_m = altitude_m;
        lastAltitudeMs = now;
        verticalSpeed_mps = 0.0f;
        altitudePrimed = true;
        return;
    }

    // Unsigned arithmetic, so this stays correct across the millis() rollover.
    // The interval is measured between successful barometer reads rather than
    // between cycles: a dropped read must not shorten the divisor and spike the
    // climb rate.
    unsigned long elapsedMs = now - lastAltitudeMs;
    if (elapsedMs == 0) {
        return;
    }
    lastAltitudeMs = now;

    float dt = (float)elapsedMs * 0.001f;
    float rawRate = (altitude_m - lastAltitude_m) / dt;
    lastAltitude_m = altitude_m;

    float alpha = dt / (VSPEED_FILTER_TAU_S + dt);
    verticalSpeed_mps += alpha * (rawRate - verticalSpeed_mps);
}

float getAltitude() {
    return altitude_m;
}

float getVerticalSpeed() {
    return verticalSpeed_mps;
}

float getHeading() {
    return heading_deg;
}

bool headingValid() {
    return magUp;
}

float getGroundSpeed() {
    return groundSpeed_mps;
}

float getLatitude() {
    return latitude_deg;
}

float getLongitude() {
    return longitude_deg;
}

bool sensorsHealthy() {
    // A dead magnetometer costs heading hold but the aircraft can still hover,
    // so it is reported through headingValid() and not here - folding it in
    // would refuse to arm an aircraft that has only lost its compass. A dead
    // barometer means no altitude at all, which is disqualifying.
    return baroUp;
}

bool hasGpsFix() {
    return gpsFix;
}