Parse GGA/GSA fix quality from NMEA, add PAIR command init sequence
Replace altitude-validity heuristic with authoritative GGA quality field (SPS/DGPS/RTK) and GSA nav mode (2D/3D) via TinyGPSCustom extractors. Send $PAIR062 commands at boot to filter NMEA output to only GGA/GSA/RMC/GSV and configure PPS for fix-only pulses. GpsPayload struct gains fix_quality field (16 -> 14 bytes packed).
This commit is contained in:
parent
80158e10d7
commit
7db4204d26
@ -234,13 +234,24 @@ with sub-microsecond precision relative to the GPS epoch. The module's RTC
|
||||
battery backup enables warm starts (~5s) after initial cold start fix (~30-60s).
|
||||
|
||||
**UART notes:** The RYS352A defaults to 115200 baud NMEA output with `GN`
|
||||
talker ID (multi-constellation). The TX line (GPIO6) is optional — only needed
|
||||
to send `$PAIR` proprietary commands (Airoha AG3352 engine) for changing
|
||||
update rate (`$PAIR050`), constellation selection (`$PAIR066`), PPS config
|
||||
(`$PAIR752`), or NMEA sentence output rates (`$PAIR062`). See
|
||||
`docs/RYS352x_PAIR_Command_Guide.md` for the full command reference.
|
||||
The firmware uses TinyGPS++ v1.1+ to parse standard GGA/RMC sentences —
|
||||
v1.1 is required for `$GNGGA`/`$GNRMC` (multi-GNSS talker ID) support.
|
||||
talker ID (multi-constellation). The TX line (GPIO6) is used at boot to send
|
||||
`$PAIR` proprietary commands (Airoha AG3352 engine) that configure the GPS
|
||||
module for satellite tracking use. See `docs/RYS352x_PAIR_Command_Guide.md`
|
||||
for the full command reference.
|
||||
|
||||
**Boot-time PAIR init sequence:** The firmware sends `$PAIR062` commands at
|
||||
startup to filter NMEA output — only GGA (position/quality), GSA (fix mode/DOP),
|
||||
RMC (time/date/speed), and GSV (satellite visibility, every 5th fix) are enabled.
|
||||
Redundant sentences (GLL, VTG, ZDA, GRS, GST, GNS) are disabled to reduce parser
|
||||
load and latency. A `$PAIR752` command configures PPS to pulse only on 2D/3D fix
|
||||
with 100ms pulse width. Each command waits for `$PAIR001` ACK; failures are
|
||||
logged but non-fatal — the GPS works with defaults if PAIR commands are unsupported.
|
||||
|
||||
The firmware uses TinyGPS++ v1.1+ with custom field extractors (`TinyGPSCustom`)
|
||||
to read GGA quality (field 6: SPS/DGPS/RTK) and GSA nav mode (field 2: 2D/3D)
|
||||
directly from the NMEA stream, replacing the earlier heuristic that inferred
|
||||
fix type from altitude validity. Both `GN` and `GP` talker ID variants are
|
||||
registered for compatibility across constellation configurations.
|
||||
|
||||
## Full GPIO Map
|
||||
|
||||
|
||||
@ -59,6 +59,10 @@
|
||||
#define BARO_REPORT_MS 1000 // 1Hz pressure/temperature
|
||||
#define STATUS_PRINT_MS 1000 // 1Hz USB serial status line
|
||||
|
||||
// GPS PAIR command init
|
||||
#define GPS_INIT_DELAY_MS 200 // Wait after UART open before sending commands
|
||||
#define PAIR_ACK_TIMEOUT_MS 500 // Timeout waiting for $PAIR001 acknowledgment
|
||||
|
||||
// --- LED ---
|
||||
#define LED_BRIGHTNESS 30 // 0-255, keep low to avoid blinding in enclosure
|
||||
#define LED_COUNT 1
|
||||
|
||||
@ -13,10 +13,10 @@ struct __attribute__((packed)) GpsPayload {
|
||||
int32_t lat_1e7; // latitude × 10^7 (0.0000001° resolution)
|
||||
int32_t lon_1e7; // longitude × 10^7
|
||||
int16_t alt_dm; // altitude in decimeters
|
||||
uint8_t fix_type; // 0=none, 2=2D, 3=3D
|
||||
uint8_t fix_type; // GSA NavMode: 0=none, 2=2D, 3=3D
|
||||
uint8_t fix_quality; // GGA Quality: 0=invalid, 1=SPS, 2=DGPS, 4=RTK
|
||||
uint8_t satellites; // visible satellite count
|
||||
uint8_t hdop_10; // HDOP × 10
|
||||
uint8_t pad[3]; // alignment
|
||||
};
|
||||
|
||||
struct __attribute__((packed)) OrientPayload {
|
||||
@ -54,6 +54,15 @@ static NimBLECharacteristic *pPpsChar = nullptr; // Sensor: PPS timestamp
|
||||
static Adafruit_NeoPixel led(LED_COUNT, PIN_LED, NEO_GRB + NEO_KHZ800);
|
||||
static HardwareSerial SerialGPS(2);
|
||||
static TinyGPSPlus gps;
|
||||
|
||||
// Custom NMEA field extractors — hooked into gps.encode() automatically
|
||||
// GGA Quality (field 6): 0=none, 1=SPS, 2=DGPS, 3=PPS, 4=RTK, 5=FloatRTK, 6=DR
|
||||
static TinyGPSCustom ggaQuality(gps, "GNGGA", 6);
|
||||
static TinyGPSCustom ggaQualityGP(gps, "GPGGA", 6);
|
||||
// GSA NavMode (field 2): 1=no fix, 2=2D, 3=3D
|
||||
static TinyGPSCustom gsaNavMode(gps, "GNGSA", 2);
|
||||
static TinyGPSCustom gsaNavModeGP(gps, "GPGSA", 2);
|
||||
|
||||
static MPU9250 imu(Wire, MPU9250_ADDR);
|
||||
static Adafruit_BMP3XX bmp;
|
||||
|
||||
@ -213,6 +222,64 @@ static void startAdvertising() {
|
||||
Serial.printf("[BLE] Advertising as \"%s\"\n", BLE_DEVICE_NAME);
|
||||
}
|
||||
|
||||
// --- GPS PAIR Command Helper ---
|
||||
|
||||
// Send a $PAIR command to the RYS352A and wait for $PAIR001 ACK.
|
||||
// body: command without $ prefix or checksum, e.g. "PAIR062,1,0"
|
||||
// Returns: 0=success, 1=processing, 2=fail, 3=unsupported, 4=param error, -1=timeout
|
||||
static int sendPairCmd(const char *body, uint16_t timeoutMs = PAIR_ACK_TIMEOUT_MS) {
|
||||
// Compute NMEA XOR checksum over the body
|
||||
uint8_t cksum = 0;
|
||||
for (const char *p = body; *p; p++) {
|
||||
cksum ^= (uint8_t)*p;
|
||||
}
|
||||
|
||||
// Send $body*XX\r\n
|
||||
char buf[80];
|
||||
snprintf(buf, sizeof(buf), "$%s*%02X\r\n", body, cksum);
|
||||
SerialGPS.print(buf);
|
||||
|
||||
// Extract command ID for matching ACK (e.g. "PAIR062" -> "062")
|
||||
// PAIR commands are "PAIRnnn,..." — the ACK references just the number
|
||||
const char *idStr = body + 4; // skip "PAIR"
|
||||
int cmdId = atoi(idStr);
|
||||
|
||||
// Wait for $PAIR001,<cmdId>,<result> or timeout
|
||||
for (int attempt = 0; attempt < 2; attempt++) {
|
||||
uint32_t start = millis();
|
||||
char line[128];
|
||||
size_t lineLen = 0;
|
||||
|
||||
while (millis() - start < timeoutMs) {
|
||||
if (SerialGPS.available()) {
|
||||
char c = SerialGPS.read();
|
||||
if (c == '\n') {
|
||||
line[lineLen] = '\0';
|
||||
// Check for $PAIR001,<cmdId>,<result>
|
||||
int ackCmd = -1, ackResult = -1;
|
||||
if (sscanf(line, "$PAIR001,%d,%d", &ackCmd, &ackResult) == 2
|
||||
&& ackCmd == cmdId) {
|
||||
if (ackResult == 1 && attempt == 0) {
|
||||
// "Processing" — retry after short delay
|
||||
delay(200);
|
||||
break;
|
||||
}
|
||||
return ackResult;
|
||||
}
|
||||
lineLen = 0;
|
||||
} else if (c != '\r' && lineLen < sizeof(line) - 1) {
|
||||
line[lineLen++] = c;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (attempt == 0 && lineLen == 0) {
|
||||
// Timeout on first attempt — don't retry
|
||||
break;
|
||||
}
|
||||
}
|
||||
return -1; // timeout
|
||||
}
|
||||
|
||||
// --- Sensor Hardware Init ---
|
||||
|
||||
static void initSensors() {
|
||||
@ -256,6 +323,31 @@ static void initSensors() {
|
||||
Serial.printf("[GPS] UART2 on GPIO%d/GPIO%d at %d baud, waiting for fix...\n",
|
||||
PIN_GPS_RX, PIN_GPS_TX, GPS_BAUD);
|
||||
|
||||
// Configure NMEA output via $PAIR commands (non-fatal if unsupported)
|
||||
delay(GPS_INIT_DELAY_MS);
|
||||
// Drain any boot-up garbage from the GPS UART
|
||||
while (SerialGPS.available()) SerialGPS.read();
|
||||
|
||||
struct { const char *cmd; const char *desc; } gpsInit[] = {
|
||||
{"PAIR062,0,1", "GGA on"},
|
||||
{"PAIR062,2,1", "GSA on"},
|
||||
{"PAIR062,4,1", "RMC on"},
|
||||
{"PAIR062,3,5", "GSV every 5"},
|
||||
{"PAIR062,1,0", "GLL off"},
|
||||
{"PAIR062,5,0", "VTG off"},
|
||||
{"PAIR062,6,0", "ZDA off"},
|
||||
{"PAIR062,7,0", "GRS off"},
|
||||
{"PAIR062,8,0", "GST off"},
|
||||
{"PAIR062,9,0", "GNS off"},
|
||||
{"PAIR752,3,100", "PPS 2D/3D fix only"},
|
||||
};
|
||||
for (auto &c : gpsInit) {
|
||||
int r = sendPairCmd(c.cmd);
|
||||
Serial.printf("[GPS] %s -> %s (%s)\n", c.cmd,
|
||||
r == 0 ? "ACK 0" : r == -1 ? "timeout" : "err",
|
||||
c.desc);
|
||||
}
|
||||
|
||||
// PPS interrupt — captures micros() on rising edge
|
||||
pinMode(PIN_GPS_PPS, INPUT);
|
||||
attachInterrupt(digitalPinToInterrupt(PIN_GPS_PPS), gpsPpsISR, RISING);
|
||||
@ -282,7 +374,17 @@ static void readSensors() {
|
||||
gp.lon_1e7 = (int32_t)(gps.location.lng() * 1e7);
|
||||
gp.alt_dm = gps.altitude.isValid()
|
||||
? (int16_t)(gps.altitude.meters() * 10) : 0;
|
||||
gp.fix_type = gps.altitude.isValid() ? 3 : 2;
|
||||
|
||||
// Authoritative fix data from custom NMEA field extractors
|
||||
const char *navVal = gsaNavMode.isUpdated() ? gsaNavMode.value()
|
||||
: gsaNavModeGP.value();
|
||||
const char *qualVal = ggaQuality.isUpdated() ? ggaQuality.value()
|
||||
: ggaQualityGP.value();
|
||||
uint8_t navMode = (navVal && navVal[0]) ? (uint8_t)atoi(navVal) : 0;
|
||||
uint8_t quality = (qualVal && qualVal[0]) ? (uint8_t)atoi(qualVal) : 0;
|
||||
|
||||
gp.fix_type = (navMode >= 2) ? navMode : 0;
|
||||
gp.fix_quality = quality;
|
||||
gp.satellites = (uint8_t)min((unsigned long)gps.satellites.value(),
|
||||
(unsigned long)255);
|
||||
float hdop = gps.hdop.hdop();
|
||||
@ -370,12 +472,25 @@ static void readSensors() {
|
||||
if (now - lastStatusPrintMs >= STATUS_PRINT_MS) {
|
||||
lastStatusPrintMs = now;
|
||||
|
||||
// GGA quality names indexed by quality field value (0-6)
|
||||
static const char *qualNames[] = {
|
||||
"none", "SPS", "DGPS", "PPS", "RTK", "FRTK", "DR"
|
||||
};
|
||||
|
||||
Serial.printf("[SNS] ");
|
||||
if (gps.location.isValid()) {
|
||||
Serial.printf("lat=%.4f lon=%.4f alt=%.1fm fix=%s sats=%d ",
|
||||
const char *navVal = gsaNavMode.isUpdated() ? gsaNavMode.value()
|
||||
: gsaNavModeGP.value();
|
||||
const char *qualVal = ggaQuality.isUpdated() ? ggaQuality.value()
|
||||
: ggaQualityGP.value();
|
||||
uint8_t navMode = (navVal && navVal[0]) ? (uint8_t)atoi(navVal) : 0;
|
||||
uint8_t quality = (qualVal && qualVal[0]) ? (uint8_t)atoi(qualVal) : 0;
|
||||
const char *dimStr = (navMode == 3) ? "3D" : (navMode == 2) ? "2D" : "??";
|
||||
const char *qualStr = (quality <= 6) ? qualNames[quality] : "?";
|
||||
|
||||
Serial.printf("lat=%.4f lon=%.4f alt=%.1fm fix=%s/%s sats=%d ",
|
||||
gps.location.lat(), gps.location.lng(),
|
||||
gps.altitude.meters(),
|
||||
gps.altitude.isValid() ? "3D" : "2D",
|
||||
gps.altitude.meters(), dimStr, qualStr,
|
||||
gps.satellites.value());
|
||||
} else {
|
||||
Serial.printf("fix=none sats=%d ", gps.satellites.value());
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user