From c19e24a3b4f646a637680165ae1d0c812abfa509 Mon Sep 17 00:00:00 2001 From: Vladslav P Date: Wed, 24 Jun 2026 03:12:24 +0300 Subject: [PATCH 1/2] rtklib_solution: implement QZGSA/QZGSV sentences Signed-off-by: Vladslav P --- src/algorithms/libs/rtklib/rtklib_solution.cc | 85 +++++++++++++++++++ 1 file changed, 85 insertions(+) diff --git a/src/algorithms/libs/rtklib/rtklib_solution.cc b/src/algorithms/libs/rtklib/rtklib_solution.cc index 609456d4b..0b7266019 100644 --- a/src/algorithms/libs/rtklib/rtklib_solution.cc +++ b/src/algorithms/libs/rtklib/rtklib_solution.cc @@ -1763,6 +1763,46 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol, } p += std::snprintf(p, MSG_TAIL, "*%02X%c%c", sum, 0x0D, 0x0A); } + /* QZGSA: QZSS */ + for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++) + { + if (!ssat[sat - 1].vs || ssat[sat - 1].azel[1] <= 0.0) + { + continue; + } + if (satsys(sat, &prn[nsat]) != SYS_QZS) + { + continue; + } + for (i = 0; i < 2; i++) + { + azel[i + nsat * 2] = ssat[sat - 1].azel[i]; + } + nsat++; + } + if (nsat > 0) + { + s = p; + p += std::snprintf(p, MAXSOLBUF, "$QZGSA,A,%d", sol->stat <= 0 ? 1 : 3); + for (i = 0; i < 12; i++) + { + if (i < nsat) + { + p += std::snprintf(p, MAX_LENGTH_INT + 2, ",%02d", prn[i]); + } + else + { + p += std::snprintf(p, COMMA_LENGTH, ","); + } + } + dops(nsat, azel, 0.0, dop); + p += std::snprintf(p, MAXSOLBUF - (p - s), ",%3.1f,%3.1f,%3.1f,5", dop[1], dop[2], dop[3]); + for (q = s + 1, sum = 0; *q; q++) + { + sum ^= *q; /* check-sum */ + } + p += std::snprintf(p, MSG_TAIL, "*%02X%c%c", sum, 0x0D, 0x0A); + } /* GAGSA: galileo */ for (sat = 1, nsat = 0; sat <= MAXSAT && nsat < 12; sat++) { @@ -2021,6 +2061,51 @@ int outnmea_gsv(unsigned char *buff, const sol_t *sol, } p += std::snprintf(p, MSG_TAIL, "*%02X%c%c", sum, 0x0D, 0x0A); } + /* QZGSV: QZSS */ + for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++) + { + if (satsys(sat, &prn) != SYS_QZS) + { + continue; + } + if (ssat[sat - 1].vs && ssat[sat - 1].azel[1] > 0.0) + { + sats[n++] = sat; + } + } + nmsg = n <= 0 ? 0 : (n - 1) / 4 + 1; + + for (i = k = 0; i < nmsg; i++) + { + s = p; + p += std::snprintf(p, MAXSOLBUF, "$QZGSV,%d,%d,%02d", nmsg, i + 1, n); + + for (j = 0; j < 4; j++, k++) + { + if (k < n) + { + satsys(sats[k], &prn); /* 193-206 */ + az = ssat[sats[k] - 1].azel[0] * R2D; + if (az < 0.0) + { + az += 360.0; + } + el = ssat[sats[k] - 1].azel[1] * R2D; + snr = ssat[sats[k] - 1].snr[0] * 0.25; + p += std::snprintf(p, MAXSOLBUF - (s - p), ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr); + } + else + { + p += std::snprintf(p, MAXSOLBUF - (s - p), ",,,,"); + } + } + p += std::snprintf(p, MAXSOLBUF - (s - p), ",1"); /* L1C/A */ + for (q = s + 1, sum = 0; *q; q++) + { + sum ^= *q; /* check-sum */ + } + p += std::snprintf(p, MSG_TAIL, "*%02X%c%c", sum, 0x0D, 0x0A); + } /* BDGSV: beidou */ for (sat = 1, n = 0; sat < MAXSAT && n < 12; sat++) { From 137f15c2aa899dc5d1a2b7140ae0667af480c0ba Mon Sep 17 00:00:00 2001 From: Vladislav P Date: Fri, 3 Jul 2026 13:23:27 +0300 Subject: [PATCH 2/2] nmea_printer: add GPS and QZSS GSA/GSV tests Signed-off-by: Vladislav P --- .../pvt/nmea_printer_test.cc | 49 ++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc index a87759752..2e83b287f 100644 --- a/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc +++ b/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc @@ -165,7 +165,30 @@ TEST_F(NmeaPrinterTest, PrintLine) pvt_solution->pvt_sol.rr[5] = 0.0; pvt_solution->pvt_sol.stat = 1; // SOLQ_FIX pvt_solution->pvt_sol.time = gtime; - + pvt_solution->pvt_ssat[0].vs = 1; + pvt_solution->pvt_ssat[0].azel[0] = 3.514475; + pvt_solution->pvt_ssat[0].azel[1] = 0.644151; + pvt_solution->pvt_ssat[0].vsat[0] = 48; + pvt_solution->pvt_ssat[0].snr[0] = 204; + pvt_solution->pvt_ssat[0].fix[0] = 48; + pvt_solution->pvt_ssat[0].slip[0] = 48; + pvt_solution->pvt_ssat[0].half[0] = 48; + pvt_solution->pvt_ssat[96].vs = 1; + pvt_solution->pvt_ssat[96].azel[0] = 0.808051; + pvt_solution->pvt_ssat[96].azel[1] = 1.489340; + pvt_solution->pvt_ssat[96].vsat[0] = 48; + pvt_solution->pvt_ssat[96].snr[0] = 203; + pvt_solution->pvt_ssat[96].fix[0] = 48; + pvt_solution->pvt_ssat[96].slip[0] = 48; + pvt_solution->pvt_ssat[96].half[0] = 48; + pvt_solution->pvt_ssat[97].vs = 1; + pvt_solution->pvt_ssat[97].azel[0] = 2.968734; + pvt_solution->pvt_ssat[97].azel[1] = 0.950019; + pvt_solution->pvt_ssat[97].vsat[0] = 48; + pvt_solution->pvt_ssat[97].snr[0] = 213; + pvt_solution->pvt_ssat[97].fix[0] = 48; + pvt_solution->pvt_ssat[97].slip[0] = 48; + pvt_solution->pvt_ssat[97].half[0] = 48; bool flag_nmea_output_file = true; ASSERT_NO_THROW({ std::shared_ptr nmea_printer = std::make_shared(filename, flag_nmea_output_file, false, ""); @@ -175,6 +198,10 @@ TEST_F(NmeaPrinterTest, PrintLine) std::ifstream test_file(filename); std::string line; std::string GPRMC("$GPRMC"); + std::string GPGSA("$GPGSA"); + std::string GPGSV("$GPGSV"); + std::string QZGSA("$QZGSA"); + std::string QZGSV("$QZGSV"); if (test_file.is_open()) { while (getline(test_file, line)) @@ -184,6 +211,26 @@ TEST_F(NmeaPrinterTest, PrintLine) { EXPECT_EQ(line, "$GPRMC,225436.00,A,4916.4497617,N,12311.1202744,W,0.00,0.00,191194,0.0,E,D*21\r"); } + found = line.find(GPGSA); + if (found != std::string::npos) + { + EXPECT_EQ(line, "$GPGSA,A,3,01,,,,,,,,,,,,0.0,0.0,0.0,1*2E\r"); + } + found = line.find(GPGSV); + if (found != std::string::npos) + { + EXPECT_EQ(line, "$GPGSV,1,1,01,01,37,201,51,,,,,,,,,,,,,1*57\r"); + } + found = line.find(QZGSA); + if (found != std::string::npos) + { + EXPECT_EQ(line, "$QZGSA,A,3,194,195,,,,,,,,,,,0.0,0.0,0.0,5*36\r"); + } + found = line.find(QZGSV); + if (found != std::string::npos) + { + EXPECT_EQ(line, "$QZGSV,1,1,02,194,85,046,51,195,54,170,53,,,,,,,,,1*71\r"); + } } test_file.close(); }