225 lines
6.3 KiB
C++
225 lines
6.3 KiB
C++
// gps.hpp — u-blox NMEA GPS reader over UART
|
|
// No external libs; uses POSIX termios + read().
|
|
//
|
|
// Hardware (THE TRUTH):
|
|
// GPIO14/TXD0 → UBLOX_TX (/dev/serial0)
|
|
// GPIO15/RXD0 → UBLOX_RX
|
|
// Default baud: 38400 (factory default on this module)
|
|
//
|
|
// Enable UART: sudo raspi-config → Interfaces → Serial Port
|
|
// "login shell over serial" = No, "serial port hardware" = Yes
|
|
// Then reboot. Check: ls -la /dev/serial0
|
|
#pragma once
|
|
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
|
|
#include <fcntl.h>
|
|
#include <termios.h>
|
|
#include <unistd.h>
|
|
|
|
namespace gps {
|
|
|
|
struct Fix {
|
|
bool valid = false;
|
|
double lat = 0.0; // degrees, positive=N
|
|
double lon = 0.0; // degrees, positive=E
|
|
float speed_knots = 0.0f;
|
|
char time[12]{}; // HHMMSS.ss (raw NMEA)
|
|
char date[7]{}; // DDMMYY
|
|
};
|
|
|
|
struct Config {
|
|
const char *dev = "/dev/serial0";
|
|
int baud = 38400; // confirmed working baud for this module
|
|
bool verbose = false;
|
|
};
|
|
|
|
class Reader {
|
|
public:
|
|
bool begin(const Config &cfg);
|
|
bool begin(const char *dev = "/dev/serial0", int baud = 38400);
|
|
void end();
|
|
|
|
// Read one NMEA sentence (null-terminated, includes '\n').
|
|
// timeout_ms: 0 = block forever, >0 = return false after timeout.
|
|
bool readLine(char *buf, size_t cap, uint32_t timeout_ms = 0);
|
|
|
|
// Parse $GPRMC or $GNRMC. Returns false if wrong sentence type or fix void.
|
|
static bool parseRmc(const char *sentence, Fix &fix);
|
|
|
|
// Verify NMEA checksum: XOR of bytes between '$' and '*'.
|
|
static bool verifyChecksum(const char *sentence);
|
|
|
|
private:
|
|
int fd_ = -1;
|
|
bool verbose_ = false;
|
|
};
|
|
|
|
// ---- Implementation ---------------------------------------------------------
|
|
|
|
inline bool Reader::begin(const Config &c)
|
|
{
|
|
verbose_ = c.verbose;
|
|
return begin(c.dev, c.baud);
|
|
}
|
|
|
|
inline bool Reader::begin(const char *dev, int baud)
|
|
{
|
|
fd_ = ::open(dev, O_RDWR | O_NOCTTY | O_NDELAY);
|
|
if (fd_ < 0) {
|
|
if (verbose_)
|
|
std::fprintf(stderr, "[gps] cannot open %s: %m\n", dev);
|
|
return false;
|
|
}
|
|
::fcntl(fd_, F_SETFL, 0); // blocking reads
|
|
|
|
termios t{};
|
|
if (tcgetattr(fd_, &t) < 0) {
|
|
if (verbose_) std::fprintf(stderr, "[gps] tcgetattr failed: %m\n");
|
|
::close(fd_); fd_ = -1;
|
|
return false;
|
|
}
|
|
|
|
speed_t spd;
|
|
switch (baud) {
|
|
case 4800: spd = B4800; break;
|
|
case 9600: spd = B9600; break;
|
|
case 19200: spd = B19200; break;
|
|
case 38400: spd = B38400; break;
|
|
case 57600: spd = B57600; break;
|
|
case 115200: spd = B115200; break;
|
|
default:
|
|
if (verbose_)
|
|
std::fprintf(stderr, "[gps] unknown baud %d, defaulting to 38400\n", baud);
|
|
spd = B38400;
|
|
}
|
|
cfsetispeed(&t, spd);
|
|
cfsetospeed(&t, spd);
|
|
cfmakeraw(&t);
|
|
t.c_cc[VMIN] = 1;
|
|
t.c_cc[VTIME] = 0;
|
|
if (tcsetattr(fd_, TCSANOW, &t) < 0) {
|
|
if (verbose_) std::fprintf(stderr, "[gps] tcsetattr failed: %m\n");
|
|
::close(fd_); fd_ = -1;
|
|
return false;
|
|
}
|
|
tcflush(fd_, TCIOFLUSH);
|
|
|
|
if (verbose_)
|
|
std::fprintf(stderr, "[gps] opened %s at %d baud\n", dev, baud);
|
|
return true;
|
|
}
|
|
|
|
inline void Reader::end()
|
|
{
|
|
if (fd_ >= 0) { ::close(fd_); fd_ = -1; }
|
|
}
|
|
|
|
inline bool Reader::readLine(char *buf, size_t cap, uint32_t timeout_ms)
|
|
{
|
|
using clock = std::chrono::steady_clock;
|
|
auto deadline = clock::now() + std::chrono::milliseconds(timeout_ms);
|
|
|
|
auto timed_read = [&](char &c) -> bool {
|
|
if (timeout_ms == 0) {
|
|
return ::read(fd_, &c, 1) == 1;
|
|
}
|
|
// Non-blocking check with timeout.
|
|
while (true) {
|
|
ssize_t r = ::read(fd_, &c, 1);
|
|
if (r == 1) return true;
|
|
if (clock::now() >= deadline) return false;
|
|
// Brief yield to avoid spinning.
|
|
struct timespec ts = { 0, 1'000'000 }; // 1 ms
|
|
nanosleep(&ts, nullptr);
|
|
}
|
|
};
|
|
|
|
size_t n = 0;
|
|
char c;
|
|
|
|
// Wait for '$' (start of sentence).
|
|
do {
|
|
if (!timed_read(c)) return false;
|
|
} while (c != '$');
|
|
buf[n++] = '$';
|
|
|
|
while (n < cap - 1) {
|
|
if (!timed_read(c)) return false;
|
|
buf[n++] = c;
|
|
if (c == '\n') break;
|
|
}
|
|
buf[n] = '\0';
|
|
return true;
|
|
}
|
|
|
|
inline bool Reader::verifyChecksum(const char *sentence)
|
|
{
|
|
// Checksum is XOR of all bytes between '$' (exclusive) and '*' (exclusive).
|
|
const char *p = sentence;
|
|
if (*p == '$') ++p;
|
|
|
|
uint8_t calc = 0;
|
|
while (*p && *p != '*') calc ^= (uint8_t)*p++;
|
|
if (*p != '*') return false; // no checksum present
|
|
|
|
unsigned int given = 0;
|
|
if (std::sscanf(p + 1, "%02X", &given) != 1) return false;
|
|
return (uint8_t)given == calc;
|
|
}
|
|
|
|
// Parse $GPRMC or $GNRMC.
|
|
// Format: $GNRMC,HHMMSS.ss,A,DDMM.MMMM,N,DDDMM.MMMM,E,spd,crs,DDMMYY,...*CS
|
|
inline bool Reader::parseRmc(const char *sentence, Fix &fix)
|
|
{
|
|
if (!sentence) return false;
|
|
if (std::strncmp(sentence, "$GPRMC", 6) != 0 &&
|
|
std::strncmp(sentence, "$GNRMC", 6) != 0)
|
|
return false;
|
|
|
|
// Make a mutable copy for tokenization (strip the *CS checksum tail).
|
|
char buf[128];
|
|
std::strncpy(buf, sentence, sizeof(buf) - 1);
|
|
buf[sizeof(buf) - 1] = '\0';
|
|
// Terminate at '*' so strtok doesn't swallow the checksum as a field.
|
|
char *star = std::strchr(buf, '*');
|
|
if (star) *star = '\0';
|
|
|
|
const char *fields[13]{};
|
|
int nf = 0;
|
|
char *p = buf;
|
|
while (*p && nf < 13) {
|
|
fields[nf++] = p;
|
|
char *next = std::strchr(p, ',');
|
|
if (!next) break;
|
|
*next = '\0';
|
|
p = next + 1;
|
|
}
|
|
if (nf < 10) return false;
|
|
|
|
if (fields[2][0] != 'A') { fix.valid = false; return false; }
|
|
|
|
std::strncpy(fix.time, fields[1], sizeof(fix.time) - 1);
|
|
|
|
double raw_lat = std::atof(fields[3]);
|
|
int lat_deg = (int)(raw_lat / 100.0);
|
|
fix.lat = lat_deg + (raw_lat - lat_deg * 100.0) / 60.0;
|
|
if (fields[4][0] == 'S') fix.lat = -fix.lat;
|
|
|
|
double raw_lon = std::atof(fields[5]);
|
|
int lon_deg = (int)(raw_lon / 100.0);
|
|
fix.lon = lon_deg + (raw_lon - lon_deg * 100.0) / 60.0;
|
|
if (fields[6][0] == 'W') fix.lon = -fix.lon;
|
|
|
|
fix.speed_knots = (float)std::atof(fields[7]);
|
|
std::strncpy(fix.date, fields[9], sizeof(fix.date) - 1);
|
|
fix.valid = true;
|
|
return true;
|
|
}
|
|
|
|
} // namespace gps
|