roro9stack/src/services/gnss_service.cpp

#include "services/gnss_service.h"

#include <Arduino.h>
#include <SD.h>

#include <algorithm>
#include <cstdio>

#include "platform/console.h"

namespace roro {

namespace {
const char* fixName(gnss::FixType f) {
    return f == gnss::FixType::ThreeD ? "3D" : f == gnss::FixType::TwoD ? "2D" : "none";
}
}  // namespace

GnssService::GnssService(const Settings& settings, ClockService& clock, StorageService& storage, EventBus& bus)
    : settings_(settings), clock_(clock), storage_(storage), bus_(bus) {
    parser_.onLine = [this](const std::string& line) {
        lastLineMs_ = millis();
        if (echo_) console.printf("nmea: %s\n", line.c_str());
    };
}

void GnssService::start() {
    open(millis());
    storage_.runJob([this]() { closeUnfinishedTracks(); });
}

void GnssService::stop() { close(); }

void GnssService::open(uint32_t nowMs) {
    Serial1.setRxBufferSize(512);  // before begin(): over a second of NMEA, read every 50 ms
    Serial1.begin(kBaud, SERIAL_8N1, kRxPin, kTxPin);
    open_ = true;
    active_ = false;  // tick() wakes it or puts it to sleep, per the setting
    standbyMs_ = 0;
}

void GnssService::wake(uint32_t nowMs) {
    send("PCAS10,0");  // hot start: ends a standby, keeps what the receiver knows
    active_ = true;
    openedMs_ = nowMs;
    firstFixMs_ = 0;
    lastLineMs_ = 0;
    lastFix_ = gnss::FixType::None;
    console.println("gnss: on");
}

void GnssService::standby(uint32_t nowMs) {
    if (tracking()) stopTrack();
    send("PCAS12,65535");
    if (active_) console.println("gnss: off (receiver in standby)");
    active_ = false;
    standbyMs_ = nowMs;
}

void GnssService::close() {
    if (!open_) return;
    Serial1.end();
    open_ = active_ = false;
    lastLineMs_ = 0;
}

void GnssService::tick(uint32_t nowMs) {
    if (!open_) open(nowMs);
    bool wanted = settings_.getBool(Setting::GnssEnabled) && !held_;
    if (wanted && !active_) wake(nowMs);
    if (!wanted && (active_ || !standbyMs_ || nowMs - standbyMs_ >= kStandbyRenewMs)) standby(nowMs);

    char buf[128];
    for (int avail; (avail = Serial1.available()) > 0;) {
        size_t n = Serial1.read(reinterpret_cast<uint8_t*>(buf), std::min<size_t>(avail, sizeof buf));
        if (n == 0) break;
        bytes_ += n;
        if (active_) parser_.feed(buf, n);  // in standby: the last lines before it, discarded
    }

    if (!active_) return;
    const gnss::GnssState& s = parser_.state();
    if (s.fix != lastFix_) {
        if (s.fix != gnss::FixType::None && !firstFixMs_) {
            firstFixMs_ = nowMs - openedMs_;
            console.printf("gnss: first Fix after %lu s\n", (unsigned long)(firstFixMs_ / 1000));
        }
        console.printf("gnss: Fix %s, %d satellites used\n", fixName(s.fix), s.satellitesUsed);
        lastFix_ = s.fix;
    }
    recordPoint(nowMs);
    // GNSS time is the most trusted source (ClockModel): set it with a Fix, then now and then.
    if (s.timeValid && s.fix != gnss::FixType::None && (!clockSetMs_ || nowMs - clockSetMs_ >= kClockEveryMs)) {
        if (clock_.set(s.utcSeconds(), TimeSource::Gnss) && !clockSetMs_) console.println("gnss: clock set");
        clockSetMs_ = nowMs;
    }
}

void GnssService::notify(const std::string& text) {
    bus_.publish(Event::withText(EventType::Notification, text.c_str(), static_cast<int32_t>(NotificationLevel::Info)));
}

std::string GnssService::startTrack(uint32_t nowMs) {
    if (tracking()) return "";
    if (!active_) return "GNSS is off";
    if (!storage_.state().present) return "No SD card";
    int64_t now = clock_.utcNow();
    if (now < 0) return "Waiting for the time (a Fix or Wi-Fi)";
    trackPath_ = gnss::trackPath(now);
    std::string name = trackPath_.substr(trackPath_.rfind('/') + 1);
    name = name.substr(0, name.size() - 4);  // without ".gpx"
    storage_.appendLine(trackPath_, gnss::gpx::header(name), true);
    trackPoints_ = 0;
    trackStartMs_ = nowMs;
    trackRule_.reset();
    console.printf("gnss: Track %s started\n", trackPath_.c_str());
    notify("Track started");
    return "";
}

void GnssService::stopTrack() {
    if (!tracking()) return;
    storage_.appendLine(trackPath_, gnss::gpx::footer(), true);
    console.printf("gnss: Track %s saved, %d points\n", trackPath_.c_str(), trackPoints_);
    notify("Track saved: " + std::to_string(trackPoints_) + (trackPoints_ == 1 ? " point" : " points"));
    trackPath_.clear();
}

void GnssService::recordPoint(uint32_t nowMs) {
    const gnss::GnssState& s = parser_.state();
    if (!tracking() || !s.positionValid || !s.timeValid) return;
    if (!trackRule_.due(nowMs, s.latitude, s.longitude)) return;
    storage_.appendLine(trackPath_,
                        gnss::gpx::point(s.latitude, s.longitude, s.altitudeValid, s.altitudeM, s.utcSeconds()), true);
    trackPoints_++;
}

// A Track cut short by a power loss or a crash has no footer: GPX readers refuse it. Close it.
void GnssService::closeUnfinishedTracks() {
    File dir = SD.open("/gnss/tracks");
    if (!dir || !dir.isDirectory()) return;
    int closed = 0;
    for (File f = dir.openNextFile(); f; f = dir.openNextFile()) {
        std::string name = f.name();
        if (f.isDirectory() || name.size() < 4 || name.compare(name.size() - 4, 4, ".gpx") != 0) continue;
        char tail[32] = {};
        size_t size = f.size();
        f.seek(size > sizeof tail - 1 ? size - (sizeof tail - 1) : 0);
        f.read(reinterpret_cast<uint8_t*>(tail), sizeof tail - 1);
        f.close();
        if (gnss::gpx::finished(tail)) continue;
        File out = SD.open(("/gnss/tracks/" + name).c_str(), FILE_APPEND);
        if (out) {
            out.print(("\n" + gnss::gpx::footer() + "\n").c_str());
            out.close();
            closed++;
        }
    }
    if (closed) console.printf("gnss: closed %d unfinished Track(s)\n", closed);
}

bool GnssService::send(const std::string& body) {
    if (!open_) return false;
    uint8_t sum = 0;
    for (char c : body) sum ^= static_cast<uint8_t>(c);
    char tail[8];
    std::snprintf(tail, sizeof tail, "*%02X\r\n", sum);
    std::string line = "$" + body + tail;
    Serial1.write(reinterpret_cast<const uint8_t*>(line.data()), line.size());
    return true;
}

void GnssService::printStatus(Print& out, uint32_t nowMs) const {
    if (!active_) return (void)out.println("gnss: off (receiver in standby)");
    const gnss::GnssState& s = parser_.state();
    out.printf("gnss: %s, Fix %s, %d used, %u in view, HDOP %.1f\n",
               receiving(nowMs) ? "receiving" : "silent", fixName(s.fix), s.satellitesUsed,
               (unsigned)s.satellites.size(), s.hdop);
    if (s.positionValid)
        out.printf("gnss: %.6f %.6f, %s%.0f m, %.1f km/h\n", s.latitude, s.longitude,
                   s.altitudeValid ? "" : "altitude unknown ", s.altitudeM, s.speedKmh);
    if (s.timeValid)
        out.printf("gnss: %04d-%02d-%02d %02d:%02d:%02d UTC\n", s.year, s.month, s.day, s.hour, s.minute, s.second);
    int perSystem[8] = {}, usedPerSystem[8] = {};
    for (auto& sat : s.satellites) {
        perSystem[static_cast<int>(sat.system)]++;
        if (sat.used) usedPerSystem[static_cast<int>(sat.system)]++;
    }
    out.print("gnss: in view (used)");
    for (int c = 1; c < 7; c++)
        if (perSystem[c])
            out.printf(" %s %d (%d)", gnss::constellationName(static_cast<gnss::Constellation>(c)), perSystem[c],
                       usedPerSystem[c]);
    out.println();
    if (tracking())
        out.printf("gnss: recording %s, %d points, %lu s\n", trackPath_.c_str(), trackPoints_,
                   (unsigned long)((nowMs - trackStartMs_) / 1000));
    out.printf("gnss: up %lu s, first Fix %s, %lu bytes, %lu sentences, %lu bad\n", (unsigned long)((nowMs - openedMs_) / 1000),
               firstFixMs_ ? (std::to_string(firstFixMs_ / 1000) + " s").c_str() : "not yet", (unsigned long)bytes_,
               (unsigned long)parser_.goodSentences(), (unsigned long)parser_.badSentences());
}

}  // namespace roro