diff --git a/CHANGELOG.md b/CHANGELOG.md index 89a9aa39..bd80245a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,14 @@ # Changelog +## [3.4.9] 2024-05-03 +- Added a few options to controll how SPARTN messages are generated: + - `--sf055-default` to set the default value for the ionospheric quality if not available. + - `--sf042-override` to force the tropospheric quality to a specific value (previous `--ura-override` was also used for this). + - `--sf042-default` to set the default value for the tropospheric quality if not available. + - `--filter-by-ocb` to filter out ionopsheric residuals for satellites not in the OCB. + - `--ignore-l2l` to ignore L2L biases. +- Fixed a bug where parsing NMEA GGA would fail if age of corrections was not provided. + ## [3.4.8] 2024-04-26 - Added support for age of correction when using NMEA. @@ -9,7 +18,7 @@ - Added new format option `lrf-uper` to output RTCM framed UPER encoded 3GPP LPP messages. - Updated 3GPP LPP version from Release 16.4.0 to Release 18.1.0. - HA-GNSS-Metrics data is now included in the `ProvideLocationInformation` message. -- Fix memory leak from ProvideLocationInformation. +- Fixed memory leak from ProvideLocationInformation. ## [3.4.6] 2024-04-04 - You can optionally include/exclude which generators to build by using the CMake options `-DINCLUDE_GENERATOR_*`. By default, RTCM and SPARTN generators are included and the old SPARTN generator is excluded. diff --git a/CMakeLists.txt b/CMakeLists.txt index f4afd3cf..363b6fbb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,8 +36,8 @@ find_package(OpenSSL REQUIRED) endif (USE_OPENSSL) add_definitions(-D_POSIX_C_SOURCE=200809L) -add_definitions(-DCLIENT_VERSION="3.4.8") -add_definitions(-DCLIENT_VERSION_INT=0x030408) +add_definitions(-DCLIENT_VERSION="3.4.9") +add_definitions(-DCLIENT_VERSION_INT=0x030409) if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") add_definitions(-DCOMPILER_CANNOT_DEDUCE_UNREACHABLE=1) diff --git a/README.md b/README.md index 0a51374d..a0111db5 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # SUPL 3GPP LPP client -![version](https://img.shields.io/badge/version-3.4.8-green) +![version](https://img.shields.io/badge/version-3.4.9-green) ![license](https://img.shields.io/badge/license-MXM-blue) This project is a set of libraries, examples and tools to facilitate the development of 3GPP LPP clients. diff --git a/examples/lpp/ssr_example.cpp b/examples/lpp/ssr_example.cpp index 6a50d04d..62265825 100644 --- a/examples/lpp/ssr_example.cpp +++ b/examples/lpp/ssr_example.cpp @@ -35,7 +35,12 @@ static bool gForceIodeContinuity; static bool gAverageZenithDelay; static bool gEnableIodeShift; static int gSf055Override; +static int gSf055Default; +static int gSf042Override; +static int gSf042Default; static bool gIncreasingSiou; +static bool gFilterByOcb; +static bool gIgnoreL2L; static bool gPrintRtcm; static Options gOptions; static ControlParser gControlParser; @@ -57,7 +62,9 @@ static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message* [[noreturn]] void execute(Options options, ssr_example::Format format, int ura_override, bool ublox_clock_correction, bool force_continuity, bool average_zenith_delay, bool enable_iode_shift, int sf055_override, - bool increasing_siou, bool print_rtcm) { + int sf055_default, int sf042_override, int sf042_default, + bool increasing_siou, bool filter_by_ocb, bool ignore_l2l, + bool print_rtcm) { gOptions = std::move(options); gFormat = format; gUraOverride = ura_override; @@ -66,7 +73,12 @@ static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message* gAverageZenithDelay = average_zenith_delay; gEnableIodeShift = enable_iode_shift; gSf055Override = sf055_override; + gSf055Default = sf055_default; + gSf042Override = sf042_override; + gSf042Default = sf042_default; gIncreasingSiou = increasing_siou; + gFilterByOcb = filter_by_ocb; + gIgnoreL2L = ignore_l2l; gPrintRtcm = print_rtcm; auto& cell_options = gOptions.cell_options; @@ -159,12 +171,15 @@ static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message* } else { gSpartnGeneratorNew.set_iode_shift(false); } - if (gSf055Override >= 0) { - gSpartnGeneratorNew.set_ionosphere_quality_override(gSf055Override); - } - if (gIncreasingSiou) { - gSpartnGeneratorNew.set_increasing_siou(true); - } + if (gSf055Override >= 0) gSpartnGeneratorNew.set_sf055_override(gSf055Override); + if (gSf055Default >= 0) gSpartnGeneratorNew.set_sf055_default(gSf055Default); + if (gSf042Override >= 0) gSpartnGeneratorNew.set_sf042_override(gSf042Override); + if (gSf042Default >= 0) gSpartnGeneratorNew.set_sf042_default(gSf042Default); + + if (gIncreasingSiou) gSpartnGeneratorNew.set_increasing_siou(true); + if (gFilterByOcb) gSpartnGeneratorNew.set_filter_by_ocb(true); + if (gIgnoreL2L) gSpartnGeneratorNew.set_ignore_l2l(true); + #endif LPP_Client client{false /* experimental segmentation support */}; @@ -405,7 +420,12 @@ void SsrCommand::parse(args::Subparser& parser) { delete mAverageZenithDelayArg; delete mEnableIodeShift; delete mSf055Override; + delete mSf055Default; + delete mSf042Override; + delete mSf042Default; delete mIncreasingSiou; + delete mFilterByOcb; + delete mIgnoreL2L; delete mPrintRTCMArg; mFormatArg = new args::ValueFlag(parser, "format", "Format of the output", @@ -452,10 +472,27 @@ void SsrCommand::parse(args::Subparser& parser) { "Override the SF055 value, value will be clamped between 0-15. " "Where 0 indicates that the value is invalid.", {"sf055-override"}, args::Options::Single); + mSf055Default = + new args::ValueFlag(parser, "sf055-default", + "Set the default SF055 value, value will be clamped between 0-15. " + "Where 0 indicates that the value is invalid.", + {"sf055-default"}, args::Options::Single); + + mSf042Override = new args::ValueFlag( + parser, "sf042-override", "Override the SF042 value, value will be clamped between 0-7.", + {"sf042-override"}, args::Options::Single); + mSf042Default = new args::ValueFlag( + parser, "sf042-default", "Set the default SF042 value, value will be clamped between 0-7.", + {"sf042-default"}, args::Options::Single); mIncreasingSiou = new args::Flag(parser, "increasing-siou", "Enable the increasing SIoU feature for SPARTN", {"increasing-siou"}); + mFilterByOcb = new args::Flag( + parser, "filter-by-ocb", + "Only include ionospheric residual satellites that also have OCB corrections", + {"filter-by-ocb"}); + mIgnoreL2L = new args::Flag(parser, "ignore-l2l", "Ignore L2L biases", {"ignore-l2l"}); mPrintRTCMArg = new args::Flag(parser, "print_rtcm", "Print RTCM messages info (only used for LRF-UPER)", @@ -522,18 +559,50 @@ void SsrCommand::execute(Options options) { if (sf055_override > 15) sf055_override = 15; } + auto sf055_default = -1; + if (*mSf055Default) { + sf055_default = mSf055Default->Get(); + if (sf055_default < 0) sf055_default = 0; + if (sf055_default > 15) sf055_default = 15; + } + + auto sf042_override = -1; + if (*mSf042Override) { + sf042_override = mSf042Override->Get(); + if (sf042_override < 0) sf042_override = 0; + if (sf042_override > 7) sf042_override = 7; + } + + auto sf042_default = -1; + if (*mSf042Default) { + sf042_default = mSf042Default->Get(); + if (sf042_default < 0) sf042_default = 0; + if (sf042_default > 7) sf042_default = 7; + } + auto increasing_siou = false; if (*mIncreasingSiou) { increasing_siou = mIncreasingSiou->Get(); } + auto filter_by_ocb = false; + if (*mFilterByOcb) { + filter_by_ocb = mFilterByOcb->Get(); + } + + auto ignore_l2l = false; + if (*mIgnoreL2L) { + ignore_l2l = mIgnoreL2L->Get(); + } + auto print_rtcm = false; if (*mPrintRTCMArg) { print_rtcm = true; } ::execute(std::move(options), format, ura_override, ublox_clock_correction, force_continuity, - average_zenith_delay, iode_shift, sf055_override, increasing_siou, print_rtcm); + average_zenith_delay, iode_shift, sf055_override, sf055_default, sf042_override, + sf042_default, increasing_siou, filter_by_ocb, ignore_l2l, print_rtcm); } } // namespace ssr_example diff --git a/examples/lpp/ssr_example.h b/examples/lpp/ssr_example.h index 9986608d..59d6aaef 100644 --- a/examples/lpp/ssr_example.h +++ b/examples/lpp/ssr_example.h @@ -23,7 +23,9 @@ class SsrCommand final : public Command { : Command("ssr", "Request State-space Representation (SSR) data from the location server"), mFormatArg(nullptr), mUraOverrideArg(nullptr), mUbloxClockCorrectionArg(nullptr), mForceContinuityArg(nullptr), mAverageZenithDelayArg(nullptr), mEnableIodeShift(nullptr), - mSf055Override(nullptr), mIncreasingSiou(nullptr), mPrintRTCMArg(nullptr) {} + mSf055Override(nullptr), mSf055Default(nullptr), mSf042Override(nullptr), + mSf042Default(nullptr), mIncreasingSiou(nullptr), mFilterByOcb(nullptr), + mIgnoreL2L(nullptr), mPrintRTCMArg(nullptr) {} ~SsrCommand() override { delete mFormatArg; @@ -33,7 +35,12 @@ class SsrCommand final : public Command { delete mAverageZenithDelayArg; delete mEnableIodeShift; delete mSf055Override; + delete mSf055Default; + delete mSf042Override; + delete mSf042Default; delete mIncreasingSiou; + delete mFilterByOcb; + delete mIgnoreL2L; delete mPrintRTCMArg; } @@ -48,7 +55,12 @@ class SsrCommand final : public Command { args::Flag* mAverageZenithDelayArg; args::Flag* mEnableIodeShift; args::ValueFlag* mSf055Override; + args::ValueFlag* mSf055Default; + args::ValueFlag* mSf042Override; + args::ValueFlag* mSf042Default; args::Flag* mIncreasingSiou; + args::Flag* mFilterByOcb; + args::Flag* mIgnoreL2L; args::Flag* mPrintRTCMArg; }; diff --git a/generator/spartn2/CMakeLists.txt b/generator/spartn2/CMakeLists.txt index 4f0f5525..d1cf08e6 100644 --- a/generator/spartn2/CMakeLists.txt +++ b/generator/spartn2/CMakeLists.txt @@ -16,7 +16,7 @@ target_link_libraries(generator_spartn2 PRIVATE asn1::generated::lpp asn1::helpe target_link_libraries(generator_spartn2 PRIVATE utility) if (SPARTN_DEBUG_PRINT) -target_compile_definitions(generator_spartn2 PRIVATE SPARTN_DEBUG_PRINT) +target_compile_definitions(generator_spartn2 PRIVATE SPARTN_DEBUG_PRINT=2) endif (SPARTN_DEBUG_PRINT) setup_target(generator_spartn2) diff --git a/generator/spartn2/builder.cpp b/generator/spartn2/builder.cpp index 6c23dda7..0053979e 100644 --- a/generator/spartn2/builder.cpp +++ b/generator/spartn2/builder.cpp @@ -6,13 +6,14 @@ Builder::Builder(uint32_t capacity) : mData(capacity), mBitOffset(0) {} -void Builder::double_to_bits(double min_range, double max_range, double resolution, double value, +double Builder::double_to_bits(double min_range, double max_range, double resolution, double value, uint8_t bits) { auto clamped_value = std::max(min_range, std::min(max_range, value)); auto scaled_value = (clamped_value - min_range) / resolution; auto rounded_value = std::lround(scaled_value); auto unsigned_value = static_cast(rounded_value); this->bits(unsigned_value, bits); + return rounded_value * resolution + min_range; } void Builder::reserve(uint32_t bits) { diff --git a/generator/spartn2/builder.hpp b/generator/spartn2/builder.hpp index 3a817d83..f1202317 100644 --- a/generator/spartn2/builder.hpp +++ b/generator/spartn2/builder.hpp @@ -20,7 +20,7 @@ class Builder { inline void b(bool value) { bits(static_cast(value), 1); } - void double_to_bits(double min_range, double max_range, double resolution, double value, + double double_to_bits(double min_range, double max_range, double resolution, double value, uint8_t bits); // TODO: float, double diff --git a/generator/spartn2/constant.hpp b/generator/spartn2/constant.hpp new file mode 100644 index 00000000..24d66297 --- /dev/null +++ b/generator/spartn2/constant.hpp @@ -0,0 +1,420 @@ +#pragma once +#include + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wreserved-macro-identifier" +#pragma GCC diagnostic ignored "-Wreserved-identifier" +#pragma GCC diagnostic ignored "-Wundef" +#pragma GCC diagnostic ignored "-Wold-style-cast" +#include +#pragma GCC diagnostic pop + +#define X 255 +#define INVALID_MAPPING 255 + +static SPARTN_CONSTEXPR uint8_t GPS_TO_SPARTN[24] = { + 0, // L1 C/A -> L1C + X, // L1C + X, // L2C + X, // L5 + X, // L1 P + X, // L1 Z-tracking + X, // L2 C/A + X, // L2 P + 1, // L2 Z-tracking -> L2W + X, // L2 L2C(M) + 2, // L2 L2C(L) -> L2L + X, // L2 L2C(M+L) + X, // L5 I + 3, // L5 Q -> L5Q + X, // L5 I+Q + X, // L1 L1C(D) + X, // L1 L1C(P) + X, // L1 L1C(D+P) + // Reserved + X, + X, + X, + X, + X, + X, +}; + +static SPARTN_CONSTEXPR uint8_t GPS_MAPPING[32] = { + X, // L1 C/A + X, // L1C + X, // L2C + X, // L5 + X, // L1 P + X, // L1 Z-tracking + X, // L2 C/A + X, // L2 P + X, // L2 Z-tracking + X, // L2 L2C(M) + X, // L2 L2C(L) + 10, // L2 L2C(M+L) -> L2 L2C(L) + X, // L5 I + X, // L5 Q + X, // L5 I+Q + X, // L1 L1C(D) + X, // L1 L1C(P) + X, // L1 L1C(D+P) + // Reserved + X, + X, + X, + X, + X, + X, +}; + +#define GPS_L1 1575.42 +#define GPS_L2 1227.60 +#define GPS_L5 1176.45 +static SPARTN_CONSTEXPR double GPS_FREQ[24] = { + GPS_L1, // L1 C/A + GPS_L1, // L1C + GPS_L2, // L2C + GPS_L5, // L5 + GPS_L1, // L1 P + GPS_L1, // L1 Z-tracking + GPS_L2, // L2 C/A + GPS_L2, // L2 P + GPS_L2, // L2 Z-tracking + GPS_L2, // L2 L2C(M) + GPS_L2, // L2 L2C(L) + GPS_L2, // L2 L2C(M+L) + GPS_L5, // L5 I + GPS_L5, // L5 Q + GPS_L5, // L5 I+Q + GPS_L1, // L1 L1C(D) + GPS_L1, // L1 L1C(P) + GPS_L1, // L1 L1C(D+P) + // Reserved + 0, + 0, + 0, + 0, + 0, + 0, +}; + +static SPARTN_CONSTEXPR const char* GPS_SIGNAL_NAMES[24] = { + "L1 C/A", + "L1C", + "L2C", + "L5", + "L1 P", + "L1 Z-tracking", + "L2 C/A", + "L2 P", + "L2 Z-tracking", + "L2 L2C(M)", + "L2 L2C(L)", + "L2 L2C(M+L)", + "L5 I", + "L5 Q", + "L5 I+Q", + "L1 L1C(D)", + "L1 L1C(P)", + "L1 L1C(D+P)", + // Reserved + "Reserved", + "Reserved", + "Reserved", + "Reserved", + "Reserved", + "Reserved", +}; + +static SPARTN_CONSTEXPR uint8_t GLO_TO_SPARTN[24] = { + 0, // G1 C/A -> L1C + 1, // G2 C/A -> L2C + X, // G3 + X, // G1 P + X, // G2 P + X, // G1a(D) + X, // G1a(P) + X, // G1a(D+P) + X, // G2a(I) + X, // G2a(P) + X, // G2a(I+P) + X, // G3 I + X, // G3 Q + X, // G3 I+Q + // Reserved + X, + X, + X, + X, + X, + X, + X, + X, + X, + X, +}; + +static SPARTN_CONSTEXPR uint8_t GLO_MAPPING[32] = { + X, // G1 C/A + X, // G2 C/A + X, // G3 + X, // G1 P + X, // G2 P + X, // G1a(D) + X, // G1a(P) + X, // G1a(D+P) + X, // G2a(I) + X, // G2a(P) + X, // G2a(I+P) + X, // G3 I + X, // G3 Q + X, // G3 I+Q + // Reserved + X, + X, + X, + X, + X, + X, + X, + X, + X, + X, +}; + +static SPARTN_CONSTEXPR double GLO_FREQ[24] = { + 1.0, // G1 C/A + 1.0, // G2 C/A + 1.0, // G3 + 1.0, // G1 P + 1.0, // G2 P + 1.0, // G1a(D) + 1.0, // G1a(P) + 1.0, // G1a(D+P) + 1.0, // G2a(I) + 1.0, // G2a(P) + 1.0, // G2a(I+P) + 1.0, // G3 I + 1.0, // G3 Q + 1.0, // G3 I+Q + // Reserved + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, +}; + +static SPARTN_CONSTEXPR const char* GLO_SIGNAL_NAMES[24] = { + "G1 C/A", + "G2 C/A", + "G3", + "G1 P", + "G2 P", + "G1a(D)", + "G1a(P)", + "G1a(D+P)", + "G2a(I)", + "G2a(P)", + "G2a(I+P)", + "G3 I", + "G3 Q", + "G3 I+Q", + // Reserved + "Reserved", + "Reserved", + "Reserved", + "Reserved", + "Reserved", + "Reserved", + "Reserved", + "Reserved", + "Reserved", +}; + +static SPARTN_CONSTEXPR uint8_t GAL_TO_SPARTN[24] = { + X, // E1 + X, // E5A + X, // E5B + X, // E6 + X, // E5A+E5B + 0, // E1 C No data -> L1C + X, // E1 A + X, // E1 B I/NAV OS/CS/SoL + X, // E1 B+C + X, // E1 A+B+C + X, // E6 C + X, // E6 A + X, // E6 B + X, // E6 B+C + X, // E6 A+B+C + X, // E5B I + 2, // E5B Q -> L7Q + X, // E5B I+Q + X, // E5(A+B) I + X, // E5(A+B) Q + X, // E5(A+B) I+Q + X, // E5A I + 1, // E5A Q -> L5Q + X, // E5A I+Q +}; + +static SPARTN_CONSTEXPR uint8_t GAL_MAPPING[32] = { + X, // E1 + X, // E5A + X, // E5B + X, // E6 + X, // E5A+E5B + X, // E1 C No data + X, // E1 A + X, // E1 B I/NAV OS/CS/SoL + 5, // E1 B+C + X, // E1 A+B+C + X, // E6 C + X, // E6 A + X, // E6 B + X, // E6 B+C + X, // E6 A+B+C + X, // E5B I + X, // E5B Q + X, // E5B I+Q + X, // E5(A+B) I + X, // E5(A+B) Q + 16, // E5(A+B) I+Q + X, // E5A I + X, // E5A Q + X, // E5A I+Q +}; + +#define GAL_E1 1575.420 +#define GAL_E6 1278.750 +#define GAL_E5 1191.795 +#define GAL_E5a 1176.450 +#define GAL_E5b 1207.140 +static SPARTN_CONSTEXPR double GAL_FREQ[24] = { + GAL_E1, // E1 + GAL_E5a, // E5A + GAL_E5b, // E5B + GAL_E6, // E6 + 0.0, // E5A+E5B + GAL_E1, // E1 C No data + GAL_E1, // E1 A + GAL_E1, // E1 B I/NAV OS/CS/SoL + GAL_E1, // E1 B+C + GAL_E1, // E1 A+B+C + GAL_E6, // E6 C + GAL_E6, // E6 A + GAL_E6, // E6 B + GAL_E6, // E6 B+C + GAL_E6, // E6 A+B+C + GAL_E5b, // E5B I + GAL_E5b, // E5B Q + GAL_E5b, // E5B I+Q + GAL_E5, // E5(A+B) I + GAL_E5, // E5(A+B) Q + GAL_E5, // E5(A+B) I+Q + GAL_E5a, // E5A I + GAL_E5a, // E5A Q + GAL_E5a, // E5A I+Q +}; + +static SPARTN_CONSTEXPR const char* GAL_SIGNAL_NAMES[24] = { + "E1", "E5A", "E5B", "E6", "E5A+E5B", "E1 C No data", + "E1 A", "E1 B I/NAV", "E1 B+C", "E1 A+B+C", "E6 C", "E6 A", + "E6 B", "E6 B+C", "E6 A+B+C", "E5B I", "E5B Q", "E5B I+Q", + "E5(A+B) I", "E5(A+B) Q", "E5(A+B) I+Q", "E5A I", "E5A Q", "E5A I+Q", +}; + +struct SystemMapping { + long gnss_id; + long signal_count; + uint8_t const* to_spartn; + uint8_t const* mapping; + double const* freq; + char const* const* signal_names; + + char const* signal_name(long signal_id) const { + if (signal_id < signal_count) return signal_names[signal_id]; + return "Unknown"; + } +}; + +static SPARTN_CONSTEXPR SystemMapping GPS_SM = { + GNSS_ID__gnss_id_gps, 24, GPS_TO_SPARTN, GPS_MAPPING, GPS_FREQ, GPS_SIGNAL_NAMES, +}; + +static SPARTN_CONSTEXPR SystemMapping GLO_SM = { + GNSS_ID__gnss_id_glonass, 24, GLO_TO_SPARTN, GLO_MAPPING, GLO_FREQ, GLO_SIGNAL_NAMES, +}; + +static SPARTN_CONSTEXPR SystemMapping GAL_SM = { + GNSS_ID__gnss_id_galileo, 24, GAL_TO_SPARTN, GAL_MAPPING, GAL_FREQ, GAL_SIGNAL_NAMES, +}; + +static SPARTN_CONSTEXPR const char* GPS_PHASE_BIAS_TYPES[4] = { + "L1C", + "L2W", + "L2L", + "L5Q", +}; + +static SPARTN_CONSTEXPR const char* GLO_PHASE_BIAS_TYPES[2] = { + "L1C", + "L2C", +}; + +static SPARTN_CONSTEXPR const char* GAL_PHASE_BIAS_TYPES[3] = { + "L1C", + "L5Q", + "L7Q", +}; + +static SPARTN_CONSTEXPR const char* GPS_CODE_BIAS_TYPES[4] = { + "C1C", + "C2W", + "C2L", + "C5Q", +}; + +static SPARTN_CONSTEXPR const char* GLO_CODE_BIAS_TYPES[2] = { + "C1C", + "C2C", +}; + +static SPARTN_CONSTEXPR const char* GAL_CODE_BIAS_TYPES[3] = { + "C1C", + "C5Q", + "C7Q", +}; + +static char const* bias_type_name(long gnss_id, bool is_phase, uint8_t type) { + if (gnss_id == GNSS_ID__gnss_id_gps) { + if (is_phase) { + if (type < 4) return GPS_PHASE_BIAS_TYPES[type]; + } else { + if (type < 4) return GPS_CODE_BIAS_TYPES[type]; + } + } else if (gnss_id == GNSS_ID__gnss_id_glonass) { + if (is_phase) { + if (type < 2) return GLO_PHASE_BIAS_TYPES[type]; + } else { + if (type < 2) return GLO_CODE_BIAS_TYPES[type]; + } + } else if (gnss_id == GNSS_ID__gnss_id_galileo) { + if (is_phase) { + if (type < 3) return GAL_PHASE_BIAS_TYPES[type]; + } else { + if (type < 3) return GAL_CODE_BIAS_TYPES[type]; + } + } + + return "Unknown"; +} + +#undef X diff --git a/generator/spartn2/data.hpp b/generator/spartn2/data.hpp index 871fcfd2..9c5e1c78 100644 --- a/generator/spartn2/data.hpp +++ b/generator/spartn2/data.hpp @@ -118,6 +118,9 @@ struct OcbCorrections { // this is the union of all satellite ids that have at least // one correction type. std::vector satellites() const; + + // Check if there is _any_ correction for the given satellite. + bool has_satellite(long id) const; }; struct OcbData { diff --git a/generator/spartn2/generator.cpp b/generator/spartn2/generator.cpp index a295937a..417a453f 100644 --- a/generator/spartn2/generator.cpp +++ b/generator/spartn2/generator.cpp @@ -12,13 +12,13 @@ #include #include #include +#include #include #include #include #include #include #include -#include #pragma GCC diagnostic pop #include @@ -28,10 +28,11 @@ namespace generator { namespace spartn { Generator::Generator() - : mGenerationIndex(0), mNextAreaId(1), mUraOverride(-1), mContinuityIndicator(-1), - mUBloxClockCorrection(false), mIonosphereQualityOverride(-1), - mIonosphereQualityDefault(0 /* SF055(0) = invalid */), mComputeAverageZenithDelay(false), - mGroupByEpochTime(false), mIodeShift(true), mIncreasingSiou(false), mSiouIndex(1), + : mGenerationIndex(0), mNextAreaId(0), mUraOverride(-1), mContinuityIndicator(-1), + mUBloxClockCorrection(false), mSf055Override(-1), mSf055Default(0 /* SF055(0) = invalid */), + mSf042Override(-1), mSf042Default(7 /* SF042(7) = >0.320m */), + mComputeAverageZenithDelay(false), mGroupByEpochTime(false), mIodeShift(true), + mIncreasingSiou(false), mSiouIndex(1), mFilterByOcb(false), mIgnoreL2L(false), mGenerateGad(true), mGenerateOcb(true), mGenerateHpac(true), mGpsSupported(true), mGlonassSupported(true), mGalileoSupported(true), mBeidouSupported(false) {} @@ -64,7 +65,7 @@ std::vector Generator::generate(LPP_Message const* lpp_message) { auto iods = mCorrectionData->iods(); for (auto iod : iods) { #ifdef SPARTN_DEBUG_PRINT - printf("-- iod=%ld\n", iod); + printf("-- iod=%hu\n", iod); #endif auto ocb = mCorrectionData->ocb(iod); @@ -86,7 +87,7 @@ std::vector Generator::generate(LPP_Message const* lpp_message) { if (mCorrectionData->find_gad_epoch_time(iod, &gad_epoch_time)) { for (auto set_id : set_ids) { #ifdef SPARTN_DEBUG_PRINT - printf("GAD: set=%ld, iod=%ld, time=%u\n", set_id, iod, + printf("GAD: set=%hu, iod=%hu, time=%u\n", set_id, iod, gad_epoch_time.rounded_seconds); #endif @@ -94,7 +95,7 @@ std::vector Generator::generate(LPP_Message const* lpp_message) { } } else { #ifdef SPARTN_DEBUG_PRINT - printf("GAD: no epoch time for iod=%ld\n", iod); + printf("GAD: no epoch time for iod=%hu\n", iod); #endif } } diff --git a/generator/spartn2/hpac.cpp b/generator/spartn2/hpac.cpp index 177f84f8..2f0969bd 100644 --- a/generator/spartn2/hpac.cpp +++ b/generator/spartn2/hpac.cpp @@ -12,10 +12,10 @@ #include #include #include -#include -#include -#include #include +#include +#include +#include #include #pragma GCC diagnostic pop @@ -264,14 +264,14 @@ static double compute_average_zentith_delay(GNSS_SSR_GriddedCorrection_r16 const } static void troposphere_data_block(MessageBuilder& builder, - GNSS_SSR_GriddedCorrection_r16 const& data, int ura_override, - bool use_average_zenith_delay) { + GNSS_SSR_GriddedCorrection_r16 const& data, int sf042_override, + int sf042_default, bool use_average_zenith_delay, bool calculate_sf051) { // NOTE(ewasjon): Use a polynomial of degree 0, as we don't have a polynomial in 3GPP // LPP. This will result in a constant value for the troposphere correction. builder.sf041(0 /* T_00 */); - if (ura_override >= 0) { - uint8_t value = ura_override > 7 ? 7 : static_cast(ura_override); + if (sf042_override >= 0) { + uint8_t value = sf042_override > 7 ? 7 : static_cast(sf042_override); builder.sf042_raw(value); } else if (data.troposphericDelayQualityIndicator_r16) { // TODO(ewasjon): Refactor as function in decode namespace @@ -282,7 +282,7 @@ static void troposphere_data_block(MessageBuilder& builder auto q_meter = q / 1000.0; builder.sf042(q_meter); } else { - builder.sf042_raw(7); // TODO(ewasjon): Add a comment about why we are using 7 + builder.sf042_raw(sf042_default); } // NOTE(ewasjon): SPARTN have an average hydrostatic delay for all grid points. 3GPP LPP only @@ -320,11 +320,11 @@ static void troposphere_data_block(MessageBuilder& builder } if (within_range(-0.252, 0.252, average_zenith_delay)) { - builder.sf044(0); // Small coefficient block - builder.sf045(average_zenith_delay); // T_00 + builder.sf044(0); // Small coefficient block + average_zenith_delay = builder.sf045(average_zenith_delay); // T_00 } else if (within_range(-1.020, 1.020, average_zenith_delay)) { - builder.sf044(1); // Small coefficient block - builder.sf048(average_zenith_delay); // T_00 + builder.sf044(1); // Small coefficient block + average_zenith_delay = builder.sf048(average_zenith_delay); // T_00 } else { builder.sf044(0); // Small coefficient block builder.sf045(0.0); // T_0 @@ -335,8 +335,28 @@ static void troposphere_data_block(MessageBuilder& builder printf(" average_zenith_delay: %f\n", average_zenith_delay); #endif - // TODO(ewasjon): [low-priority] Compute the minimum residual field size for all grid points. - builder.sf051(1); // Large residuals + // Compute the residual field size for all grid points + auto residual_field_size = 0; + if (calculate_sf051) { + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (!element->tropospericDelayCorrection_r16) continue; + + auto& grid_point = *element->tropospericDelayCorrection_r16; + auto residual = decode::tropoWetVerticalDelay_r16(grid_point.tropoWetVerticalDelay_r16); + auto total_residual = residual - average_zenith_delay; + if (within_range(-0.124, 0.124, total_residual)) + residual_field_size = std::max(residual_field_size, 0U); + else + residual_field_size = std::max(residual_field_size, 1U); + } + } else { + // If we're not calculating the residual field size, always use the largest size + residual_field_size = 1; + } + + builder.sf051(residual_field_size); for (int i = 0; i < list.count; i++) { auto element = list.array[i]; @@ -345,13 +365,20 @@ static void troposphere_data_block(MessageBuilder& builder #ifdef SPARTN_DEBUG_PRINT printf(" grid[%2d] = invalid\n", i); #endif - builder.sf053_invalid(); + if (residual_field_size == 0) + builder.sf052_invalid(); + else + builder.sf053_invalid(); } else { auto& grid_point = *element->tropospericDelayCorrection_r16; auto residual = decode::tropoWetVerticalDelay_r16(grid_point.tropoWetVerticalDelay_r16); - builder.sf053(residual - average_zenith_delay); + auto total_residual = residual - average_zenith_delay; + if (residual_field_size == 0) + builder.sf052(total_residual); + else + builder.sf053(total_residual); #ifdef SPARTN_DEBUG_PRINT - printf(" grid[%2d] = %f\n", i, residual - average_zenith_delay); + printf(" grid[%2d] = %f\n", i, total_residual); #endif } } @@ -421,11 +448,20 @@ static void ionosphere_data_block_1(MessageBuilder& builder, HpacSatellite const if (sf055_override >= 0) { builder.sf055_raw(sf055_override); +#ifdef SPARTN_DEBUG_PRINT + printf(" sf055: %d [override]\n", sf055_override); +#endif } else { auto q = decode::stecQualityIndicator_r16(element->stecQualityIndicator_r16); if (q.invalid) { +#ifdef SPARTN_DEBUG_PRINT + printf(" sf055: %d [default]\n", sf055_default); +#endif builder.sf055_raw(sf055_default); } else { +#ifdef SPARTN_DEBUG_PRINT + printf(" sf055: %f\n", q.value); +#endif builder.sf055(q.value); } } @@ -499,7 +535,7 @@ static void ionosphere_data_block_2(MessageBuilder& builder, long grid_points, } static void ionosphere_data_block(MessageBuilder& builder, CorrectionPointSet& correction_point_set, - HpacCorrections& corrections, long gnss_id, + HpacCorrections& corrections, long gnss_id, OcbCorrections* ocb, int ionosphere_block_type, int sf055_override, int sf055_default) { auto satellites = corrections.satellites(); @@ -508,9 +544,19 @@ static void ionosphere_data_block(MessageBuilder& builder, CorrectionPointSet& c // NOTE(ewasjon): Remove all satellites that does not have STEC corrections. // TODO(ewasjon): [low-priority] Add _WHY_ we're removing satellites without STEC auto it = satellites.begin(); - for (; it != satellites.end(); it++) { + for (; it != satellites.end();) { if (!it->stec) { +#ifdef SPARTN_DEBUG_PRINT + printf(" removed satellite=%u [stec]\n", it->prn()); +#endif + it = satellites.erase(it); + } else if (ocb && !ocb->has_satellite(it->id)) { +#ifdef SPARTN_DEBUG_PRINT + printf(" removed satellite=%u [ocb]\n", it->prn()); +#endif it = satellites.erase(it); + } else { + it++; } } } @@ -522,6 +568,10 @@ static void ionosphere_data_block(MessageBuilder& builder, CorrectionPointSet& c for (auto& satellite : satellites) { if (!satellite.stec) continue; +#ifdef SPARTN_DEBUG_PRINT + printf(" satellite=%u\n", satellite.prn()); +#endif + ionosphere_data_block_1(builder, satellite, equation_type, sf055_override, sf055_default); if (ionosphere_block_type == 2) { @@ -534,6 +584,8 @@ void Generator::generate_hpac(uint16_t iod) { auto hpac_data = mCorrectionData->hpac(iod); if (!hpac_data) return; + auto ocb_data = mCorrectionData->ocb(iod); + std::vector messages; for (auto& kvp : hpac_data->mKeyedCorrections) { if (!mGpsSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_gps) continue; @@ -559,8 +611,20 @@ void Generator::generate_hpac(uint16_t iod) { if (cps_it == mCorrectionPointSets.end()) continue; auto& correction_point_set = *(cps_it->second.get()); + OcbCorrections* ocb_corrections = nullptr; + if (ocb_data && mFilterByOcb) { + auto ocb_key = OcbKey{gnss_id, epoch_time}; + if (!mGroupByEpochTime) { + ocb_key.epoch_time = 0; + } + auto ocb_it = ocb_data->mKeyedCorrections.find(ocb_key); + if (ocb_it != ocb_data->mKeyedCorrections.end()) { + ocb_corrections = &ocb_it->second; + } + } + #ifdef SPARTN_DEBUG_PRINT - printf("HPAC: time=%u, set=%ld, gnss=%ld, iod=%ld\n", epoch_time, set_id, gnss_id, iod); + printf("HPAC: time=%u, set=%hu, gnss=%ld, iod=%hu\n", epoch_time, set_id, gnss_id, iod); printf(" area_id=%u\n", correction_point_set.area_id); #endif @@ -593,15 +657,17 @@ void Generator::generate_hpac(uint16_t iod) { // Troposphere data block if (troposphere_block_type != 0) { - troposphere_data_block(builder, *corrections.gridded, mUraOverride, - mComputeAverageZenithDelay); + // TODO(ewasjon): [low-priority] Expose this as a option + auto calculate_sf051 = mComputeAverageZenithDelay; + troposphere_data_block(builder, *corrections.gridded, mSf042Override, mSf042Default, + mComputeAverageZenithDelay, calculate_sf051); } // Ionosphere data block if (ionosphere_block_type != 0) { ionosphere_data_block(builder, correction_point_set, corrections, gnss_id, - ionosphere_block_type, mIonosphereQualityOverride, - mIonosphereQualityDefault); + ocb_corrections, ionosphere_block_type, mSf055Override, + mSf055Default); } } diff --git a/generator/spartn2/include/generator/spartn2/generator.hpp b/generator/spartn2/include/generator/spartn2/generator.hpp index 98d37ce1..bbcbb7e0 100644 --- a/generator/spartn2/include/generator/spartn2/generator.hpp +++ b/generator/spartn2/include/generator/spartn2/generator.hpp @@ -59,11 +59,15 @@ class Generator { } void set_iode_shift(bool iode_shift) { mIodeShift = iode_shift; } - void set_ionosphere_quality_override(int ionosphere_quality_override) { - mIonosphereQualityOverride = ionosphere_quality_override; - } + void set_sf055_override(int sf055_override) { mSf055Override = sf055_override; } + void set_sf055_default(int sf055_default) { mSf055Default = sf055_default; } + + void set_sf042_override(int sf042_override) { mSf042Override = sf042_override; } + void set_sf042_default(int sf042_default) { mSf042Default = sf042_default; } void set_increasing_siou(bool increasing_siou) { mIncreasingSiou = increasing_siou; } + void set_filter_by_ocb(bool filter_by_ocb) { mFilterByOcb = filter_by_ocb; } + void set_ignore_l2l(bool ignore_l2l) { mIgnoreL2L = ignore_l2l; } void set_generate_ocb(bool generate_ocb) { mGenerateOcb = generate_ocb; } void set_generate_hpac(bool generate_hpac) { mGenerateHpac = generate_hpac; } @@ -106,9 +110,10 @@ class Generator { double mContinuityIndicator; // <0 = no override bool mUBloxClockCorrection; - // SF055: - int mIonosphereQualityOverride; // <0 = no override - int mIonosphereQualityDefault; + int mSf055Override; // <0 = no override + int mSf055Default; + int mSf042Override; // <0 = no override + int mSf042Default; bool mComputeAverageZenithDelay; bool mGroupByEpochTime; @@ -116,6 +121,9 @@ class Generator { bool mIncreasingSiou; uint16_t mSiouIndex; + bool mFilterByOcb; + bool mIgnoreL2L; + bool mGenerateGad; bool mGenerateOcb; bool mGenerateHpac; diff --git a/generator/spartn2/message.hpp b/generator/spartn2/message.hpp index da6e3cc3..8dba4bde 100644 --- a/generator/spartn2/message.hpp +++ b/generator/spartn2/message.hpp @@ -379,22 +379,22 @@ class MessageBuilder { inline void sf044(uint8_t size) { mBuilder.bits(size, 1); } // SF045 - Small troposphere coefficient T_00 - inline void sf045(double value) { mBuilder.double_to_bits(-0.252, 0.252, 0.004, value, 7); } + inline double sf045(double value) { return mBuilder.double_to_bits(-0.252, 0.252, 0.004, value, 7); } // SF046 - Troposphere polynomial coefficient T_10/T_01 - inline void sf046(double value) { mBuilder.double_to_bits(-0.063, 0.063, 0.001, value, 7); } + inline double sf046(double value) { return mBuilder.double_to_bits(-0.063, 0.063, 0.001, value, 7); } // SF047 - Small troposphere coefficient T_11 - inline void sf047(double value) { mBuilder.double_to_bits(-0.051, 0.051, 0.0002, value, 9); } + inline double sf047(double value) { return mBuilder.double_to_bits(-0.051, 0.051, 0.0002, value, 9); } // SF048 - Large troposphere coefficient T_00 - inline void sf048(double value) { mBuilder.double_to_bits(-1.020, 1.020, 0.004, value, 9); } + inline double sf048(double value) { return mBuilder.double_to_bits(-1.020, 1.020, 0.004, value, 9); } // SF049 - Large troposphere coefficient T_10/T_01 - inline void sf049(double value) { mBuilder.double_to_bits(-0.255, 0.255, 0.001, value, 9); } + inline double sf049(double value) { return mBuilder.double_to_bits(-0.255, 0.255, 0.001, value, 9); } // SF050 - Large troposphere coefficient T_11 - inline void sf050(double value) { mBuilder.double_to_bits(-0.2046, 0.2046, 0.0002, value, 11); } + inline double sf050(double value) { return mBuilder.double_to_bits(-0.2046, 0.2046, 0.0002, value, 11); } // SF051 - Troposphere residual field size inline void sf051(uint8_t size) { mBuilder.bits(size, 1); } diff --git a/generator/spartn2/ocb.cpp b/generator/spartn2/ocb.cpp index 6899b758..c46556ff 100644 --- a/generator/spartn2/ocb.cpp +++ b/generator/spartn2/ocb.cpp @@ -1,3 +1,4 @@ +#include "constant.hpp" #include "data.hpp" #include "decode.hpp" #include "generator.hpp" @@ -158,6 +159,55 @@ std::vector OcbCorrections::satellites() const { return result; } +bool OcbCorrections::has_satellite(long id) const { + if (orbit) { + auto& list = orbit->ssr_OrbitCorrectionList_r15.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (element->svID_r15.satellite_id == id) return true; + } + } + + if (clock) { + auto& list = clock->ssr_ClockCorrectionList_r15.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (element->svID_r15.satellite_id == id) return true; + } + } + + if (code_bias) { + auto& list = code_bias->ssr_CodeBiasSatList_r15.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (element->svID_r15.satellite_id == id) return true; + } + } + + if (phase_bias) { + auto& list = phase_bias->ssr_PhaseBiasSatList_r16.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (element->svID_r16.satellite_id == id) return true; + } + } + + if (ura) { + auto& list = ura->ssr_URA_SatList_r16.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (element->svID_r16.satellite_id == id) return true; + } + } + + return false; +} + void CorrectionData::add_correction(long gnss_id, GNSS_SSR_OrbitCorrections_r15* orbit) { if (!orbit) return; auto iod = static_cast(orbit->iod_ssr_r15); @@ -250,264 +300,41 @@ struct Bias { static Bias invalid() { return Bias{-1, 0.0, 0.0, false, 0, false}; } }; -#define X 255 - -static SPARTN_CONSTEXPR uint8_t GPS_TO_SPARTN[24] = { - 0, // L1 C/A -> L1C - X, // L1C - X, // L2C - X, // L5 - X, // L1 P - X, // L1 Z-tracking - X, // L2 C/A - X, // L2 P - 1, // L2 Z-tracking -> L2W - X, // L2 L2C(M) - 2, // L2 L2C(L) -> L2L - X, // L2 L2C(M+L) - X, // L5 I - 3, // L5 Q -> L5Q - X, // L5 I+Q - X, // L1 L1C(D) - X, // L1 L1C(P) - X, // L1 L1C(D+P) - // Reserved - X, - X, - X, - X, - X, - X, -}; - -static SPARTN_CONSTEXPR uint8_t GPS_MAPPING[32] = { - X, // L1 C/A - X, // L1C - X, // L2C - X, // L5 - X, // L1 P - X, // L1 Z-tracking - X, // L2 C/A - X, // L2 P - X, // L2 Z-tracking - X, // L2 L2C(M) - X, // L2 L2C(L) - 10, // L2 L2C(M+L) -> L2 L2C(L) - X, // L5 I - X, // L5 Q - X, // L5 I+Q - X, // L1 L1C(D) - X, // L1 L1C(P) - X, // L1 L1C(D+P) - // Reserved - X, - X, - X, - X, - X, - X, -}; - -#define GPS_L1 1575.42 -#define GPS_L2 1227.60 -#define GPS_L5 1176.45 -static SPARTN_CONSTEXPR double GPS_FREQ[24] = { - GPS_L1, // L1 C/A - GPS_L1, // L1C - GPS_L2, // L2C - GPS_L5, // L5 - GPS_L1, // L1 P - GPS_L1, // L1 Z-tracking - GPS_L2, // L2 C/A - GPS_L2, // L2 P - GPS_L2, // L2 Z-tracking - GPS_L2, // L2 L2C(M) - GPS_L2, // L2 L2C(L) - GPS_L2, // L2 L2C(M+L) - GPS_L5, // L5 I - GPS_L5, // L5 Q - GPS_L5, // L5 I+Q - GPS_L1, // L1 L1C(D) - GPS_L1, // L1 L1C(P) - GPS_L1, // L1 L1C(D+P) - // Reserved - 0, - 0, - 0, - 0, - 0, - 0, -}; - -static Bias gps_bias_from_signal(long signal_id, double correction, double continuity_indicator, - bool fix_flag) { - if (signal_id >= 24) { +static Bias bias_from_signal(SystemMapping const* mapping, bool is_phase, long from_id, + double correction, double continuity_indicator, bool fix_flag) { + if (from_id >= mapping->signal_count) { return Bias::invalid(); } - auto type = GPS_TO_SPARTN[signal_id]; - if (type == X) { - auto from_id = signal_id; - auto to_id = GPS_MAPPING[from_id]; - if (to_id == X) { + auto type = mapping->to_spartn[from_id]; + if (type == INVALID_MAPPING) { + auto to_id = mapping->mapping[from_id]; + if (to_id == INVALID_MAPPING) { return Bias::invalid(); - } - - auto from_freq = GPS_FREQ[from_id]; - auto to_freq = GPS_FREQ[to_id]; - auto scale = from_freq / to_freq; - auto shifted_correction = correction * scale; -#ifdef SPARTN_DEBUG_PRINT - printf(" GPS: mapping signal id %2ld to %2u (%.2f / %.2f = %.4f)\n", from_id, to_id, - from_freq, to_freq, scale); -#endif - - return gps_bias_from_signal(to_id, shifted_correction, continuity_indicator, fix_flag); - } - - return Bias{signal_id, correction, continuity_indicator, fix_flag, type, false}; -} - -static Bias glo_bias_from_signal(long signal_id, double correction, double continuity_indicator, - bool fix_flag) { - if (signal_id >= 32) { - return Bias::invalid(); - } - - static SPARTN_CONSTEXPR uint8_t MAPPABLE[32] = { - 1, 2, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // - 0, 0, - }; - - auto type = MAPPABLE[signal_id]; - if (type > 0) { - return Bias{ - signal_id, correction, continuity_indicator, fix_flag, static_cast(type - 1), - false}; - } else { - return Bias::invalid(); - } -} - -static SPARTN_CONSTEXPR uint8_t GAL_TO_SPARTN[24] = { - X, // E1 - X, // E5A - X, // E5B - X, // E6 - X, // E5A+E5B - 0, // E1 C No data -> L1C - X, // E1 A - X, // E1 B I/NAV OS/CS/SoL - X, // E1 B+C - X, // E1 A+B+C - X, // E6 C - X, // E6 A - X, // E6 B - X, // E6 B+C - X, // E6 A+B+C - X, // E5B I - 2, // E5B Q -> L7Q - X, // E5B I+Q - X, // E5(A+B) I - X, // E5(A+B) Q - X, // E5(A+B) I+Q - X, // E5A I - 1, // E5A Q -> L5Q - X, // E5A I+Q -}; - -static SPARTN_CONSTEXPR uint8_t GAL_MAPPING[32] = { - X, // E1 - X, // E5A - X, // E5B - X, // E6 - X, // E5A+E5B - X, // E1 C No data - X, // E1 A - X, // E1 B I/NAV OS/CS/SoL - 5, // E1 B+C - X, // E1 A+B+C - X, // E6 C - X, // E6 A - X, // E6 B - X, // E6 B+C - X, // E6 A+B+C - X, // E5B I - X, // E5B Q - X, // E5B I+Q - X, // E5(A+B) I - X, // E5(A+B) Q - 16, // E5(A+B) I+Q - X, // E5A I - X, // E5A Q - X, // E5A I+Q -}; - -#define GAL_E1 1575.420 -#define GAL_E6 1278.750 -#define GAL_E5 1191.795 -#define GAL_E5a 1176.450 -#define GAL_E5b 1207.140 -static SPARTN_CONSTEXPR double GAL_FREQ[24] = { - GAL_E1, // E1 - GAL_E5a, // E5A - GAL_E5b, // E5B - GAL_E6, // E6 - 0.0, // E5A+E5B - GAL_E1, // E1 C No data - GAL_E1, // E1 A - GAL_E1, // E1 B I/NAV OS/CS/SoL - GAL_E1, // E1 B+C - GAL_E1, // E1 A+B+C - GAL_E6, // E6 C - GAL_E6, // E6 A - GAL_E6, // E6 B - GAL_E6, // E6 B+C - GAL_E6, // E6 A+B+C - GAL_E5b, // E5B I - GAL_E5b, // E5B Q - GAL_E5b, // E5B I+Q - GAL_E5, // E5(A+B) I - GAL_E5, // E5(A+B) Q - GAL_E5, // E5(A+B) I+Q - GAL_E5a, // E5A I - GAL_E5a, // E5A Q - GAL_E5a, // E5A I+Q -}; - -static Bias gal_bias_from_signal(long signal_id, double correction, double continuity_indicator, - bool fix_flag) { - if (signal_id >= 24) { - return Bias::invalid(); - } - - auto type = GAL_TO_SPARTN[signal_id]; - if (type == X) { - auto from_id = signal_id; - auto to_id = GAL_MAPPING[from_id]; - if (to_id == X) { + } else if (to_id >= mapping->signal_count) { return Bias::invalid(); } - auto from_freq = GAL_FREQ[from_id]; - auto to_freq = GAL_FREQ[to_id]; + auto from_freq = mapping->freq[from_id]; + auto to_freq = mapping->freq[to_id]; auto scale = from_freq / to_freq; auto shifted_correction = correction * scale; #ifdef SPARTN_DEBUG_PRINT - printf(" GAL: mapping signal id %2ld to %2u (%.2f / %.2f = %.4f)\n", from_id, to_id, - from_freq, to_freq, scale); + printf(" from: %2ld '%-16s' %7.2f\n", from_id, mapping->signal_name(from_id), + correction); + printf(" to: %2hhu '%-16s' %7.2f\n", to_id, mapping->signal_name(to_id), + shifted_correction); #endif - return gal_bias_from_signal(to_id, shifted_correction, continuity_indicator, fix_flag); + auto new_bias = bias_from_signal(mapping, is_phase, to_id, shifted_correction, + continuity_indicator, fix_flag); + new_bias.mapped = true; + return new_bias; } - return Bias{signal_id, correction, continuity_indicator, fix_flag, type, false}; + return Bias{from_id, correction, continuity_indicator, fix_flag, type, false}; } -#undef X - static bool phase_bias_fix_flag(SSR_PhaseBiasSignalElement_r16 const& signal) { if (!signal.phaseBiasIntegerIndicator_r16) { // TODO(ewasjon): What should we do here if the fix flag information is missing? The @@ -524,11 +351,14 @@ static bool phase_bias_fix_flag(SSR_PhaseBiasSignalElement_r16 const& signal) { } template -static std::map phase_biases(SSR_PhaseBiasSatElement_r16 const& satellite, - BiasToSignal* bias_to_signal) { +static std::map phase_biases(SystemMapping const* mapping, + SSR_PhaseBiasSatElement_r16 const& satellite, + BiasToSignal* bias_to_signal, bool ignore_l2l) { std::map biases_by_type; + printf(" PHASE BIAS:\n"); auto& list = satellite.ssr_PhaseBiasSignalList_r16.list; + for (int i = 0; i < list.count; i++) { auto element = list.array[i]; if (!element) continue; @@ -539,31 +369,46 @@ static std::map phase_biases(SSR_PhaseBiasSatElement_r16 const& s 320.0; // TODO(ewasjon): [low-priority] Compute the continuity indicator. auto fix_flag = phase_bias_fix_flag(*element); - auto bias = (*bias_to_signal)(signal_id, correction, continuity_indicator, fix_flag); + auto bias = + (*bias_to_signal)(mapping, true, signal_id, correction, continuity_indicator, fix_flag); if (bias.signal_id == -1) { +#if defined(SPARTN_DEBUG_PRINT) && SPARTN_DEBUG_PRINT > 1 + printf(" ? %2ld '%-16s' %7.2f\n", signal_id, mapping->signal_name(signal_id), + correction); +#endif + continue; + } + + if (mapping->gnss_id == GNSS_ID__gnss_id_gps && bias.type == 2 && ignore_l2l) { #ifdef SPARTN_DEBUG_PRINT - printf(" unsupported bias for signal id %2ld\n", signal_id); + printf(" -%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, true, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif continue; } if (biases_by_type.count(bias.type) == 0) { #ifdef SPARTN_DEBUG_PRINT - printf(" adding bias type %u (id=%ld)\n", bias.type, bias.signal_id); + printf(" +%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, true, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif biases_by_type[bias.type] = bias; } else if (!bias.mapped && biases_by_type[bias.type].mapped) { #ifdef SPARTN_DEBUG_PRINT // If the bias is mapped and the new bias is not mapped, then we want to prioritize // the "original" bias. - printf(" replacing bias type %u (id=%ld) with %u (id=%ld)\n", - biases_by_type[bias.type].type, biases_by_type[bias.type].signal_id, bias.type, - bias.signal_id); + printf(" =%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, true, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif biases_by_type[bias.type] = bias; } else { #ifdef SPARTN_DEBUG_PRINT - printf(" duplicate bias type %u\n", bias.type); + printf(" !%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, true, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif } } @@ -572,10 +417,13 @@ static std::map phase_biases(SSR_PhaseBiasSatElement_r16 const& s } template -static std::map code_biases(SSR_CodeBiasSatElement_r15 const& satellite, - BiasToSignal* bias_to_signal) { +static std::map code_biases(SystemMapping const* mapping, + SSR_CodeBiasSatElement_r15 const& satellite, + BiasToSignal* bias_to_signal, bool ignore_l2l) { std::map biases_by_type; + printf(" CODE BIAS:\n"); + auto& list = satellite.ssr_CodeBiasSignalList_r15.list; for (int i = 0; i < list.count; i++) { auto element = list.array[i]; @@ -584,31 +432,45 @@ static std::map code_biases(SSR_CodeBiasSatElement_r15 const& sat auto signal_id = decode::signal_id(element->signal_and_tracking_mode_ID_r15); auto correction = decode::codeBias_r15(element->codeBias_r15); - auto bias = (*bias_to_signal)(signal_id, correction, 0.0, false); + auto bias = (*bias_to_signal)(mapping, false, signal_id, correction, 0.0, false); if (bias.signal_id == -1) { +#if defined(SPARTN_DEBUG_PRINT) && SPARTN_DEBUG_PRINT > 1 + printf(" ? %2ld '%-16s' %7.2f\n", signal_id, mapping->signal_name(signal_id), + correction); +#endif + continue; + } + + if (mapping->gnss_id == GNSS_ID__gnss_id_gps && bias.type == 2 && ignore_l2l) { #ifdef SPARTN_DEBUG_PRINT - printf(" unsupported bias for signal id %2ld\n", signal_id); + printf(" -%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, true, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif continue; } if (biases_by_type.count(bias.type) == 0) { #ifdef SPARTN_DEBUG_PRINT - printf(" adding bias type %u (id=%ld)\n", bias.type, bias.signal_id); + printf(" +%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, false, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif biases_by_type[bias.type] = bias; } else if (!bias.mapped && biases_by_type[bias.type].mapped) { #ifdef SPARTN_DEBUG_PRINT // If the bias is mapped and the new bias is not mapped, then we want to prioritize // the "original" bias. - printf(" replacing bias type %u (id=%ld) with %u (id=%ld)\n", - biases_by_type[bias.type].type, biases_by_type[bias.type].signal_id, bias.type, - bias.signal_id); + printf(" =%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, false, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif biases_by_type[bias.type] = bias; } else { #ifdef SPARTN_DEBUG_PRINT - printf(" duplicate bias type %u\n", bias.type); + printf(" !%s (%u) %2ld '%-16s' %7.2f\n", + bias_type_name(mapping->gnss_id, false, bias.type), bias.type, bias.signal_id, + mapping->signal_name(bias.signal_id), bias.correction); #endif } } @@ -616,13 +478,14 @@ static std::map code_biases(SSR_CodeBiasSatElement_r15 const& sat return biases_by_type; } -static void generate_gps_bias_block(MessageBuilder& builder, +static void generate_gps_bias_block(MessageBuilder& builder, SystemMapping const* mapping, SSR_CodeBiasSatElement_r15 const* code_bias, - SSR_PhaseBiasSatElement_r16 const* phase_bias) { + SSR_PhaseBiasSatElement_r16 const* phase_bias, + bool ignore_l2l) { if (!phase_bias) { builder.sf025_raw(false, 0); } else { - auto types_of_biases = phase_biases(*phase_bias, &gps_bias_from_signal); + auto types_of_biases = phase_biases(mapping, *phase_bias, &bias_from_signal, ignore_l2l); builder.sf025(&types_of_biases); for (auto& kvp : types_of_biases) { @@ -635,7 +498,7 @@ static void generate_gps_bias_block(MessageBuilder& builder, if (!code_bias) { builder.sf027_raw(false, 0); } else { - auto types_of_biases = code_biases(*code_bias, &gps_bias_from_signal); + auto types_of_biases = code_biases(mapping, *code_bias, &bias_from_signal, ignore_l2l); builder.sf027(&types_of_biases); for (auto& kvp : types_of_biases) { @@ -644,13 +507,14 @@ static void generate_gps_bias_block(MessageBuilder& builder, } } -static void generate_glo_bias_block(MessageBuilder& builder, +static void generate_glo_bias_block(MessageBuilder& builder, SystemMapping const* mapping, SSR_CodeBiasSatElement_r15 const* code_bias, - SSR_PhaseBiasSatElement_r16 const* phase_bias) { + SSR_PhaseBiasSatElement_r16 const* phase_bias, + bool ignore_l2l) { if (!phase_bias) { builder.sf026_raw(false, 0); } else { - auto types_of_biases = phase_biases(*phase_bias, &glo_bias_from_signal); + auto types_of_biases = phase_biases(mapping, *phase_bias, &bias_from_signal, ignore_l2l); builder.sf026(&types_of_biases); for (auto& kvp : types_of_biases) { @@ -663,7 +527,7 @@ static void generate_glo_bias_block(MessageBuilder& builder, if (!code_bias) { builder.sf028_raw(false, 0); } else { - auto types_of_biases = code_biases(*code_bias, &glo_bias_from_signal); + auto types_of_biases = code_biases(mapping, *code_bias, &bias_from_signal, ignore_l2l); builder.sf028(&types_of_biases); for (auto& kvp : types_of_biases) { @@ -672,13 +536,14 @@ static void generate_glo_bias_block(MessageBuilder& builder, } } -static void generate_gal_bias_block(MessageBuilder& builder, +static void generate_gal_bias_block(MessageBuilder& builder, SystemMapping const* mapping, SSR_CodeBiasSatElement_r15 const* code_bias, - SSR_PhaseBiasSatElement_r16 const* phase_bias) { + SSR_PhaseBiasSatElement_r16 const* phase_bias, + bool ignore_l2l) { if (!phase_bias) { builder.sf102_raw(false, 0); } else { - auto types_of_biases = phase_biases(*phase_bias, &gal_bias_from_signal); + auto types_of_biases = phase_biases(mapping, *phase_bias, &bias_from_signal, ignore_l2l); builder.sf102(&types_of_biases); for (auto& kvp : types_of_biases) { @@ -691,7 +556,7 @@ static void generate_gal_bias_block(MessageBuilder& builder, if (!code_bias) { builder.sf105_raw(false, 0); } else { - auto types_of_biases = code_biases(*code_bias, &gal_bias_from_signal); + auto types_of_biases = code_biases(mapping, *code_bias, &bias_from_signal, ignore_l2l); builder.sf105(&types_of_biases); for (auto& kvp : types_of_biases) { @@ -727,7 +592,7 @@ void Generator::generate_ocb(uint16_t iod) { auto satellites = corrections.satellites(); #ifdef SPARTN_DEBUG_PRINT - printf("OCB: time=%u, gnss=%ld, iod=%ld\n", epoch_time, gnss_id, iod); + printf("OCB: time=%u, gnss=%ld, iod=%hu\n", epoch_time, gnss_id, iod); for (auto& satellite : satellites) { printf(" satellite: %4ld ", satellite.id); if (satellite.orbit) { @@ -826,7 +691,7 @@ void Generator::generate_ocb(uint16_t iod) { auto c2 = decode::delta_Clock_C1_r15(clock.delta_Clock_C2_r15); // t_0 = epochTime + (0.5 * ssrUpdateInterval) - // TODO(ewasjon): [low-priority] Include SSR update interval.This is fine not to + // TODO(ewasjon): [low-priority] Include SSR update interval. This is fine not to // include while we are using t=t0. auto t0 = corrections.epoch_time.seconds; // TODO(ewasjon): [low-priority] We don't have an actual time available, is it @@ -862,13 +727,16 @@ void Generator::generate_ocb(uint16_t iod) { if (satellite.code_bias || satellite.phase_bias) { switch (gnss_id) { case GNSS_ID__gnss_id_gps: - generate_gps_bias_block(builder, satellite.code_bias, satellite.phase_bias); + generate_gps_bias_block(builder, &GPS_SM, satellite.code_bias, + satellite.phase_bias, mIgnoreL2L); break; case GNSS_ID__gnss_id_glonass: - generate_glo_bias_block(builder, satellite.code_bias, satellite.phase_bias); + generate_glo_bias_block(builder, &GLO_SM, satellite.code_bias, + satellite.phase_bias, mIgnoreL2L); break; case GNSS_ID__gnss_id_galileo: - generate_gal_bias_block(builder, satellite.code_bias, satellite.phase_bias); + generate_gal_bias_block(builder, &GAL_SM, satellite.code_bias, + satellite.phase_bias, mIgnoreL2L); break; default: SPARTN_UNREACHABLE(); } diff --git a/receiver/nmea/gga.cpp b/receiver/nmea/gga.cpp index 56639cb9..511b7e94 100644 --- a/receiver/nmea/gga.cpp +++ b/receiver/nmea/gga.cpp @@ -191,8 +191,10 @@ std::unique_ptr GgaMessage::parse(std::string prefix, std::string const success &= parse_altitude(tokens[10], tokens[11], message->mGeoidSeparation); if (tokens.size() > 12) { - success &= parse_age_of_differential_corrections(tokens[12], - message->mAgeOfDifferentialCorrections); + if (!parse_age_of_differential_corrections(tokens[12], + message->mAgeOfDifferentialCorrections)) { + message->mAgeOfDifferentialCorrections = 0; + } } else { message->mAgeOfDifferentialCorrections = 0; }