twisla/roro9stack
Public Access
1
0
Fork 0
Code Issues 51 Pull Requests 1 Actions 2 Packages Releases 15 Activity

M2 step 2: NMEA parser (host-tested)

Browse Source 
lib/gnss: RMC, GGA, GSA and GSV into one GnssState: Fix type, position,
altitude, speed, course, HDOP, UTC time (trusted only with a Fix) and the
satellites in view across constellations. GSV sequences are kept per
constellation and signal band and merged per satellite with the stronger
SNR; GSA's system ID marks which satellites are used. 16 tests, using
lines captured from the device.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01EhqxQ49eCju4CzKYNjZzwT
This commit is contained in:
twisla
2026-10-04 22:05:01 +02:00
co-authored by Claude Opus 5.5
parent f812c62a3d
commit c9b1ecb772
3 changed files with 532 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/gnss/src/nmea_parser.cpp
+231
View File
@@ -0,0 +1,231 @@
#include "nmea_parser.h"
#include <cstdlib>
namespace roro::gnss {
namespace {
constexpr double kKmhPerKnot = 1.852;
int hexValue(char c) {
if (c >= '0' && c <= '9') return c - '0';
if (c >= 'A' && c <= 'F') return c - 'A' + 10;
if (c >= 'a' && c <= 'f') return c - 'a' + 10;
return -1;
}
Constellation fromTalker(const std::string& t) {
if (t == "GP") return Constellation::Gps;
if (t == "GL") return Constellation::Glonass;
if (t == "GA") return Constellation::Galileo;
if (t == "GB" || t == "BD") return Constellation::BeiDou;
if (t == "GQ" || t == "QZ") return Constellation::Qzss;
if (t == "GI") return Constellation::Navic;
return Constellation::Unknown; // GN: combined
}
// GSA's system ID field (NMEA 4.10).
Constellation fromSystemId(int id) {
switch (id) {
case 1: return Constellation::Gps;
case 2: return Constellation::Glonass;
case 3: return Constellation::Galileo;
case 4: return Constellation::BeiDou;
case 5: return Constellation::Qzss;
case 6: return Constellation::Navic;
default: return Constellation::Unknown;
}
}
bool number(const std::string& s, double& out) {
if (s.empty()) return false;
char* end;
out = std::strtod(s.c_str(), &end);
return *end == '\0';
}
int integer(const std::string& s, int fallback = 0) {
double v;
return number(s, v) ? static_cast<int>(v) : fallback;
}
// "ddmm.mmmm" / "dddmm.mmmm" with its hemisphere letter.
bool coordinate(const std::string& value, const std::string& hemisphere, double& out) {
double raw;
if (!number(value, raw) || hemisphere.empty()) return false;
int degrees = static_cast<int>(raw / 100);
out = degrees + (raw - degrees * 100) / 60.0;
if (hemisphere == "S" || hemisphere == "W") out = -out;
return true;
}
// Days since 1970-01-01 for a civil date (Howard Hinnant's algorithm).
int64_t daysFromCivil(int y, int m, int d) {
y -= m <= 2;
const int64_t era = (y >= 0 ? y : y - 399) / 400;
const unsigned yoe = static_cast<unsigned>(y - era * 400);
const unsigned doy = (153 * (m + (m > 2 ? -3 : 9)) + 2) / 5 + d - 1;
const unsigned doe = yoe * 365 + yoe / 4 - yoe / 100 + doy;
return era * 146097 + static_cast<int64_t>(doe) - 719468;
}
} // namespace
const char* constellationName(Constellation c) {
switch (c) {
case Constellation::Gps: return "GPS";
case Constellation::Glonass: return "GLONASS";
case Constellation::Galileo: return "Galileo";
case Constellation::BeiDou: return "BeiDou";
case Constellation::Qzss: return "QZSS";
case Constellation::Navic: return "NavIC";
default: return "?";
}
}
int64_t GnssState::utcSeconds() const {
return daysFromCivil(year, month, day) * 86400 + hour * 3600 + minute * 60 + second;
}
void NmeaParser::feed(const char* data, size_t len) {
for (size_t i = 0; i < len; i++) {
char c = data[i];
if (c == '\n') {
if (!overflow_ && !line_.empty()) sentence(line_);
line_.clear();
overflow_ = false;
} else if (c != '\r') {
if (line_.size() >= kMaxLine) overflow_ = true;
else line_ += c;
}
}
}
bool NmeaParser::sentence(const std::string& line) {
size_t star = line.rfind('*');
if (line.size() < 7 || line[0] != '$' || star == std::string::npos || star + 3 != line.size()) {
bad_++;
return false;
}
uint8_t sum = 0;
for (size_t i = 1; i < star; i++) sum ^= static_cast<uint8_t>(line[i]);
int hi = hexValue(line[star + 1]), lo = hexValue(line[star + 2]);
if (hi < 0 || lo < 0 || sum != (hi << 4 | lo)) {
bad_++;
return false;
}
good_++;
Fields f;
size_t start = 1;
for (size_t i = 1; i <= star; i++)
if (i == star || line[i] == ',') {
f.push_back(line.substr(start, i - start));
start = i + 1;
}
if (f[0].size() < 5) return true;
std::string talker = f[0].substr(0, 2), type = f[0].substr(f[0].size() - 3);
if (type == "RMC") rmc(f);
else if (type == "GGA") gga(f);
else if (type == "GSA") gsa(f, fromTalker(talker));
else if (type == "GSV") gsv(f, fromTalker(talker));
return true;
}
void NmeaParser::rmc(const Fields& f) {
if (f.size() < 10) return;
bool active = f[2] == "A";
double lat, lon;
state_.positionValid = active && coordinate(f[3], f[4], lat) && coordinate(f[5], f[6], lon);
if (state_.positionValid) {
state_.latitude = lat;
state_.longitude = lon;
}
double knots, course;
state_.speedKmh = active && number(f[7], knots) ? static_cast<float>(knots * kKmhPerKnot) : 0;
state_.courseValid = active && number(f[8], course);
if (state_.courseValid) state_.courseDeg = static_cast<float>(course);
const std::string &t = f[1], &d = f[9];
state_.timeValid = active && t.size() >= 6 && d.size() == 6;
if (state_.timeValid) {
state_.hour = integer(t.substr(0, 2));
state_.minute = integer(t.substr(2, 2));
state_.second = integer(t.substr(4, 2));
state_.day = integer(d.substr(0, 2));
state_.month = integer(d.substr(2, 2));
state_.year = 2000 + integer(d.substr(4, 2));
}
}
void NmeaParser::gga(const Fields& f) {
if (f.size() < 10) return;
int quality = integer(f[6]);
double lat, lon, hdop, alt;
state_.positionValid = quality > 0 && coordinate(f[2], f[3], lat) && coordinate(f[4], f[5], lon);
if (state_.positionValid) {
state_.latitude = lat;
state_.longitude = lon;
}
state_.satellitesUsed = integer(f[7]);
if (number(f[8], hdop)) state_.hdop = static_cast<float>(hdop);
state_.altitudeValid = quality > 0 && number(f[9], alt);
if (state_.altitudeValid) state_.altitudeM = static_cast<float>(alt);
}
void NmeaParser::gsa(const Fields& f, Constellation talker) {
if (f.size() < 18) return;
int mode = integer(f[2], 1);
state_.fix = mode == 3 ? FixType::ThreeD : mode == 2 ? FixType::TwoD : FixType::None;
Constellation system = f.size() > 18 ? fromSystemId(integer(f[18])) : talker;
if (system == Constellation::Unknown) return; // can't tell whose satellites these are
std::set<int>& used = used_[static_cast<int>(system)];
used.clear();
for (size_t i = 3; i <= 14; i++)
if (!f[i].empty()) used.insert(integer(f[i]));
rebuildSatellites();
}
void NmeaParser::gsv(const Fields& f, Constellation talker) {
if (f.size() < 4 || talker == Constellation::Unknown) return;
int total = integer(f[1]), number = integer(f[2]);
size_t extra = f.size() - 4;
int signal = extra % 4 == 1 ? integer(f.back()) : 0; // NMEA 4.10 signal ID, last
auto key = std::make_pair(static_cast<int>(talker), signal);
std::vector<Satellite>& pending = pending_[key];
if (number == 1) pending.clear();
for (size_t i = 4; i + 3 < f.size(); i += 4) {
Satellite s;
s.system = talker;
s.prn = integer(f[i]);
s.elevation = integer(f[i + 1]);
s.azimuth = integer(f[i + 2]);
s.snr = integer(f[i + 3], -1);
if (s.prn > 0) pending.push_back(s);
}
if (number == total) {
inView_[key] = pending;
pending.clear();
rebuildSatellites();
}
}
void NmeaParser::rebuildSatellites() {
std::vector<Satellite> merged;
for (auto& [key, list] : inView_)
for (const Satellite& s : list) {
Satellite* same = nullptr;
for (auto& m : merged)
if (m.system == s.system && m.prn == s.prn) same = &m;
if (!same) merged.push_back(s);
else if (s.snr > same->snr) same->snr = s.snr;
}
for (auto& s : merged) {
auto it = used_.find(static_cast<int>(s.system));
s.used = it != used_.end() && it->second.count(s.prn) > 0;
}
state_.satellites = std::move(merged);
}
} // namespace roro::gnss
lib/gnss/src/nmea_parser.h
+78
View File
@@ -0,0 +1,78 @@
#pragma once
#include <cstddef>
#include <cstdint>
#include <map>
#include <set>
#include <string>
#include <utility>
#include <vector>
namespace roro::gnss {
enum class Constellation : uint8_t { Unknown, Gps, Glonass, Galileo, BeiDou, Qzss, Navic };
const char* constellationName(Constellation c);
enum class FixType : uint8_t { None, TwoD, ThreeD };
struct Satellite {
Constellation system = Constellation::Unknown;
int prn = 0;
int elevation = 0; // degrees above the horizon
int azimuth = 0; // degrees from north, clockwise
int snr = -1; // dB-Hz; -1 when not tracked
bool used = false; // part of the current Fix
};
// Everything the receiver has said, as of the last sentence (see CONTEXT.md: Fix).
struct GnssState {
FixType fix = FixType::None;
bool positionValid = false;
double latitude = 0, longitude = 0; // degrees, south and west negative
bool altitudeValid = false;
float altitudeM = 0; // above mean sea level
float speedKmh = 0;
bool courseValid = false;
float courseDeg = 0;
int satellitesUsed = 0;
float hdop = 99.9f;
// UTC, trusted only with a Fix: the receiver reports a time without one, from its own clock.
bool timeValid = false;
int year = 0, month = 0, day = 0, hour = 0, minute = 0, second = 0;
std::vector<Satellite> satellites; // in view, all constellations, one entry per satellite
int64_t utcSeconds() const; // seconds since 1970-01-01 UTC; meaningful when timeValid
};
// NMEA 0183 (4.10 style, as the Cap's AT6668 sends it): RMC, GGA, GSA and GSV, with the
// constellation taken from the talker ID or GSA's system ID field. Host-tested; no hardware here.
class NmeaParser {
public:
// Raw bytes from the UART, in any chunks.
void feed(const char* data, size_t len);
// One sentence without its line end. False if malformed or its checksum is wrong.
bool sentence(const std::string& line);
const GnssState& state() const { return state_; }
uint32_t goodSentences() const { return good_; }
uint32_t badSentences() const { return bad_; }
private:
using Fields = std::vector<std::string>;
void rmc(const Fields& f);
void gga(const Fields& f);
void gsa(const Fields& f, Constellation talker);
void gsv(const Fields& f, Constellation talker);
void rebuildSatellites();
static constexpr size_t kMaxLine = 120;
std::string line_;
bool overflow_ = false;
uint32_t good_ = 0, bad_ = 0;
GnssState state_;
// GSV sequences per (constellation, signal): pending until their last message.
std::map<std::pair<int, int>, std::vector<Satellite>> pending_, inView_;
std::map<int, std::set<int>> used_; // per constellation, from GSA
};
} // namespace roro::gnss
test/test_nmea/test_nmea.cpp
+223
View File
@@ -0,0 +1,223 @@
#include <unity.h>
#include <cstdio>
#include <string>
#include "nmea_parser.h"
using namespace roro;
using namespace roro::gnss;
void setUp() {}
void tearDown() {}
// "$<body>*<checksum>": the checksum is computed here, so scenarios stay readable.
static std::string nmea(const std::string& body) {
uint8_t sum = 0;
for (char c : body) sum ^= static_cast<uint8_t>(c);
char tail[4];
std::snprintf(tail, sizeof tail, "*%02X", sum);
return "$" + body + tail;
}
static void feedLine(NmeaParser& p, const std::string& line) {
std::string withEnd = line + "\r\n";
p.feed(withEnd.data(), withEnd.size());
}
void test_checksum_is_verified() {
NmeaParser p;
TEST_ASSERT_TRUE(p.sentence("$GNVTG,,,,,,,,,N*2E")); // captured from the device
TEST_ASSERT_FALSE(p.sentence("$GNVTG,,,,,,,,,N*2F"));
TEST_ASSERT_FALSE(p.sentence("$GNVTG,,,,,,,,,N"));
TEST_ASSERT_FALSE(p.sentence("GNVTG,,,,,,,,,N*2E"));
TEST_ASSERT_EQUAL_UINT32(3, p.badSentences());
TEST_ASSERT_EQUAL_UINT32(1, p.goodSentences());
}
void test_no_fix_as_captured_indoors() {
NmeaParser p;
feedLine(p, "$GNRMC,200231.00,V,,,,,,,041026,,,N,V*1A");
feedLine(p, "$GNGGA,200231.00,,,,,0,00,25.5,,,,,,*48");
feedLine(p, "$GNGSA,A,1,,,,,,,,,,,,,25.5,25.5,25.5,1*01");
feedLine(p, "$GPGSV,1,1,01,25,41,108,17,1*58");
feedLine(p, "$GLGSV,1,1,00,1*78");
const GnssState& s = p.state();
TEST_ASSERT_TRUE(s.fix == FixType::None);
TEST_ASSERT_FALSE(s.positionValid);
TEST_ASSERT_FALSE(s.timeValid); // a time without a Fix isn't trusted
TEST_ASSERT_EQUAL(1, static_cast<int>(s.satellites.size()));
TEST_ASSERT_TRUE(s.satellites[0].system == Constellation::Gps);
TEST_ASSERT_EQUAL(25, s.satellites[0].prn);
TEST_ASSERT_EQUAL(41, s.satellites[0].elevation);
TEST_ASSERT_EQUAL(108, s.satellites[0].azimuth);
TEST_ASSERT_EQUAL(17, s.satellites[0].snr);
TEST_ASSERT_FALSE(s.satellites[0].used);
}
void test_rmc_with_a_fix_gives_position_time_speed_course() {
NmeaParser p;
feedLine(p, nmea("GNRMC,123519.00,A,5050.8136,N,00421.1532,E,10.0,84.4,041026,,,A,V"));
const GnssState& s = p.state();
TEST_ASSERT_TRUE(s.positionValid);
TEST_ASSERT_FLOAT_WITHIN(1e-6, 50.0 + 50.8136 / 60.0, s.latitude);
TEST_ASSERT_FLOAT_WITHIN(1e-6, 4.0 + 21.1532 / 60.0, s.longitude);
TEST_ASSERT_TRUE(s.timeValid);
TEST_ASSERT_EQUAL(2026, s.year);
TEST_ASSERT_EQUAL(10, s.month);
TEST_ASSERT_EQUAL(4, s.day);
TEST_ASSERT_EQUAL(12, s.hour);
TEST_ASSERT_EQUAL(35, s.minute);
TEST_ASSERT_EQUAL(19, s.second);
TEST_ASSERT_FLOAT_WITHIN(0.01, 18.52, s.speedKmh); // 10 knots
TEST_ASSERT_TRUE(s.courseValid);
TEST_ASSERT_FLOAT_WITHIN(0.01, 84.4, s.courseDeg);
}
void test_southern_and_western_hemispheres_are_negative() {
NmeaParser p;
feedLine(p, nmea("GNRMC,000000.00,A,3352.0000,S,15112.6000,W,0.0,,010126,,,A,V"));
TEST_ASSERT_FLOAT_WITHIN(1e-6, -(33.0 + 52.0 / 60.0), p.state().latitude);
TEST_ASSERT_FLOAT_WITHIN(1e-6, -(151.0 + 12.6 / 60.0), p.state().longitude);
TEST_ASSERT_FALSE(p.state().courseValid); // empty course field
}
void test_utc_seconds_from_the_fix_time() {
NmeaParser p;
feedLine(p, nmea("GNRMC,123519.00,A,5050.8136,N,00421.1532,E,0.0,,041026,,,A,V"));
TEST_ASSERT_EQUAL_INT64(1791117319LL, p.state().utcSeconds()); // 2026-10-04 12:35:19 UTC
}
void test_gga_gives_fix_quality_altitude_satellites_and_hdop() {
NmeaParser p;
feedLine(p, nmea("GNGGA,123519.00,5050.8136,N,00421.1532,E,1,09,0.9,61.4,M,46.9,M,,"));
feedLine(p, nmea("GNGSA,A,3,05,13,15,18,23,,,,,,,,1.6,0.9,1.3,1"));
const GnssState& s = p.state();
TEST_ASSERT_TRUE(s.positionValid);
TEST_ASSERT_EQUAL(9, s.satellitesUsed);
TEST_ASSERT_FLOAT_WITHIN(0.01, 0.9, s.hdop);
TEST_ASSERT_TRUE(s.altitudeValid);
TEST_ASSERT_FLOAT_WITHIN(0.01, 61.4, s.altitudeM);
TEST_ASSERT_TRUE(s.fix == FixType::ThreeD);
}
void test_fix_type_comes_from_gsa_mode() {
NmeaParser p;
feedLine(p, nmea("GNGSA,A,2,05,13,15,,,,,,,,,,2.1,1.9,1.0,1"));
TEST_ASSERT_TRUE(p.state().fix == FixType::TwoD);
feedLine(p, nmea("GNGSA,A,1,,,,,,,,,,,,,25.5,25.5,25.5,1"));
TEST_ASSERT_TRUE(p.state().fix == FixType::None);
}
void test_losing_the_fix_drops_the_position() {
NmeaParser p;
feedLine(p, nmea("GNRMC,123519.00,A,5050.8136,N,00421.1532,E,0.0,,041026,,,A,V"));
TEST_ASSERT_TRUE(p.state().positionValid);
feedLine(p, nmea("GNRMC,123520.00,V,,,,,,,041026,,,N,V"));
TEST_ASSERT_FALSE(p.state().positionValid);
TEST_ASSERT_FALSE(p.state().timeValid);
}
void test_gsv_sequence_spans_several_messages() {
NmeaParser p;
feedLine(p, nmea("GPGSV,2,1,05,05,60,120,40,13,45,200,35,15,30,300,30,18,20,050,25,1"));
TEST_ASSERT_EQUAL(0, static_cast<int>(p.state().satellites.size())); // committed at the last message
feedLine(p, nmea("GPGSV,2,2,05,23,10,010,,1"));
const auto& sats = p.state().satellites;
TEST_ASSERT_EQUAL(5, static_cast<int>(sats.size()));
TEST_ASSERT_EQUAL(-1, sats[4].snr); // not tracked: empty SNR
}
void test_each_constellation_keeps_its_own_satellites() {
NmeaParser p;
feedLine(p, nmea("GPGSV,1,1,01,05,60,120,40,1"));
feedLine(p, nmea("GLGSV,1,1,01,70,30,90,33,1"));
feedLine(p, nmea("GAGSV,1,1,01,11,20,45,28,7"));
feedLine(p, nmea("GBGSV,1,1,01,24,50,270,36,1"));
feedLine(p, nmea("GQGSV,1,1,01,02,70,180,41,1"));
const auto& sats = p.state().satellites;
TEST_ASSERT_EQUAL(5, static_cast<int>(sats.size()));
int seen[8] = {};
for (auto& s : sats) seen[static_cast<int>(s.system)]++;
TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Gps)]);
TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Glonass)]);
TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Galileo)]);
TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::BeiDou)]);
TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Qzss)]);
}
void test_a_satellite_on_two_bands_counts_once_with_the_stronger_signal() {
NmeaParser p;
feedLine(p, nmea("GPGSV,1,1,01,05,60,120,30,1")); // L1
feedLine(p, nmea("GPGSV,1,1,01,05,60,120,42,8")); // L5
const auto& sats = p.state().satellites;
TEST_ASSERT_EQUAL(1, static_cast<int>(sats.size()));
TEST_ASSERT_EQUAL(42, sats[0].snr);
}
void test_an_empty_gsv_clears_that_constellation() {
NmeaParser p;
feedLine(p, nmea("GLGSV,1,1,01,70,30,90,33,1"));
TEST_ASSERT_EQUAL(1, static_cast<int>(p.state().satellites.size()));
feedLine(p, "$GLGSV,1,1,00,1*78"); // captured
TEST_ASSERT_EQUAL(0, static_cast<int>(p.state().satellites.size()));
}
void test_gsa_marks_the_satellites_used_per_system() {
NmeaParser p;
feedLine(p, nmea("GPGSV,1,1,02,05,60,120,40,13,45,200,35,1"));
feedLine(p, nmea("GBGSV,1,1,01,05,50,270,36,1")); // BeiDou 5, not GPS 5
feedLine(p, nmea("GNGSA,A,3,05,,,,,,,,,,,,1.6,0.9,1.3,1")); // GPS: 5 used
feedLine(p, nmea("GNGSA,A,3,,,,,,,,,,,,,1.6,0.9,1.3,4")); // BeiDou: none
for (auto& s : p.state().satellites) {
bool expected = s.system == Constellation::Gps && s.prn == 5;
TEST_ASSERT_EQUAL(expected, s.used);
}
}
void test_split_feeds_and_noise_are_handled() {
NmeaParser p;
std::string a = "garbage\r\n" + nmea("GNGGA,123519.00,5050.8136,N,00421.1532,E,1,09,0.9,61.4,M,46.9,M,,");
std::string b = "\r\n";
p.feed(a.data(), 20);
p.feed(a.data() + 20, a.size() - 20);
TEST_ASSERT_FALSE(p.state().positionValid); // no line end yet
p.feed(b.data(), b.size());
TEST_ASSERT_TRUE(p.state().positionValid);
}
void test_an_overlong_line_is_dropped() {
NmeaParser p;
std::string junk(300, 'x');
junk += "\r\n";
p.feed(junk.data(), junk.size());
feedLine(p, nmea("GNGGA,123519.00,5050.8136,N,00421.1532,E,1,09,0.9,61.4,M,46.9,M,,"));
TEST_ASSERT_TRUE(p.state().positionValid);
}
void test_constellation_names() {
TEST_ASSERT_EQUAL_STRING("GPS", constellationName(Constellation::Gps));
TEST_ASSERT_EQUAL_STRING("Galileo", constellationName(Constellation::Galileo));
TEST_ASSERT_EQUAL_STRING("BeiDou", constellationName(Constellation::BeiDou));
}
int main() {
UNITY_BEGIN();
RUN_TEST(test_checksum_is_verified);
RUN_TEST(test_no_fix_as_captured_indoors);
RUN_TEST(test_rmc_with_a_fix_gives_position_time_speed_course);
RUN_TEST(test_southern_and_western_hemispheres_are_negative);
RUN_TEST(test_utc_seconds_from_the_fix_time);
RUN_TEST(test_gga_gives_fix_quality_altitude_satellites_and_hdop);
RUN_TEST(test_fix_type_comes_from_gsa_mode);
RUN_TEST(test_losing_the_fix_drops_the_position);
RUN_TEST(test_gsv_sequence_spans_several_messages);
RUN_TEST(test_each_constellation_keeps_its_own_satellites);
RUN_TEST(test_a_satellite_on_two_bands_counts_once_with_the_stronger_signal);
RUN_TEST(test_an_empty_gsv_clears_that_constellation);
RUN_TEST(test_gsa_marks_the_satellites_used_per_system);
RUN_TEST(test_split_feeds_and_noise_are_handled);
RUN_TEST(test_an_overlong_line_is_dropped);
RUN_TEST(test_constellation_names);
return UNITY_END();
}