NMEASatellites.cc

Go to the documentation of this file.

#include "NMEASatellites.h"
 
#include <algorithm>
#include <iterator>
#include <utility>
 
#include "NMEASentence.h"
 
namespace {
constexpr size_t GSV_PAGE_COUNT_FIELD = 1;
constexpr size_t GSV_PAGE_NUMBER_FIELD = 2;
constexpr size_t GSV_SATELLITE_COUNT_FIELD = 3;
constexpr size_t GSV_FIRST_SATELLITE_FIELD = 4;
constexpr size_t GSV_SATELLITE_FIELDS = 4;
constexpr int GSV_SATELLITES_PER_PAGE = 4;
constexpr int MAX_GSV_PAGES = 64;
constexpr int MAX_GSV_SATELLITES = MAX_GSV_PAGES * GSV_SATELLITES_PER_PAGE;
constexpr int MAX_GSV_SIGNAL_ID = 15;
constexpr int MAX_GSV_SATELLITE_ID = 999;
 
bool validNavigation(const NMEA::Sentence& input)
{
    const auto status = NMEA::navigationStatus(input);
    if (!status) {
        return false;
    }
    if (input.type() == "GGA") {
        return NMEA::gga(input).has_value() || (input.count >= NMEA::Field::GGA_MIN_FIELDS && !status->valid);
    }
    if (input.type() != "RMC" || input.count < NMEA::Field::RMC_MIN_FIELDS ||
        !NMEA::rmcDate(input.fields[NMEA::Field::RMC_DATE])) {
        return false;
    }
    return !status->valid || NMEA::rmc(input).has_value();
}
}  // namespace
 
namespace NMEA {
GPSConstellation satelliteConstellation(std::string_view talker, std::optional<int> systemId,
                                        std::optional<int> satelliteId)
{
    using Constellation = GPSConstellation;
    namespace Id = SatelliteIds;
    constexpr int GPS_SYSTEM_ID = 1;
    constexpr int GLONASS_SYSTEM_ID = 2;
    constexpr int GALILEO_SYSTEM_ID = 3;
    constexpr int BEIDOU_SYSTEM_ID = 4;
    constexpr int QZSS_SYSTEM_ID = 5;
    constexpr int NAVIC_SYSTEM_ID = 6;
    constexpr int SBAS_LAST_PRN = 158;
    constexpr int QZSS_LEGACY_LAST_NMEA_ID = 200;
    constexpr int BEIDOU_UNAMBIGUOUS_FIRST_NMEA_ID = Id::QZSS_LAST_NMEA_ID + 1;
 
    if ((talker == "GP" || (talker == "GN" && (!systemId || *systemId == GPS_SYSTEM_ID))) && satelliteId &&
        ((*satelliteId >= Id::SBAS_FIRST_NMEA_ID && *satelliteId <= Id::SBAS_LAST_NMEA_ID) ||
         (*satelliteId >= Id::SBAS_FIRST_PRN && *satelliteId <= SBAS_LAST_PRN))) {
        return Constellation::SBAS;
    }
    if (talker == "GP") {
        return Constellation::GPS;
    }
    if (talker == "GL") {
        return Constellation::GLONASS;
    }
    if (talker == "GA") {
        return Constellation::Galileo;
    }
    if (talker == "GB" || talker == "BD") {
        return Constellation::BeiDou;
    }
    if (talker == "GQ" || talker == "PQ" || talker == "QZ") {
        return Constellation::QZSS;
    }
    if (talker == "GI") {
        return Constellation::NavIC;
    }
    if (talker != "GN") {
        return Constellation::Unknown;
    }
    if (systemId) {
        switch (*systemId) {
            case GPS_SYSTEM_ID:
                return Constellation::GPS;
            case GLONASS_SYSTEM_ID:
                return Constellation::GLONASS;
            case GALILEO_SYSTEM_ID:
                return Constellation::Galileo;
            case BEIDOU_SYSTEM_ID:
                return Constellation::BeiDou;
            case QZSS_SYSTEM_ID:
                return Constellation::QZSS;
            case NAVIC_SYSTEM_ID:
                return Constellation::NavIC;
            default:
                return Constellation::Unknown;
        }
    }
    if (!satelliteId) {
        return Constellation::Unknown;
    }
    const int id = *satelliteId;
    // SBAS identifiers were classified above, so the remaining low range is GPS.
    if (id >= Id::FIRST_LOCAL_ID && id < Id::SBAS_FIRST_NMEA_ID) {
        return Constellation::GPS;
    }
    if (id >= Id::GLONASS_FIRST_NMEA_ID && id <= Id::GLONASS_LAST_NMEA_ID) {
        return Constellation::GLONASS;
    }
    if (id >= Id::QZSS_FIRST_NMEA_ID && id <= QZSS_LEGACY_LAST_NMEA_ID) {
        return Constellation::QZSS;
    }
    // Qt's legacy BeiDou range starts at 201; u-blox extended QZSS ends at 202.
    // Without explicit context, neither interpretation is safe for 201/202.
    if ((id >= BEIDOU_UNAMBIGUOUS_FIRST_NMEA_ID && id <= Id::BEIDOU_LEGACY_LAST_NMEA_ID) ||
        (id >= Id::BEIDOU_EXTENDED_FIRST_NMEA_ID && id <= Id::BEIDOU_EXTENDED_LAST_NMEA_ID)) {
        return Constellation::BeiDou;
    }
    if (id >= Id::GALILEO_FIRST_NMEA_ID && id <= Id::GALILEO_LAST_NMEA_ID) {
        return Constellation::Galileo;
    }
    return Constellation::Unknown;
}
 
std::optional<GSV> gsv(const Sentence& input)
{
    if (input.type() != "GSV" || input.count < GSV_FIRST_SATELLITE_FIELD) {
        return {};
    }
    const auto& f = input.fields;
    const auto total = number<int>(f[GSV_PAGE_COUNT_FIELD]);
    const auto message = number<int>(f[GSV_PAGE_NUMBER_FIELD]);
    const auto count = number<int>(f[GSV_SATELLITE_COUNT_FIELD]);
    const auto system = satelliteConstellation(input.talker(), {}, {});
    if (!total || !message || !count || (system == GPSConstellation::Unknown && input.talker() != "GN") || *total < 1 ||
        *total > MAX_GSV_PAGES || *message < 1 || *message > *total || *count < 0 || *count > MAX_GSV_SATELLITES ||
        *total != std::max(1, (*count + GSV_SATELLITES_PER_PAGE - 1) / GSV_SATELLITES_PER_PAGE)) {
        return {};
    }
    const int entries = std::min(GSV_SATELLITES_PER_PAGE, *count - (*message - 1) * GSV_SATELLITES_PER_PAGE);
    const size_t end = GSV_FIRST_SATELLITE_FIELD + entries * GSV_SATELLITE_FIELDS;
    const size_t fields = input.count - GSV_FIRST_SATELLITE_FIELD;
    const size_t satelliteSlots = fields / GSV_SATELLITE_FIELDS;
    const size_t trailingFields = fields % GSV_SATELLITE_FIELDS;
    if (satelliteSlots < static_cast<size_t>(entries) || satelliteSlots > GSV_SATELLITES_PER_PAGE ||
        trailingFields > 1) {
        return {};
    }
    const size_t signalIndex = GSV_FIRST_SATELLITE_FIELD + satelliteSlots * GSV_SATELLITE_FIELDS;
    // Some receivers retain all four satellite slots on short or empty pages.
    for (size_t index = end; index < signalIndex; ++index) {
        if (!f[index].empty()) {
            return {};
        }
    }
    GSV result{system, *total, *message, *count};
    if (trailingFields == 1 && !f[signalIndex].empty()) {
        const auto signal = number<int>(f[signalIndex], HEX_BASE);
        if (!signal || *signal < 0 || *signal > MAX_GSV_SIGNAL_ID) {
            return {};
        }
        result.signal = *signal;
    }
    for (size_t index = GSV_FIRST_SATELLITE_FIELD; index < end; index += GSV_SATELLITE_FIELDS) {
        const auto id = number<int>(f[index]);
        if (!id || *id < 1 || *id > MAX_GSV_SATELLITE_ID) {
            return {};
        }
        SatelliteData satellite;
        satellite.constellation = satelliteConstellation(input.talker(), {}, *id);
        if (satellite.constellation == GPSConstellation::Unknown) {
            return {};
        }
        satellite.id = satelliteId(satellite.constellation, *id);
        result.satellites.push_back(satellite);
    }
    return result;
}
 
void SatelliteAssembler::clear()
{
    _views.clear();
    _used.clear();
    _time.reset();
    _batchStartedUs.reset();
    _lastAcceptedUs = 0;
}
 
SatelliteAssembler::Update SatelliteAssembler::ingest(const Sentence& input, uint64_t receivedAtUs)
{
    Update update{false, flushDue(receivedAtUs)};
    const auto complete = [&]() {
        auto epoch = flush();
        update.completed.insert(update.completed.end(), std::make_move_iterator(epoch.begin()),
                                std::make_move_iterator(epoch.end()));
    };
    if (validNavigation(input)) {
        const auto time = utcMilliseconds(input.fields[Field::UTC_TIME]);
        if (time && time != _time) {
            complete();
            _time = time;
        }
        return update;
    }
    if (input.type() == "GSV") {
        const auto parsed = gsv(input);
        if (!parsed) {
            return update;
        }
        const auto& page = *parsed;
        auto& signalReports = _views[page.constellation];
        if (page.message == 1 && signalReports[page.signal].complete()) {
            complete();
        }
        auto& report = _views[page.constellation][page.signal];
        if (page.message == 1) {
            report = {page.messages, page.satelliteCount, 1, receivedAtUs, {}};
        }
        if (report.messages != page.messages || report.count != page.satelliteCount || report.next != page.message) {
            _views[page.constellation].erase(page.signal);
            return update;
        }
        report.satellites.insert(report.satellites.end(), page.satellites.begin(), page.satellites.end());
        report.timestamp = std::min(report.timestamp, receivedAtUs);
        ++report.next;
        update.accepted = true;
    } else if (input.type() == "GSA" && input.count >= Field::GSA_MIN_FIELDS) {
        const auto& f = input.fields;
        const auto fix = number<unsigned>(f[Field::GSA_DIMENSION]);
        if (!fix || *fix < FixDimension::NO_FIX || *fix > FixDimension::THREE_D) {
            return update;
        }
        std::optional<int> system;
        if (input.talker() == "GN" && input.count > Field::GSA_SYSTEM_ID && !f[Field::GSA_SYSTEM_ID].empty()) {
            system = number<int>(f[Field::GSA_SYSTEM_ID], HEX_BASE);
            if (!system) {
                return update;
            }
        }
        std::map<GPSConstellation, Used> reports;
        const auto explicitSystem = satelliteConstellation(input.talker(), system, {});
        if (explicitSystem != GPSConstellation::Unknown) {
            reports[explicitSystem].timestamp = receivedAtUs;
        }
        if (explicitSystem == GPSConstellation::GPS) {
            reports[GPSConstellation::SBAS].timestamp = receivedAtUs;
        }
        for (size_t index = Field::GSA_FIRST_SATELLITE; index < Field::GSA_FIRST_SATELLITE + Field::GSA_SATELLITE_SLOTS;
             ++index) {
            if (f[index].empty()) {
                continue;
            }
            const auto id = number<int>(f[index]);
            if (!id || *id <= 0) {
                return update;
            }
            const auto constellation = satelliteConstellation(input.talker(), system, id);
            if (constellation == GPSConstellation::Unknown) {
                return update;
            }
            auto& report = reports[constellation];
            report.timestamp = receivedAtUs;
            if (*fix != FixDimension::NO_FIX) {
                report.ids.insert(satelliteId(constellation, *id));
            }
        }
        if (reports.empty()) {
            return update;
        }
        if (!_views.empty() && std::any_of(reports.begin(), reports.end(),
                                           [this](const auto& report) { return _used.contains(report.first); })) {
            complete();
        }
        for (auto& [constellation, report] : reports) {
            _used[constellation] = std::move(report);
        }
        update.accepted = true;
    }
    if (update.accepted) {
        if (!_batchStartedUs) {
            _batchStartedUs = receivedAtUs;
        }
        _lastAcceptedUs = receivedAtUs;
    }
    return update;
}
 
std::optional<uint64_t> SatelliteAssembler::deadlineUs() const
{
    if (!_batchStartedUs) {
        return std::nullopt;
    }
    return std::min(*_batchStartedUs + static_cast<uint64_t>(BATCH_TIMEOUT.count()),
                    _lastAcceptedUs + static_cast<uint64_t>(IDLE_TIMEOUT.count()));
}
 
SatelliteEpoch SatelliteAssembler::flushDue(uint64_t nowUs)
{
    const auto deadline = deadlineUs();
    return deadline && nowUs >= *deadline ? flush() : SatelliteEpoch{};
}
 
SatelliteEpoch SatelliteAssembler::flush()
{
    _batchStartedUs.reset();
    std::map<GPSConstellation, SatelliteSystem> systems;
    std::map<GPSConstellation, std::set<int>> satellites;
    const auto includeView = [&systems](GPSConstellation constellation, uint64_t timestamp) {
        auto& out = systems[constellation];
        out.constellation = constellation;
        out.inViewTimestampUs = out.inViewTimestampUs ? std::min(out.inViewTimestampUs, timestamp) : timestamp;
    };
    for (const auto& [system, signalReports] : _views) {
        for (const auto& [signal, report] : signalReports) {
            if (!report.complete()) {
                continue;
            }
            // GN identifies coverage only through its entries, including across pages and signals.
            if (system != GPSConstellation::Unknown) {
                includeView(system, report.timestamp);
                if (system == GPSConstellation::GPS) {
                    includeView(GPSConstellation::SBAS, report.timestamp);
                }
            }
            for (const auto& satellite : report.satellites) {
                includeView(satellite.constellation, report.timestamp);
                satellites[satellite.constellation].insert(satellite.id);
            }
        }
    }
    for (auto& [system, view] : satellites) {
        systems[system].inView = static_cast<int>(view.size());
    }
    for (const auto& [system, used] : _used) {
        auto& out = systems[system];
        out.constellation = system;
        out.inUseTimestampUs = used.timestamp;
        out.inUse = static_cast<int>(used.ids.size());
    }
    SatelliteEpoch result;
    for (auto& [system, report] : systems) {
        result.push_back(std::move(report));
    }
    _views.clear();
    _used.clear();
    return result;
}
}  // namespace NMEA