diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 00000000..daf9b4eb --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,28 @@ +# Changelog + +## [3.4.0] +- Support for receivers that communicate using NMEA protocol has been added. The following sentences are now supported: + - GGA + - GST + - VTG +- Added a new example has been added to demonstrate how to use the NMEA receiver. +- Added a command-line argument `--print-receiver-messages` (or `--prm`) to print all received messages to the standard output. This option is valid for any receiver (NMEA or u-Blox). +- Fixed a bug related to ASN.1 BIT STRING encoding. The MSB and LSB were swapped. This should not affect OSR or SSR assistance data request. +- Fixed a bug where the SUPL identity was not properly encoded when using IMSI and MSISDN. A value for ABCD was encoded as BADC. The fix is only active when the argument `--supl-identity-fix` is passed. +- Added way to send a "fake" location has been added with the command-line arguments `--location-info`, `--latitude`, `--longitude`, and `--altitude`. There is also an option to always send location information, even if the location server does not request it. This is done with the command-line argument `--force-location-info`. +- Added a option (`--rtcm-print`) to disable printing the "RTCM: XXXX bytes | " lines when using OSR and RTCM generation has been added. +- Added two SPARTN generators: + - `spartn` - The original SPARTN generator. + - `spartn2` - A new SPARTN generator that is based on the original SPARTN generator. It's recommended to use this generator instead of the original SPARTN generator. +- Added support to output ASN.1 UPER encoded 3GPP LPP ProvideAssistanceData messages from the `example_lpp`. +- Moved `build-with-gcc4.8.sh` to `docker/verify-with-gcc4.8.sh` and removed the use of `sudo` in the script. +- Changed to use `TAI_Time` instead of `time_t` to represent estimation time in the `LocationInformation` structure. +- `provide_location_information_callback_ublox` now sets the `location_info->time` field to the time received from the receiver instead of the current time. +- Fixed a bug where `--ublox-serial-data-bits` was used instead of `--serial-data-bits` for the output serial configuration. +- Fixed command-line argument `--version` (or `-v`) to _actually_ print the version. +- Added a new example `example_lpp2spartn` to demonstrate how to convert LPP to SPARTN. It takes ASN.1 UPER messages from STDIN and converts them to SPARTN messages. +- Added a new example `example_ntrip` which can be used to connect to an NTRIP caster and receiver RTCM messages. +- Added a new interface `stdin` that can be used to read data from STDIN. +- `ProvideLocationInformation` now uses MeasurementReferenceTime to provide a precise time estimate with a 250ns resolution. +- Fixed a bug where UBX messages would be missed due to the way we were reading from the interface. +- Fixed a bug where the bitfields in UBX-NAV-PVT were not properly right-shifted down to the LSB. `diff_soln` would report `0b00` or `0b10` instead of `0b0` or `0b1`. diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a66a26d..196c6d12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,8 @@ option(USE_ASAN "USE_ASAN" OFF) option(ASN_DEBUG "ASN_DEBUG" OFF) option(INTERFACE_FD_DEBUG "Print FD debug information" OFF) option(RECEIVER_UBLOX_THREADED "Print Receiver u-blox (threaded) debug information" OFF) +option(SPARTN_DEBUG_PRINT "Print SPARTN debug information" OFF) +option(RECEIVER_NMEA_DEBUG "Print Receiver NMEA debug information" OFF) option (FORCE_COLORED_OUTPUT "Always produce ANSI-colored output (GNU/Clang only)." TRUE) if (FORCE_COLORED_OUTPUT) @@ -25,6 +27,9 @@ if (USE_OPENSSL) find_package(OpenSSL REQUIRED) endif (USE_OPENSSL) +add_definitions(-D_POSIX_C_SOURCE=200809L) +add_definitions(-DCLIENT_VERSION="3.4.0") + if(${ASN_DEBUG}) add_definitions(-DASN_EMIT_DEBUG=1) endif(${ASN_DEBUG}) @@ -37,7 +42,13 @@ add_subdirectory("libs") add_subdirectory("interface") add_subdirectory("asn.1") add_subdirectory("generator/rtcm") +add_subdirectory("generator/spartn") +add_subdirectory("generator/spartn2") add_subdirectory("receiver/ublox") +add_subdirectory("receiver/nmea") add_subdirectory("examples/ublox") +add_subdirectory("examples/nmea") add_subdirectory("examples/lpp") +add_subdirectory("examples/lpp2spartn") +add_subdirectory("examples/ntrip") diff --git a/README.md b/README.md index 7da36a28..1a248279 100644 --- a/README.md +++ b/README.md @@ -1,43 +1,48 @@ -# SUPL-3GPP-LPP-client +# SUPL 3GPP LPP client -SUPL-3GPP-LPP-client is an example client for that connects to a location server using SUPL and requests assistance data using 3GPP LPP (Release 16.4). The code contains libraries for building a custom client for your specific needs. The example client is written in C++ and is designed to be portable and easy to use. It should be sufficient to get started or use as a simple client for testing. +![version](https://img.shields.io/badge/version-3.4.0-green) +![license](https://img.shields.io/badge/license-MXM-blue) -The source code for the ASN.1 definitions from 3GPP LPP Release 16.4 are generated by ASN.1 C (https://github.com/vlm/asn1c). All other code is in C++ and should be portable to must unix-like systems. The example client is tested on Ubuntu 20.04 and Raspberry PI OS (32-bit) with gcc-4.8. +This project is a set of libraries, examples and tools to facilitate the development of 3GPP LPP clients. ---- +## Libraries +* 3GPP LPP client - A library that can be used to communicate with a SUPL server and request assistance data. +* RTCM converter - Converts RTCM messages to 3GPP LPP messages +* SPARTN converter - Converts 3GPP LPP messages to SPARTN messages +* Interface - A set of interfaces that can be read or written to, e.g. a device, a file, a socket, etc. +* u-Blox Receiver - A library that can be used to communicate with a u-Blox receiver. +* NMEA Receiver - A library that can be used to communicate with a GNSS receiver that supports NMEA. -## Table of Contents -- [Examples](#examples) -- [Prerequisites](#prerequisites) -- [Building](#building) -- [Usage](#usage) -- [License](#license) +## Examples +* [3GPP LPP example](/examples/lpp/README.md) - `example-lpp` - Simple example of requesting assistance data from a SUPL server, converting it, and sending it to a GNSS receiver. Supports OSR, SSR, and basic retrieval of A-GNSS data. This is almost a fully implemented client. +* [NTRIP example](/examples/ntrip/README.md) - `example-ntrip` - Example that connects to an NTRIP caster, requesting RTCM data, and sending it to a GNSS receiver. +* [u-Blox example](/examples/ublox/README.md) - `example-ublox` - Example that will communicate with a u-Blox receiver and print all received messages to the console. +* [NMEA example](/examples/nmea/README.md) - `example-nmea` - Example that will communicate with a GNSS receiver (that supports NMEA) and print all received messages to the console. ---- +## Build -## Examples -The repository has a few examples: -- [LPP](/examples/lpp/README.md) - Example client that requests assistance data from a location server. It supports OSR, SSR, and AGNSS requests. The assistance data can be outputted to a file, serial port, TCP, UDP or stdout. It can also be converted to RTCM messages that can be transmitted to any GNSS receiver that supports it. -- [u-blox](/examples/ublox/README.md) - Example program that demonstrates how to use the u-Blox receiver library. The program takes an interface and port associated with the receiver as arguments. It will the connect, configure the u-blox to output UBX-NAV-PVT, and print all received messages to stdout. - -## Prerequisites -These are the prerequisites required for building: +First install the dependencies: ```bash sudo apt install g++ cmake libssl-dev ninja-build ``` -# Building -Building using CMake and Ninja: +Clone the repository: ```bash -cmake -S . -B build -G Ninja -cmake --build build --config Debug +git clone git@github.com:Ericsson/SUPL-3GPP-LPP-client.git +cd SUPL-3GPP-LPP-client ``` -After a successful build, the example executables should be located in the `build` folder with a prefix of `example-`. E.g., `build/example-lpp`. - -### GCC-4.8 +Setup the build: +```bash +mkdir build +cd build +cmake .. -GNinja -DCMAKE_BUILD_TYPE=Debug +``` -To verify compatibility with gcc-4.8 and older compilers, use the build script (`build-with-gcc4.8.sh`), that will build the client with gcc 4.8. This help guarantee that the source code will compile on smaller devices such as a Raspberry PI or BeagleBone. This is done with the help of a docker image with the compiler preinstalled. (This requires that you have docker installed) +Build the project (from the build directory): +```bash +ninja +``` ## License See [LICENSE](/LICENSE.txt) file. diff --git a/build-with-gcc4.8.sh b/docker/verify-with-gcc4.8.sh similarity index 66% rename from build-with-gcc4.8.sh rename to docker/verify-with-gcc4.8.sh index 6354ff1a..a519dd90 100755 --- a/build-with-gcc4.8.sh +++ b/docker/verify-with-gcc4.8.sh @@ -10,5 +10,5 @@ cp -r receiver build/docker/. cp -r libs build/docker/. cp CMakeLists.txt build/docker/. -sudo docker build . -f docker/Dockerfile.gcc-4.8 -t build:gcc-4.8 -sudo docker build ./build -f docker/Dockerfile.build.gcc-4.8 +docker build . -f docker/Dockerfile.gcc-4.8 -t build:gcc-4.8 +docker build build -f docker/Dockerfile.build.gcc-4.8 diff --git a/examples/lpp/CMakeLists.txt b/examples/lpp/CMakeLists.txt index c762f781..264ea210 100644 --- a/examples/lpp/CMakeLists.txt +++ b/examples/lpp/CMakeLists.txt @@ -14,8 +14,11 @@ target_include_directories(example_lpp PRIVATE "./") target_link_libraries(example_lpp PRIVATE utility modem lpplib Threads::Threads) target_link_libraries(example_lpp PRIVATE args) target_link_libraries(example_lpp PRIVATE generator::rtcm) +target_link_libraries(example_lpp PRIVATE generator::spartn) +target_link_libraries(example_lpp PRIVATE generator::spartn2) target_link_libraries(example_lpp PRIVATE dependency::interface) target_link_libraries(example_lpp PRIVATE receiver::ublox) +target_link_libraries(example_lpp PRIVATE receiver::nmea) target_link_libraries(example_lpp PRIVATE asn1::generated asn1::helper) set_target_properties(example_lpp PROPERTIES OUTPUT_NAME "example-lpp") diff --git a/examples/lpp/README.md b/examples/lpp/README.md index ebd79dc0..384183e7 100644 --- a/examples/lpp/README.md +++ b/examples/lpp/README.md @@ -1,6 +1,6 @@ # Example - LPP Client -This is a simple client examples that requests assistance data from a location server. It supports OSR, SSR, and AGNSS requests. The assistance data can be outputted to a file, serial port, TCP, UDP or stdout. It can also be converted to RTCM messages that can be transmitted to any GNSS receiver that supports it. +This sample code is a simple client that asks for assistance data from a location server. It can handle OSR, SSR, and AGNSS requests. The assistance data can converted to RTCM or SPARTN before being sent to a GNSS receiver or other interface. The client also supports to 3GPP LPP Provide Location Information, which can be used to send the device's location to the location server. ## Usage @@ -12,95 +12,127 @@ There are a few required arguments: - `-i` or `--ci` - The Cell Identity ``` -./example-lpp COMMAND {OPTIONS} + ./example-lpp COMMAND {OPTIONS} -Example - LPP Client + 3GPP LPP Example (3.4.0) - This sample code is a simple client that asks for + assistance data from a location server. It can handle OSR, SSR, and AGNSS + requests. The assistance data can converted to RTCM or SPARTN before being + sent to a GNSS receiver or other interface. The client also supports to 3GPP + LPP Provide Location Information, which can be used to send the device's + location to the location server. -OPTIONS: + OPTIONS: - -?, --help Display this help menu - -v, --version Display version information - Commands: - osr Request observation data from a - location server - ssr Request State-space Representation - (SSR) data from the location server - agnss Request Assisted GNSS data from the - location server - Location Server: - -h[host], --host=[host] Host - -p[port], --port=[port] Port - Default: 5431 - -s, --ssl TLS - Default: false - Identity: - --msisdn=[msisdn] MSISDN - --imsi=[imsi] IMSI - Default: 2460813579 - --ipv4=[ipv4] IPv4 - Cell Information: - -c[mcc], --mcc=[mcc] Mobile Country Code - -n[mnc], --mnc=[mnc] Mobile Network Code - -t[tac], --lac=[tac], --tac=[tac] Tracking Area Code - -i[ci], --ci=[ci] Cell Identity - Modem: - --modem=[device] Device - --modem-baud=[baud_rate] Baud Rate - u-blox Receiver: - --ublox-port=[port] The port used on the u-blox receiver, - used by configuration. - One of: uart1, uart2, i2c, usb - Default: (by interface) - Serial: - --ublox-serial=[device] Device - --ublox-serial-baud=[baud_rate] Baud Rate + -?, --help Display this help menu + -v, --version Display version information + Commands: + osr Request observation data from a + location server + ssr Request State-space Representation + (SSR) data from the location server + agnss Request Assisted GNSS data from the + location server + Location Server: + -h[host], --host=[host] Host + -p[port], --port=[port] Port + Default: 5431 + -s, --ssl TLS + Default: false + Identity: + --msisdn=[msisdn] MSISDN + --imsi=[imsi] IMSI + Default: 2460813579 + --ipv4=[ipv4] IPv4 + --supl-identity-fix Use SUPL Identity Fix + Cell Information: + -c[mcc], --mcc=[mcc] Mobile Country Code + -n[mnc], --mnc=[mnc] Mobile Network Code + -t[tac], --lac=[tac], --tac=[tac] Tracking Area Code + -i[ci], --ci=[ci] Cell Identity + Modem: + --modem=[device] Device + --modem-baud=[baud_rate] Baud Rate + u-blox Receiver: + --ublox-port=[port] The port used on the u-blox receiver, + used by configuration. + One of: uart1, uart2, i2c, usb + Default: (by interface) + Serial: + --ublox-serial=[device] Device + --ublox-serial-baud=[baud_rate] Baud Rate + Default: 115200 + --ublox-serial-data=[data_bits] Data Bits + One of: 5, 6, 7, 8 + Default: 8 + --ublox-serial-stop=[stop_bits] Stop Bits + One of: 1, 2 + Default: 1 + --ublox-serial-parity=[parity_bits] + Parity Bits + One of: none, odd, even + Default: none + I2C: + --ublox-i2c=[device] Device + --ublox-i2c-address=[address] Address + Default: 66 + TCP: + --ublox-tcp=[ip_address] Host or IP Address + --ublox-tcp-port=[port] Port + UDP: + --ublox-udp=[ip_address] Host or IP Address + --ublox-udp-port=[port] Port + NMEA Receiver: + Serial: + --nmea-serial=[device] Device + --nmea-serial-baud=[baud_rate] Baud Rate Default: 115200 - --ublox-serial-data=[data_bits] Data Bits + --nmea-serial-data=[data_bits] Data Bits One of: 5, 6, 7, 8 Default: 8 - --ublox-serial-stop=[stop_bits] Stop Bits + --nmea-serial-stop=[stop_bits] Stop Bits One of: 1, 2 Default: 1 - --ublox-serial-parity=[parity_bits] + --nmea-serial-parity=[parity_bits] Parity Bits One of: none, odd, even Default: none - I2C: - --ublox-i2c=[device] Device - --ublox-i2c-address=[address] Address - Default: 66 - TCP: - --ublox-tcp=[ip_address] Host or IP Address - --ublox-tcp-port=[port] Port - UDP: - --ublox-udp=[ip_address] Host or IP Address - --ublox-udp-port=[port] Port - Output: - File: - --file=[file_path] Path - Serial: - --serial=[device] Device - --serial-baud=[baud_rate] Baud Rate - Default: 115200 - --serial-data=[data_bits] Data Bits - One of: 5, 6, 7, 8 - Default: 8 - --serial-stop=[stop_bits] Stop Bits - One of: 1, 2 - Default: 1 - --serial-parity=[parity_bits] Parity Bits - One of: none, odd, even - Default: none - I2C: - --i2c=[device] Device - --i2c-address=[address] Address - Default: 66 - TCP: - --tcp=[ip_address] Host or IP Address - --tcp-port=[port] Port - UDP: - --udp=[ip_address] Host or IP Address - --udp-port=[port] Port - Stdout: - --stdout Stdout + Output: + File: + --file=[file_path] Path + Serial: + --serial=[device] Device + --serial-baud=[baud_rate] Baud Rate + Default: 115200 + --serial-data=[data_bits] Data Bits + One of: 5, 6, 7, 8 + Default: 8 + --serial-stop=[stop_bits] Stop Bits + One of: 1, 2 + Default: 1 + --serial-parity=[parity_bits] Parity Bits + One of: none, odd, even + Default: none + I2C: + --i2c=[device] Device + --i2c-address=[address] Address + Default: 66 + TCP: + --tcp=[ip_address] Host or IP Address + --tcp-port=[port] Port + UDP: + --udp=[ip_address] Host or IP Address + --udp-port=[port] Port + Stdout: + --stdout Stdout + Location Infomation: + --location-info Location Information + --force-location-info Force Location Information (always + send even if not requested) + --latitude=[latitude] Latitude + Default: 69.0599730655754 + --longitude=[longitude] Longitude + Default: 20.54864403253676 + --altitude=[altitude] Altitude + Default: 0 + ``` diff --git a/examples/lpp/agnss_example.cpp b/examples/lpp/agnss_example.cpp index 8b80d778..7ec6d48f 100644 --- a/examples/lpp/agnss_example.cpp +++ b/examples/lpp/agnss_example.cpp @@ -42,6 +42,11 @@ void execute(Options options, agnss_example::Format format) { } LPP_Client client{false /* enable experimental segmentation support */}; + + if (!identity_options.use_supl_identity_fix) { + client.use_incorrect_supl_identity(); + } + if (identity_options.imsi) { client.set_identity_imsi(*identity_options.imsi); } else if (identity_options.msisdn) { @@ -51,7 +56,7 @@ void execute(Options options, agnss_example::Format format) { } else { throw std::runtime_error("No identity provided"); } - + if (!client.connect(location_server_options.host.c_str(), location_server_options.port, location_server_options.ssl, gCell)) { throw std::runtime_error("Unable to connect to location server"); diff --git a/examples/lpp/location_information.cpp b/examples/lpp/location_information.cpp index ae3bc260..97848334 100644 --- a/examples/lpp/location_information.cpp +++ b/examples/lpp/location_information.cpp @@ -1,17 +1,20 @@ #include "location_information.h" #include #include +#include +#include #include #include #include #include "lpp/location_information.h" +#include "options.hpp" #include "utility/types.h" bool provide_location_information_callback(UNUSED LocationInformation& location, UNUSED HaGnssMetrics& metrics, UNUSED void* userdata) { #if 0 // Example implementation - location.time = time(NULL); + location.tai_time = TAI_Time::now(); location.latitude = 20; location.longitude = 25; location.altitude = 30; @@ -36,15 +39,20 @@ bool provide_location_information_callback(UNUSED LocationInformation& location, } bool provide_location_information_callback_ublox(UNUSED LocationInformation& location, - UNUSED HaGnssMetrics& metrics, - UNUSED void* userdata) { + UNUSED HaGnssMetrics& metrics, void* userdata) { auto receiver = reinterpret_cast(userdata); if (!receiver) return false; auto nav_pvt = receiver->nav_pvt(); - if(!nav_pvt) return false; + if (!nav_pvt) return false; - location.time = time(NULL); // TODO(ewasjon): use time from nav_pvt + // TODO(ewasjon): Should we use the system time if the UTC time from u-blox is invalid? + if (!nav_pvt->valid_time()) { + printf("u-blox time is invalid\n"); + return false; + } + + location.tai_time = nav_pvt->tai_time(); location.latitude = nav_pvt->latitude(); location.longitude = nav_pvt->longitude(); location.altitude = nav_pvt->altitude(); @@ -80,6 +88,68 @@ bool provide_location_information_callback_ublox(UNUSED LocationInformation& loc return true; } +bool provide_location_information_callback_nmea(LocationInformation& location, + HaGnssMetrics& metrics, void* userdata) { + auto receiver = reinterpret_cast(userdata); + if (!receiver) return false; + + // NOTE(ewasjon): We require GGA, VTG, and GST message to produce a valid location information, + // if either is missing we will skip sending a provider location information message. + auto gga = receiver->gga(); + auto vtg = receiver->vtg(); + auto gst = receiver->gst(); + if (!gga || !vtg || !gst) { + return false; + } + + location.tai_time = gga->time_of_day(); + location.latitude = gga->latitude(); + location.longitude = gga->longitude(); + location.altitude = gga->altitude(); + location.horizontal_accuracy = gst->horizontal_position_error(); + location.horizontal_speed = vtg->speed_over_ground(); + location.horizontal_speed_accuracy = 0; + location.bearing = vtg->true_course_over_ground(); + + // TODO(ewasjon): Are these not available in NMEA? + location.vertical_accuracy = gst->vertical_position_error(); + location.vertical_speed = 0; + location.vertical_speed_accuracy = 0; + location.vertical_velocity_direction = VerticalDirection::UP; + + switch (gga->fix_quality()) { + case receiver::nmea::GgaFixQuality::Invalid: metrics.fixq = FixQuality::INVALID; break; + case receiver::nmea::GgaFixQuality::GpsFix: metrics.fixq = FixQuality::STANDALONE; break; + case receiver::nmea::GgaFixQuality::DgpsFix: metrics.fixq = FixQuality::DGPS_FIX; break; + case receiver::nmea::GgaFixQuality::PpsFix: metrics.fixq = FixQuality::PPS_FIX; break; + case receiver::nmea::GgaFixQuality::RtkFixed: metrics.fixq = FixQuality::RTK_FIX; break; + case receiver::nmea::GgaFixQuality::RtkFloat: metrics.fixq = FixQuality::RTK_FLOAT; break; + case receiver::nmea::GgaFixQuality::DeadReckoning: + metrics.fixq = FixQuality::DEAD_RECKONING; + break; + default: metrics.fixq = FixQuality::INVALID; + } + + metrics.sats = static_cast(gga->satellites_in_view()); + metrics.age = 0; // TODO: ? + metrics.hdop = gga->h_dop(); + metrics.pdop = 0; + return true; +} + +bool provide_location_information_callback_fake(UNUSED LocationInformation& location, + UNUSED HaGnssMetrics& metrics, void* userdata) { + auto options = reinterpret_cast(userdata); + if (!options) return false; + + location.tai_time = TAI_Time::now(); + location.latitude = options->latitude; + location.longitude = options->longitude; + location.altitude = options->altitude; + + return true; +} + bool provide_ecid_callback(ECIDInformation& ecid, void* userdata) { auto modem = reinterpret_cast(userdata); if (!modem) return false; diff --git a/examples/lpp/location_information.h b/examples/lpp/location_information.h index b886293e..f5c49a35 100644 --- a/examples/lpp/location_information.h +++ b/examples/lpp/location_information.h @@ -3,9 +3,14 @@ struct LocationInformation; struct HaGnssMetrics; struct ECIDInformation; +struct LocationInformationOptions; bool provide_location_information_callback(LocationInformation& location, HaGnssMetrics& metrics, void* userdata); bool provide_location_information_callback_ublox(LocationInformation& location, HaGnssMetrics& metrics, void* userdata); +bool provide_location_information_callback_nmea(LocationInformation& location, + HaGnssMetrics& metrics, void* userdata); +bool provide_location_information_callback_fake(LocationInformation& location, + HaGnssMetrics& metrics, void* userdata); bool provide_ecid_callback(ECIDInformation& ecid, void* userdata); diff --git a/examples/lpp/options.cpp b/examples/lpp/options.cpp index 4be1bd9a..d81da4b1 100644 --- a/examples/lpp/options.cpp +++ b/examples/lpp/options.cpp @@ -40,6 +40,11 @@ args::ValueFlag msisdn{ identity, "msisdn", "MSISDN", {"msisdn"}, args::Options::Single}; args::ValueFlag imsi{identity, "imsi", "IMSI", {"imsi"}, args::Options::Single}; args::ValueFlag ipv4{identity, "ipv4", "IPv4", {"ipv4"}, args::Options::Single}; +args::Flag use_supl_identity_fix{identity, + "supl-identity-fix", + "Use SUPL Identity Fix", + {"supl-identity-fix"}, + args::Options::Single}; // // Cell Information @@ -156,6 +161,52 @@ args::ValueFlag ublox_udp_ip_address{ args::ValueFlag ublox_udp_port{ ublox_udp_device, "port", "Port", {"ublox-udp-port"}, args::Options::Single}; +// +// NMEA +// + +args::Group nmea_receiver_group{ + "NMEA Receiver:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::Group nmea_serial_group{ + nmea_receiver_group, + "Serial:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag nmea_serial_device{ + nmea_receiver_group, "device", "Device", {"nmea-serial"}, args::Options::Single}; +args::ValueFlag nmea_serial_baud_rate{ + nmea_receiver_group, "baud_rate", "Baud Rate", {"nmea-serial-baud"}, args::Options::Single}; +args::ValueFlag nmea_serial_data_bits{ + nmea_receiver_group, "data_bits", "Data Bits", {"nmea-serial-data"}, args::Options::Single}; +args::ValueFlag nmea_serial_stop_bits{ + nmea_receiver_group, "stop_bits", "Stop Bits", {"nmea-serial-stop"}, args::Options::Single}; +args::ValueFlag nmea_serial_parity_bits{nmea_receiver_group, + "parity_bits", + "Parity Bits", + {"nmea-serial-parity"}, + args::Options::Single}; + +// +// Output +// + +args::Group other_receiver_group{ + "Other Receiver Options:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::Flag print_messages{other_receiver_group, + "print-receiver-messages", + "Print Receiver Messages", + {"print-receiver-messages", "prm"}, + args::Options::Single}; + // // Output // @@ -231,6 +282,34 @@ args::Group stdout_output{ }; args::Flag stdout_output_flag{stdout_output, "stdout", "Stdout", {"stdout"}, args::Options::Single}; +// +// Location Information +// + +args::Group location_infomation{ + "Location Infomation:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::Flag li_enable{ + location_infomation, "location-info", "Location Information", + {"location-info"}, args::Options::Single, +}; +args::Flag li_force{ + location_infomation, + "force-location-info", + "Force Location Information (always send even if not requested)", + {"force-location-info"}, + args::Options::Single, +}; +args::ValueFlag li_latitude{ + location_infomation, "latitude", "Latitude", {"latitude"}, args::Options::Single}; +args::ValueFlag li_longitude{ + location_infomation, "longitude", "Longitude", {"longitude"}, args::Options::Single}; +args::ValueFlag li_altitude{ + location_infomation, "altitude", "Altitude", {"altitude"}, args::Options::Single}; + // // Options // @@ -255,6 +334,7 @@ LocationServerOptions parse_location_server_options() { IdentityOptions parse_identity_options() { IdentityOptions identity{}; + identity.use_supl_identity_fix = false; if (msisdn) { identity.msisdn = std::unique_ptr{new unsigned long{msisdn.Get()}}; @@ -272,6 +352,10 @@ IdentityOptions parse_identity_options() { identity.imsi = std::unique_ptr{new unsigned long{2460813579lu}}; } + if (use_supl_identity_fix) { + identity.use_supl_identity_fix = true; + } + return identity; } @@ -330,8 +414,8 @@ OutputOptions parse_output_options() { } auto data_bits = DataBits::EIGHT; - if (ublox_serial_data_bits) { - switch (ublox_serial_data_bits.Get()) { + if (serial_data_bits) { + switch (serial_data_bits.Get()) { case 5: data_bits = DataBits::FIVE; break; case 6: data_bits = DataBits::SIX; break; case 7: data_bits = DataBits::SEVEN; break; @@ -341,8 +425,8 @@ OutputOptions parse_output_options() { } auto stop_bits = StopBits::ONE; - if (ublox_serial_stop_bits) { - switch (ublox_serial_stop_bits.Get()) { + if (serial_stop_bits) { + switch (serial_stop_bits.Get()) { case 1: stop_bits = StopBits::ONE; break; case 2: stop_bits = StopBits::TWO; break; default: throw args::ValidationError("Invalid stop bits"); @@ -350,12 +434,12 @@ OutputOptions parse_output_options() { } auto parity_bit = ParityBit::NONE; - if (ublox_serial_parity_bits) { - if (ublox_serial_parity_bits.Get() == "none") { + if (serial_parity_bits) { + if (serial_parity_bits.Get() == "none") { parity_bit = ParityBit::NONE; - } else if (ublox_serial_parity_bits.Get() == "odd") { + } else if (serial_parity_bits.Get() == "odd") { parity_bit = ParityBit::ODD; - } else if (ublox_serial_parity_bits.Get() == "even") { + } else if (serial_parity_bits.Get() == "even") { parity_bit = ParityBit::EVEN; } else { throw args::ValidationError("Invalid parity bits"); @@ -544,16 +628,107 @@ static Port ublox_parse_port() { } } +static bool print_receiver_options_parse() { + if (print_messages) { + return true; + } else { + return false; + } +} + static UbloxOptions ublox_parse_options() { if (ublox_serial_device || ublox_i2c_device || ublox_tcp_ip_address || ublox_udp_ip_address) { - auto port = ublox_parse_port(); - auto interface = ublox_parse_interface(); - return UbloxOptions{port, std::move(interface)}; + auto port = ublox_parse_port(); + auto interface = ublox_parse_interface(); + auto print_messages = print_receiver_options_parse(); + return UbloxOptions{port, std::move(interface), print_messages}; } else { return UbloxOptions{}; } } +static NmeaOptions nmea_parse_options() { + if (nmea_serial_device) { + uint32_t baud_rate = 115200; + if (nmea_serial_baud_rate) { + if (nmea_serial_baud_rate.Get() < 0) { + throw args::ValidationError("nmea-serial-baud-rate must be positive"); + } + + baud_rate = static_cast(nmea_serial_baud_rate.Get()); + } + + auto data_bits = DataBits::EIGHT; + if (nmea_serial_data_bits) { + switch (nmea_serial_data_bits.Get()) { + case 5: data_bits = DataBits::FIVE; break; + case 6: data_bits = DataBits::SIX; break; + case 7: data_bits = DataBits::SEVEN; break; + case 8: data_bits = DataBits::EIGHT; break; + default: throw args::ValidationError("Invalid data bits"); + } + } + + auto stop_bits = StopBits::ONE; + if (nmea_serial_stop_bits) { + switch (nmea_serial_stop_bits.Get()) { + case 1: stop_bits = StopBits::ONE; break; + case 2: stop_bits = StopBits::TWO; break; + default: throw args::ValidationError("Invalid stop bits"); + } + } + + auto parity_bit = ParityBit::NONE; + if (nmea_serial_parity_bits) { + if (nmea_serial_parity_bits.Get() == "none") { + parity_bit = ParityBit::NONE; + } else if (nmea_serial_parity_bits.Get() == "odd") { + parity_bit = ParityBit::ODD; + } else if (nmea_serial_parity_bits.Get() == "even") { + parity_bit = ParityBit::EVEN; + } else { + throw args::ValidationError("Invalid parity bits"); + } + } + + auto interface = interface::Interface::serial(nmea_serial_device.Get(), baud_rate, + data_bits, stop_bits, parity_bit); + auto print_messages = print_receiver_options_parse(); + return NmeaOptions{std::unique_ptr(interface), print_messages}; + } else { + return NmeaOptions{}; + } +} + +static LocationInformationOptions parse_location_information_options() { + LocationInformationOptions location_information{}; + location_information.latitude = 69.0599730655754; + location_information.longitude = 20.54864403253676; + location_information.altitude = 0; + location_information.force = false; + + if (li_force) { + location_information.force = true; + } + + if (li_enable) { + location_information.enabled = true; + if (li_latitude) { + location_information.latitude = li_latitude.Get(); + } + + if (li_longitude) { + location_information.longitude = li_longitude.Get(); + } + + if (li_altitude) { + location_information.altitude = li_altitude.Get(); + } + } + + return location_information; +} + // // Option Parser // @@ -570,11 +745,12 @@ void OptionParser::add_command(std::unique_ptr command) { int OptionParser::parse_and_execute(int argc, char** argv) { args::ArgumentParser parser( - "Example - LPP Client", - "This is a simple client examples that requests assistance data from a location server. It " - "supports OSR, SSR, and AGNSS requests. The assistance data can be outputted to a file, " - "serial port, TCP, UDP or stdout. It can also be converted to RTCM messages that can be " - "transmitted to any GNSS receiver that supports it."); + "3GPP LPP Example (" CLIENT_VERSION + ") - This sample code is a simple client that asks for assistance data from a location " + "server. It can handle OSR, SSR, and AGNSS requests. The assistance data can converted to " + "RTCM or SPARTN before being sent to a GNSS receiver or other interface. The client also " + "supports to 3GPP LPP Provide Location Information, which can be used to send the device's " + "location to the location server."); args::HelpFlag help{parser, "help", "Display this help menu", {'?', "help"}}; args::Flag version{parser, "version", "Display version information", {'v', "version"}}; @@ -590,33 +766,32 @@ int OptionParser::parse_and_execute(int argc, char** argv) { std::vector> args_commands; for (auto& command : mCommands) { auto command_ptr = command.get(); - args_commands.emplace_back( - new args::Command(commands, command->name(), command->description(), - [command_ptr](args::Subparser& parser) { - command_ptr->parse(parser); - parser.Parse(); - - Options options{}; - options.location_server_options = parse_location_server_options(); - options.identity_options = parse_identity_options(); - options.cell_options = parse_cell_options(); - options.modem_options = parse_modem_options(); - options.output_options = parse_output_options(); - options.ublox_options = ublox_parse_options(); - command_ptr->execute(std::move(options)); - })); + args_commands.emplace_back(new args::Command( + commands, command->name(), command->description(), + [command_ptr](args::Subparser& parser) { + command_ptr->parse(parser); + parser.Parse(); + + Options options{}; + options.location_server_options = parse_location_server_options(); + options.identity_options = parse_identity_options(); + options.cell_options = parse_cell_options(); + options.modem_options = parse_modem_options(); + options.output_options = parse_output_options(); + options.ublox_options = ublox_parse_options(); + options.nmea_options = nmea_parse_options(); + options.location_information_options = parse_location_information_options(); + command_ptr->execute(std::move(options)); + })); } // Defaults ublox_i2c_address.HelpDefault("66"); ublox_serial_baud_rate.HelpDefault("115200"); - ublox_serial_data_bits.HelpDefault("8"); ublox_serial_data_bits.HelpChoices({"5", "6", "7", "8"}); - ublox_serial_stop_bits.HelpDefault("1"); ublox_serial_stop_bits.HelpChoices({"1", "2"}); - ublox_serial_parity_bits.HelpDefault("none"); ublox_serial_parity_bits.HelpChoices({ "none", @@ -632,6 +807,18 @@ int OptionParser::parse_and_execute(int argc, char** argv) { "usb", }); + nmea_serial_baud_rate.HelpDefault("115200"); + nmea_serial_data_bits.HelpDefault("8"); + nmea_serial_data_bits.HelpChoices({"5", "6", "7", "8"}); + nmea_serial_stop_bits.HelpDefault("1"); + nmea_serial_stop_bits.HelpChoices({"1", "2"}); + nmea_serial_parity_bits.HelpDefault("none"); + nmea_serial_parity_bits.HelpChoices({ + "none", + "odd", + "even", + }); + location_server_port.HelpDefault("5431"); location_server_ssl.HelpDefault("false"); imsi.HelpDefault("2460813579"); @@ -652,17 +839,29 @@ int OptionParser::parse_and_execute(int argc, char** argv) { "even", }); + li_latitude.HelpDefault("69.0599730655754"); + li_longitude.HelpDefault("20.54864403253676"); + li_altitude.HelpDefault("0"); + // Globals args::GlobalOptions location_server_globals{parser, location_server}; args::GlobalOptions identity_globals{parser, identity}; args::GlobalOptions cell_information_globals{parser, cell_information}; args::GlobalOptions modem_globals{parser, modem}; args::GlobalOptions ublox_receiver_globals{parser, ublox_receiver_group}; + args::GlobalOptions nmea_receiver_globals{parser, nmea_receiver_group}; + args::GlobalOptions other_receiver_globals{parser, other_receiver_group}; args::GlobalOptions output_globals{parser, output}; + args::GlobalOptions location_information_globals{parser, location_infomation}; // Parse try { parser.ParseCLI(argc, argv); + if (version) { + std::cout << "3GPP LPP Example (" << CLIENT_VERSION << ")" << std::endl; + return 0; + } + return 0; } catch (const args::ValidationError& e) { std::cerr << e.what() << std::endl; diff --git a/examples/lpp/options.hpp b/examples/lpp/options.hpp index 22023de9..52ccce5e 100644 --- a/examples/lpp/options.hpp +++ b/examples/lpp/options.hpp @@ -24,6 +24,9 @@ struct IdentityOptions { std::unique_ptr imsi; /// Identify the device with IPv4 address. std::unique_ptr ipv4; + + /// Whether to switch the order of the digits in the identity. + bool use_supl_identity_fix; }; /// Cell options. @@ -60,16 +63,42 @@ struct UbloxOptions { receiver::ublox::Port port; /// Interface to use for communication with the receiver. std::unique_ptr interface; + /// Whether to print messages. + bool print_messages; +}; + +/// Nmea options. +struct NmeaOptions { + /// Interface to use for communication with the receiver. + std::unique_ptr interface; + /// Whether to print messages. + bool print_messages; +}; + +/// Location information options. +struct LocationInformationOptions { + /// Enable fake location information. + bool enabled; + /// Force location information to be sent, even if it hasn't been requested. + bool force; + /// Fake latitude. + double latitude; + /// Fake longitude. + double longitude; + /// Fake altitude. + double altitude; }; /// Options. struct Options { - LocationServerOptions location_server_options; - IdentityOptions identity_options; - CellOptions cell_options; - ModemOptions modem_options; - OutputOptions output_options; - UbloxOptions ublox_options; + LocationServerOptions location_server_options; + IdentityOptions identity_options; + CellOptions cell_options; + ModemOptions modem_options; + OutputOptions output_options; + UbloxOptions ublox_options; + NmeaOptions nmea_options; + LocationInformationOptions location_information_options; }; /// Command. diff --git a/examples/lpp/osr_example.cpp b/examples/lpp/osr_example.cpp index bb2d507c..f2a0dfc9 100644 --- a/examples/lpp/osr_example.cpp +++ b/examples/lpp/osr_example.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -10,28 +11,35 @@ using RtcmGenerator = std::unique_ptr; using UReceiver = receiver::ublox::ThreadedReceiver; +using NReceiver = receiver::nmea::ThreadedReceiver; static CellID gCell; static osr_example::Format gFormat; static RtcmGenerator gGenerator; static generator::rtcm::MessageFilter gFilter; static Options gOptions; +static bool gPrintRtcm; static std::unique_ptr gModem; static std::unique_ptr gUbloxReceiver; +static std::unique_ptr gNmeaReceiver; static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message*, void*); -void execute(Options options, osr_example::Format format, osr_example::MsmType msm_type) { - gOptions = std::move(options); - gFormat = format; - - auto& cell_options = gOptions.cell_options; - auto& location_server_options = gOptions.location_server_options; - auto& identity_options = gOptions.identity_options; - auto& modem_options = gOptions.modem_options; - auto& output_options = gOptions.output_options; - auto& ublox_options = gOptions.ublox_options; +void execute(Options options, osr_example::Format format, osr_example::MsmType msm_type, + bool print_rtcm) { + gOptions = std::move(options); + gFormat = format; + gPrintRtcm = print_rtcm; + + auto& cell_options = gOptions.cell_options; + auto& location_server_options = gOptions.location_server_options; + auto& identity_options = gOptions.identity_options; + auto& modem_options = gOptions.modem_options; + auto& output_options = gOptions.output_options; + auto& ublox_options = gOptions.ublox_options; + auto& nmea_options = gOptions.nmea_options; + auto& location_information_options = gOptions.location_information_options; gCell = CellID{ .mcc = cell_options.mcc, @@ -113,16 +121,30 @@ void execute(Options options, osr_example::Format format, osr_example::MsmType m ublox_options.interface->open(); ublox_options.interface->print_info(); - gUbloxReceiver = std::unique_ptr( - new UReceiver(ublox_options.port, std::move(ublox_options.interface))); + gUbloxReceiver = std::unique_ptr(new UReceiver( + ublox_options.port, std::move(ublox_options.interface), ublox_options.print_messages)); gUbloxReceiver->start(); } + if (nmea_options.interface) { + printf("[nmea]\n"); + nmea_options.interface->open(); + nmea_options.interface->print_info(); + + gNmeaReceiver = std::unique_ptr( + new NReceiver(std::move(nmea_options.interface), nmea_options.print_messages)); + gNmeaReceiver->start(); + } + // Create RTCM generator for converting LPP messages to RTCM messages. gGenerator = std::unique_ptr(new generator::rtcm::Generator()); LPP_Client client{false /* enable experimental segmentation support */}; + if (!identity_options.use_supl_identity_fix) { + client.use_incorrect_supl_identity(); + } + if (identity_options.imsi) { client.set_identity_imsi(*identity_options.imsi); } else if (identity_options.msisdn) { @@ -133,8 +155,32 @@ void execute(Options options, osr_example::Format format, osr_example::MsmType m throw std::runtime_error("No identity provided"); } - client.provide_location_information_callback(gUbloxReceiver.get(), - provide_location_information_callback_ublox); + printf("[location information]\n"); + if (gUbloxReceiver.get()) { + printf(" source: ublox\n"); + client.provide_location_information_callback(gUbloxReceiver.get(), + provide_location_information_callback_ublox); + } else if (gNmeaReceiver.get()) { + printf(" source: nmea\n"); + client.provide_location_information_callback(gNmeaReceiver.get(), + provide_location_information_callback_nmea); + } else if (location_information_options.enabled) { + printf(" source: simulated\n"); + client.provide_location_information_callback(&location_information_options, + provide_location_information_callback_fake); + } else { + printf(" source: none\n"); + client.provide_location_information_callback(nullptr, + provide_location_information_callback); + } + + if (location_information_options.force) { + client.force_location_information(); + printf(" force: true\n"); + } else { + printf(" force: false\n"); + } + client.provide_ecid_callback(gModem.get(), provide_ecid_callback); if (!client.connect(location_server_options.host.c_str(), location_server_options.port, @@ -174,16 +220,18 @@ static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message* if (gFormat == osr_example::Format::RTCM) { auto messages = gGenerator->generate(message, gFilter); - size_t length = 0; - for (auto& message : messages) { - length += message.data().size(); - } + if (gPrintRtcm) { + size_t length = 0; + for (auto& message : messages) { + length += message.data().size(); + } - printf("RTCM: %4zu bytes | ", length); - for (auto& message : messages) { - printf("%4i ", message.id()); + printf("RTCM: %4zu bytes | ", length); + for (auto& message : messages) { + printf("%4i ", message.id()); + } + printf("\n"); } - printf("\n"); for (auto& message : messages) { auto buffer = message.data().data(); @@ -200,6 +248,15 @@ static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message* interface->write(buffer, size); } } + } else if (gNmeaReceiver) { + auto interface = gNmeaReceiver->interface(); + if (interface) { + for (auto& message : messages) { + auto buffer = message.data().data(); + auto size = message.data().size(); + interface->write(buffer, size); + } + } } } else if (gFormat == osr_example::Format::XER) { std::stringstream buffer; @@ -224,6 +281,7 @@ void OsrCommand::parse(args::Subparser& parser) { // NOTE: parse may be called multiple times delete mFormatArg; delete mMsmTypeArg; + delete mPrintRTCMArg; mFormatArg = new args::ValueFlag(parser, "format", "Format", {'f', "format"}, args::Options::Single); @@ -234,6 +292,10 @@ void OsrCommand::parse(args::Subparser& parser) { {'y', "msm_type"}, args::Options::Single); mMsmTypeArg->HelpDefault("any"); mMsmTypeArg->HelpChoices({"any", "4", "5", "6", "7"}); + + // the default value is true, thus this is a negated flag + mPrintRTCMArg = new args::Flag(parser, "print_rtcm", "Do not print RTCM messages info", + {"rtcm-print"}, args::Options::Single); } void OsrCommand::execute(Options options) { @@ -266,7 +328,12 @@ void OsrCommand::execute(Options options) { } } - ::execute(std::move(options), format, msm_type); + auto print_rtcm = true; + if (*mPrintRTCMArg) { + print_rtcm = false; + } + + ::execute(std::move(options), format, msm_type, print_rtcm); } } // namespace osr_example diff --git a/examples/lpp/osr_example.h b/examples/lpp/osr_example.h index b93d4e3b..6fa6b838 100644 --- a/examples/lpp/osr_example.h +++ b/examples/lpp/osr_example.h @@ -20,11 +20,12 @@ class OsrCommand final : public Command { public: OsrCommand() : Command("osr", "Request observation data from a location server"), mFormatArg(nullptr), - mMsmTypeArg(nullptr) {} + mMsmTypeArg(nullptr), mPrintRTCMArg(nullptr) {} ~OsrCommand() override { delete mFormatArg; delete mMsmTypeArg; + delete mPrintRTCMArg; } void parse(args::Subparser& parser) override; @@ -33,6 +34,7 @@ class OsrCommand final : public Command { private: args::ValueFlag* mFormatArg; args::ValueFlag* mMsmTypeArg; + args::Flag* mPrintRTCMArg; }; } // namespace osr_example diff --git a/examples/lpp/ssr_example.cpp b/examples/lpp/ssr_example.cpp index 7d3d6147..999e2f77 100644 --- a/examples/lpp/ssr_example.cpp +++ b/examples/lpp/ssr_example.cpp @@ -1,35 +1,56 @@ #include "ssr_example.h" +#include +#include +#include #include #include #include #include +#include +#include #include #include #include "location_information.h" -static CellID gCell; -static std::unique_ptr gModem; -static ssr_example::Format gFormat; -static int gUraOverride; -static bool gUBloxClockCorrection; -static bool gForceIodeContinuity; -static Options gOptions; +using UReceiver = receiver::ublox::ThreadedReceiver; +using NReceiver = receiver::nmea::ThreadedReceiver; + +static CellID gCell; +static ssr_example::Format gFormat; +static int gUraOverride; +static bool gUBloxClockCorrection; +static bool gForceIodeContinuity; +static bool gAverageZenithDelay; +static bool gEnableIodeShift; +static Options gOptions; +static SPARTN_Generator gSpartnGeneratorOld; +static generator::spartn::Generator gSpartnGeneratorNew; + +static std::unique_ptr gModem; +static std::unique_ptr gUbloxReceiver; +static std::unique_ptr gNmeaReceiver; static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message*, void*); void execute(Options options, ssr_example::Format format, int ura_override, - bool ublox_clock_correction, bool force_continuity) { + bool ublox_clock_correction, bool force_continuity, bool average_zenith_delay, + bool enable_iode_shift) { gOptions = std::move(options); gFormat = format; gUraOverride = ura_override; gUBloxClockCorrection = ublox_clock_correction; gForceIodeContinuity = force_continuity; + gAverageZenithDelay = average_zenith_delay; + gEnableIodeShift = enable_iode_shift; - auto& cell_options = gOptions.cell_options; - auto& location_server_options = gOptions.location_server_options; - auto& identity_options = gOptions.identity_options; - auto& modem_options = gOptions.modem_options; - auto& output_options = gOptions.output_options; + auto& cell_options = gOptions.cell_options; + auto& location_server_options = gOptions.location_server_options; + auto& identity_options = gOptions.identity_options; + auto& modem_options = gOptions.modem_options; + auto& output_options = gOptions.output_options; + auto& ublox_options = gOptions.ublox_options; + auto& nmea_options = gOptions.nmea_options; + auto& location_information_options = gOptions.location_information_options; gCell = CellID{ .mcc = cell_options.mcc, @@ -66,8 +87,46 @@ void execute(Options options, ssr_example::Format format, int ura_override, interface->print_info(); } + if (ublox_options.interface) { + printf("[ublox]\n"); + ublox_options.interface->open(); + ublox_options.interface->print_info(); + + gUbloxReceiver = std::unique_ptr(new UReceiver( + ublox_options.port, std::move(ublox_options.interface), ublox_options.print_messages)); + gUbloxReceiver->start(); + } + + if (nmea_options.interface) { + printf("[nmea]\n"); + nmea_options.interface->open(); + nmea_options.interface->print_info(); + + gNmeaReceiver = std::unique_ptr( + new NReceiver(std::move(nmea_options.interface), nmea_options.print_messages)); + gNmeaReceiver->start(); + } + + gSpartnGeneratorNew.set_ura_override(gUraOverride); + gSpartnGeneratorNew.set_ublox_clock_correction(gUBloxClockCorrection); + if (gForceIodeContinuity) { + gSpartnGeneratorNew.set_continuity_indicator(320.0); + } + if (gAverageZenithDelay) { + gSpartnGeneratorNew.set_compute_average_zenith_delay(true); + } + if (gEnableIodeShift) { + gSpartnGeneratorNew.set_iode_shift(true); + } else { + gSpartnGeneratorNew.set_iode_shift(false); + } + LPP_Client client{false /* experimental segmentation support */}; + if (!identity_options.use_supl_identity_fix) { + client.use_incorrect_supl_identity(); + } + if (identity_options.imsi) { client.set_identity_imsi(*identity_options.imsi); } else if (identity_options.msisdn) { @@ -78,7 +137,32 @@ void execute(Options options, ssr_example::Format format, int ura_override, throw std::runtime_error("No identity provided"); } - client.provide_location_information_callback(NULL, provide_location_information_callback); + printf("[location information]\n"); + if (gUbloxReceiver.get()) { + printf(" source: ublox\n"); + client.provide_location_information_callback(gUbloxReceiver.get(), + provide_location_information_callback_ublox); + } else if (gNmeaReceiver.get()) { + printf(" source: nmea\n"); + client.provide_location_information_callback(gNmeaReceiver.get(), + provide_location_information_callback_nmea); + } else if (location_information_options.enabled) { + printf(" source: simulated\n"); + client.provide_location_information_callback(&location_information_options, + provide_location_information_callback_fake); + } else { + printf(" source: none\n"); + client.provide_location_information_callback(nullptr, + provide_location_information_callback); + } + + if (location_information_options.force) { + client.force_location_information(); + printf(" force: true\n"); + } else { + printf(" force: false\n"); + } + client.provide_ecid_callback(gModem.get(), provide_ecid_callback); if (!client.connect(location_server_options.host.c_str(), location_server_options.port, @@ -105,8 +189,55 @@ void execute(Options options, ssr_example::Format format, int ura_override, } } -static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message* message, void*) { - if (gFormat == ssr_example::Format::XER) { +static void assistance_data_callback(LPP_Client* client, LPP_Transaction*, LPP_Message* message, + void*) { + if (gFormat == ssr_example::Format::SPARTN_OLD) { + auto messages = gSpartnGeneratorOld.generate(message, gUraOverride, gUBloxClockCorrection, + gForceIodeContinuity); + for (auto& msg : messages) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : gOptions.output_options.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + + if (gUbloxReceiver) { + auto interface = gUbloxReceiver->interface(); + if (interface) { + interface->write(bytes.data(), bytes.size()); + } + } else if (gNmeaReceiver) { + auto interface = gNmeaReceiver->interface(); + if (interface) { + interface->write(bytes.data(), bytes.size()); + } + } + } + } else if (gFormat == ssr_example::Format::SPARTN_NEW) { + auto messages = gSpartnGeneratorNew.generate(message); + for (auto& msg : messages) { + auto data = msg.build(); + if (data.size() == 0) { + printf("Size of SPARTN payload is above the 1024 byte limit\n"); + continue; + } + + for (auto& interface : gOptions.output_options.interfaces) { + interface->write(data.data(), data.size()); + } + + if (gUbloxReceiver) { + auto interface = gUbloxReceiver->interface(); + if (interface) { + interface->write(data.data(), data.size()); + } + } else if (gNmeaReceiver) { + auto interface = gNmeaReceiver->interface(); + if (interface) { + interface->write(data.data(), data.size()); + } + } + } + } else if (gFormat == ssr_example::Format::XER) { std::stringstream buffer; xer_encode( &asn_DEF_LPP_Message, message, XER_F_BASIC, @@ -120,6 +251,15 @@ static void assistance_data_callback(LPP_Client*, LPP_Transaction*, LPP_Message* for (auto& interface : gOptions.output_options.interfaces) { interface->write(message.c_str(), message.size()); } + } else if (gFormat == ssr_example::Format::ASN1_UPER) { + auto octet = client->encode(message); + if (octet) { + for (auto& interface : gOptions.output_options.interfaces) { + interface->write(octet->buf, octet->size); + } + + ASN_STRUCT_FREE(asn_DEF_OCTET_STRING, octet); + } } else { throw std::runtime_error("Unsupported format"); } @@ -133,11 +273,13 @@ void SsrCommand::parse(args::Subparser& parser) { delete mUraOverrideArg; delete mUbloxClockCorrectionArg; delete mForceContinuityArg; + delete mAverageZenithDelayArg; + delete mEnableIodeShift; mFormatArg = new args::ValueFlag(parser, "format", "Format of the output", {"format"}, args::Options::Single); mFormatArg->HelpDefault("xer"); - mFormatArg->HelpChoices({"xer"}); + mFormatArg->HelpChoices({"xer", "spartn", "spartn-old", "asn1-uper"}); mUraOverrideArg = new args::ValueFlag( parser, "ura", @@ -152,6 +294,14 @@ void SsrCommand::parse(args::Subparser& parser) { mForceContinuityArg = new args::Flag(parser, "force-continuity", "Force SF022 (IODE Continuity) to be 320 secs", {"force-continuity"}); + + mAverageZenithDelayArg = + new args::Flag(parser, "average-zenith-delay", + "Compute the average zenith delay and differential for residuals", + {"average-zenith-delay"}); + + mEnableIodeShift = new args::Flag( + parser, "iode-shift", "Enable the IODE shift to fix data stream issues", {"iode-shift"}); } void SsrCommand::execute(Options options) { @@ -159,6 +309,12 @@ void SsrCommand::execute(Options options) { if (*mFormatArg) { if (mFormatArg->Get() == "xer") { format = ssr_example::Format::XER; + } else if (mFormatArg->Get() == "spartn") { + format = ssr_example::Format::SPARTN_NEW; + } else if (mFormatArg->Get() == "spartn-old") { + format = ssr_example::Format::SPARTN_OLD; + } else if (mFormatArg->Get() == "asn1-uper") { + format = ssr_example::Format::ASN1_UPER; } else { throw args::ValidationError("Invalid format"); } @@ -179,7 +335,18 @@ void SsrCommand::execute(Options options) { force_continuity = mForceContinuityArg->Get(); } - ::execute(std::move(options), format, ura_override, ublox_clock_correction, force_continuity); + auto average_zenith_delay = false; + if (*mAverageZenithDelayArg) { + average_zenith_delay = mAverageZenithDelayArg->Get(); + } + + auto iode_shift = false; + if (*mEnableIodeShift) { + iode_shift = mEnableIodeShift->Get(); + } + + ::execute(std::move(options), format, ura_override, ublox_clock_correction, force_continuity, + average_zenith_delay, iode_shift); } } // namespace ssr_example diff --git a/examples/lpp/ssr_example.h b/examples/lpp/ssr_example.h index 8310a32f..d7069b5b 100644 --- a/examples/lpp/ssr_example.h +++ b/examples/lpp/ssr_example.h @@ -5,6 +5,9 @@ namespace ssr_example { enum class Format { XER, + SPARTN_NEW, + SPARTN_OLD, + ASN1_UPER, }; class SsrCommand final : public Command { @@ -12,13 +15,16 @@ class SsrCommand final : public Command { SsrCommand() : Command("ssr", "Request State-space Representation (SSR) data from the location server"), mFormatArg(nullptr), mUraOverrideArg(nullptr), mUbloxClockCorrectionArg(nullptr), - mForceContinuityArg(nullptr) {} + mForceContinuityArg(nullptr), mAverageZenithDelayArg(nullptr), mEnableIodeShift(nullptr) { + } ~SsrCommand() override { delete mFormatArg; delete mUraOverrideArg; delete mUbloxClockCorrectionArg; delete mForceContinuityArg; + delete mAverageZenithDelayArg; + delete mEnableIodeShift; } void parse(args::Subparser& parser) override; @@ -29,6 +35,8 @@ class SsrCommand final : public Command { args::ValueFlag* mUraOverrideArg; args::Flag* mUbloxClockCorrectionArg; args::Flag* mForceContinuityArg; + args::Flag* mAverageZenithDelayArg; + args::Flag* mEnableIodeShift; }; } // namespace ssr_example diff --git a/examples/lpp2spartn/CMakeLists.txt b/examples/lpp2spartn/CMakeLists.txt new file mode 100644 index 00000000..a2cddedd --- /dev/null +++ b/examples/lpp2spartn/CMakeLists.txt @@ -0,0 +1,20 @@ +add_executable(example_lpp2spartn + "main.cpp" + "options.cpp" +) +add_executable(examples::lpp2spartn ALIAS example_lpp2spartn) + +target_include_directories(example_lpp2spartn PRIVATE "./") +target_link_libraries(example_lpp2spartn PRIVATE args) +target_link_libraries(example_lpp2spartn PRIVATE generator::spartn) +target_link_libraries(example_lpp2spartn PRIVATE generator::spartn2) +target_link_libraries(example_lpp2spartn PRIVATE asn1::generated asn1::helper) +target_link_libraries(example_lpp2spartn PRIVATE dependency::interface) + +set_target_properties(example_lpp2spartn PROPERTIES OUTPUT_NAME "example-lpp2spartn") +set_target_properties(example_lpp2spartn PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}") + +if (USE_ASAN) +target_compile_options(example_lpp2spartn PRIVATE -fsanitize=address,undefined,leak) +target_link_libraries(example_lpp2spartn PRIVATE -fsanitize=address,undefined,leak) +endif (USE_ASAN) diff --git a/examples/lpp2spartn/README.md b/examples/lpp2spartn/README.md new file mode 100644 index 00000000..7cdd67b6 --- /dev/null +++ b/examples/lpp2spartn/README.md @@ -0,0 +1,49 @@ +# LPP2SPARTN Example + +This sample code takes UPER encoded 3GPP LPP messages from STDIN and transforms them into SPARTN messages. + +## Usage + +``` + ./example-lpp2spartn {OPTIONS} + + LPP2SPARTN Example (3.4.0) - This sample code takes UPER encoded 3GPP LPP + messages from STDIN and transforms them into SPARTN messages. + + OPTIONS: + + -?, --help Display this help menu + -v, --version Display version information + Options: + --format=[format] Format + One of: spartn, spartn-old + --iode-shift IODE Shift + Output: + File: + --file=[file_path] Path + Serial: + --serial=[device] Device + --serial-baud=[baud_rate] Baud Rate + Default: 115200 + --serial-data=[data_bits] Data Bits + One of: 5, 6, 7, 8 + Default: 8 + --serial-stop=[stop_bits] Stop Bits + One of: 1, 2 + Default: 1 + --serial-parity=[parity_bits] Parity Bits + One of: none, odd, even + Default: none + I2C: + --i2c=[device] Device + --i2c-address=[address] Address + Default: 66 + TCP: + --tcp=[ip_address] Host or IP Address + --tcp-port=[port] Port + UDP: + --udp=[ip_address] Host or IP Address + --udp-port=[port] Port + Stdout: + --stdout Stdout +``` diff --git a/examples/lpp2spartn/main.cpp b/examples/lpp2spartn/main.cpp new file mode 100644 index 00000000..53fcf998 --- /dev/null +++ b/examples/lpp2spartn/main.cpp @@ -0,0 +1,196 @@ +#include +#include +#include +#include +#include +#include "options.hpp" + +struct StdinStream { + StdinStream() { + fd = fileno(stdin); + if (fd < 0) throw std::runtime_error("Failed to open stdin"); + } + + ~StdinStream() { close(fd); } + + // Read from stdin all fill the buffer + bool read() { + auto bytes_read = 0; + uint8_t temp[1024]; + for (;;) { + // check if there is data to be read + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(fd, &readfds); + timeval timeout{0, 0}; + auto ret = select(fd + 1, &readfds, NULL, NULL, &timeout); + if (ret < 0) { + throw std::runtime_error("Failed to read from stdin"); + } else if (ret == 0) { + // no data to be read + break; + } + + // read data + auto bytes = ::read(fd, temp, sizeof(temp)); + if (bytes < 0) { + throw std::runtime_error("Failed to read from stdin"); + } else if (bytes == 0) { + break; + } + + // append to buffer + buffer.insert(buffer.end(), temp, temp + bytes); + bytes_read += bytes; + } + + return bytes_read > 0; + } + + void clear(size_t bytes) { buffer.erase(buffer.begin(), buffer.begin() + bytes); } + + int fd; + std::vector buffer; +}; + +#define ALLOC_ZERO(T) ((T*)calloc(1, sizeof(T))) +LPP_Message* lpp_decode(StdinStream& stream) { + asn_codec_ctx_t stack_ctx{}; + stack_ctx.max_stack_size = 1024 * 1024 * 4; + + LPP_Message* lpp = ALLOC_ZERO(LPP_Message); + asn_dec_rval_t rval = uper_decode_complete(&stack_ctx, &asn_DEF_LPP_Message, (void**)&lpp, + stream.buffer.data(), stream.buffer.size()); + if (rval.code != RC_OK) { + free(lpp); + return NULL; + } + + stream.clear(rval.consumed); + return lpp; +} + +LPP_Message* next_message(StdinStream& stream) { + if (stream.buffer.empty()) { + if (!stream.read()) return NULL; + } + + if (stream.buffer.size() > 0) { + auto message = lpp_decode(stream); + if (message) return message; + if (!stream.read()) return NULL; + } + + return NULL; +} + +int main(int argc, char** argv) { + auto options = parse_configuration(argc, argv); + auto& output = options.output; + auto& spartn = options.spartn; + + StdinStream stream{}; + SPARTN_Generator old_generator{}; + + auto gUraOverride = 2; + auto gUBloxClockCorrection = true; + auto gForceIodeContinuity = true; + + generator::spartn::Generator new_generator{}; + + new_generator.set_ura_override(gUraOverride); + new_generator.set_ublox_clock_correction(gUBloxClockCorrection); + if (gForceIodeContinuity) { + new_generator.set_continuity_indicator(320.0); + } + + if (spartn.iode_shift) { + new_generator.set_iode_shift(true); + } else { + new_generator.set_iode_shift(false); + } + + for (;;) { + auto message = next_message(stream); + if (!message) break; + + if (options.format == Format::SPARTN_NEW) { + auto messages2 = new_generator.generate(message); + for (auto& msg : messages2) { + auto data = msg.build(); + for (auto& interface : output.interfaces) { + interface->write(data.data(), data.size()); + } + } + } else if (options.format == Format::SPARTN_OLD) { + auto messages = old_generator.generate(message, gUraOverride, gUBloxClockCorrection, + gForceIodeContinuity); + + // NOTE(ewasjon): This looks stupid, and it is, but for testing purposes we want to + // output the messages in a specific order. Will be fixed in the future. + for (auto& msg : messages) { + if (msg->message_type == 2 && msg->message_sub_type == 0) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : output.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + } + } + + for (auto& msg : messages) { + if (msg->message_type == 0 && msg->message_sub_type == 0) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : output.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + } + } + + for (auto& msg : messages) { + if (msg->message_type == 0 && msg->message_sub_type == 1) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : output.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + } + } + + for (auto& msg : messages) { + if (msg->message_type == 0 && msg->message_sub_type == 2) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : output.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + } + } + for (auto& msg : messages) { + if (msg->message_type == 1 && msg->message_sub_type == 0) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : output.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + } + } + for (auto& msg : messages) { + if (msg->message_type == 1 && msg->message_sub_type == 1) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : output.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + } + } + for (auto& msg : messages) { + if (msg->message_type == 1 && msg->message_sub_type == 2) { + auto bytes = SPARTN_Transmitter::build(msg); + for (auto& interface : output.interfaces) { + interface->write(bytes.data(), bytes.size()); + } + } + } + } + + ASN_STRUCT_FREE(asn_DEF_LPP_Message, message); + } + + return 0; +} diff --git a/examples/lpp2spartn/options.cpp b/examples/lpp2spartn/options.cpp new file mode 100644 index 00000000..b6d189d7 --- /dev/null +++ b/examples/lpp2spartn/options.cpp @@ -0,0 +1,297 @@ +#include "options.hpp" +#include + +using namespace interface; + +args::Group arguments{"Arguments:"}; + +// +// Format +// + +args::Group format{ + "Options:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::ValueFlag format_value{ + format, "format", "Format", {"format"}, args::Options::Single}; + +args::Flag iode_shift{format, "iode-shift", "IODE Shift", {"iode-shift"}, args::Options::Single}; + +Format parse_format_options() { + if (!format_value) { + throw args::RequiredError("format"); + } + + if (format_value.Get() == "spartn") { + return Format::SPARTN_NEW; + } else if (format_value.Get() == "spartn-old") { + return Format::SPARTN_OLD; + } else { + throw args::ValidationError("Invalid format"); + } +} + +SpartnOptions parse_spartn_options() { + SpartnOptions options{}; + options.iode_shift = false; + + if (iode_shift) { + options.iode_shift = iode_shift.Get(); + } + + return options; +} + +// +// Output +// + +args::Group output{ + "Output:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::Group file_output{ + output, + "File:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag file_path{ + file_output, "file_path", "Path", {"file"}, args::Options::Single}; + +args::Group serial_output{ + output, + "Serial:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag serial_device{ + serial_output, "device", "Device", {"serial"}, args::Options::Single}; +args::ValueFlag serial_baud_rate{ + serial_output, "baud_rate", "Baud Rate", {"serial-baud"}, args::Options::Single}; +args::ValueFlag serial_data_bits{ + serial_output, "data_bits", "Data Bits", {"serial-data"}, args::Options::Single}; +args::ValueFlag serial_stop_bits{ + serial_output, "stop_bits", "Stop Bits", {"serial-stop"}, args::Options::Single}; +args::ValueFlag serial_parity_bits{ + serial_output, "parity_bits", "Parity Bits", {"serial-parity"}, args::Options::Single}; + +args::Group i2c_output{ + output, + "I2C:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag i2c_device{ + i2c_output, "device", "Device", {"i2c"}, args::Options::Single}; +args::ValueFlag i2c_address{ + i2c_output, "address", "Address", {"i2c-address"}, args::Options::Single}; + +args::Group tcp_output{ + output, + "TCP:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag tcp_ip_address{ + tcp_output, "ip_address", "Host or IP Address", {"tcp"}, args::Options::Single}; +args::ValueFlag tcp_port{tcp_output, "port", "Port", {"tcp-port"}, args::Options::Single}; + +args::Group udp_output{ + output, + "UDP:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag udp_ip_address{ + udp_output, "ip_address", "Host or IP Address", {"udp"}, args::Options::Single}; +args::ValueFlag udp_port{udp_output, "port", "Port", {"udp-port"}, args::Options::Single}; + +args::Group stdout_output{ + output, + "Stdout:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::Flag stdout_output_flag{stdout_output, "stdout", "Stdout", {"stdout"}, args::Options::Single}; + +OutputOptions parse_output_options() { + OutputOptions output{}; + + if (file_path) { + auto interface = interface::Interface::file(file_path.Get(), true); + output.interfaces.emplace_back(interface); + } + + if (serial_device || serial_baud_rate) { + if (!serial_device) { + throw args::RequiredError("serial_device"); + } + + uint32_t baud_rate = 115200; + if (serial_baud_rate) { + if (serial_baud_rate.Get() < 0) { + throw args::ValidationError("serial_baud_rate must be positive"); + } + + baud_rate = static_cast(serial_baud_rate.Get()); + } + + auto data_bits = DataBits::EIGHT; + if (serial_data_bits) { + switch (serial_data_bits.Get()) { + case 5: data_bits = DataBits::FIVE; break; + case 6: data_bits = DataBits::SIX; break; + case 7: data_bits = DataBits::SEVEN; break; + case 8: data_bits = DataBits::EIGHT; break; + default: throw args::ValidationError("Invalid data bits"); + } + } + + auto stop_bits = StopBits::ONE; + if (serial_stop_bits) { + switch (serial_stop_bits.Get()) { + case 1: stop_bits = StopBits::ONE; break; + case 2: stop_bits = StopBits::TWO; break; + default: throw args::ValidationError("Invalid stop bits"); + } + } + + auto parity_bit = ParityBit::NONE; + if (serial_parity_bits) { + if (serial_parity_bits.Get() == "none") { + parity_bit = ParityBit::NONE; + } else if (serial_parity_bits.Get() == "odd") { + parity_bit = ParityBit::ODD; + } else if (serial_parity_bits.Get() == "even") { + parity_bit = ParityBit::EVEN; + } else { + throw args::ValidationError("Invalid parity bits"); + } + } + + auto interface = interface::Interface::serial(serial_device.Get(), baud_rate, data_bits, + stop_bits, parity_bit); + output.interfaces.emplace_back(interface); + } + + if (i2c_device || i2c_address) { + if (!i2c_device) { + throw args::RequiredError("i2c_device"); + } + + if (!i2c_address) { + throw args::RequiredError("i2c_address"); + } + + auto interface = interface::Interface::i2c(i2c_device.Get(), i2c_address.Get()); + output.interfaces.emplace_back(interface); + } + + if (tcp_ip_address || tcp_port) { + if (!tcp_ip_address) { + throw args::RequiredError("tcp_ip_address"); + } + + if (!tcp_port) { + throw args::RequiredError("tcp_port"); + } + + auto interface = + interface::Interface::tcp(tcp_ip_address.Get(), tcp_port.Get(), true /* reconnect */); + output.interfaces.emplace_back(interface); + } + + if (udp_ip_address || udp_port) { + if (!udp_ip_address) { + throw args::RequiredError("udp_ip_address"); + } + + if (!udp_port) { + throw args::RequiredError("udp_port"); + } + + auto interface = + interface::Interface::udp(udp_ip_address.Get(), udp_port.Get(), true /* reconnect */); + output.interfaces.emplace_back(interface); + } + + if (stdout_output_flag) { + auto interface = interface::Interface::stdout(); + output.interfaces.emplace_back(interface); + } + + for (auto& interface : output.interfaces) { + interface->open(); + } + + return output; +} + +Options parse_configuration(int argc, char** argv) { + args::ArgumentParser parser("LPP2SPARTN Example (" CLIENT_VERSION + ") - This sample code takes UPER encoded 3GPP LPP messages from " + "STDIN and transforms them into SPARTN messages."); + + args::HelpFlag help{parser, "help", "Display this help menu", {'?', "help"}}; + args::Flag version{parser, "version", "Display version information", {'v', "version"}}; + + format_value.HelpChoices({ + "spartn", + "spartn-old", + }); + + i2c_address.HelpDefault("66"); + serial_baud_rate.HelpDefault("115200"); + + serial_data_bits.HelpDefault("8"); + serial_data_bits.HelpChoices({"5", "6", "7", "8"}); + + serial_stop_bits.HelpDefault("1"); + serial_stop_bits.HelpChoices({"1", "2"}); + + serial_parity_bits.HelpDefault("none"); + serial_parity_bits.HelpChoices({ + "none", + "odd", + "even", + }); + + // Globals + args::GlobalOptions format_globals{parser, format}; + args::GlobalOptions output_globals{parser, output}; + + try { + parser.ParseCLI(argc, argv); + + if (version) { + std::cout << "LPP2SPARTN Example (" CLIENT_VERSION << ")" << std::endl; + exit(0); + } + + return Options{ + .format = parse_format_options(), + .output = parse_output_options(), + .spartn = parse_spartn_options(), + }; + } catch (const args::ValidationError& e) { + std::cerr << e.what() << std::endl; + parser.Help(std::cerr); + exit(1); + } catch (const args::Help&) { + std::cout << parser; + exit(0); + } catch (const args::ParseError& e) { + std::cerr << e.what() << std::endl; + std::cerr << parser; + exit(1); + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + exit(1); + } +} diff --git a/examples/lpp2spartn/options.hpp b/examples/lpp2spartn/options.hpp new file mode 100644 index 00000000..74ace7c3 --- /dev/null +++ b/examples/lpp2spartn/options.hpp @@ -0,0 +1,31 @@ +#pragma once +#include +#include +#include + +/// Format options. +enum class Format { + /// SPARTN format, using the old SPARTN generator. + SPARTN_OLD, + /// SPARTN format, using the new SPARTN generator. + SPARTN_NEW, +}; + +/// SPARTN options. +struct SpartnOptions { + bool iode_shift; +}; + +/// Output options. +struct OutputOptions { + /// Interfaces to output data to. + std::vector> interfaces; +}; + +struct Options { + Format format; + OutputOptions output; + SpartnOptions spartn; +}; + +extern Options parse_configuration(int argc, char** argv); diff --git a/examples/nmea/CMakeLists.txt b/examples/nmea/CMakeLists.txt new file mode 100644 index 00000000..f151d7ac --- /dev/null +++ b/examples/nmea/CMakeLists.txt @@ -0,0 +1,18 @@ +add_executable(example_nmea + "main.cpp" + "options.cpp" +) +add_executable(examples::nmea ALIAS example_nmea) + +target_include_directories(example_nmea PRIVATE "./") +target_link_libraries(example_nmea PRIVATE args) +target_link_libraries(example_nmea PRIVATE receiver::nmea) +target_link_libraries(example_nmea PRIVATE dependency::interface) + +set_target_properties(example_nmea PROPERTIES OUTPUT_NAME "example-nmea") +set_target_properties(example_nmea PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}") + +if (USE_ASAN) +target_compile_options(example_nmea PRIVATE -fsanitize=address,undefined,leak) +target_link_libraries(example_nmea PRIVATE -fsanitize=address,undefined,leak) +endif (USE_ASAN) diff --git a/examples/nmea/README.md b/examples/nmea/README.md new file mode 100644 index 00000000..250bfe25 --- /dev/null +++ b/examples/nmea/README.md @@ -0,0 +1,38 @@ +# NMEA Example + +This is an example program that demonstrates how to use the NMEA receiver library. The program takes an interface and port associated with the receiver as arguments. It will print all received messages to stdout. + +## Usage + +``` + ./example-nmea {OPTIONS} + + NMEA Example (3.4.0) - This is an example program that demonstrates how to + use the NMEA receiver library. The program takes an interface and port + associated with the receiver as arguments and print all received messages. + + OPTIONS: + + -?, --help Display this help menu + -v, --version Display version information + Interface: + Serial: + --serial=[device] Device + --serial-baud=[baud_rate] Baud Rate + Default: 115200 + --serial-data=[data_bits] Data Bits + One of: 5, 6, 7, 8 + Default: 8 + --serial-stop=[stop_bits] Stop Bits + One of: 1, 2 + Default: 1 + --serial-parity=[parity_bits] Parity Bits + One of: none, odd, even + Default: none + I2C: + --i2c=[device] Device + --i2c-address=[address] Address + Default: 66 + Stdin: + --stdin Enable stdin +``` diff --git a/examples/nmea/main.cpp b/examples/nmea/main.cpp new file mode 100644 index 00000000..745c9499 --- /dev/null +++ b/examples/nmea/main.cpp @@ -0,0 +1,33 @@ +#include "options.hpp" + +#include +#include +#include +#include + +using namespace receiver::nmea; + +static void receiver_loop(NmeaReceiver& receiver) { + receiver.interface().print_info(); + + printf("[nmea]\n"); + printf("-----------------------------------------------------\n"); + + for (;;) { + // Wait for the next nmea message. + auto message = receiver.wait_for_message(); + if (message) { + message->print(); + } else { + break; + } + + usleep(10 * 1000); + } +} + +int main(int argc, char** argv) { + auto receiver = parse_configuration(argc, argv); + receiver_loop(*receiver.get()); + return 0; +} diff --git a/examples/nmea/options.cpp b/examples/nmea/options.cpp new file mode 100644 index 00000000..cb8d6c60 --- /dev/null +++ b/examples/nmea/options.cpp @@ -0,0 +1,209 @@ +#include +#include +#include + +using namespace interface; +using namespace receiver::nmea; + +args::Group arguments{"Arguments:"}; + +// +// Interface +// + +args::Group interface_group{ + "Interface:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::Group serial_group{ + interface_group, + "Serial:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag serial_device{ + serial_group, "device", "Device", {"serial"}, args::Options::Single}; +args::ValueFlag serial_baud_rate{ + serial_group, "baud_rate", "Baud Rate", {"serial-baud"}, args::Options::Single}; +args::ValueFlag serial_data_bits{ + serial_group, "data_bits", "Data Bits", {"serial-data"}, args::Options::Single}; +args::ValueFlag serial_stop_bits{ + serial_group, "stop_bits", "Stop Bits", {"serial-stop"}, args::Options::Single}; +args::ValueFlag serial_parity_bits{ + serial_group, "parity_bits", "Parity Bits", {"serial-parity"}, args::Options::Single}; + +args::Group i2c_group{ + interface_group, + "I2C:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag i2c_device{ + i2c_group, "device", "Device", {"i2c"}, args::Options::Single}; +args::ValueFlag i2c_address{ + i2c_group, "address", "Address", {"i2c-address"}, args::Options::Single}; + +args::Group stdin_group{ + interface_group, + "Stdin:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::Flag stdin_device{stdin_group, "", "Enable stdin", {"stdin"}, args::Options::Single}; + +// +// +// + +static std::unique_ptr parse_serial() { + assert(serial_device); + + auto baud_rate = 115200; + if (serial_baud_rate) { + baud_rate = serial_baud_rate.Get(); + } + + auto data_bits = DataBits::EIGHT; + if (serial_data_bits) { + switch (serial_data_bits.Get()) { + case 5: data_bits = DataBits::FIVE; break; + case 6: data_bits = DataBits::SIX; break; + case 7: data_bits = DataBits::SEVEN; break; + case 8: data_bits = DataBits::EIGHT; break; + default: throw args::ValidationError("Invalid data bits"); + } + } + + auto stop_bits = StopBits::ONE; + if (serial_stop_bits) { + switch (serial_stop_bits.Get()) { + case 1: stop_bits = StopBits::ONE; break; + case 2: stop_bits = StopBits::TWO; break; + default: throw args::ValidationError("Invalid stop bits"); + } + } + + auto parity_bit = ParityBit::NONE; + if (serial_parity_bits) { + if (serial_parity_bits.Get() == "none") { + parity_bit = ParityBit::NONE; + } else if (serial_parity_bits.Get() == "odd") { + parity_bit = ParityBit::ODD; + } else if (serial_parity_bits.Get() == "even") { + parity_bit = ParityBit::EVEN; + } else { + throw args::ValidationError("Invalid parity bits"); + } + } + + switch (data_bits) { + case DataBits::FIVE: printf(" data bits: 5\n"); break; + case DataBits::SIX: printf(" data bits: 6\n"); break; + case DataBits::SEVEN: printf(" data bits: 7\n"); break; + case DataBits::EIGHT: printf(" data bits: 8\n"); break; + } + + switch (stop_bits) { + case StopBits::ONE: printf(" stop bits: 1\n"); break; + case StopBits::TWO: printf(" stop bits: 2\n"); break; + } + + switch (parity_bit) { + case ParityBit::NONE: printf(" parity bit: none\n"); break; + case ParityBit::ODD: printf(" parity bit: odd\n"); break; + case ParityBit::EVEN: printf(" parity bit: even\n"); break; + } + + return std::unique_ptr( + Interface::serial(serial_device.Get(), baud_rate, data_bits, stop_bits, parity_bit)); +} + +static std::unique_ptr parse_i2c() { + assert(i2c_device); + + auto address = 66; + if (i2c_address) { + address = i2c_address.Get(); + } + + return std::unique_ptr(Interface::i2c(i2c_device.Get(), address)); +} + +static std::unique_ptr parse_interface() { + if (serial_device) { + return parse_serial(); + } else if (i2c_device) { + return parse_i2c(); + } else if (stdin_device) { + return std::unique_ptr(Interface::stdin()); + } else { + throw args::RequiredError("No interface specified: --serial, --i2c, or --stdin"); + } +} + +static std::unique_ptr create_receiver() { + auto interface = parse_interface(); + interface->open(); + if (!interface->is_open()) { + throw std::runtime_error("Could not open interface"); + } + + auto receiver = std::unique_ptr(new NmeaReceiver(std::move(interface))); + return receiver; +} + +std::unique_ptr parse_configuration(int argc, char** argv) { + args::ArgumentParser parser( + "NMEA Example (" CLIENT_VERSION + ") - This is an example program that demonstrates how to use the NMEA receiver " + "library. The program takes an interface and port associated with the " + "receiver as arguments and print all received messages."); + + args::HelpFlag help{parser, "help", "Display this help menu", {'?', "help"}}; + args::Flag version{parser, "version", "Display version information", {'v', "version"}}; + + i2c_address.HelpDefault("66"); + serial_baud_rate.HelpDefault("115200"); + + serial_data_bits.HelpDefault("8"); + serial_data_bits.HelpChoices({"5", "6", "7", "8"}); + + serial_stop_bits.HelpDefault("1"); + serial_stop_bits.HelpChoices({"1", "2"}); + + serial_parity_bits.HelpDefault("none"); + serial_parity_bits.HelpChoices({ + "none", + "odd", + "even", + }); + + args::GlobalOptions interface_globals{parser, interface_group}; + + try { + parser.ParseCLI(argc, argv); + + if (version) { + std::cout << "NMEA Example (" << CLIENT_VERSION << ")" << std::endl; + exit(0); + } + + return create_receiver(); + } catch (const args::ValidationError& e) { + std::cerr << e.what() << std::endl; + parser.Help(std::cerr); + exit(1); + } catch (const args::Help&) { + std::cout << parser; + exit(0); + } catch (const args::ParseError& e) { + std::cerr << e.what() << std::endl; + std::cerr << parser; + exit(1); + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + exit(1); + } +} diff --git a/examples/nmea/options.hpp b/examples/nmea/options.hpp new file mode 100644 index 00000000..7314be8e --- /dev/null +++ b/examples/nmea/options.hpp @@ -0,0 +1,10 @@ +#pragma once +#include + +namespace receiver { +namespace nmea { +class NmeaReceiver; +} // namespace nmea +} // namespace receiver + +extern std::unique_ptr parse_configuration(int argc, char** argv); diff --git a/examples/ntrip/CMakeLists.txt b/examples/ntrip/CMakeLists.txt new file mode 100644 index 00000000..15e010e2 --- /dev/null +++ b/examples/ntrip/CMakeLists.txt @@ -0,0 +1,17 @@ +add_executable(example_ntrip + "main.cpp" + "options.cpp" +) +add_executable(examples::ntrip ALIAS example_ntrip) + +target_include_directories(example_ntrip PRIVATE "./") +target_link_libraries(example_ntrip PRIVATE args) +target_link_libraries(example_ntrip PRIVATE dependency::interface) + +set_target_properties(example_ntrip PROPERTIES OUTPUT_NAME "example-ntrip") +set_target_properties(example_ntrip PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}") + +if (USE_ASAN) +target_compile_options(example_ntrip PRIVATE -fsanitize=address,undefined,leak) +target_link_libraries(example_ntrip PRIVATE -fsanitize=address,undefined,leak) +endif (USE_ASAN) diff --git a/examples/ntrip/README.md b/examples/ntrip/README.md new file mode 100644 index 00000000..469e35ae --- /dev/null +++ b/examples/ntrip/README.md @@ -0,0 +1,55 @@ +# NTRIP Example + +This sample code illustrates the process of utilizing the NTRIP client to establish a communication link with a caster and transmit the data to a serial port, file, or stdout. + +## Usage + +``` + ./example-ntrip {OPTIONS} + + NTRIP Example (3.4.0) - This sample code illustrates the process of + utilizing the NTRIP client to establish a communication link with a caster + and transmit the data to a serial port, file, or stdout. + + OPTIONS: + + -?, --help Display this help menu + -v, --version Display version information + NTRIP: + --host=[hostname] Hostname + --port=[port] Port + Default: 2101 + --mountpoint=[mountpoint] Mountpoint + --username=[username] Username + --password=[password] Password + --nmea=[nmea_string] NMEA String + Output: + File: + --file=[file_path] Path + Serial: + --serial=[device] Device + --serial-baud=[baud_rate] Baud Rate + Default: 115200 + --serial-data=[data_bits] Data Bits + One of: 5, 6, 7, 8 + Default: 8 + --serial-stop=[stop_bits] Stop Bits + One of: 1, 2 + Default: 1 + --serial-parity=[parity_bits] Parity Bits + One of: none, odd, even + Default: none + I2C: + --i2c=[device] Device + --i2c-address=[address] Address + Default: 66 + TCP: + --tcp=[ip_address] Host or IP Address + --tcp-port=[port] Port + UDP: + --udp=[ip_address] Host or IP Address + --udp-port=[port] Port + Stdout: + --stdout Stdout + +``` diff --git a/examples/ntrip/main.cpp b/examples/ntrip/main.cpp new file mode 100644 index 00000000..842b765d --- /dev/null +++ b/examples/ntrip/main.cpp @@ -0,0 +1,183 @@ +#include +#include +#include +#include +#include +#include +#include "options.hpp" + +// resolve sockaddr from hostname +static sockaddr_in resolve(const std::string& hostname, uint16_t port) { + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + + auto host = gethostbyname(hostname.c_str()); + if (!host) throw std::runtime_error("Failed to resolve hostname"); + + memcpy(&addr.sin_addr, host->h_addr_list[0], host->h_length); + return addr; +} + +static std::string base64_encode(uint8_t* data, size_t size) { + static const char* table = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + + std::string encoded; + encoded.reserve((size + 2) / 3 * 4); + + for (size_t i = 0; i < size; i += 3) { + uint32_t temp = 0; + temp |= data[i + 0] << 16; + if (i + 1 < size) temp |= data[i + 1] << 8; + if (i + 2 < size) temp |= data[i + 2] << 0; + + encoded += table[(temp >> 18) & 0x3f]; + encoded += table[(temp >> 12) & 0x3f]; + if (i + 1 < size) { + encoded += table[(temp >> 6) & 0x3f]; + } else { + encoded += '='; + } + if (i + 2 < size) { + encoded += table[(temp >> 0) & 0x3f]; + } else { + encoded += '='; + } + } + + return encoded; +} + +// username:password -> base64 +static std::string authorization_basic(const std::string& username, const std::string& password) { + auto data = username + ":" + password; + auto size = data.size(); + auto temp = (uint8_t*)malloc(size); + memcpy(temp, data.c_str(), size); + auto encoded = base64_encode(temp, size); + free(temp); + return encoded; +} + +// hexdump with ascii +static void hexdump(const void* data, size_t size) { + auto bytes = (const uint8_t*)data; + for (size_t i = 0; i < size; i += 16) { + printf("%08lx: ", i); + + // hex + for (size_t j = 0; j < 16; j++) { + if (i + j < size) { + printf("%02x ", bytes[i + j]); + } else { + printf(" "); + } + } + + // ascii + printf(" "); + for (size_t j = 0; j < 16; j++) { + if (i + j < size) { + auto c = bytes[i + j]; + if (c >= 0x20 && c <= 0x7e) { + printf("%c", c); + } else { + printf("."); + } + } else { + printf(" "); + } + } + + printf("\n"); + } +} + +class Ntrip { +public: + Ntrip(sockaddr_in addr) { + mSocket = socket(AF_INET, SOCK_STREAM, 0); + if (mSocket < 0) throw std::runtime_error("Failed to create socket"); + + if (connect(mSocket, (sockaddr*)&addr, sizeof(addr)) < 0) { + throw std::runtime_error("Failed to connect to host"); + } + } + + ~Ntrip() { close(mSocket); } + + void authorize(std::string username, std::string password) { + mUsername = username; + mPassword = password; + } + + void request(const std::string& mountpoint) { + std::string request = "GET /" + mountpoint + " HTTP/1.0\r\n"; + request += "User-Agent: NTRIP 2.0/SUPL-3GPP-LPP-client\r\n"; + request += "Accept: */*\r\n"; + request += "Connection: close\r\n"; + if (!mUsername.empty() && !mPassword.empty()) { + request += "Authorization: Basic " + authorization_basic(mUsername, mPassword) + "\r\n"; + } + request += "\r\n"; + + if (send(mSocket, request.c_str(), request.size(), 0) < 0) { + throw std::runtime_error("Failed to send request"); + } + } + + void nmea_update(const std::string& nmea) { + std::string request = nmea + "\r\n"; + if (send(mSocket, request.c_str(), request.size(), 0) < 0) { + throw std::runtime_error("Failed to send request"); + } + } + + ssize_t read(void* buffer, size_t size) { return ::read(mSocket, buffer, size); } + +private: + int mSocket; + std::string mUsername; + std::string mPassword; +}; + +int main(int argc, char** argv) { + auto options = parse_configuration(argc, argv); + auto& host = options.host; + auto& output = options.output; + + // resolve hostname + auto addr = resolve(host.hostname, host.port); + + // connect to host + Ntrip ntrip(addr); + ntrip.authorize(host.username, host.password); + + if (host.mountpoint) { + ntrip.request(*host.mountpoint); + } else { + ntrip.request(""); + } + + if (!host.nmea.empty()) { + ntrip.nmea_update(host.nmea); + } + + char temp[4096]; + for (;;) { + auto bytes = ntrip.read(temp, sizeof(temp)); + if (bytes < 0) { + throw std::runtime_error("Failed to read from NTRIP"); + } else if (bytes == 0) { + break; + } + + hexdump(temp, bytes); + + for (auto& interface : output.interfaces) { + interface->write(temp, bytes); + } + } + + return 0; +} diff --git a/examples/ntrip/options.cpp b/examples/ntrip/options.cpp new file mode 100644 index 00000000..b0b4e17e --- /dev/null +++ b/examples/ntrip/options.cpp @@ -0,0 +1,322 @@ +#include "options.hpp" +#include + +using namespace interface; + +args::Group arguments{"Arguments:"}; + +// +// NTRIP +// + +args::Group ntrip{ + arguments, + "NTRIP:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::ValueFlag ntrip_hostname{ + ntrip, "hostname", "Hostname", {"host"}, args::Options::Single}; +args::ValueFlag ntrip_port{ntrip, "port", "Port", {"port"}, args::Options::Single}; +args::ValueFlag ntrip_mountpoint{ + ntrip, "mountpoint", "Mountpoint", {"mountpoint"}, args::Options::Single}; +args::ValueFlag ntrip_username{ + ntrip, "username", "Username", {"username"}, args::Options::Single}; +args::ValueFlag ntrip_password{ + ntrip, "password", "Password", {"password"}, args::Options::Single}; + +args::ValueFlag nmea_string{ + ntrip, "nmea_string", "NMEA String", {"nmea"}, args::Options::Single}; + +HostOptions parse_host_options() { + if (!ntrip_hostname) { + throw args::RequiredError("ntrip_hostname"); + } + + std::unique_ptr mountpoint; + if (ntrip_mountpoint) { + mountpoint = std::unique_ptr(new std::string(ntrip_mountpoint.Get())); + } + + uint16_t port = 2101; + if (ntrip_port) { + if (ntrip_port.Get() < 0) { + throw args::ValidationError("ntrip_port must be positive"); + } + + port = static_cast(ntrip_port.Get()); + } + + std::string username; + if (ntrip_username) { + username = ntrip_username.Get(); + } + + std::string password; + if (ntrip_password) { + password = ntrip_password.Get(); + } + + std::string nmea; + if (nmea_string) { + nmea = nmea_string.Get(); + } + + return HostOptions{ + .hostname = ntrip_hostname.Get(), + .port = port, + .mountpoint = std::move(mountpoint), + .username = username, + .password = password, + .nmea = nmea, + }; +} + +// +// Output +// + +args::Group output{ + "Output:", + args::Group::Validators::AllChildGroups, + args::Options::Global, +}; + +args::Group file_output{ + output, + "File:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag file_path{ + file_output, "file_path", "Path", {"file"}, args::Options::Single}; + +args::Group serial_output{ + output, + "Serial:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag serial_device{ + serial_output, "device", "Device", {"serial"}, args::Options::Single}; +args::ValueFlag serial_baud_rate{ + serial_output, "baud_rate", "Baud Rate", {"serial-baud"}, args::Options::Single}; +args::ValueFlag serial_data_bits{ + serial_output, "data_bits", "Data Bits", {"serial-data"}, args::Options::Single}; +args::ValueFlag serial_stop_bits{ + serial_output, "stop_bits", "Stop Bits", {"serial-stop"}, args::Options::Single}; +args::ValueFlag serial_parity_bits{ + serial_output, "parity_bits", "Parity Bits", {"serial-parity"}, args::Options::Single}; + +args::Group i2c_output{ + output, + "I2C:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag i2c_device{ + i2c_output, "device", "Device", {"i2c"}, args::Options::Single}; +args::ValueFlag i2c_address{ + i2c_output, "address", "Address", {"i2c-address"}, args::Options::Single}; + +args::Group tcp_output{ + output, + "TCP:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag tcp_ip_address{ + tcp_output, "ip_address", "Host or IP Address", {"tcp"}, args::Options::Single}; +args::ValueFlag tcp_port{tcp_output, "port", "Port", {"tcp-port"}, args::Options::Single}; + +args::Group udp_output{ + output, + "UDP:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::ValueFlag udp_ip_address{ + udp_output, "ip_address", "Host or IP Address", {"udp"}, args::Options::Single}; +args::ValueFlag udp_port{udp_output, "port", "Port", {"udp-port"}, args::Options::Single}; + +args::Group stdout_output{ + output, + "Stdout:", + args::Group::Validators::AllOrNone, + args::Options::Global, +}; +args::Flag stdout_output_flag{stdout_output, "stdout", "Stdout", {"stdout"}, args::Options::Single}; + +OutputOptions parse_output_options() { + OutputOptions output{}; + + if (file_path) { + auto interface = interface::Interface::file(file_path.Get(), true); + output.interfaces.emplace_back(interface); + } + + if (serial_device || serial_baud_rate) { + if (!serial_device) { + throw args::RequiredError("serial_device"); + } + + uint32_t baud_rate = 115200; + if (serial_baud_rate) { + if (serial_baud_rate.Get() < 0) { + throw args::ValidationError("serial_baud_rate must be positive"); + } + + baud_rate = static_cast(serial_baud_rate.Get()); + } + + auto data_bits = DataBits::EIGHT; + if (serial_data_bits) { + switch (serial_data_bits.Get()) { + case 5: data_bits = DataBits::FIVE; break; + case 6: data_bits = DataBits::SIX; break; + case 7: data_bits = DataBits::SEVEN; break; + case 8: data_bits = DataBits::EIGHT; break; + default: throw args::ValidationError("Invalid data bits"); + } + } + + auto stop_bits = StopBits::ONE; + if (serial_stop_bits) { + switch (serial_stop_bits.Get()) { + case 1: stop_bits = StopBits::ONE; break; + case 2: stop_bits = StopBits::TWO; break; + default: throw args::ValidationError("Invalid stop bits"); + } + } + + auto parity_bit = ParityBit::NONE; + if (serial_parity_bits) { + if (serial_parity_bits.Get() == "none") { + parity_bit = ParityBit::NONE; + } else if (serial_parity_bits.Get() == "odd") { + parity_bit = ParityBit::ODD; + } else if (serial_parity_bits.Get() == "even") { + parity_bit = ParityBit::EVEN; + } else { + throw args::ValidationError("Invalid parity bits"); + } + } + + auto interface = interface::Interface::serial(serial_device.Get(), baud_rate, data_bits, + stop_bits, parity_bit); + output.interfaces.emplace_back(interface); + } + + if (i2c_device || i2c_address) { + if (!i2c_device) { + throw args::RequiredError("i2c_device"); + } + + if (!i2c_address) { + throw args::RequiredError("i2c_address"); + } + + auto interface = interface::Interface::i2c(i2c_device.Get(), i2c_address.Get()); + output.interfaces.emplace_back(interface); + } + + if (tcp_ip_address || tcp_port) { + if (!tcp_ip_address) { + throw args::RequiredError("tcp_ip_address"); + } + + if (!tcp_port) { + throw args::RequiredError("tcp_port"); + } + + auto interface = + interface::Interface::tcp(tcp_ip_address.Get(), tcp_port.Get(), true /* reconnect */); + output.interfaces.emplace_back(interface); + } + + if (udp_ip_address || udp_port) { + if (!udp_ip_address) { + throw args::RequiredError("udp_ip_address"); + } + + if (!udp_port) { + throw args::RequiredError("udp_port"); + } + + auto interface = + interface::Interface::udp(udp_ip_address.Get(), udp_port.Get(), true /* reconnect */); + output.interfaces.emplace_back(interface); + } + + if (stdout_output_flag) { + auto interface = interface::Interface::stdout(); + output.interfaces.emplace_back(interface); + } + + for (auto& interface : output.interfaces) { + interface->open(); + } + + return output; +} + +Options parse_configuration(int argc, char** argv) { + args::ArgumentParser parser("NTRIP Example (" CLIENT_VERSION + ") - This sample code illustrates the process of utilizing the " + "NTRIP client to establish a communication link with a caster and " + "transmit the data to a serial port, file, or stdout."); + + args::HelpFlag help{parser, "help", "Display this help menu", {'?', "help"}}; + args::Flag version{parser, "version", "Display version information", {'v', "version"}}; + + ntrip_port.HelpDefault("2101"); + + i2c_address.HelpDefault("66"); + serial_baud_rate.HelpDefault("115200"); + + serial_data_bits.HelpDefault("8"); + serial_data_bits.HelpChoices({"5", "6", "7", "8"}); + + serial_stop_bits.HelpDefault("1"); + serial_stop_bits.HelpChoices({"1", "2"}); + + serial_parity_bits.HelpDefault("none"); + serial_parity_bits.HelpChoices({ + "none", + "odd", + "even", + }); + + // Globals + args::GlobalOptions ntrip_globals{parser, ntrip}; + args::GlobalOptions output_globals{parser, output}; + + try { + parser.ParseCLI(argc, argv); + + if (version) { + std::cout << "NTRIP Example (" CLIENT_VERSION ")" << std::endl; + exit(0); + } + + return Options{ + .host = parse_host_options(), + .output = parse_output_options(), + }; + } catch (const args::ValidationError& e) { + std::cerr << e.what() << std::endl; + parser.Help(std::cerr); + exit(1); + } catch (const args::Help&) { + std::cout << parser; + exit(0); + } catch (const args::ParseError& e) { + std::cerr << e.what() << std::endl; + std::cerr << parser; + exit(1); + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + exit(1); + } +} diff --git a/examples/ntrip/options.hpp b/examples/ntrip/options.hpp new file mode 100644 index 00000000..22e9f557 --- /dev/null +++ b/examples/ntrip/options.hpp @@ -0,0 +1,38 @@ +#pragma once +#include +#include +#include + +/// Host options. +struct HostOptions { + /// Hostname to connect to. + std::string hostname; + + /// Port to connect to. + uint16_t port; + + /// Mountpoint to connect to. + std::unique_ptr mountpoint; + + /// Username to connect with. + std::string username; + + /// Password to connect with. + std::string password; + + /// Nmea String + std::string nmea; +}; + +/// Output options. +struct OutputOptions { + /// Interfaces to output data to. + std::vector> interfaces; +}; + +struct Options { + HostOptions host; + OutputOptions output; +}; + +extern Options parse_configuration(int argc, char** argv); diff --git a/generator/rtcm/extract/extract.hpp b/generator/rtcm/extract/extract.hpp index 12476784..d0ca592a 100644 --- a/generator/rtcm/extract/extract.hpp +++ b/generator/rtcm/extract/extract.hpp @@ -62,7 +62,7 @@ static GenericGnssId gnss_id(const GNSS_SystemTime& src_time) { return GenericGnssId::GPS; } -static TAI_Time epoch_time(const GNSS_SystemTime& src_time) { +static ts::TAI_Time epoch_time(const GNSS_SystemTime& src_time) { auto day_number = decode::day_number(src_time); auto time_of_day_seconds = decode::time_of_day(src_time); auto time_of_day_fraction = decode::time_of_day_fraction(src_time); @@ -73,12 +73,12 @@ static TAI_Time epoch_time(const GNSS_SystemTime& src_time) { auto gnss = decode::gnss_id(src_time); switch (gnss) { - case GenericGnssId::GPS: return TAI_Time(GPS_Time(day_number, time_of_day)); - case GenericGnssId::GLONASS: return TAI_Time(GLO_Time(day_number, time_of_day)); - case GenericGnssId::GALILEO: return TAI_Time(GST_Time(day_number, time_of_day)); - case GenericGnssId::BEIDOU: return TAI_Time(BDT_Time(day_number, time_of_day)); + case GenericGnssId::GPS: return ts::TAI_Time(ts::GPS_Time(day_number, time_of_day)); + case GenericGnssId::GLONASS: return ts::TAI_Time(ts::GLO_Time(day_number, time_of_day)); + case GenericGnssId::GALILEO: return ts::TAI_Time(ts::GST_Time(day_number, time_of_day)); + case GenericGnssId::BEIDOU: return ts::TAI_Time(ts::BDT_Time(day_number, time_of_day)); } - return TAI_Time::now(); + return ts::TAI_Time::now(); } } // namespace decode diff --git a/generator/rtcm/include/generator/rtcm/types.hpp b/generator/rtcm/include/generator/rtcm/types.hpp index 46bd33e9..17e40d5f 100644 --- a/generator/rtcm/include/generator/rtcm/types.hpp +++ b/generator/rtcm/include/generator/rtcm/types.hpp @@ -1,7 +1,7 @@ #pragma once -#include -#include #include +#include +#include #ifndef RTCM_EXPLICIT #define RTCM_EXPLICIT explicit diff --git a/generator/rtcm/messages/helper.hpp b/generator/rtcm/messages/helper.hpp index d1a6d1ba..dca123d2 100644 --- a/generator/rtcm/messages/helper.hpp +++ b/generator/rtcm/messages/helper.hpp @@ -10,15 +10,15 @@ #define ROUND(x) (floor((x) + 0.5)) -static void epoch_time(Encoder& encoder, const TAI_Time& time, GenericGnssId gnss) { +static void epoch_time(Encoder& encoder, const ts::TAI_Time& time, GenericGnssId gnss) { switch (gnss) { case GenericGnssId::GPS: { - auto tow = GPS_Time(time).time_of_week(); + auto tow = ts::GPS_Time(time).time_of_week(); auto milliseconds = tow.full_seconds() * 1000; encoder.u32(30, static_cast(milliseconds)); } break; case GenericGnssId::GLONASS: { - auto glo = GLO_Time(time); + auto glo = ts::GLO_Time(time); auto dow = glo.days() % 7; auto tow = glo.time_of_day(); auto milliseconds = tow.full_seconds() * 1000; @@ -26,12 +26,12 @@ static void epoch_time(Encoder& encoder, const TAI_Time& time, GenericGnssId gns encoder.u32(27, static_cast(milliseconds)); } break; case GenericGnssId::GALILEO: { - auto tow = GST_Time(time).time_of_week(); + auto tow = ts::GST_Time(time).time_of_week(); auto milliseconds = tow.full_seconds() * 1000; encoder.u32(30, static_cast(milliseconds)); } break; case GenericGnssId::BEIDOU: { - auto tow = BDT_Time(time).time_of_week(); + auto tow = ts::BDT_Time(time).time_of_week(); auto milliseconds = tow.full_seconds() * 1000; encoder.u32(30, static_cast(milliseconds)); } break; diff --git a/generator/rtcm/messages/msm.cpp b/generator/rtcm/messages/msm.cpp index abd08e9a..19186f32 100644 --- a/generator/rtcm/messages/msm.cpp +++ b/generator/rtcm/messages/msm.cpp @@ -1,9 +1,10 @@ #include "msm.hpp" +#include "encoder.hpp" +#include "helper.hpp" + #include #include #include -#include "encoder.hpp" -#include "helper.hpp" using namespace generator::rtcm; diff --git a/generator/rtcm/messages/residuals.cpp b/generator/rtcm/messages/residuals.cpp index be381f45..0f5fe6ef 100644 --- a/generator/rtcm/messages/residuals.cpp +++ b/generator/rtcm/messages/residuals.cpp @@ -7,7 +7,7 @@ using namespace generator::rtcm; extern generator::rtcm::Message generate_1030(const Residuals& residuals) { auto message_id = 1030U; - auto time = GPS_Time(residuals.time).time_of_week().seconds(); + auto time = ts::GPS_Time(residuals.time).time_of_week().seconds(); auto encoder = Encoder(); encoder.u16(12, message_id); @@ -48,7 +48,7 @@ extern generator::rtcm::Message generate_1030(const Residuals& residuals) { extern generator::rtcm::Message generate_1031(const Residuals& residuals) { auto message_id = 1031U; - auto time = GLO_Time(residuals.time).time_of_day().seconds(); + auto time = ts::GLO_Time(residuals.time).time_of_day().seconds(); auto encoder = Encoder(); encoder.u16(12, message_id); diff --git a/generator/rtcm/rtk_data.hpp b/generator/rtcm/rtk_data.hpp index 93822831..4ef6c1e3 100644 --- a/generator/rtcm/rtk_data.hpp +++ b/generator/rtcm/rtk_data.hpp @@ -75,7 +75,7 @@ struct Satellite { }; struct Observations { - TAI_Time time; + ts::TAI_Time time; std::vector signals; std::vector satellites; @@ -102,7 +102,7 @@ struct Residuals { double s_ld; }; - TAI_Time time; + ts::TAI_Time time; uint32_t reference_station_id; uint32_t n_refs; std::vector satellites; diff --git a/generator/rtcm/time/bdt_time.cpp b/generator/rtcm/time/bdt_time.cpp index 45e454e5..b8994e9c 100644 --- a/generator/rtcm/time/bdt_time.cpp +++ b/generator/rtcm/time/bdt_time.cpp @@ -1,6 +1,7 @@ #include "bdt_time.hpp" #include "utc_time.hpp" +namespace ts { static TsInt leap_seconds_utc_bdt() { // NOTE(ewasjon): There are 33 leapseconds between the start of UTC and the start of BDT, after // that BDT includes leapseconds similar to UTC. @@ -69,3 +70,4 @@ BDT_Time BDT_Time::now() { Timestamp BDT_Time::utc_timestamp() const { return bdt_2_utc(tm); } +} // namespace ts diff --git a/generator/rtcm/time/bdt_time.hpp b/generator/rtcm/time/bdt_time.hpp index e9a468d9..4bff3baa 100644 --- a/generator/rtcm/time/bdt_time.hpp +++ b/generator/rtcm/time/bdt_time.hpp @@ -1,6 +1,7 @@ #pragma once #include "timestamp.hpp" +namespace ts { class TAI_Time; class UTC_Time; class BDT_Time { @@ -30,3 +31,4 @@ class BDT_Time { // away from TAI as of (2022-09-22) and changes with added or subtracted leap-seconds. Timestamp tm; }; +} // namespace ts diff --git a/generator/rtcm/time/glo_time.cpp b/generator/rtcm/time/glo_time.cpp index 69aacd64..671e55a9 100644 --- a/generator/rtcm/time/glo_time.cpp +++ b/generator/rtcm/time/glo_time.cpp @@ -1,6 +1,7 @@ #include "glo_time.hpp" #include "utc_time.hpp" +namespace ts { static Timestamp utc_2_glo(Timestamp timestamp) { timestamp.add(3 * HOUR_IN_SECONDS); @@ -52,3 +53,4 @@ GLO_Time GLO_Time::now() { Timestamp GLO_Time::utc_timestamp() const { return glo_2_utc(tm); } +} // namespace ts diff --git a/generator/rtcm/time/glo_time.hpp b/generator/rtcm/time/glo_time.hpp index b4fffa1f..a5dcbf45 100644 --- a/generator/rtcm/time/glo_time.hpp +++ b/generator/rtcm/time/glo_time.hpp @@ -1,6 +1,7 @@ #pragma once #include "timestamp.hpp" +namespace ts { class TAI_Time; class UTC_Time; class GLO_Time { @@ -27,3 +28,4 @@ class GLO_Time { private: Timestamp tm; }; +} // namespace ts diff --git a/generator/rtcm/time/gps_time.cpp b/generator/rtcm/time/gps_time.cpp index f44f630a..d3dd40fb 100644 --- a/generator/rtcm/time/gps_time.cpp +++ b/generator/rtcm/time/gps_time.cpp @@ -1,6 +1,7 @@ #include "gps_time.hpp" #include "utc_time.hpp" +namespace ts { static TsInt leap_seconds_utc_gps() { // TODO(ewasjon): This will not always be correct. Use LeapSeconds:: // instead. @@ -77,3 +78,4 @@ GPS_Time GPS_Time::now() { Timestamp GPS_Time::utc_timestamp() const { return gps_2_utc(tm); } +} // namespace ts diff --git a/generator/rtcm/time/gps_time.hpp b/generator/rtcm/time/gps_time.hpp index c28e1b04..4eaf0c46 100644 --- a/generator/rtcm/time/gps_time.hpp +++ b/generator/rtcm/time/gps_time.hpp @@ -1,6 +1,7 @@ #pragma once #include "timestamp.hpp" +namespace ts { class UTC_Time; class TAI_Time; class GPS_Time { @@ -28,3 +29,4 @@ class GPS_Time { private: Timestamp tm; }; +} // namespace ts diff --git a/generator/rtcm/time/gst_time.cpp b/generator/rtcm/time/gst_time.cpp index 962256e7..35739d5d 100644 --- a/generator/rtcm/time/gst_time.cpp +++ b/generator/rtcm/time/gst_time.cpp @@ -1,6 +1,7 @@ #include "gst_time.hpp" #include "utc_time.hpp" +namespace ts { static TsInt leap_seconds_utc_gst() { // NOTE(ewasjon): The leapseconds difference between UTC and GST is 13 (because of the begining) // and the difference between in leapseconds sinec the start (which is 5 as of 2022:09:22). Or @@ -82,3 +83,4 @@ GST_Time GST_Time::now() { Timestamp GST_Time::utc_timestamp() const { return gst_2_utc(tm); } +} // namespace ts diff --git a/generator/rtcm/time/gst_time.hpp b/generator/rtcm/time/gst_time.hpp index 564bb6d5..0e08e00c 100644 --- a/generator/rtcm/time/gst_time.hpp +++ b/generator/rtcm/time/gst_time.hpp @@ -1,6 +1,7 @@ #pragma once #include "timestamp.hpp" +namespace ts { class TAI_Time; class UTC_Time; class GST_Time { @@ -27,3 +28,4 @@ class GST_Time { private: Timestamp tm; }; +} // namespace ts diff --git a/generator/rtcm/time/tai_time.cpp b/generator/rtcm/time/tai_time.cpp index 2ad20c45..7eb35b47 100644 --- a/generator/rtcm/time/tai_time.cpp +++ b/generator/rtcm/time/tai_time.cpp @@ -1,6 +1,7 @@ #include "tai_time.hpp" #include "utc_time.hpp" +namespace ts { static Timestamp utc_2_tai(Timestamp timestamp) { timestamp.subtract(LeapSeconds::count()); return timestamp; @@ -28,3 +29,4 @@ TAI_Time TAI_Time::now() { Timestamp TAI_Time::utc_timestamp() const { return tai_2_utc(tm); } +} // namespace ts diff --git a/generator/rtcm/time/tai_time.hpp b/generator/rtcm/time/tai_time.hpp index 2af8b79d..97c8d4ab 100644 --- a/generator/rtcm/time/tai_time.hpp +++ b/generator/rtcm/time/tai_time.hpp @@ -1,7 +1,9 @@ #pragma once -#include #include "timestamp.hpp" +#include + +namespace ts { class GPS_Time; class GLO_Time; class GST_Time; @@ -34,3 +36,4 @@ class TAI_Time { // january 1970). Timestamp tm; }; +} // namespace ts diff --git a/generator/rtcm/time/timestamp.hpp b/generator/rtcm/time/timestamp.hpp index 32ec93f2..7a348a4b 100644 --- a/generator/rtcm/time/timestamp.hpp +++ b/generator/rtcm/time/timestamp.hpp @@ -1,16 +1,18 @@ #pragma once +#include #include "types.hpp" -using TsInt = int64_t; +namespace ts { +using TsInt = int64_t; using TsFloat = double; -RTCM_CONSTEXPR static TsInt DAYS_PER_WEEK = 7LL; -RTCM_CONSTEXPR static TsInt DAYS_PER_YEAR = 365LL; -RTCM_CONSTEXPR static TsInt MINUTE_IN_SECONDS = 60LL; -RTCM_CONSTEXPR static TsInt HOUR_IN_SECONDS = MINUTE_IN_SECONDS * 60LL; -RTCM_CONSTEXPR static TsInt DAY_IN_SECONDS = HOUR_IN_SECONDS * 24LL; -RTCM_CONSTEXPR static TsInt WEEK_IN_SECONDS = DAY_IN_SECONDS * DAYS_PER_WEEK; -RTCM_CONSTEXPR static TsInt YEAR_IN_SECONDS = DAY_IN_SECONDS * 365LL; +RTCM_CONSTEXPR static TsInt DAYS_PER_WEEK = 7LL; +RTCM_CONSTEXPR static TsInt DAYS_PER_YEAR = 365LL; +RTCM_CONSTEXPR static TsInt MINUTE_IN_SECONDS = 60LL; +RTCM_CONSTEXPR static TsInt HOUR_IN_SECONDS = MINUTE_IN_SECONDS * 60LL; +RTCM_CONSTEXPR static TsInt DAY_IN_SECONDS = HOUR_IN_SECONDS * 24LL; +RTCM_CONSTEXPR static TsInt WEEK_IN_SECONDS = DAY_IN_SECONDS * DAYS_PER_WEEK; +RTCM_CONSTEXPR static TsInt YEAR_IN_SECONDS = DAY_IN_SECONDS * 365LL; RTCM_CONSTEXPR static TsInt MILLISECONDS_PER_SECOND = 1000LL; // Total time in seconds (+fractions) since a start date. The date depends on @@ -27,9 +29,11 @@ class Timestamp { normalize(); } - RTCM_NODISCARD TsInt seconds() const { return mSeconds; } + RTCM_NODISCARD TsInt seconds() const { return mSeconds; } RTCM_NODISCARD TsFloat fraction() const { return mFraction; } - RTCM_NODISCARD TsFloat full_seconds() const { return static_cast(mSeconds) + mFraction; } + RTCM_NODISCARD TsFloat full_seconds() const { + return static_cast(mSeconds) + mFraction; + } RTCM_NODISCARD Timestamp operator+(const Timestamp& other) const { return Timestamp{seconds() + other.seconds(), fraction() + other.fraction()}; @@ -61,7 +65,7 @@ class Timestamp { } private: - TsInt mSeconds; + TsInt mSeconds; TsFloat mFraction; }; @@ -71,3 +75,4 @@ class LeapSeconds { return 37; // TODO(ewasjon): SHOULD NOT BE HARDCODED! } }; +} // namespace ts diff --git a/generator/rtcm/time/utc_time.cpp b/generator/rtcm/time/utc_time.cpp index 8f8962f5..18d2337e 100644 --- a/generator/rtcm/time/utc_time.cpp +++ b/generator/rtcm/time/utc_time.cpp @@ -1,13 +1,16 @@ #include "utc_time.hpp" -#include -#include -#include #include "bdt_time.hpp" #include "glo_time.hpp" #include "gps_time.hpp" #include "gst_time.hpp" #include "tai_time.hpp" +#include +#include +#include +#include + +namespace ts { // NOTE: The day each month of the year starts with. RTCM_CONSTEXPR static std::array day_of_year = { 1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, @@ -154,3 +157,4 @@ UTC_Time UTC_Time::now() { return UTC_Time(timestamp); } +} // namespace ts diff --git a/generator/rtcm/time/utc_time.hpp b/generator/rtcm/time/utc_time.hpp index 02613cc5..6ed25023 100644 --- a/generator/rtcm/time/utc_time.hpp +++ b/generator/rtcm/time/utc_time.hpp @@ -1,7 +1,9 @@ #pragma once -#include #include "timestamp.hpp" +#include + +namespace ts { class TAI_Time; class GPS_Time; class GLO_Time; @@ -30,3 +32,4 @@ class UTC_Time { private: Timestamp tm; }; +} // namespace ts diff --git a/generator/spartn/CMakeLists.txt b/generator/spartn/CMakeLists.txt new file mode 100644 index 00000000..d50235da --- /dev/null +++ b/generator/spartn/CMakeLists.txt @@ -0,0 +1,42 @@ + +add_library(generator_spartn STATIC + "generator.cpp" + "time.cpp" + "transmitter.cpp" +) +add_library(generator::spartn ALIAS generator_spartn) + +target_include_directories(generator_spartn PRIVATE "./" "include/generator/spartn/") +target_include_directories(generator_spartn PUBLIC "include/") +target_link_libraries(generator_spartn PRIVATE asn1::generated asn1::helper) +target_link_libraries(generator_spartn PRIVATE utility) + +if (USE_ASAN) +target_compile_options(generator_spartn PRIVATE -fsanitize=address,undefined,leak) +target_link_libraries(generator_spartn PRIVATE -fsanitize=address,undefined,leak) +endif (USE_ASAN) + +target_compile_options(generator_spartn PRIVATE + "-Wall" + "-Wextra" + "-Wpedantic" + "-Wnon-virtual-dtor" + "-Wold-style-cast" + "-Wcast-align" + "-Woverloaded-virtual" + "-Wsign-conversion" + "-Wno-conversion" + "-Wno-old-style-cast" +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(generator_spartn PRIVATE + "-Wmisleading-indentation" + ) +endif() + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU") + target_compile_options(generator_spartn PRIVATE + "-Wno-missing-field-initializers" + ) +endif() \ No newline at end of file diff --git a/generator/spartn/generator.cpp b/generator/spartn/generator.cpp new file mode 100644 index 00000000..587d72e7 --- /dev/null +++ b/generator/spartn/generator.cpp @@ -0,0 +1,1821 @@ +#include "generator.h" +#include + +std::vector> +SPARTN_Generator::generate(const LPP_Message* const lpp_message, const int8_t qualityoveride, + const bool ublox_clock_correction, const bool force_continuity) { + if (!lpp_message->lpp_MessageBody) { + std::cout << "[INFO] no message body!" << std::endl; + return {}; + } + + if (lpp_message->lpp_MessageBody->present != LPP_MessageBody_PR_c1) { + std::cout << "[INFO] no c1!" << std::endl; + return {}; + } + + if (lpp_message->lpp_MessageBody->choice.c1.present != + LPP_MessageBody__c1_PR_provideAssistanceData) { + std::cout << "[INFO] no provideAssistanceData!" << std::endl; + return {}; + } + + auto& provide_assistance_data_base = + lpp_message->lpp_MessageBody->choice.c1.choice.provideAssistanceData; + if (provide_assistance_data_base.criticalExtensions.present != + ProvideAssistanceData__criticalExtensions_PR_c1) { + std::cout << "[INFO] no c1!" << std::endl; + return {}; + } + + if (provide_assistance_data_base.criticalExtensions.choice.c1.present != + ProvideAssistanceData__criticalExtensions__c1_PR_provideAssistanceData_r9) { + std::cout << "[INFO] no provideAssistanceData_r9!" << std::endl; + return {}; + } + + this->prov_ass_data_ = + &provide_assistance_data_base.criticalExtensions.choice.c1.choice.provideAssistanceData_r9; + this->qualityoveride_ = qualityoveride; + this->ublox_clock_correction = ublox_clock_correction; + this->force_continuity = force_continuity; + + auto provide_assistance_data = this->prov_ass_data_->a_gnss_ProvideAssistanceData; + if (!provide_assistance_data) { + std::cout << "[INFO] no a_gnss_ProvideAssistanceData!" << std::endl; + return {}; + } + + std::cout << "[INFO] Generating messages" << std::endl; + std::vector> messages = {}; + + this->ocb_corrections_ = SPARTN_Generator::group_ocb_corrections( + this->prov_ass_data_->a_gnss_ProvideAssistanceData->gnss_GenericAssistData); + + this->hpac_corrections_ = group_hpac_corrections( + this->prov_ass_data_->a_gnss_ProvideAssistanceData->gnss_GenericAssistData, + this->prov_ass_data_->a_gnss_ProvideAssistanceData->gnss_CommonAssistData); + + this->intersect_svs(); + + SPARTN_Generator::generate_ocb_message(messages); + SPARTN_Generator::generate_gad_message(messages); + SPARTN_Generator::generate_hpac_messages(messages); + + return messages; +} + +void SPARTN_Generator::intersect_svs() { + std::map> ocb_svs = {}; + std::set ocb_gnss_ids = {}; + for (const auto& ocb_corr : this->ocb_corrections_) { + const uint16_t gnss_id = ocb_corr.gnss_id.gnss_id; + ocb_gnss_ids.insert(gnss_id); + if (ocb_svs.find(gnss_id) == ocb_svs.end()) { + ocb_svs[gnss_id] = {}; + } + + if (ocb_corr.orbit_corrs) { + const auto& orb_corrs = ocb_corr.orbit_corrs->ssr_OrbitCorrectionList_r15.list; + for (int i = 0; i < orb_corrs.count; i++) { + ocb_svs[gnss_id].insert(orb_corrs.array[i]->svID_r15.satellite_id); + } + } + if (ocb_corr.clock_corrs) { + const auto& clock_corrs = ocb_corr.clock_corrs->ssr_ClockCorrectionList_r15.list; + for (int i = 0; i < clock_corrs.count; i++) { + ocb_svs[gnss_id].insert(clock_corrs.array[i]->svID_r15.satellite_id); + } + } + if (ocb_corr.code_bias_corrs) { + const auto& code_bias_corrs = ocb_corr.code_bias_corrs->ssr_CodeBiasSatList_r15.list; + for (int i = 0; i < code_bias_corrs.count; i++) { + ocb_svs[gnss_id].insert(code_bias_corrs.array[i]->svID_r15.satellite_id); + } + } + if (ocb_corr.phase_bias_corrs) { + const auto& phase_bias_corrs = ocb_corr.phase_bias_corrs->ssr_PhaseBiasSatList_r16.list; + for (int i = 0; i < phase_bias_corrs.count; i++) { + ocb_svs[gnss_id].insert(phase_bias_corrs.array[i]->svID_r16.satellite_id); + } + } + } + + std::map> hpac_svs = {}; + std::set hpac_gnss_ids = {}; + for (const auto& hpac_corr : this->hpac_corrections_) { + const uint16_t gnss_id = hpac_corr.gnss_id.gnss_id; + hpac_gnss_ids.insert(gnss_id); + if (hpac_svs.find(gnss_id) == hpac_svs.end()) { + hpac_svs[gnss_id] = {}; + } + + if (hpac_corr.stec_corrections) { + const auto& stec_corrs = hpac_corr.stec_corrections->stec_SatList_r16.list; + + for (int i = 0; i < stec_corrs.count; i++) { + hpac_svs[gnss_id].insert(stec_corrs.array[i]->svID_r16.satellite_id); + } + } + } + + std::vector common_gnss = {}; + std::set_intersection(ocb_gnss_ids.begin(), ocb_gnss_ids.end(), hpac_gnss_ids.begin(), + hpac_gnss_ids.end(), std::back_inserter(common_gnss)); + + for (const auto& gnss_id : common_gnss) { + const auto& ocb = ocb_svs[gnss_id]; + const auto& hpac = hpac_svs[gnss_id]; + + this->sv_intersections_[gnss_id] = {}; + auto& inter = this->sv_intersections_[gnss_id]; + + std::set_intersection(ocb.begin(), ocb.end(), hpac.begin(), hpac.end(), + std::back_inserter(inter)); + } +} + +void SPARTN_Generator::generate_ocb_message( + std::vector>& messages) { + if (!this->prov_ass_data_->a_gnss_ProvideAssistanceData->gnss_GenericAssistData) { + std::cout << "[INFO] no generic assistance data!" << std::endl; + return; + } + + const auto& gen_ass = + this->prov_ass_data_->a_gnss_ProvideAssistanceData->gnss_GenericAssistData; + + const auto set_size = this->ocb_corrections_.size(); + size_t set_num = 0; + for (const auto& this_assist_data : this->ocb_corrections_) { + std::unique_ptr this_message(new SPARTN_Message); + std::unique_ptr this_header(new SPARTN_Message_Header); + auto& satellite_blocks = this_message->data; + + const auto gnss_id = (GNSS_ID__gnss_id)this_assist_data.gnss_id.gnss_id; + const std::time_t generation_time = std::time(nullptr); + + std::cout << std::endl << "===== NEW OCB MESSAGE ["; + std::cout << Constants::constellation_names.at(gnss_id); + std::cout << "] =====" << std::endl; + + const auto& orb_corrs = this_assist_data.orbit_corrs; + + const auto& clk_corrs = this_assist_data.clock_corrs; + + const auto& code_corrs = this_assist_data.code_bias_corrs; + + const auto& phase_corrs = this_assist_data.phase_bias_corrs; + + const std::map all_sv_ocbs = + SPARTN_Generator::get_ocb_for_svs(orb_corrs, clk_corrs, code_corrs, phase_corrs); + + // NOTE: remove "unallowed" satellites, if we don't do this then the + // bitmask will be incorrect. + std::map sv_ocbs = {}; + const auto& allowed_svs = this->sv_intersections_.at(gnss_id); + for (const auto& sv_ocb : all_sv_ocbs) { + if (std::count(allowed_svs.begin(), allowed_svs.end(), sv_ocb.first) != 0) { + sv_ocbs[sv_ocb.first] = sv_ocb.second; + } else { + std::cout << "[INFO] SV: " << sv_ocb.first << " is not allowed" << std::endl; + } + } + + const GNSS_SystemTime* const gnss_time = + SPARTN_Generator::get_gnss_systemtime(this_assist_data); + + SPARTN_Generator::add_time_to_message(this_message, gnss_time, gnss_id); + + this_message->message_type = 0; + + const int8_t subtype = Constants::constellation_subtypes.at(gnss_id); + if (subtype == -1) { + std::cout << "[ERR] unsupported GNSS: " << gnss_id << std::endl; + continue; + } + this_message->message_sub_type = subtype; + + { + SPARTN_Generator::add_field_to_header(this_header, 5, 9, this_assist_data.iod_ssr); + + // end of OCB set for epoch + SPARTN_Generator::add_field_to_header(this_header, 10, 1, + (int64_t)(++set_num == set_size)); + + SPARTN_Generator::add_field_to_header(this_header, 69, 1, 0); + SPARTN_Generator::add_field_to_header(this_header, 8, 1, 0); + + if (SPARTN_Generator::check_is_itrf(orb_corrs)) { + SPARTN_Generator::add_field_to_header(this_header, 9, 1, 0); + } else { + std::cout << "[INFO] Got unsupported reference datum" << std::endl; + continue; + } + + const uint8_t ephm_id = Constants::ephemeris_id.at(gnss_id); + const uint8_t ephm_bit_count = Constants::ephemeris_bit_count.at(gnss_id); + const uint8_t ephm_bits = Constants::ephemeris_bits.at(gnss_id); + const uint8_t sat_mask_id = Constants::satellite_mask_ids.at(gnss_id); + const uint8_t sat_mask_bit_count = Constants::satellite_mask_sizes.at(gnss_id); + + const auto sat_mask_bits = + SPARTN_Generator::get_satellite_mask_from_ocbs(sv_ocbs, sat_mask_bit_count, 2); + + SPARTN_Generator::add_field_to_header(this_header, ephm_id, ephm_bit_count, ephm_bits); + std::unique_ptr sat_mask_field( + new SPARTN_Field(sat_mask_id, sat_mask_bit_count, sat_mask_bits)); + this_header->fields.push(std::move(sat_mask_field)); + } + + this_message->message_header = std::move(this_header); + + for (const auto& sv_ocb : sv_ocbs) { + const auto svid = sv_ocb.first; + + std::unique_ptr this_sat_block(new SPARTN_Block); + std::unique_ptr this_sat_block_pre(new SPARTN_Block); + const auto& ocb_corr = sv_ocb.second; + uint8_t ocb_mask = 0; + + if (orb_corrs && this->generate_orbit_block(this_sat_block_pre, std::get<0>(ocb_corr), + gnss_id, generation_time)) { + ocb_mask |= 4; + } else { + std::cout << "[INFO] no orbit corrections for SV: " << svid << std::endl; + } + + if (clk_corrs && this->generate_clock_block( + this_sat_block_pre, std::get<1>(ocb_corr), this_assist_data.ura, + gnss_id, generation_time, gnss_time, + clk_corrs->ssrUpdateInterval_r15, clk_corrs->iod_ssr_r15)) { + ocb_mask |= 2; + } else { + std::cout << "[INFO] no clock corrections for SV: " << svid << std::endl; + } + + if (code_corrs && this->generate_bias_block(this_sat_block_pre, std::get<2>(ocb_corr), + std::get<3>(ocb_corr), gnss_id)) { + ocb_mask |= 1; + } + + SPARTN_Generator::add_field_to_block(this_sat_block, 13, 1, 0); + + SPARTN_Generator::add_field_to_block(this_sat_block, 14, 3, ocb_mask); + + if (this->last_generation_time_.find(gnss_id) == this->last_generation_time_.end()) { + this->last_generation_time_[gnss_id] = {}; + } + + if (this->last_generation_time_.at(gnss_id).find(svid) == + this->last_generation_time_.at(gnss_id).end()) { + this->last_generation_time_.at(gnss_id)[svid] = generation_time; + } + + const auto since_last_generation = + generation_time - this->last_generation_time_.at(gnss_id).at(svid); + uint8_t spartn_continuity = 7; + + for (uint8_t i = 1; i < Constants::spartn_continuity_num; i++) { + if (since_last_generation < Constants::allowed_iode_continuity_values[i]) { + spartn_continuity = i - 1; + break; + } + } + + if (this->force_continuity) { + SPARTN_Generator::add_field_to_block(this_sat_block, 15, 3, 7 /* 320 secs */); + } else { + SPARTN_Generator::add_field_to_block(this_sat_block, 15, 3, spartn_continuity); + } + + if (spartn_continuity == 7) { + this->last_generation_time_.at(gnss_id).at(svid) = generation_time; + } + + /* + * Fields come before blocks in the satellite block, but we need to + * know the result of the block to fill in the OCB mask. So store + * the blocks in a temp queue and figure out the mask, add the + * fields then move the data from the temp queue to the actual one. + */ + while (!this_sat_block_pre->data.empty()) { + this_sat_block->data.push(std::move(this_sat_block_pre->data.front())); + this_sat_block_pre->data.pop(); + } + + satellite_blocks.push(std::move(this_sat_block)); + } + + messages.push_back(std::move(this_message)); + + // TODO: REMOVE + // messages.clear(); + // printf("-----------------------------------------------------------\n"); + } +} + +bool SPARTN_Generator::generate_orbit_block( + const std::unique_ptr& sat_block, + const SSR_OrbitCorrectionSatelliteElement_r15* const orbit_correction, + const GNSS_ID__gnss_id gnss_id, const std::time_t generation_time) { + if (!orbit_correction) { + return false; + } + + std::unique_ptr orbit_block(new SPARTN_Block); + + const uint8_t iod_id = Constants::iode_ids.at(gnss_id); + const uint8_t iod_bit_size = Constants::iode_bit_size.at(gnss_id); + + const auto& iod_bit_string = orbit_correction->iod_r15; + long iod_dec = 0; + for (size_t i = 0; i < iod_bit_string.size; i++) { + iod_dec <<= 8; + iod_dec |= iod_bit_string.buf[i]; + } + iod_dec >>= iod_bit_string.bits_unused; + iod_dec >>= 3; // TODO: is this right? + + const auto svid = orbit_correction->svID_r15.satellite_id; + if (this->iods_.find(gnss_id) == this->iods_.end()) { + this->iods_[gnss_id] = {}; + } + + if (this->iods_.at(gnss_id).find(svid) == this->iods_.at(gnss_id).end()) { + this->iods_.at(gnss_id)[svid] = {iod_dec, generation_time}; + } + + if (this->iods_.at(gnss_id).at(svid).first != iod_dec) { + this->iods_.at(gnss_id)[svid] = {iod_dec, generation_time}; + } + + SPARTN_Generator::add_field_to_block(orbit_block, iod_id, iod_bit_size, iod_dec); + + double val_dec = SPARTN_Generator::decode_value_lpp(orbit_correction->delta_radial_r15, + Constants::orbit_lpp_radial_res); + + if (SPARTN_Generator::check_within_range(val_dec, Constants::orbit_clock_spartn_rng_min)) { + SPARTN_Generator::add_field_to_block( + orbit_block, 20, 14, + SPARTN_Generator::encode_value_spartn(val_dec, Constants::orbit_clock_spartn_rng_min, + Constants::orbit_clock_spartn_res)); + } else { + std::cout << "[INFO] Radial correction is out of range: " << val_dec << std::endl; + return false; + } + + val_dec = SPARTN_Generator::decode_value_lpp(orbit_correction->delta_AlongTrack_r15, + Constants::orbit_lpp_along_res); + + if (SPARTN_Generator::check_within_range(val_dec, Constants::orbit_clock_spartn_rng_min)) { + SPARTN_Generator::add_field_to_block( + orbit_block, 20, 14, + SPARTN_Generator::encode_value_spartn(val_dec, Constants::orbit_clock_spartn_rng_min, + Constants::orbit_clock_spartn_res)); + } else { + std::cout << "[INFO] Along correction is out of range: " << val_dec << std::endl; + return false; + } + + val_dec = SPARTN_Generator::decode_value_lpp(orbit_correction->delta_CrossTrack_r15, + Constants::orbit_lpp_cross_res); + + if (SPARTN_Generator::check_within_range(val_dec, Constants::orbit_clock_spartn_rng_min)) { + SPARTN_Generator::add_field_to_block( + orbit_block, 20, 14, + SPARTN_Generator::encode_value_spartn(val_dec, Constants::orbit_clock_spartn_rng_min, + Constants::orbit_clock_spartn_res)); + } else { + std::cout << "[INFO] Cross correction is out of range: " << val_dec << std::endl; + return false; + } + + SPARTN_Generator::add_field_to_block(orbit_block, 21, 0, 0); + + SPARTN_Generator::add_block_to_block(sat_block, orbit_block); + + return true; +} + +bool SPARTN_Generator::generate_clock_block( + const std::unique_ptr& sat_block, + const SSR_ClockCorrectionSatelliteElement_r15* const clock_correction, + const GNSS_SSR_URA_r16* const URA, const GNSS_ID__gnss_id gnss_id, + const std::time_t generation_time, const GNSS_SystemTime* const gnss_systemtime, + const long update_interval, const uint8_t iode) { + if (!clock_correction) { + return false; + } + + std::unique_ptr clock_block(new SPARTN_Block); + + const auto svid = clock_correction->svID_r15.satellite_id; + const std::pair& iod_time = + this->iods_.find(gnss_id)->second.find(svid)->second; + + const auto since_last_iod_change = generation_time - iod_time.second; + uint8_t spartn_iode = 7; + + for (uint8_t i = 1; i < Constants::spartn_continuity_num; i++) { + if (since_last_iod_change < Constants::allowed_iode_continuity_values[i]) { + spartn_iode = i - 1; + break; + } + } + + if (this->force_continuity) { + SPARTN_Generator::add_field_to_block(clock_block, 22, 3, 7 /* 320 secs */); + } else { + SPARTN_Generator::add_field_to_block(clock_block, 22, 3, spartn_iode); + } + + if (spartn_iode == 7) { + this->iods_.at(gnss_id)[svid] = {iode, generation_time}; + } + + /** +---------------------------------------------------------------------+ + * | Clock Correction Calculation | + * +---------------------------------------------------------------------+ + * | LPP gives clock corrections as a polynomial, where-as SPARTN uses a | + * | single lump value. Therefore we must calculate the lump value from | + * | the given polynomial. | + * | | + * | delta_c = c0 + c1(t - t0) + [c2(t-t0)] ^ 2 | + * | where: c0, c1, c2 are coefficients given by the LPP message, | + * | t is the generation time of the LPP message, | + * | t0 is the reference time. | + * | | + * | t0 = epochTime + (0.5 * ssrUpdateInterval) | + * +---------------------------------------------------------------------+ + */ + + const double c0 = SPARTN_Generator::decode_value_lpp(clock_correction->delta_Clock_C0_r15, + Constants::clock_c0_lpp_res); + double c1 = 0; + double c2 = 0; + + if (clock_correction->delta_Clock_C1_r15) { + if (*clock_correction->delta_Clock_C1_r15 != 0) { + c1 = SPARTN_Generator::decode_value_lpp(*clock_correction->delta_Clock_C1_r15, + Constants::clock_c1_lpp_res); + } + } + + if (clock_correction->delta_Clock_C2_r15) { + if (*clock_correction->delta_Clock_C2_r15 != 0) { + c2 = SPARTN_Generator::decode_value_lpp(*clock_correction->delta_Clock_C2_r15, + Constants::clock_c2_lpp_res); + } + } + + // TODO: Why are we rounding TimeOfDayFrac_msec? + const long tod = gnss_systemtime->gnss_TimeOfDay + + (gnss_systemtime->gnss_TimeOfDayFrac_msec ? + *gnss_systemtime->gnss_TimeOfDayFrac_msec >= 500 ? 1 : 0 : + 0); + + auto standard_day_number = + (double)SPARTN_LPP_Time::standard_day_number(gnss_systemtime->gnss_DayNumber, gnss_id); + auto epoch_time = (standard_day_number * Constants::seconds_in_day) + (double)tod; + + auto update_interval_in_seconds = Constants::update_intervals.at(update_interval); + auto reference_time = epoch_time + (0.5 * update_interval_in_seconds); + + auto delta_gen_ref = reference_time - reference_time; // TODO: what to do here? + auto delta_c = c0 + (c1 * delta_gen_ref) + pow(c2 * delta_gen_ref, 2); + + if (this->ublox_clock_correction) { + delta_c = -delta_c; + } + + if (SPARTN_Generator::check_within_range(delta_c, Constants::orbit_clock_spartn_rng_min)) { + SPARTN_Generator::add_field_to_block( + clock_block, 20, 14, + SPARTN_Generator::encode_value_spartn(delta_c, Constants::orbit_clock_spartn_rng_min, + Constants::orbit_clock_spartn_res)); + } else { + std::cout << "[INFO] Clock correction is out of range: " << delta_c << std::endl; + return false; + } + + if (URA == nullptr) { + SPARTN_Generator::add_field_to_block(clock_block, 24, 3, + this->qualityoveride_ < 0 ? 0 : + 7 < this->qualityoveride_ ? 7 : + this->qualityoveride_); + } else { + std::cout << "[INFO] URA present" << std::endl; + const auto& sats = URA->ssr_URA_SatList_r16.list; + + for (int i = 0; i < sats.count; i++) { + const SSR_URA_SatElement_r16* const this_sat = sats.array[i]; + + if (this_sat->svID_r16.satellite_id == clock_correction->svID_r15.satellite_id) { + const auto ura_bits = (*this_sat->ssr_URA_r16.buf) >> this_sat->ssr_URA_r16.size; + const auto ura_class = ura_bits >> 3; + const auto ura_value = ura_bits & 3; + const auto q = ((pow(3, ura_class)) * ((1 + ((double)ura_value))) - 1) / 100; + + for (size_t ura_idx = 0; ura_idx < Constants::ura_values.size(); ura_idx++) { + if (q <= Constants::ura_values[ura_idx]) { + SPARTN_Generator::add_field_to_block(clock_block, 24, 3, (int64_t)ura_idx); + break; + } + } + break; + } + } + + // TODO: what to do if we don't find the SV? + } + + SPARTN_Generator::add_block_to_block(sat_block, clock_block); + + return true; +} + +bool SPARTN_Generator::generate_bias_block( + const std::unique_ptr& sat_block, + const SSR_CodeBiasSatElement_r15* const sat_code_biases, + const SSR_PhaseBiasSatElement_r16* const sat_phase_biases, const GNSS_ID__gnss_id gnss_id) { + std::unique_ptr bias_block(new SPARTN_Block); + + const SSR_CodeBiasSignalList_r15* code_biases = + sat_code_biases ? &sat_code_biases->ssr_CodeBiasSignalList_r15 : nullptr; + + std::map lpp_to_spartn_bias = {}; + Constants::bias_ids_size bis; + + switch (gnss_id) { + case GNSS_ID__gnss_id_gps: { + lpp_to_spartn_bias = Constants::gps_lpp_to_spartn_bias; + bis = Constants::gps_bias_ids_size; + break; + } + case GNSS_ID__gnss_id_qzss: { + lpp_to_spartn_bias = Constants::qzss_lpp_to_spartn_bias; + bis = Constants::qzss_bias_ids_size; + break; + } + case GNSS_ID__gnss_id_galileo: { + lpp_to_spartn_bias = Constants::gal_lpp_to_spartn_bias; + bis = Constants::gal_bias_ids_size; + break; + } + case GNSS_ID__gnss_id_glonass: { + lpp_to_spartn_bias = Constants::glo_lpp_to_spartn_bias; + bis = Constants::glo_bias_ids_size; + break; + } + case GNSS_ID__gnss_id_bds: { + lpp_to_spartn_bias = Constants::bds_lpp_to_spartn_bias; + bis = Constants::bds_bias_ids_size; + break; + } + default: { + std::cout << "[INFO] bias, unsupported GNSS: " << gnss_id << std::endl; + return false; + } + } + + this->generate_phase_bias_block(bias_block, sat_phase_biases, lpp_to_spartn_bias, bis, gnss_id); + + SPARTN_Generator::generate_code_bias_block(bias_block, code_biases, lpp_to_spartn_bias, bis); + + SPARTN_Generator::add_block_to_block(sat_block, bias_block); + + return true; +} + +void SPARTN_Generator::generate_phase_bias_block( + const std::unique_ptr& bias_block, + const SSR_PhaseBiasSatElement_r16* const phase_biases, + std::map lpp_to_spartn_bias, const Constants::bias_ids_size bis, + const GNSS_ID__gnss_id gnss_id) { + if (phase_biases == nullptr) { + std::cout << "[INFO] no phase bias for SV" << std::endl; + + SPARTN_Generator::add_field_to_block(bias_block, bis.phase_id, bis.bit_count, 0); + + return; + } + + const auto phase_list = phase_biases->ssr_PhaseBiasSignalList_r16.list; + std::vector> sig_corrs = {}; + + for (int i = 0; i < phase_list.count; i++) { + const SSR_PhaseBiasSignalElement_r16* const this_phase_bias = phase_list.array[i]; + + const uint8_t lpp_code_bias_type = + SPARTN_Generator::get_signalid(this_phase_bias->signal_and_tracking_mode_ID_r16); + + if (lpp_to_spartn_bias.find(lpp_code_bias_type) == lpp_to_spartn_bias.end()) { + std::cout << "[INFO] got unsupported signal for phase bias: " << (int)lpp_code_bias_type + << std::endl; + continue; + } + + const long code_type = lpp_to_spartn_bias.at(lpp_code_bias_type); + + sig_corrs.emplace_back(std::make_pair(code_type, this_phase_bias)); + } + + std::bitset phase_bias_mask = 0; + + std::queue> phase_bias_blocks = {}; + + const auto svid = phase_biases->svID_r16.satellite_id; + + static constexpr std::bitset new_sig = 1; + const uint8_t mask_length = bis.bit_count - 1; + for (const std::pair& this_sig_corr : sig_corrs) { + const auto* const this_phase_bias = this_sig_corr.second; + + bool fix_flag = false; + + const auto* const integer_indicator = this_phase_bias->phaseBiasIntegerIndicator_r16; + + if (integer_indicator) { + switch (*integer_indicator) { + case 0: + case 1: { + fix_flag = true; + break; + } + case 2: { + fix_flag = false; + break; + } + default: { + std::cout << "[INFO] Unknown phase bias integer indicator: " + << *this_phase_bias->phaseBiasIntegerIndicator_r16 << std::endl; + continue; + } + } + } else { + fix_flag = true; + } + + std::unique_ptr this_phase_bias_block(new SPARTN_Block); + + SPARTN_Generator::add_field_to_block(this_phase_bias_block, 23, 1, (int64_t)fix_flag); + + if (this->discontinuities_.find(gnss_id) == this->discontinuities_.end()) { + this->discontinuities_[gnss_id] = {}; + } + + if (this->discontinuities_.at(gnss_id).find(svid) == + this->discontinuities_.at(gnss_id).end()) { + this->discontinuities_.at(gnss_id)[svid] = {}; + } + + if (this->discontinuities_.at(gnss_id).at(svid).find(this_sig_corr.first) == + this->discontinuities_.at(gnss_id).at(svid).end()) { + this->discontinuities_.at(gnss_id).at(svid)[this_sig_corr.first] = {-1, 0}; + } + + LastDiscontinuity& this_last_disc = + this->discontinuities_.at(gnss_id).at(svid).at(this_sig_corr.first); + + if (this_last_disc.discontinuity_indicator != + this_phase_bias->phaseDiscontinuityIndicator_r16) { + // can narrow here because indicator has max of 3 + this_last_disc = {(int8_t)this_phase_bias->phaseDiscontinuityIndicator_r16, + std::time(nullptr)}; + } + + const auto since_last_disc = std::time(nullptr) - this_last_disc.update_time; + + uint8_t spartn_continuity = 7; + + for (uint8_t i = 1; i < Constants::spartn_continuity_num; i++) { + if (since_last_disc < Constants::allowed_iode_continuity_values[i]) { + spartn_continuity = i - 1; + break; + } + } + + if (this->force_continuity) { + SPARTN_Generator::add_field_to_block(this_phase_bias_block, 15, 3, 7 /* 320 secs */); + } else { + SPARTN_Generator::add_field_to_block(this_phase_bias_block, 15, 3, spartn_continuity); + } + + if (spartn_continuity == 7) { + this->discontinuities_.at(gnss_id).at(svid)[this_sig_corr.first] = {-1, 0}; + } + + const double val_dec = SPARTN_Generator::decode_value_lpp( + this_sig_corr.second->phaseBias_r16, Constants::phase_bias_lpp_res); + + if (SPARTN_Generator::check_within_range(val_dec, Constants::phase_bias_spartn_rng_min)) { + SPARTN_Generator::add_field_to_block( + this_phase_bias_block, 20, 14, + SPARTN_Generator::encode_value_spartn(val_dec, Constants::phase_bias_spartn_rng_min, + Constants::phase_bias_spartn_res)); + } else { + continue; + } + phase_bias_blocks.push(std::move(this_phase_bias_block)); + + phase_bias_mask |= new_sig << (mask_length - this_sig_corr.first - 1); + } + + SPARTN_Generator::add_field_to_block(bias_block, bis.phase_id, bis.bit_count, + (int64_t)phase_bias_mask.to_ullong()); + + while (!phase_bias_blocks.empty()) { + SPARTN_Generator::add_block_to_block(bias_block, phase_bias_blocks.front()); + phase_bias_blocks.pop(); + } +} + +bool SPARTN_Generator::generate_code_bias_block(const std::unique_ptr& bias_block, + const SSR_CodeBiasSignalList_r15* const code_biases, + std::map lpp_to_spartn_bias, + const Constants::bias_ids_size bis) { + if (!code_biases) { + return false; + } + + std::vector> signal_corrs = {}; + for (int i = 0; i < code_biases->list.count; i++) { + const SSR_CodeBiasSignalElement_r15* const this_code_bias = code_biases->list.array[i]; + uint8_t spartn_signal_id; + + const uint8_t lpp_code_bias_type = + SPARTN_Generator::get_signalid(this_code_bias->signal_and_tracking_mode_ID_r15); + + if (lpp_to_spartn_bias.find(lpp_code_bias_type) == lpp_to_spartn_bias.end()) { + std::cout << "INFO[code]: unsupported signal id: " << (int)lpp_code_bias_type + << std::endl; + continue; + } + + const uint8_t code_type = lpp_to_spartn_bias.at(lpp_code_bias_type); + const auto correction = SPARTN_Generator::decode_value_lpp(this_code_bias->codeBias_r15, + Constants::bias_lpp_res); + + signal_corrs.emplace_back(std::make_pair(code_type, correction)); + } + + std::bitset code_bias_mask = 0; + std::vector encoded_values = {}; + static constexpr std::bitset new_sig = 1; + const uint8_t mask_length = bis.bit_count - 1; + + for (const auto& type_corr : signal_corrs) { + if (SPARTN_Generator::check_within_range(type_corr.second, + Constants::code_bias_spartn_rng_min)) { + code_bias_mask |= new_sig << (mask_length - type_corr.first - 1); + + encoded_values.emplace_back(SPARTN_Generator::encode_value_spartn( + type_corr.second, Constants::code_bias_spartn_rng_min, Constants::bias_spartn_res)); + } + } + + SPARTN_Generator::add_field_to_block(bias_block, bis.code_id, bis.bit_count, + (int64_t)code_bias_mask.to_ullong()); + + for (const long& v : encoded_values) { + SPARTN_Generator::add_field_to_block(bias_block, 29, 11, v); + } + + return true; +} + +void SPARTN_Generator::generate_hpac_messages( + std::vector>& messages) const { + if (!this->prov_ass_data_->a_gnss_ProvideAssistanceData->gnss_GenericAssistData) { + std::cout << "[INFO] no generic assistance data!" << std::endl; + return; + } + + const auto& gen_ass = + this->prov_ass_data_->a_gnss_ProvideAssistanceData->gnss_GenericAssistData; + for (const auto& this_assist_data : this->hpac_corrections_) { + const auto gnss_id = this_assist_data.gnss_id.gnss_id; + + std::unique_ptr this_message(new SPARTN_Message); + this_message->message_type = 1; + this_message->message_sub_type = Constants::gnss_id_to_hpac_subtype.at(gnss_id); + + const GNSS_SystemTime* const gnss_time = + SPARTN_Generator::get_gnss_systemtime(this_assist_data); + + SPARTN_Generator::add_time_to_message(this_message, gnss_time, (GNSS_ID__gnss_id)gnss_id); + + std::unique_ptr this_header(new SPARTN_Message_Header); + SPARTN_Generator::add_field_to_header(this_header, 5, 9, this_assist_data.iod_ssr); + SPARTN_Generator::add_field_to_header(this_header, 68, 4, 0); + SPARTN_Generator::add_field_to_header(this_header, 69, 1, 0); + + std::cout << std::endl << "==== HPAC MESSAGE ["; + std::cout << Constants::constellation_names.at(gnss_id); + std::cout << "] =====" << std::endl; + + if (!this_assist_data.correction_points) { + std::cout << "[INFO] no correction points!" << std::endl; + continue; + } + + if (!this_assist_data.stec_corrections) { + std::cout << "[INFO] no STEC corrections!" << std::endl; + continue; + } + + if (!this_assist_data.gridded_corrections) { + std::cout << "[INFO] no gridded corrections!" << std::endl; + continue; + } + + std::unique_ptr this_atmo_block(new SPARTN_Block); + + if (!SPARTN_Generator::generate_area_data(this_atmo_block, this_assist_data)) { + continue; + } + this->generate_tropo_data(this_atmo_block, this_assist_data.gridded_corrections); + this->generate_iono_data(this_atmo_block, this_assist_data); + + this_message->data.push(std::move(this_atmo_block)); + + SPARTN_Generator::add_field_to_header( + this_header, 30, 5, + (int64_t)(this_message->data.size() - 1)); // SPARTN counts from zero here + + this_message->message_header = std::move(this_header); + + messages.push_back(std::move(this_message)); + } +} + +bool SPARTN_Generator::generate_area_data(const std::unique_ptr& atmo_block, + const HPACCorrection& hpac_corr) { + std::unique_ptr area_data_block(new SPARTN_Block); + + const CorrectionPoint& corr_points = *hpac_corr.correction_points; + + SPARTN_Generator::add_field_to_block(area_data_block, 31, 8, corr_points.id); + + uint8_t num_of_grid_points = corr_points.num_of_grid_points; + + SPARTN_Generator::add_field_to_block(area_data_block, 39, 7, num_of_grid_points); + + /* [SPARTN] 0: none 1: poly 2: poly & grid. i.e. if we want to define + * residuals for the grid, we must provide a poly. The issue is that LPP + * does not provide a poly for the tropo, so this value can either only be + * zero or two. + * + * This is also for the entire grid, in SPARTN, either every grid point has + * a poly / poly & grid, or it does not. In LPP, it is per grid point, i.e. + * one grid point could have just a poly and another could have nothing and + * so on. + */ + uint8_t tropo_block_indicator = 2; + const auto grid_list = hpac_corr.gridded_corrections->gridList_r16.list; + + for (int i = 0; i < grid_list.count; i++) { + if (!grid_list.array[i]->tropospericDelayCorrection_r16) { + tropo_block_indicator = 0; + break; + } + } + + SPARTN_Generator::add_field_to_block(area_data_block, 40, 2, tropo_block_indicator); + + uint8_t iono_block_indicator = 0; + + if (hpac_corr.stec_corrections) { + iono_block_indicator++; + + if (hpac_corr.gridded_corrections) { + iono_block_indicator++; + } + } + + SPARTN_Generator::add_field_to_block(area_data_block, 40, 2, iono_block_indicator); + + SPARTN_Generator::add_block_to_block(atmo_block, area_data_block); + return true; +} + +void SPARTN_Generator::generate_tropo_data( + const std::unique_ptr& atmo_block, + const GNSS_SSR_GriddedCorrection_r16* const gridded_corrections) const { + if (!gridded_corrections) { + std::cout << "[INFO] no tropo data" << std::endl; + return; + } + + std::unique_ptr tropo_data_block(new SPARTN_Block); + + if (!this->generate_tropo_coef(tropo_data_block, gridded_corrections)) { + return; + } + SPARTN_Generator::generate_tropo_grid(tropo_data_block, gridded_corrections); + + SPARTN_Generator::add_block_to_block(atmo_block, tropo_data_block); +} + +bool SPARTN_Generator::generate_tropo_coef( + const std::unique_ptr& tropo_data_block, + const GNSS_SSR_GriddedCorrection_r16* const gridded_corrections) const { + std::unique_ptr tropo_coef_block(new SPARTN_Block); + + const auto grid_list = gridded_corrections->gridList_r16.list; + + /* + * LPP gives no coefficients for the troposphere but SPARTN requires at + * least T00, so just give a constant zero. + */ + SPARTN_Generator::add_field_to_block(tropo_coef_block, 41, 3, 0 /* just T00 */); + + const auto* const qual_indi = gridded_corrections->troposphericDelayQualityIndicator_r16; + const uint8_t qual_class = qual_indi ? *qual_indi->buf >> 3 : 0; + const uint8_t qual_value = qual_indi ? *qual_indi->buf & 3 : 0; + + uint8_t tropo_qual = 7; + + if (qual_class == 0 && qual_value == 0) { + tropo_qual = this->qualityoveride_ < 0 ? 0 : + 7 < this->qualityoveride_ ? 7 : + this->qualityoveride_; + } else if (qual_class <= 1 && qual_value < 1) { + const double q = (pow(3, qual_class)) * (1 + ((double)qual_value / 4)) - 1; + auto q_meter = q / 1000.0; + + for (uint64_t i = 0; i < Constants::spartn_tropo_qual_num; i++) { + if (Constants::spartn_tropo_qualities_maximums[i] < q_meter) { + tropo_qual = i; + break; + } + } + } + + printf("tropo class=%i, value=%i => qual=%i\n", qual_class, qual_value, tropo_qual); + + SPARTN_Generator::add_field_to_block(tropo_coef_block, 42, 3, tropo_qual); + + /* + * SPARTN only specifies the average hydrostatic delay, where-as LPP will + * give a delay for each grid point. Therefore we calculate the average + * from LPP + */ + double hydrostatic_delay_average = 0; + double count = 0; + for (int i = 0; i < grid_list.count; i++) { + if (!grid_list.array[i]->tropospericDelayCorrection_r16) { + return false; + } + + const long this_delay = + grid_list.array[i]->tropospericDelayCorrection_r16->tropoHydroStaticVerticalDelay_r16; + + const auto val_dec = + SPARTN_Generator::decode_value_lpp(this_delay, Constants::tropo_dry_res); + + if (SPARTN_Generator::check_within_range(val_dec, Constants::tropo_dry_spartn_rng_min)) { + hydrostatic_delay_average += val_dec; + ++count; + } + } + hydrostatic_delay_average /= count; + + SPARTN_Generator::add_field_to_block( + tropo_coef_block, 43, 8, + SPARTN_Generator::encode_value_spartn(hydrostatic_delay_average, + Constants::tropo_dry_spartn_rng_min, + Constants::tropo_dry_res)); + + SPARTN_Generator::add_field_to_block(tropo_coef_block, 44, 1, 0); + + { + /* + * LPP contains no troposphere coefficients, but SPARTN requires at + * least one set with at least T00 defined, so add a 0 one. + * + * We use a constant value of 63 because: + * (0 - -0.252) / 0.0004 = 63 + */ + std::unique_ptr zero_coef(new SPARTN_Block); + SPARTN_Generator::add_field_to_block(zero_coef, 45, 7, 63); + SPARTN_Generator::add_block_to_block(tropo_coef_block, zero_coef); + } + + SPARTN_Generator::add_block_to_block(tropo_data_block, tropo_coef_block); + return true; +} + +void SPARTN_Generator::generate_tropo_grid( + const std::unique_ptr& tropo_data_block, + const GNSS_SSR_GriddedCorrection_r16* const gridded_corrections) { + std::unique_ptr grid_block(new SPARTN_Block); + + SPARTN_Generator::add_field_to_block(grid_block, 51, 1, 1); + + const auto gridlist = gridded_corrections->gridList_r16.list; + + for (int i = 0; i < gridlist.count; i++) { + const long wetdelay = + gridlist.array[i]->tropospericDelayCorrection_r16->tropoWetVerticalDelay_r16; + + const double val_dec = + SPARTN_Generator::decode_value_lpp(wetdelay, Constants::wetdelay_res); + + // scale and res are the same between LPP and SPARTN so no need to + // check range + SPARTN_Generator::add_field_to_block( + grid_block, 53, 8, + SPARTN_Generator::encode_value_spartn(val_dec, Constants::wetdelay_spartn_rng_min, + Constants::wetdelay_res)); + } + + SPARTN_Generator::add_block_to_block(tropo_data_block, grid_block); +} + +void SPARTN_Generator::generate_iono_data(const std::unique_ptr& atmo_block, + const HPACCorrection& hpac_corr) const { + std::unique_ptr iono_data_block(new SPARTN_Block); + /* + * When mergeing the STEC_Corrections and GriddedCorrections, it is + * possible to safely assume that the correction ID is the same for all + * IEs. This is because GNSS-SSR-CorrectionPoints is sent once per message + * and contains only one ID for the entire message. + */ + std::vector satellite_data = {}; + + const auto& sat_list = hpac_corr.stec_corrections->stec_SatList_r16.list; + const auto& gridded_corrections = hpac_corr.gridded_corrections->gridList_r16.list; + const auto gnss_id = hpac_corr.gnss_id; + const auto& allowed_svs = this->sv_intersections_.at(gnss_id.gnss_id); + + for (int i = 0; i < sat_list.count; i++) { + const STEC_SatElement_r16* const this_sat = sat_list.array[i]; + const auto svid = this_sat->svID_r16.satellite_id; + + if (std::count(allowed_svs.begin(), allowed_svs.end(), svid) == 0) { + if (std::count(allowed_svs.begin(), allowed_svs.end(), svid) == 0) { + // std::cout << "[INFO] " << (int)svid << " not allowed in iono" + // << std::endl; + continue; + } + } + + satellite_data.emplace_back(IonosphereSatelliteData{(uint64_t)svid, this_sat, {}}); + } + + for (int i = 0; i < gridded_corrections.count; i++) { + const GridElement_r16* const this_grid_elm = gridded_corrections.array[i]; + + if (!this_grid_elm->stec_ResidualSatList_r16) { + continue; + } + + const auto reslist = this_grid_elm->stec_ResidualSatList_r16->list; + + for (int j = 0; j < reslist.count; j++) { + const long this_sv = reslist.array[j]->svID_r16.satellite_id; + + for (IonosphereSatelliteData& iono_data : satellite_data) { + if (iono_data.SVID == static_cast(this_sv)) { + iono_data.residuals.emplace_back(reslist.array[j]); + break; + } + } + } + } + + std::unique_ptr iono_block(new SPARTN_Block); + + const uint8_t sat_mask_bit_count = Constants::satellite_mask_sizes.at(gnss_id.gnss_id); + const uint8_t field_id = Constants::satellite_mask_ids.at(gnss_id.gnss_id); + + uint8_t min_coefs_found = 3; + std::bitset sat_mask_bitset = 2; + sat_mask_bitset <<= sat_mask_bit_count - 2; + + const uint16_t sat_mask_length = sat_mask_bit_count - 3; + static constexpr std::bitset new_sv = 1; + + for (const IonosphereSatelliteData& this_sat : satellite_data) { + uint8_t coefs_found = 0; + if (this_sat.sat_elm->stec_C01_r16) { + coefs_found++; + if (this_sat.sat_elm->stec_C10_r16) { + coefs_found++; + if (this_sat.sat_elm->stec_C11_r16) { + coefs_found++; + } + } + } + + if (coefs_found < min_coefs_found) { + min_coefs_found = coefs_found; + } + + sat_mask_bitset |= new_sv << (sat_mask_length - this_sat.SVID); + } + + SPARTN_Generator::add_field_to_block(iono_block, 54, 3, min_coefs_found); + + std::unique_ptr satellite_mask( + new SPARTN_Field(field_id, sat_mask_bit_count, sat_mask_bitset)); + iono_block->data.push(std::move(satellite_mask)); + + SPARTN_Generator::add_block_to_block(iono_data_block, iono_block); + + for (const IonosphereSatelliteData& this_sat : satellite_data) { + std::unique_ptr iono_sat_block(new SPARTN_Block); + + std::pair, bool> coefs_res_size = + SPARTN_Generator::generate_iono_coef_block(this_sat.sat_elm, min_coefs_found); + + this->generate_iono_sat_block(iono_sat_block, this_sat.sat_elm, coefs_res_size.second); + SPARTN_Generator::add_block_to_block(iono_sat_block, coefs_res_size.first); + + SPARTN_Generator::generate_iono_grid_block(iono_sat_block, this_sat.residuals); + SPARTN_Generator::add_block_to_block(iono_data_block, iono_sat_block); + } + SPARTN_Generator::add_block_to_block(atmo_block, iono_data_block); +} + +void SPARTN_Generator::generate_iono_sat_block(const std::unique_ptr& iono_data_block, + const STEC_SatElement_r16* const& sat_elm, + const bool residual_size) const { + std::unique_ptr iono_poly_block(new SPARTN_Block); + + uint8_t spartn_iono_quality = 0; + + auto stec_buf = *sat_elm->stecQualityIndicator_r16.buf; + + uint8_t stec_class = stec_buf & 7; + stec_class = ((stec_class & 1) << 2) | ((stec_class & 2) << 0) | ((stec_class & 4) >> 2); + uint8_t stec_value = stec_buf >> 3; + stec_value = ((stec_value & 1) << 2) | ((stec_value & 2) << 0) | ((stec_value & 4) >> 2); + const uint8_t index = (8 * stec_class) + stec_value; + + if (index == 64) { + spartn_iono_quality = 15; + } else if (index == 0) { + spartn_iono_quality = this->qualityoveride_ < 0 ? 0 : + 15 < this->qualityoveride_ ? 15 : + this->qualityoveride_; + } else { + const double lpp_stec_qual = Constants::lpp_stec_qualities_maxiumums.at(64 - index); + + for (uint64_t i = 0; i < Constants::spartn_stec_qual_num; i++) { + const double this_max = Constants::spartn_stec_qualities_maximums.at(i); + + if (this_max > lpp_stec_qual) { + spartn_iono_quality = i + 1; + break; + } + } + } + + printf("iono class=%i, value=%i, index=%2i => qual=%2i\n", stec_class, stec_value, index, + spartn_iono_quality); + + SPARTN_Generator::add_field_to_block(iono_poly_block, 55, 4, spartn_iono_quality); + + SPARTN_Generator::add_field_to_block(iono_poly_block, 56, 1, (int64_t)residual_size); + + SPARTN_Generator::add_block_to_block(iono_data_block, iono_poly_block); +} + +std::pair, bool> +SPARTN_Generator::generate_iono_coef_block(const STEC_SatElement_r16* const& sat_elm, + const uint8_t equ_type) { + std::unique_ptr iono_coef_block(new SPARTN_Block); + + bool use_small_coef = true; + + double c00_dec = -1; + double c01_dec = -1; + double c10_dec = -1; + double c11_dec = -1; + uint8_t coef_counter = 0; + + double val_dec = + SPARTN_Generator::decode_value_lpp(sat_elm->stec_C00_r16, Constants::iono_lpp_c00_res); + + if (SPARTN_Generator::check_within_range(val_dec, Constants::iono_spartn_c00_small_rng_min)) { + c00_dec = val_dec; + } else if (SPARTN_Generator::check_within_range(val_dec, + Constants::iono_spartn_c00_large_rng_min)) { + use_small_coef = false; + c00_dec = val_dec; + } + + if (coef_counter < equ_type && c00_dec != -1 && sat_elm->stec_C01_r16) { + coef_counter++; + val_dec = + SPARTN_Generator::decode_value_lpp(*sat_elm->stec_C01_r16, Constants::iono_lpp_c01_res); + + if (use_small_coef && SPARTN_Generator::check_within_range( + val_dec, Constants::iono_spartn_c01_small_rng_min)) { + c01_dec = val_dec; + } else { + use_small_coef = false; + + if (SPARTN_Generator::check_within_range(val_dec, + Constants::iono_spartn_c01_large_rng_min)) { + c01_dec = val_dec; + } + } + + if (coef_counter < equ_type && c01_dec != -1 && sat_elm->stec_C10_r16) { + coef_counter++; + val_dec = SPARTN_Generator::decode_value_lpp(*sat_elm->stec_C10_r16, + Constants::iono_lpp_c10_res); + + if (use_small_coef && SPARTN_Generator::check_within_range( + val_dec, Constants::iono_spartn_c10_small_rng_min)) { + c10_dec = val_dec; + } else { + use_small_coef = false; + + if (SPARTN_Generator::check_within_range( + val_dec, Constants::iono_spartn_c10_large_rng_min)) { + c10_dec = val_dec; + } + } + + if (coef_counter < equ_type && c10_dec != -1 && sat_elm->stec_C11_r16) { + val_dec = SPARTN_Generator::decode_value_lpp(*sat_elm->stec_C11_r16, + Constants::iono_lpp_c11_res); + + if (use_small_coef && SPARTN_Generator::check_within_range( + val_dec, Constants::iono_spartn_c11_small_rng_min)) { + c11_dec = val_dec; + } else { + use_small_coef = false; + + if (SPARTN_Generator::check_within_range( + val_dec, Constants::iono_spartn_c11_large_rng_min)) { + c11_dec = val_dec; + } + } + } + } + } + + const uint8_t base_bit_count = use_small_coef ? 12 : 14; + const uint8_t base_id = use_small_coef ? 57 : 60; + + if (c00_dec != -1) { + SPARTN_Generator::add_field_to_block( + iono_coef_block, base_id, base_bit_count, + SPARTN_Generator::encode_value_spartn(c00_dec, + use_small_coef ? + Constants::iono_spartn_c00_small_rng_min : + Constants::iono_spartn_c00_large_rng_min, + Constants::iono_spartn_c00_res)); + } else { + throw std::invalid_argument("did not get a valid c00 value"); + } + + if (c01_dec != -1) { + SPARTN_Generator::add_field_to_block( + iono_coef_block, base_id + 1, base_bit_count, + SPARTN_Generator::encode_value_spartn(c01_dec, + use_small_coef ? + Constants::iono_spartn_c01_small_rng_min : + Constants::iono_spartn_c01_large_rng_min, + Constants::iono_spartn_c01_res)); + } + + if (c10_dec != -1) { + SPARTN_Generator::add_field_to_block( + iono_coef_block, base_id + 1, base_bit_count, + SPARTN_Generator::encode_value_spartn(c10_dec, + use_small_coef ? + Constants::iono_spartn_c10_small_rng_min : + Constants::iono_spartn_c10_large_rng_min, + Constants::iono_spartn_c10_res)); + } + + if (c11_dec != -1) { + SPARTN_Generator::add_field_to_block( + iono_coef_block, base_id + 2, base_bit_count + 1, + SPARTN_Generator::encode_value_spartn(c11_dec, + use_small_coef ? + Constants::iono_spartn_c11_small_rng_min : + Constants::iono_spartn_c11_large_rng_min, + Constants::iono_spartn_c11_res)); + } + + return std::make_pair(std::move(iono_coef_block), !use_small_coef); +} + +void SPARTN_Generator::generate_iono_grid_block( + const std::unique_ptr& iono_data_block, + const std::vector& residuals) { + std::unique_ptr grid_block(new SPARTN_Block); + + // small = 0, medium = 1, large = 2, extra large = 3 + uint8_t residual_size = 0; + std::vector decoded_residuals = {}; + for (const STEC_ResidualSatElement_r16* const residual_sat : residuals) { + const long this_res = SPARTN_Generator::get_residual(residual_sat); + if (this_res == 99999) { + throw std::invalid_argument("[ERR] unset residual, grid invliad. unrecoverable state."); + } + + const double val_dec = + SPARTN_Generator::decode_value_lpp(this_res, Constants::residual_res); + + uint8_t this_residual_size = 0; + if (SPARTN_Generator::check_within_range(val_dec, + Constants::residual_spartn_small_rng_min)) { + this_residual_size = 0; + } else if (SPARTN_Generator::check_within_range( + val_dec, Constants::residual_spartn_medium_rng_min)) { + this_residual_size = 1; + } else if (SPARTN_Generator::check_within_range(val_dec, + Constants::residual_spartn_large_rng_min)) { + this_residual_size = 2; + } else if (SPARTN_Generator::check_within_range( + val_dec, Constants::residual_spartn_extra_large_rng_min)) { + this_residual_size = 3; + } else { + continue; + } + + decoded_residuals.emplace_back(val_dec); + + if (this_residual_size > residual_size) { + residual_size = this_residual_size; + } + } + + uint8_t field_id = 64; + uint8_t field_size = 4; + double res_rng_min = Constants::residual_spartn_small_rng_min; + + if (residual_size == 1) { + field_id = 65; + field_size = 7; + res_rng_min = Constants::residual_spartn_medium_rng_min; + } else if (residual_size == 2) { + field_id = 66; + field_size = 10; + res_rng_min = Constants::residual_spartn_large_rng_min; + } else if (residual_size == 3) { + field_id = 67; + field_size = 14; + res_rng_min = Constants::residual_spartn_extra_large_rng_min; + } + + SPARTN_Generator::add_field_to_block(grid_block, 63, 2, residual_size); + + for (const double& residual : decoded_residuals) { + SPARTN_Generator::add_field_to_block( + grid_block, field_id, field_size, + SPARTN_Generator::encode_value_spartn(residual, res_rng_min, Constants::residual_res)); + } + + SPARTN_Generator::add_block_to_block(iono_data_block, grid_block); +} + +void SPARTN_Generator::generate_gad_message( + std::vector>& messages) { + if (this->hpac_corrections_.size() == 0) { + std::cout << "[INFO] [GAD] only send GAD with HPAC" << std::endl; + return; + } else if (!this->last_correction_point_) { + std::cout << "[INFO] [GAD] no correction points (stored)" << std::endl; + return; + } + + // NOTE(ewasjon): LPP doesn't include IOD SSR for GAD, so we use one from the HPAC corrections. + auto iod_ssr = this->hpac_corrections_.at(0).iod_ssr; + + std::cout << std::endl << "==== NEW GAD MESSAGE ====" << std::endl; + std::unique_ptr this_message(new SPARTN_Message); + + this_message->message_type = 2; + this_message->message_sub_type = 0; + + std::unique_ptr this_header(new SPARTN_Message_Header); + SPARTN_Generator::add_field_to_header(this_header, 5, 9, iod_ssr); + SPARTN_Generator::add_field_to_header(this_header, 68, 4, 0); + SPARTN_Generator::add_field_to_header(this_header, 69, 1, 0); + + // LPP only provides one area at a time + // SPARTN counts from zero + SPARTN_Generator::add_field_to_header(this_header, 30, 5, 0); + + this_message->message_header = std::move(this_header); + + const uint32_t area_id = this->last_correction_point_->id; + + SPARTN_Generator::generate_gad_message_array(this_message, *this->last_correction_point_, + area_id); + + messages.push_back(std::move(this_message)); +} + +// NOTE(ewasjon): This function may not be correct. +void SPARTN_Generator::generate_gad_message_list( + const std::unique_ptr& gad_message, + const GNSS_SSR_ListOfCorrectionPoints_r16& corrpoints, const uint32_t area_id) { + std::unique_ptr area_def(new SPARTN_Block); + + SPARTN_Generator::add_field_to_block(area_def, 31, 8, area_id); + + SPARTN_Generator::add_field_to_block(area_def, 32, 11, corrpoints.referencePointLatitude_r16); + + SPARTN_Generator::add_field_to_block(area_def, 33, 12, corrpoints.referencePointLongitude_r16); + + SPARTN_Generator::add_field_to_block(area_def, 34, 3, + corrpoints.relativeLocationsList_r16.list.count); + + SPARTN_Generator::add_field_to_block(area_def, 35, 3, + corrpoints.relativeLocationsList_r16.list.count); + + const uint16_t curr_delta_lat = + corrpoints.relativeLocationsList_r16.list.array[0]->deltaLatitude_r16; + const uint16_t curr_delta_lng = + corrpoints.relativeLocationsList_r16.list.array[0]->deltaLongitude_r16; + + // LPP spec allows delta to be -ve & 0, SPARTN does not. we check max after + // sanity check + if (curr_delta_lng <= 0) { + std::cout << "[INFO] got a negative longitude delta" << std::endl; + return; + } + + if (curr_delta_lat <= 0) { + std::cout << "[INFO] got a negative latitude delta" << std::endl; + return; + } + + /* sanity check */ + for (int i = 1; i < corrpoints.relativeLocationsList_r16.list.count; i++) { + const RelativeLocation* const this_elm = corrpoints.relativeLocationsList_r16.list.array[i]; + + if (this_elm->deltaLatitude_r16 != curr_delta_lat) { + std::cout << "[INFO] got different delta lat from list, aborting" << std::endl; + return; + } + if (this_elm->deltaLongitude_r16 != curr_delta_lng) { + std::cout << "[INFO] got different delta lng from list, aborting" << std::endl; + return; + } + } + + const auto dec_delta_lat = + SPARTN_Generator::decode_value_lpp(curr_delta_lat, Constants::delta_lpp_res); + const auto dec_delta_lng = + SPARTN_Generator::decode_value_lpp(curr_delta_lng, Constants::delta_lpp_res); + + if (dec_delta_lat > Constants::delta_spartn_rng_max) { + std::cout << "[INFO] got a latitude delta outside range" << std::endl; + return; + } + + if (dec_delta_lng > Constants::delta_spartn_rng_max) { + std::cout << "[INFO] got a longitude delta outside range" << std::endl; + return; + } + + SPARTN_Generator::add_field_to_block( + area_def, 36, 5, + SPARTN_Generator::encode_value_spartn(dec_delta_lat, Constants::delta_spartn_rng_min, + Constants::delta_spartn_res)); + SPARTN_Generator::add_field_to_block( + area_def, 37, 5, + SPARTN_Generator::encode_value_spartn(dec_delta_lng, Constants::delta_spartn_rng_min, + Constants::delta_spartn_res)); + + gad_message->data.push(std::move(area_def)); +} + +void SPARTN_Generator::generate_gad_message_array( + const std::unique_ptr& gad_message, const CorrectionPoint& corrpoints, + const uint32_t area_id) { + std::unique_ptr area_def(new SPARTN_Block); + + SPARTN_Generator::add_field_to_block(area_def, 31, 8, area_id); + + auto ref_point_lat_lpp = corrpoints.referencePointLatitude_r16; + auto ref_point_lng_lpp = corrpoints.referencePointLongitude_r16; + + auto ref_point_lat_real = + ((double)ref_point_lat_lpp / Constants::two_to_the_fourteen) * Constants::deg_in_lat; + auto ref_point_lng_real = + ((double)ref_point_lng_lpp / Constants::two_to_the_fifteen) * Constants::deg_in_lng; + + auto ref_point_lat_spartn = SPARTN_Generator::encode_value_spartn( + ref_point_lat_real, -1 * Constants::deg_in_lat, Constants::lat_lng_res); + auto ref_point_lng_spartn = SPARTN_Generator::encode_value_spartn( + ref_point_lng_real, -1 * Constants::deg_in_lng, Constants::lat_lng_res); + + SPARTN_Generator::add_field_to_block(area_def, 32, 11, ref_point_lat_spartn); + SPARTN_Generator::add_field_to_block(area_def, 33, 12, ref_point_lng_spartn); + + auto num_of_steps_lat = corrpoints.numberOfStepsLatitude_r16; + auto num_of_steps_lng = corrpoints.numberOfStepsLongitude_r16; + + auto grid_count_lat = num_of_steps_lat + 1; + auto grid_count_lng = num_of_steps_lng + 1; + + // NOTE(ewasjon): This cannot happen, but we check anyway + if (grid_count_lat == 0 || grid_count_lng == 0) { + std::cout << "[ERR] lpp provided a grid with zero steps"; + std::cout << ", lat steps " << num_of_steps_lat; + std::cout << ", long steps " << num_of_steps_lng << std::endl; + return; + } + + if (grid_count_lat > 8) { + std::cout << "[ERR] too many grid points in latitude: " << grid_count_lat << " (max 8)"; + return; + } else if (grid_count_lng > 8) { + std::cout << "[ERR] too many grid points in longitude: " << grid_count_lng << " (max 8)"; + return; + } + + // NOTE(ewasjon): SF034 and SF035 encode the number of grid points with 3 bits. Where 0b000 is 1 + // grid point, 0b001 is 2 grid points, etc. So we subtract 1 from the number of grid points to + // get the correct encoding. + SPARTN_Generator::add_field_to_block(area_def, 34, 3, grid_count_lat - 1); + SPARTN_Generator::add_field_to_block(area_def, 35, 3, grid_count_lng - 1); + + auto delta_lat_lpp = corrpoints.stepOfLatitude_r16; + auto delta_lng_lpp = corrpoints.stepOfLongitude_r16; + auto delta_lat_real = (double)delta_lat_lpp * Constants::step_lpp_res; + auto delta_lng_real = (double)delta_lng_lpp * Constants::step_lpp_res; + + // NOTE(ewasjon): SPARTN encodes the delta with 5 bits, for the range 0.1 to 3.2 degrees. Thus, + // the 0 integer value is not equal to 0 degrees, but rather 0.1 degrees. We subtract 0.1 from + // the delta to get the correct encoding. + auto delta_lng_spartn = + std::lround((delta_lng_real - Constants::step_spartn_res) / Constants::step_spartn_res); + auto delta_lat_spartn = + std::lround((delta_lat_real - Constants::step_spartn_res) / Constants::step_spartn_res); + + SPARTN_Generator::add_field_to_block(area_def, 36, 5, delta_lat_spartn); + SPARTN_Generator::add_field_to_block(area_def, 37, 5, delta_lng_spartn); + + gad_message->data.push(std::move(area_def)); +} + +template +void SPARTN_Generator::add_ocb_correction(std::vector& ocb_corrections, + const T* const corrs, const GNSS_SSR_URA_r16* ura, + const GNSS_ID gnss_id) { + if (!corrs) { + return; + } + + bool found_ocb = false; + long iod_ssr = 0; + for (OCBCorrection& ocb : ocb_corrections) { + if (gnss_id.gnss_id != ocb.gnss_id.gnss_id) { + continue; + } + + iod_ssr = SPARTN_Generator::get_iod(corrs, has_iod_r16{}); + + if (iod_ssr == ocb.iod_ssr) { + SPARTN_Generator::add_ocb_correction(ocb, corrs); + found_ocb = true; + break; + } + } + + if (!found_ocb) { + ocb_corrections.push_back({iod_ssr, gnss_id, nullptr, nullptr, nullptr, nullptr, ura}); + SPARTN_Generator::add_ocb_correction(ocb_corrections.back(), corrs); + } +} + +std::vector +SPARTN_Generator::group_ocb_corrections(const GNSS_GenericAssistData* const gen_ass) { + std::vector ocb_corrections = {}; + for (int i = 0; gen_ass && i < gen_ass->list.count; i++) { + const GNSS_GenericAssistDataElement* const this_assist_data = gen_ass->list.array[i]; + + const auto& gnss_id = this_assist_data->gnss_ID; + + const GNSS_SSR_OrbitCorrections_r15* orb_corrs = nullptr; + const GNSS_SSR_ClockCorrections_r15* clk_corrs = nullptr; + const GNSS_SSR_CodeBias_r15* code_corrs = nullptr; + const GNSS_SSR_PhaseBias_r16* phase_corrs = nullptr; + + const GNSS_SSR_URA_r16* ura = nullptr; + + if (this_assist_data->ext2) { + const auto& ext2 = this_assist_data->ext2; + + orb_corrs = + ext2->gnss_SSR_OrbitCorrections_r15 ? ext2->gnss_SSR_OrbitCorrections_r15 : nullptr; + + clk_corrs = + ext2->gnss_SSR_ClockCorrections_r15 ? ext2->gnss_SSR_ClockCorrections_r15 : nullptr; + + code_corrs = ext2->gnss_SSR_CodeBias_r15 ? ext2->gnss_SSR_CodeBias_r15 : nullptr; + } + + if (this_assist_data->ext3) { + const auto& ext3 = this_assist_data->ext3; + + phase_corrs = ext3->gnss_SSR_PhaseBias_r16 ? ext3->gnss_SSR_PhaseBias_r16 : nullptr; + + ura = ext3->gnss_SSR_URA_r16 ? ext3->gnss_SSR_URA_r16 : nullptr; + } + + SPARTN_Generator::add_ocb_correction(ocb_corrections, orb_corrs, ura, gnss_id); + SPARTN_Generator::add_ocb_correction(ocb_corrections, clk_corrs, ura, gnss_id); + SPARTN_Generator::add_ocb_correction(ocb_corrections, code_corrs, ura, gnss_id); + SPARTN_Generator::add_ocb_correction(ocb_corrections, phase_corrs, ura, gnss_id); + } + + return ocb_corrections; +} + +std::vector +SPARTN_Generator::group_hpac_corrections(const GNSS_GenericAssistData* const gen_ass, + const GNSS_CommonAssistData* const com_ass) { + std::vector hpac_corrections = {}; + + const GNSS_SSR_CorrectionPoints_r16* correction_points = + (com_ass && com_ass->ext2) ? com_ass->ext2->gnss_SSR_CorrectionPoints_r16 ? + com_ass->ext2->gnss_SSR_CorrectionPoints_r16 : + nullptr : + nullptr; + + if (correction_points) { + this->last_correction_point_ = nullptr; + + this->last_correction_point_ = std::unique_ptr(new CorrectionPoint{}); + this->last_correction_point_->id = correction_points->correctionPointSetID_r16; + + switch (correction_points->correctionPoints_r16.present) { + case GNSS_SSR_CorrectionPoints_r16__correctionPoints_r16_PR_listOfCorrectionPoints_r16: { + this->last_correction_point_->num_of_grid_points = + correction_points->correctionPoints_r16.choice.listOfCorrectionPoints_r16 + .relativeLocationsList_r16.list.count; + break; + } + case GNSS_SSR_CorrectionPoints_r16__correctionPoints_r16_PR_arrayOfCorrectionPoints_r16: { + const auto& array = + correction_points->correctionPoints_r16.choice.arrayOfCorrectionPoints_r16; + this->last_correction_point_->num_of_grid_points = + (array.numberOfStepsLatitude_r16 + 1) * (array.numberOfStepsLongitude_r16 + 1); + + this->last_correction_point_->referencePointLatitude_r16 = + array.referencePointLatitude_r16; + this->last_correction_point_->referencePointLongitude_r16 = + array.referencePointLongitude_r16; + this->last_correction_point_->numberOfStepsLatitude_r16 = + array.numberOfStepsLatitude_r16; + this->last_correction_point_->numberOfStepsLongitude_r16 = + array.numberOfStepsLongitude_r16; + this->last_correction_point_->stepOfLatitude_r16 = array.stepOfLatitude_r16; + this->last_correction_point_->stepOfLongitude_r16 = array.stepOfLongitude_r16; + + break; + } + default: { + this->last_correction_point_ = nullptr; + break; + } + } + } + + for (int i = 0; gen_ass && i < gen_ass->list.count; i++) { + const auto& this_assist_data = gen_ass->list.array[i]; + + const auto& gnss_id = this_assist_data->gnss_ID; + + const GNSS_SSR_GriddedCorrection_r16* gridded_corrections = nullptr; + const GNSS_SSR_STEC_Correction_r16* stec_corrections = nullptr; + + if (this_assist_data->ext3) { + const auto& ext3 = this_assist_data->ext3; + + gridded_corrections = ext3->gnss_SSR_GriddedCorrection_r16 ? + ext3->gnss_SSR_GriddedCorrection_r16 : + nullptr; + + stec_corrections = + ext3->gnss_SSR_STEC_Correction_r16 ? ext3->gnss_SSR_STEC_Correction_r16 : nullptr; + } + + if ((gridded_corrections && stec_corrections)) { + if (gridded_corrections->iod_ssr_r16 == stec_corrections->iod_ssr_r16) { + hpac_corrections.push_back({gridded_corrections->iod_ssr_r16, gnss_id, + this->last_correction_point_.get(), gridded_corrections, + stec_corrections}); + } else { + hpac_corrections.push_back({gridded_corrections->iod_ssr_r16, gnss_id, + this->last_correction_point_.get(), gridded_corrections, + nullptr}); + hpac_corrections.push_back({stec_corrections->iod_ssr_r16, gnss_id, + this->last_correction_point_.get(), nullptr, + stec_corrections}); + } + } + if (gridded_corrections && !stec_corrections) { + hpac_corrections.push_back({gridded_corrections->iod_ssr_r16, gnss_id, + this->last_correction_point_.get(), gridded_corrections, + nullptr}); + } else if (!gridded_corrections && stec_corrections) { + hpac_corrections.push_back({stec_corrections->iod_ssr_r16, gnss_id, + this->last_correction_point_.get(), nullptr, + stec_corrections}); + } + } + return hpac_corrections; +} + +static TAI_Time time_from_epoch_time(const GNSS_SystemTime* time) { + auto day_number = time->gnss_DayNumber; + auto time_of_day = time->gnss_TimeOfDay; + auto msec = time->gnss_TimeOfDayFrac_msec ? *time->gnss_TimeOfDayFrac_msec / 1000.0 : 0; + + switch (time->gnss_TimeID.gnss_id) { + case GNSS_ID__gnss_id_gps: { + auto gps_time = GPS_Time(day_number, static_cast(time_of_day) + msec); + return TAI_Time{gps_time}; + } + + case GNSS_ID__gnss_id_glonass: { + auto glo_time = GLO_Time(day_number, static_cast(time_of_day) + msec); + return TAI_Time{glo_time}; + } + + case GNSS_ID__gnss_id_galileo: { + auto gal_time = GST_Time(day_number, static_cast(time_of_day) + msec); + return TAI_Time{gal_time}; + } + + case GNSS_ID__gnss_id_bds: { + auto bds_time = BDT_Time(day_number, static_cast(time_of_day) + msec); + return TAI_Time{bds_time}; + } + + default: assert(false); + } +} + +void SPARTN_Generator::add_time_to_message(const std::unique_ptr& message, + const GNSS_SystemTime* epoch_time, + const GNSS_ID__gnss_id gnss_id) { + const long tod = + epoch_time->gnss_TimeOfDay + (epoch_time->gnss_TimeOfDayFrac_msec ? + *epoch_time->gnss_TimeOfDayFrac_msec >= 500 ? 1 : 0 : + 0); + + std::unique_ptr time( + new SPARTN_LPP_Time(gnss_id, epoch_time->gnss_DayNumber, tod)); + + message->time = std::move(time); +} + +template +void SPARTN_Generator::add_ocb_for_svs(std::map& ocb_for_svs, T corr) { + const auto corr_list = corr.list; + + for (int i = 0; i < corr_list.count; i++) { + const auto svid = SPARTN_Generator::get_svid(corr_list.array[i], + has_svid_r16{}); + + if (ocb_for_svs.find(svid) == ocb_for_svs.end()) { + OCBSatCorrection ocb_for_sv{nullptr, nullptr, nullptr, nullptr}; + ocb_for_svs[svid] = ocb_for_sv; + } + + SPARTN_Generator::add_sv(ocb_for_svs.at(svid), corr_list.array[i]); + } +} + +std::map +SPARTN_Generator::get_ocb_for_svs(const GNSS_SSR_OrbitCorrections_r15* const orbit_corrs, + const GNSS_SSR_ClockCorrections_r15* const clock_corrs, + const GNSS_SSR_CodeBias_r15* const code_bias_corrs, + const GNSS_SSR_PhaseBias_r16* const phase_bias_corrs) { + std::map ocb_for_svs = {}; + + if (orbit_corrs) { + SPARTN_Generator::add_ocb_for_svs(ocb_for_svs, orbit_corrs->ssr_OrbitCorrectionList_r15); + } + + if (clock_corrs) { + SPARTN_Generator::add_ocb_for_svs(ocb_for_svs, clock_corrs->ssr_ClockCorrectionList_r15); + } + + if (code_bias_corrs) { + SPARTN_Generator::add_ocb_for_svs(ocb_for_svs, code_bias_corrs->ssr_CodeBiasSatList_r15); + } + + if (phase_bias_corrs) { + SPARTN_Generator::add_ocb_for_svs(ocb_for_svs, phase_bias_corrs->ssr_PhaseBiasSatList_r16); + } + + return ocb_for_svs; +} diff --git a/generator/spartn/include/generator/spartn/constants.h b/generator/spartn/include/generator/spartn/constants.h new file mode 100644 index 00000000..e91eb8f2 --- /dev/null +++ b/generator/spartn/include/generator/spartn/constants.h @@ -0,0 +1,242 @@ +#ifndef SPARTN_CONSTANTS +#define SPARTN_CONSTANTS + +#include +#include +#include +#include + +namespace Constants { +static constexpr uint16_t max_payload_size = 8192; +static constexpr uint16_t max_message_size = 50115; +static constexpr uint8_t max_spartn_bit_count = 58; + +static constexpr uint8_t lpp_stec_qual_num = 63; +static constexpr uint8_t spartn_stec_qual_num = 14; +static constexpr uint8_t spartn_tropo_qual_num = 6; +static constexpr uint8_t spartn_continuity_num = 8; +static constexpr uint8_t num_of_gnss = 7; +/* + * All units are in a meters; + */ +static const std::map gnss_id_to_hpac_subtype = {{0, 0}, {3, 2}, {4, 1}}; + +static constexpr double orbit_lpp_radial_res = 0.0001; +static constexpr double orbit_lpp_along_res = 0.0004; +static constexpr double orbit_lpp_cross_res = 0.0004; + +static constexpr double clock_c0_lpp_res = 0.0001; +static constexpr double clock_c1_lpp_res = 0.000001; +static constexpr double clock_c2_lpp_res = 0.00000002; + +static constexpr double orbit_clock_spartn_res = 0.002; +static constexpr double orbit_clock_spartn_rng_min = -16.382; + +static constexpr double bias_lpp_res = 0.01; +static constexpr double bias_spartn_res = 0.02; +static constexpr double code_bias_spartn_rng_min = -20.46; +static constexpr double phase_bias_lpp_res = 0.001; +static constexpr double phase_bias_spartn_res = 0.002; +static constexpr double phase_bias_spartn_rng_min = -16.382; + +static constexpr double iono_lpp_c00_res = 0.05; +static constexpr double iono_lpp_c01_res = 0.02; +static constexpr double iono_lpp_c10_res = 0.02; +static constexpr double iono_lpp_c11_res = 0.02; +static constexpr double iono_spartn_c00_res = 0.04; +static constexpr double iono_spartn_c00_small_rng_min = -81.88; +static constexpr double iono_spartn_c00_large_rng_min = -327.64; +static constexpr double iono_spartn_c01_res = 0.008; +static constexpr double iono_spartn_c01_small_rng_min = -16.376; +static constexpr double iono_spartn_c01_large_rng_min = -65.528; +static constexpr double iono_spartn_c10_res = 0.008; +static constexpr double iono_spartn_c10_small_rng_min = -16.376; +static constexpr double iono_spartn_c10_large_rng_min = -65.528; +static constexpr double iono_spartn_c11_res = 0.002; +static constexpr double iono_spartn_c11_small_rng_min = -8.190; +static constexpr double iono_spartn_c11_large_rng_min = -32.766; + +static constexpr double tropo_dry_res = 0.004; +static constexpr double tropo_dry_spartn_rng_min = -0.508; +static constexpr double tropo_dry_lpp_offset = 2.3; +static constexpr double tropo_wet_res = 0.004; +static constexpr double tropo_wet_spartn_rng_min = -1.020; + +static constexpr double residual_res = 0.04; // same for LPP & SPARTN +static constexpr double residual_spartn_small_rng_min = -0.28; +static constexpr double residual_spartn_medium_rng_min = -2.52; +static constexpr double residual_spartn_large_rng_min = -20.44; +static constexpr double residual_spartn_extra_large_rng_min = -327.64; + +static constexpr double delta_lpp_res = 0.01; +static constexpr double delta_spartn_res = 0.1; +static constexpr double delta_spartn_rng_min = 0.1; +static constexpr double delta_spartn_rng_max = 3.2; + +static constexpr double wetdelay_res = 0.004; +static constexpr double wetdelay_spartn_rng_min = -0.508; + +static constexpr uint16_t two_to_the_fourteen = 16384; +static constexpr uint16_t two_to_the_fifteen = 32768; +static constexpr uint8_t deg_in_lat = 90; +static constexpr uint8_t deg_in_lng = 180; +static constexpr double lat_lng_res = 0.1; +static constexpr double step_lpp_res = 0.01; +static constexpr double step_spartn_res = 0.1; + +static constexpr uint32_t seconds_in_day = 86400; +static constexpr uint16_t day_delta_1970_1980 = 3657; // Jan 6 +static constexpr uint16_t day_delta_1970_1996 = 9496; // Jan 1 +static constexpr uint16_t day_delta_1970_1999 = 10825; // Aug 2 +static constexpr uint16_t day_delta_1970_2006 = 13149; // Jan 1 +static constexpr uint16_t day_delta_1970_2010 = 14610; // Jan 1 +static constexpr uint32_t second_delta_1970_2010 = 1262304000; + +// if a index of 64 is found, then there is no max... +static constexpr std::array lpp_stec_qualities_maxiumums = { + 33.6664, 30.2992, 26.9319, 23.5647, 20.1974, 16.8301, 13.4629, 12.3405, 11.2180, + 10.0956, 8.9732, 7.8508, 6.7284, 5.6059, 4.4835, 4.1094, 3.7352, 3.3611, + 2.9870, 2.6128, 2.2387, 1.8645, 1.4904, 1.3657, 1.2410, 1.1163, 0.9915, + 0.8668, 0.7421, 0.6174, 0.4927, 0.4511, 0.4096, 0.3680, 0.3264, 0.2848, + 0.2433, 0.2017, 0.1601, 0.1463, 0.1324, 0.1186, 0.1047, 0.0908, 0.0770, + 0.0631, 0.0493, 0.0447, 0.0400, 0.0354, 0.0308, 0.0262, 0.0216, 0.0169, + 0.0123, 0.0108, 0.0092, 0.0077, 0.0062, 0.0046, 0.0031, 0.0015, 0}; + +static constexpr std::array spartn_stec_qualities_maximums = { + 0.03, 0.05, 0.07, 0.14, 0.28, 0.56, 1.12, 2.24, 4.48, 8.96, 17.92, 35.84, 71.68, 143.36}; + +static constexpr std::array spartn_tropo_qualities_maximums = { + 0.010, 0.020, 0.040, 0.080, 0.160, 0.320}; + +static constexpr std::array ura_values = {0, 0.01, 0.02, 0.05, 0.1, 0.3, 1.0}; + +static constexpr std::array update_intervals = { + 1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, 900, 1800, 3600, 7200, 10800}; + +static const std::map gps_lpp_to_spartn_bias = {{0, 0}, {8, 1}, {10, 2}, {13, 3}}; +static const std::map glo_lpp_to_spartn_bias = {{0, 0}, {1, 1}}; +static const std::map gal_lpp_to_spartn_bias = {{5, 0}, {22, 1}, {16, 2}}; +static const std::map bds_lpp_to_spartn_bias = {{0, 0}, {6, 2}, {3, 3}}; +static const std::map qzss_lpp_to_spartn_bias = {{0, 0}, {8, 1}, {11, 2}}; + +static constexpr uint8_t max_bias_mask_bit_count = 9; + +typedef struct bias_ids_size { + uint8_t phase_id; + uint8_t code_id; + uint8_t bit_count; +} bias_ids_size; + +static const bias_ids_size gps_bias_ids_size = { + 25, + 27, + 7, +}; + +static const bias_ids_size glo_bias_ids_size = { + 26, + 28, + 6, +}; + +static const bias_ids_size gal_bias_ids_size = { + 102, + 105, + 9, +}; + +static const bias_ids_size bds_bias_ids_size = { + 106, + 103, + 9, +}; + +static const bias_ids_size qzss_bias_ids_size = { + 107, + 104, + 7, +}; + +/* +Generated with: + polynomial = 0x09U + initial value = 0 + input reflected = true + result reflected = true + final XOR value = 0 +*/ +static const uint8_t crc4_lookuptable[256] = { + 0x00, 0x0B, 0x05, 0x0E, 0x0A, 0x01, 0x0F, 0x04, 0x07, 0x0C, 0x02, 0x09, 0x0D, 0x06, 0x08, 0x03, + 0x0E, 0x05, 0x0B, 0x00, 0x04, 0x0F, 0x01, 0x0A, 0x09, 0x02, 0x0C, 0x07, 0x03, 0x08, 0x06, 0x0D, + 0x0F, 0x04, 0x0A, 0x01, 0x05, 0x0E, 0x00, 0x0B, 0x08, 0x03, 0x0D, 0x06, 0x02, 0x09, 0x07, 0x0C, + 0x01, 0x0A, 0x04, 0x0F, 0x0B, 0x00, 0x0E, 0x05, 0x06, 0x0D, 0x03, 0x08, 0x0C, 0x07, 0x09, 0x02, + 0x0D, 0x06, 0x08, 0x03, 0x07, 0x0C, 0x02, 0x09, 0x0A, 0x01, 0x0F, 0x04, 0x00, 0x0B, 0x05, 0x0E, + 0x03, 0x08, 0x06, 0x0D, 0x09, 0x02, 0x0C, 0x07, 0x04, 0x0F, 0x01, 0x0A, 0x0E, 0x05, 0x0B, 0x00, + 0x02, 0x09, 0x07, 0x0C, 0x08, 0x03, 0x0D, 0x06, 0x05, 0x0E, 0x00, 0x0B, 0x0F, 0x04, 0x0A, 0x01, + 0x0C, 0x07, 0x09, 0x02, 0x06, 0x0D, 0x03, 0x08, 0x0B, 0x00, 0x0E, 0x05, 0x01, 0x0A, 0x04, 0x0F, + 0x09, 0x02, 0x0C, 0x07, 0x03, 0x08, 0x06, 0x0D, 0x0E, 0x05, 0x0B, 0x00, 0x04, 0x0F, 0x01, 0x0A, + 0x07, 0x0C, 0x02, 0x09, 0x0D, 0x06, 0x08, 0x03, 0x00, 0x0B, 0x05, 0x0E, 0x0A, 0x01, 0x0F, 0x04, + 0x06, 0x0D, 0x03, 0x08, 0x0C, 0x07, 0x09, 0x02, 0x01, 0x0A, 0x04, 0x0F, 0x0B, 0x00, 0x0E, 0x05, + 0x08, 0x03, 0x0D, 0x06, 0x02, 0x09, 0x07, 0x0C, 0x0F, 0x04, 0x0A, 0x01, 0x05, 0x0E, 0x00, 0x0B, + 0x04, 0x0F, 0x01, 0x0A, 0x0E, 0x05, 0x0B, 0x00, 0x03, 0x08, 0x06, 0x0D, 0x09, 0x02, 0x0C, 0x07, + 0x0A, 0x01, 0x0F, 0x04, 0x00, 0x0B, 0x05, 0x0E, 0x0D, 0x06, 0x08, 0x03, 0x07, 0x0C, 0x02, 0x09, + 0x0B, 0x00, 0x0E, 0x05, 0x01, 0x0A, 0x04, 0x0F, 0x0C, 0x07, 0x09, 0x02, 0x06, 0x0D, 0x03, 0x08, + 0x05, 0x0E, 0x00, 0x0B, 0x0F, 0x04, 0x0A, 0x01, 0x02, 0x09, 0x07, 0x0C, 0x08, 0x03, 0x0D, 0x06}; + +/*! + This table provides a lookup table for the CCITT CRC16 algorithm + Generated with: + polynomial = 0x1021U + initial value = 0 + input reflected = false + result reflected = false + final XOR value = 0 + */ +static const uint16_t lib_crc_kCrc16qtable[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, + 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, 0x2462, 0x3443, 0x0420, 0x1401, + 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, + 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, + 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, + 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, 0x9188, 0x81A9, 0xB1CA, 0xA1EB, + 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, 0x22F3, 0x32D2, + 0x4235, 0x5214, 0x6277, 0x7256, 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xA7DB, 0xB7FA, 0x8799, 0x97B8, + 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, + 0x18C0, 0x08E1, 0x3882, 0x28A3, 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, + 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, + 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0}; + +static constexpr std::array allowed_iode_continuity_values = { + 0, 1, 5, 10, 30, 60, 120, 320}; + +static constexpr std::array iode_ids = {18, 0, 101, 99, 19, 100, 0}; + +static constexpr std::array iode_bit_size = {8, 0, 8, 10, 7, 8, 0}; + +static const std::array constellation_names = { + "GPS", "SBAS", "QZSS", "GAL", "GLO", "BDS", "NAVIC"}; + +static constexpr std::array satellite_mask_ids = {11, 0, 0, 93, 12, 0, 0}; + +static constexpr std::array satellite_mask_sizes = {58, 0, 0, 56, 50, 0, 0}; + +static constexpr std::array constellation_subtypes = {0, -1, -1, 2, 1, -1, -1}; + +static constexpr std::array ephemeris_id = {16, 0, 0, 96, 17, 0, 0}; + +static constexpr std::array ephemeris_bit_count = {2, 0, 0, 3, 2, 0, 0}; + +static constexpr std::array ephemeris_bits = {0, 0, 0, 1, 0, 0, 0}; +} // namespace Constants + +#endif diff --git a/generator/spartn/include/generator/spartn/data.h b/generator/spartn/include/generator/spartn/data.h new file mode 100644 index 00000000..6506075f --- /dev/null +++ b/generator/spartn/include/generator/spartn/data.h @@ -0,0 +1,56 @@ +#ifndef SPARTN_DATA_ +#define SPARTN_DATA_ + +#include +#include + +#include +#include +#include +#include + +class SPARTN_Data { +public: + enum class DataType { + Block, + Field, + }; + + virtual ~SPARTN_Data() = default; + virtual enum DataType get_data_type() const = 0; +}; + +class SPARTN_Block : public SPARTN_Data { +public: + virtual ~SPARTN_Block() = default; + + enum DataType get_data_type() const override { return DataType::Block; } + + std::queue> data = {}; +}; + +class SPARTN_Field : public SPARTN_Data { +public: + SPARTN_Field(const uint8_t id_, const uint8_t bit_count_, const int64_t bits_) { + this->id = id_; + this->bit_count = bit_count_; + this->bits = bits_ < 0 ? bits_ & ((1 << bit_count_) - 1) : bits_; + } + + SPARTN_Field(const uint8_t id_, const uint8_t bit_count_, + const std::bitset bits_) { + this->id = id_; + this->bit_count = bit_count_; + this->bits = bits_; + } + + virtual ~SPARTN_Field() = default; + + enum DataType get_data_type() const override { return DataType::Field; } + + uint8_t id; + uint8_t bit_count; + std::bitset bits = {}; +}; + +#endif diff --git a/generator/spartn/include/generator/spartn/generator.h b/generator/spartn/include/generator/spartn/generator.h new file mode 100644 index 00000000..dc61362b --- /dev/null +++ b/generator/spartn/include/generator/spartn/generator.h @@ -0,0 +1,946 @@ +#ifndef SPARTN_Generator_ +#define SPARTN_Generator_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef struct CorrectionPoint { + long id; + uint8_t num_of_grid_points; + + long referencePointLatitude_r16; + long referencePointLongitude_r16; + long numberOfStepsLatitude_r16; + long numberOfStepsLongitude_r16; + long stepOfLatitude_r16; + long stepOfLongitude_r16; +} CorrectionPoint; + +typedef struct HPACCorrection { + const long iod_ssr; + const GNSS_ID gnss_id; + const CorrectionPoint* correction_points; + const GNSS_SSR_GriddedCorrection_r16* gridded_corrections; + const GNSS_SSR_STEC_Correction_r16* stec_corrections; +} HPACCorrection; + +typedef struct IonosphereSatelliteData { + const uint64_t SVID; + const STEC_SatElement_r16* const sat_elm; // contains coefficients + std::vector residuals; +} IonosphereSatelliteData; + +typedef struct LastDiscontinuity { + int8_t discontinuity_indicator; + std::time_t update_time; +} LastDiscontinuity; + +typedef struct OCBCorrection { + ~OCBCorrection() = default; + + const long iod_ssr; + const GNSS_ID gnss_id; + const GNSS_SSR_OrbitCorrections_r15* orbit_corrs; + const GNSS_SSR_ClockCorrections_r15* clock_corrs; + const GNSS_SSR_CodeBias_r15* code_bias_corrs; + const GNSS_SSR_PhaseBias_r16* phase_bias_corrs; + const GNSS_SSR_URA_r16* ura; +} OCBCorrection; + +typedef std::tuple + OCBSatCorrection; + +class SPARTN_Generator { +public: + SPARTN_Generator() = default; + ~SPARTN_Generator() = default; + + /** Generate SPARTN message(s) from a single LPP message. SPARTN + * messages are represented by the internal struct SPARTN_Message, + * meaning they can be passed to the SPARTN_Transmitter::transmit + * function to be sent as a binary stream to various outputs (UART + * etc.). + * + * @param[in] lpp_message a pointer to the LPP_Message struct that is + * to be transcoded. This is stored in the class + * so that it does not need to be passed around. + * + * @param[in] uraoveride a hacky fix to set a nominal value for the + * URA if the LPP message does not include it. + * uraoveride will be clamped between 0-7. + * + * @return a vector of the transcoded SPARTN messages, + * ready to be transmitted. Messages will always + * be in the order of OCB -> GAD -> HPAC (unless + * the LPP message does not contain information + * for a full message to be made). + */ + std::vector> generate(const LPP_Message* lpp_message, + int8_t qualityoveride, + bool ublox_clock_correction, + bool force_continuity); + +private: + void intersect_svs(); + + /** Generate OCB messages from the LPP message. Currently GPS, GLONASS + * and Galileo constellations are supported, others will fail silently. + * + * @param[out] messages vector of the currently transcoded messages, + * newly generated messages will be appended to + * this vector. + */ + void generate_ocb_message(std::vector>& messages); + /** Generate the orbit block of the OCB message, for one satellite. + * + * @param[out] sat_block the SPARTN satellite block where the + * generated orbit block will be place. + * + * @param[in] orbit_correction pointer to the information from LPP on + * the orbit correction for *one* + * satellite. + * + * @param[in] gnss_id orbit_correction parameter does not + * contain knowledge of which + * constellation the satellite it is + * correcting is part of, keep store of it + * here. + * + * @param[in] generation_time time stamp of when the generation of + * the SPARTN messages started. + * + * @return true if block was successfully + * generated and added to parent block, + * false if something failed. + */ + bool generate_orbit_block(const std::unique_ptr& sat_block, + const SSR_OrbitCorrectionSatelliteElement_r15* orbit_correction, + GNSS_ID__gnss_id gnss_id, std::time_t generation_time); + /** Generate the clock block of the OCB message, for one satellite. + * + * @param[out] sat_block the SPARTN satellite block where the + * generated clock block will be place. + * + * @param[in] clock_correction pointer to the information from LPP on + * the clock correction for *one* + * satellite. + * + * @param[in] URA pointer to the user range error given + * by LPP. + * + * @param[in] gnss_id clock_correction parameter does not + * contain knowledge of which + * constellation the satellite it is + * correcting is part of, keep store of it + * here. + * + * @param[in] generation_time time stamp of when the generation of + * the SPARTN messages started. + * + * @param[in] gnss_systemtime should be retrieved from the LPP + * message. Used to generate the clock + * corrections (SF020). + * + * @param[in] update_interval should be retrieved from the LPP + * message. Used to generate the clock + * corrections (SF020). + * + * @param[in] iode Issue of Data Ephemeris from the LPP + * message. + * + * @return true if block was successfully + * generated and added to parent block, + * false if something failed. + */ + bool generate_clock_block(const std::unique_ptr& sat_block, + const SSR_ClockCorrectionSatelliteElement_r15* clock_correction, + const GNSS_SSR_URA_r16* URA, GNSS_ID__gnss_id gnss_id, + std::time_t generation_time, const GNSS_SystemTime* gnss_systemtime, + long update_interval, uint8_t iode); + /** Generate the bias block of the OCB message, for one satellite. + * + * @param[out] sat_block the SPARTN satellite block where the + * generated bias block will be place. + * + * @param[in] sat_code_biases pointer to the information from LPP on + * the code bias corrections for each + * signal for one satellite. + * + * @param[in] sat_phase_biases pointer to the information from LPP on + * the phase bias corrections for each + * signal for one satellite. + * + * @param[in] gnss_id neither correction contains knowledge + * of which constellation the satellite + * it is correcting is part of, keep + * store of it here. + * + * @return true if block was successfully + * generated and added to parent block, + * false if something failed. + */ + bool generate_bias_block(const std::unique_ptr& sat_block, + const SSR_CodeBiasSatElement_r15* sat_code_biases, + const SSR_PhaseBiasSatElement_r16* sat_phase_biases, + GNSS_ID__gnss_id gnss_id); + /** Generate a phase bias block for each signal contained within the + * LPP message, add it to the bias block for an individual satellite. + * + * @param[out] bias_block the bias block for each phase bias + * block to be added to. + * + * @param[in] phase_biases list of phase bias corrections given + * by LPP for each signal for one + * satellite. + * + * @param[in] lpp_to_spartn_bias SPARTN and LPP give each signal a + * numerical ID. Different + * constellations have different signals + * and therefore different IDs. SPARTN + * and LPP also do not agree on the IDs, + * one signal in LPP might have ID 8 but + * 1 in SPARTN. This is a mapping + * between the LPP IDs and the SPARTN + * IDs for each signal in *one* + * constellation. + * + * @param[in] bis in SPARTN, different constellations + * have different IDs for code and phase + * bias fields and different sizes. + * Store this information here. + * + * @param[in] gnss_id the phase bias correction does not + * contain knowledge of which + * constellation the satellite it is + * correcting is part of, keep store of + * it here. + */ + void generate_phase_bias_block(const std::unique_ptr& bias_block, + const SSR_PhaseBiasSatElement_r16* phase_biases, + std::map lpp_to_spartn_bias, + Constants::bias_ids_size bis, GNSS_ID__gnss_id gnss_id); + /** Generate the code bias block for a satellite. + * + * @param[out] bias_block the bias block for the code bias + * block to be added to. + * + * @param[in] code_biases list of phase bias corrections given + * by LPP for each signal for one + * satellite. + * + * @param[in] lpp_to_spartn_bias SPARTN and LPP give each signal a + * numerical ID. Different + * constellations have different signals + * and therefore different IDs. SPARTN + * and LPP also do not agree on the IDs, + * one signal in LPP might have ID 8 but + * 1 in SPARTN. This is a mapping + * between the LPP IDs and the SPARTN + * IDs for each signal in *one* + * constellation. + * + * @param[in] bis in SPARTN, different constellations + * have different IDs for code and phase + * bias fields and different sizes. + * Store this information here. + * + * @return true if all code bias block has + * been added successfully, false if + * not. + */ + static bool generate_code_bias_block(const std::unique_ptr& bias_block, + const SSR_CodeBiasSignalList_r15* code_biases, + std::map lpp_to_spartn_bias, + Constants::bias_ids_size bis); + + /** Generate HPAC messages from the LPP message. Currently GPS, GLONASS + * and Galileo constellations are supported, others will fail silently. + * + * @param[out] messages vector of the currently transcoded messages, + * newly generated messages will be appended to + * this vector. + */ + void generate_hpac_messages(std::vector>& messages) const; + /** Generate the area data block for a single correction. + * + * @param[out] atmo_block the atmosphere block for the area data block + * to be added to. + * + * @param[in] hpac_corr the HPAC corrections given by LPP, organised + * by SPARTN_Generator::group_hpac_corrections() + * + * @return true if area data block has been added + * successfully, false if not. + */ + static bool generate_area_data(const std::unique_ptr& atmo_block, + const HPACCorrection& hpac_corr); + /** Generate the troposphere data block. + * + * @param[out] atmo_block the atmosphere block for the + * troposphere data block to be added + * to. + * + * @param[in] gridded_corrections pointer to the gridded correction + * data given by LPP. + */ + void generate_tropo_data(const std::unique_ptr& atmo_block, + const GNSS_SSR_GriddedCorrection_r16* gridded_corrections) const; + /** Generate the troposphere coefficients block. + * + * @param[out] tropo_data the troposphere data block for the + * troposphere coefficients block to be + * added to. + * + * @param[in] gridded_corrections pointer to the gridded correction + * data given by LPP. + * + * @return true if block has been added + * successfully, false if not. + */ + bool generate_tropo_coef(const std::unique_ptr& tropo_data, + const GNSS_SSR_GriddedCorrection_r16* gridded_corrections) const; + /** Generate the troposphere grid block. + * + * @param[out] tropo_data the troposphere data block for the + * troposphere grid block to be added + * to. + * + * @param[in] gridded_corrections pointer to the gridded correction + * data given by LPP. + */ + static void generate_tropo_grid(const std::unique_ptr& tropo_data, + const GNSS_SSR_GriddedCorrection_r16* gridded_corrections); + /** Generate the ionosphere data block. + * + * @param[out] atmo_block the atmosphere block for the + * ionosphere data block to be added + * to. + * + * @param[in] gridded_corrections pointer to the gridded correction + * data given by LPP. + */ + void generate_iono_data(const std::unique_ptr& atmo_block, + const HPACCorrection& hpac_corr) const; + /** Generate the ionosphere satellite polynomial block for one + * satellite. + * + * @param[out] iono_data the ionosphere data block for the + * satellite block to be added to. + * + * @param[in] sat_elm ionosphere corrections for a single + * satellite + * + * @param[in] residual_size used for SF056, false means small + * coefficient block is used, true means + * large is used. + */ + void generate_iono_sat_block(const std::unique_ptr& iono_data_block, + const STEC_SatElement_r16* const& sat_elm, + bool residual_size) const; + /** Generate the ionosphere satellite coefficient block. Will generate + * either small (table 6.21) or large (table 6.22) based on the + * equ_type parameter. + * + * This function does not automatically add it's generated block to a + * parent (like other functions in this library), as the coefficients + * block must be added *after* the polynomial block but the polynomial + * block relies on information that can only be known after the + * coefficients block is generated (size of coefficients). + * + * @param[in] sat_elm ionosphere corrections for a single + * satellite + * + * @param[in] equ_type the equation type used (how many coefficients + * used) + * + * @return a pair of the newly generated coefficient block + * and if small or large coefficients can be used, + * small is false and large is true. + */ + static std::pair, bool> + generate_iono_coef_block(const STEC_SatElement_r16* const& sat_elm, uint8_t equ_type); + /** Generate the ionosphere satellite polynomial block for one + * satellite. + * + * @param[out] iono_data_block the ionosphere data block for the + * satellite block to be added to. + * + * @param[in] residuals vector of points to residuals for each + * grid point for a single satellite. + */ + static void + generate_iono_grid_block(const std::unique_ptr& iono_data_block, + const std::vector& residuals); + + /** Generate HPAC messages from the LPP message. + * + * @param[out] messages vector of the currently transcoded messages, + * newly generated messages will be appended to + * this vector. + */ + void generate_gad_message(std::vector>& messages); + /** Generate the area definition block from LPP's list representation + * of the correction points. + * + * WARNING: very much untested code. + * + * @param[out] gad_message the GAD message for the area definition + * blocks to be added to. + * + * @param[in] corrpoints correction points in LPP's list + * representation. + * + * @param[in] area_id the id of the area from LPP. + */ + static void generate_gad_message_list(const std::unique_ptr& gad_message, + const GNSS_SSR_ListOfCorrectionPoints_r16& corrpoints, + uint32_t area_id); + /** Generate the area definition block from LPP's array representation + * of the correction points. + * + * @param[out] gad_message the GAD message for the area definition + * blocks to be added to. + * + * @param[in] corrpoints correction points in LPP's array + * representation. + * + * @param[in] area_id the id of the area from LPP. + */ + static void generate_gad_message_array(const std::unique_ptr& gad_message, + const CorrectionPoint& corrpoints, uint32_t area_id); + + template + struct has_iod_r16 : std::false_type {}; + template + struct has_iod_r16().iod_ssr_r16, void())> : std::true_type {}; + + /** gets the IOD for types that have the attribute `iod_ssr_r15`. + * + * @param[in] corr the information element (typically a + * correction). + * + * @param[in] std::false_type + * + * @return the issue of data ephemeris for the IE. + */ + template + static long get_iod(const T* const corr, std::false_type) { + return corr->iod_ssr_r15; + } + + /** gets the IOD for types that have the attribute `iod_ssr_r16`. + * + * @param[in] corr the information element (typically a + * correction). + * + * @param[in] std::true_type + * + * @return the issue of data ephemeris for the IE. + */ + template + static long get_iod(const T* const corr, std::true_type) { + return corr->iod_ssr_r16; + } + + /** Overload of add_ocb_correction for the orbit corrections. + * + * @param[out] ocb_correction where the correction is be added. + * + * @param[in] orbit_corrs set of corrections to be added. + */ + static void add_ocb_correction(OCBCorrection& ocb_correction, + const GNSS_SSR_OrbitCorrections_r15* const orbit_corrs) { + ocb_correction.orbit_corrs = orbit_corrs; + } + + /** Overload of add_ocb_correction for the clock corrections. + * + * @param[out] ocb_correction where the correction is be added. + * + * @param[in] clock_corrs set of corrections to be added. + */ + static void add_ocb_correction(OCBCorrection& ocb_correction, + const GNSS_SSR_ClockCorrections_r15* const clock_corrs) { + ocb_correction.clock_corrs = clock_corrs; + } + + /** Overload of add_ocb_correction for the code bias corrections. + * + * @param[out] ocb_correction where the correction is be added. + * + * @param[in] code_corrs set of corrections to be added. + */ + static void add_ocb_correction(OCBCorrection& ocb_correction, + const GNSS_SSR_CodeBias_r15* const code_corrs) { + ocb_correction.code_bias_corrs = code_corrs; + } + + /** Overload of add_ocb_correction for the phase bias corrections. + * + * @param[out] ocb_correction where the correction is be added. + * + * @param[in] phase_corrs set of corrections to be added. + */ + static void add_ocb_correction(OCBCorrection& ocb_correction, + const GNSS_SSR_PhaseBias_r16* const phase_corrs) { + ocb_correction.phase_bias_corrs = phase_corrs; + } + + /** Add some generic orbit, clock or bias correction to the list of + * `OCBCorrection`s. If this function can't find somewhere to put the + * correction in the list (based on iod_ssr), it will create a new + * record and put it there. See comments in implementation for more + * details. + * + * There is some real grimy template stuff going on here to make this + * work... + * + * The first issue is that the type of the corrections is different, so + * we template that type (easy). + * + * Next all corrections store the iod in iod_ssr_r15, *except* for + * phase bias which uses iod_ssr_r16. Therefore we have the type + * `has_iod_r16` which takes the type of the correction as it's + * template. Using a little SFINAE this can then be converted into a + * boolean as to if iod_ssr16 is used. There is then the overloaded + * function `get_iod` that takes a bool as it's second parameter, which + * allows it to either call the function that gets the r15 or r16 + * member. + * + * The final issue is adding the correction to the right member of the + * struct. Again, overloading is used and multiple `add_ocb_correction` + * definitions with different arguments for each correction that could + * be used. + * + * @param[out] ocb_corrections a list of grouped OCB corrections where + * the new set of corrections will end up. + * + * @param[in] corrs a set of correction from LPP, i.e. + * GNSS_SSR_PhaseBias_r16. + * + * @param[in] ura the user range error is also associated + * with an iod-ssr so is grouped as well. + * its passed to this function so that it + * can be used when a new record must be + * made. + * + * @param[in] gnss_id iod-ssr only identifies a correction + * within a constellation, so keep track of + * that as well. + */ + template + static void add_ocb_correction(std::vector& ocb_corrections, const T* corrs, + const GNSS_SSR_URA_r16* ura, GNSS_ID gnss_id); + + /** The purpose of this function (and group_hpac_corrections) is that + * it is not ensured by LPP that the iod-ssr between correction types + * is the same. If the iod-ssr changes, all previous corrections must + * be discarded SPARTN has a similar idea with it's SIOU, but it only + * allows one SIOU for all correction types. Therefore we must group + * correction types by iod-ssr and create a new message for each value + * found. + * + * @param[in] gen_ass The struct from LPP that contains all of the sets + * of corrections. + * + * @return A vector of `OCBCorrection` structs, grouped by + * iod-ssr and constellation. + */ + static std::vector group_ocb_corrections(const GNSS_GenericAssistData* gen_ass); + + /** The same idea as SPARTN_Generator::group_ocb_corrections(), just + * with the HPAC IEs instead. This time, the correction points are the + * same between all of the elements in the vector. + * + * @param[in] gen_ass Generic assistance data from LPP, containing the + * gridded and STEC correction. + * + * @param[in] com_ass Common assistance data from LPP, containing the + * correction points. + * + * @return A vector of `HPACCorrection` structs, grouped by + * iod-ssr and constellation. + */ + std::vector group_hpac_corrections(const GNSS_GenericAssistData* gen_ass, + const GNSS_CommonAssistData* com_ass); + + /** Helper function to take add a `SPARTN_LPP_Time` member to a + * `SPARTN_Message`. + * + * @param[out] message a pointer to the message to have the time + * added to. + * + * @param[in] epoch_time LPP's representation of time, to be converted + * into this library's representation. + * + * @param[in] gnss_id to interpret epoch_time correctly, the + * constellation that it represents must also be + * known. + */ + static void add_time_to_message(const std::unique_ptr& message, + const GNSS_SystemTime* epoch_time, GNSS_ID__gnss_id gnss_id); + + template + struct has_svid_r16 : std::false_type {}; + template + struct has_svid_r16()->svID_r16, void())> : std::true_type {}; + + /** gets the SV-ID for types that have the attribute `svID_r15`. + * + * @param[in] corr the information element (typically a + * correction). + * + * @param[in] std::false_type + * + * @return the SV-ID that the correction is for. + */ + template + static long get_svid(const T* const corr, std::false_type) { + return corr->svID_r15.satellite_id; + } + + /** gets the SV-ID for types that have the attribute `svID_r16`. + * + * @param[in] corr the information element (typically a + * correction). + * + * @param[in] std::true_type + * + * @return the SV-ID that the correction is for. + */ + template + static long get_svid(const T* const corr, std::true_type) { + return corr->svID_r16.satellite_id; + } + + static void add_sv(OCBSatCorrection& ocb_corr, + SSR_OrbitCorrectionSatelliteElement_r15* const orb_corr) { + std::get<0>(ocb_corr) = orb_corr; + } + + static void add_sv(OCBSatCorrection& ocb_corr, + SSR_ClockCorrectionSatelliteElement_r15* const clk_corr) { + std::get<1>(ocb_corr) = clk_corr; + } + + static void add_sv(OCBSatCorrection& ocb_corr, SSR_CodeBiasSatElement_r15* const code_corr) { + std::get<2>(ocb_corr) = code_corr; + } + + static void add_sv(OCBSatCorrection& ocb_corr, SSR_PhaseBiasSatElement_r16* const phase_corr) { + std::get<3>(ocb_corr) = phase_corr; + } + + template + static void add_ocb_for_svs(std::map& ocb_for_svs, T corr); + + /** Group the OCB corrections by SV-ID. + * + * LPP & SPARTN have different ways of structuring their correction + * type and SV relationships: + * + * LPP | SPARTN + * --------------------------------|------------------------------- + * Correction Type | Each SV + * | | | + * |- Correction for SV | |- All correction types for SV + * + * For LPP each correction type is sorted in a different IE and has a + * list of SV in each of those IEs. This makes it very hard to easily + * structure the code inorder for the SPARTN structure to be generated. + * So instead we do most of the heavy lifting here and make a list of + * SVs and have all the corrections types of correction stored next to + * them. + * + * @param[in] orbit_corrs orbit corrections. + * @param[in] clock_corrs clock corrections. + * @param[in] code_bias_corrs code corrections. + * @param[in] phase_bias_corrs phase bias corrections. + * + * @return a map between SV-IDs and corrections for + * that satellite. + */ + static std::map + get_ocb_for_svs(const GNSS_SSR_OrbitCorrections_r15* orbit_corrs, + const GNSS_SSR_ClockCorrections_r15* clock_corrs, + const GNSS_SSR_CodeBias_r15* code_bias_corrs, + const GNSS_SSR_PhaseBias_r16* phase_bias_corrs); + + /** Generates a SPARTN bitmask from the output of + * SPARTN_Generator::get_ocb_for_svs(). The size of mask is the second + * largest available for each field for each constellation. + * + * @param[in] sv_ocbs a map of SV-IDs to a set of + * corrections (only the keys are + * used). + * + * @param[in] sat_mask_bit_count size of the resulting bitmask. + * + * @param[in] sat_mask_bit_count_flag prepended to the start of the + * mask, to indicate it's size, as + * defined by the SPARTN ICD. + * + * @return a bitset of length defined by + * Constants::max_spartn_bit_count. + */ + static std::bitset + get_satellite_mask_from_ocbs(const std::map& sv_ocbs, + const uint16_t sat_mask_bit_count, + const uint8_t sat_mask_bit_count_flag) { + std::bitset sat_mask_bitset = sat_mask_bit_count_flag; + sat_mask_bitset <<= sat_mask_bit_count - 2; + + const uint16_t sat_mask_length = sat_mask_bit_count - 3; + static constexpr std::bitset new_sv = 1; + + for (const auto& sv_ocb : sv_ocbs) { + sat_mask_bitset |= new_sv << (sat_mask_length - sv_ocb.first); + } + + return sat_mask_bitset; + } + + /** The value of a residual can either be stored in the b7 or b16 field + * in LPP. This function figures out which it is then retrieves the + * value. + * + * @param[in] res the residual struct from the LPP message. + * + * @return the residual's numerical value or 99999 (error value) + * if it can't figure out where the residual is stored. + * It is safe to return 99999 as the error value because + * LPP does not allow values larger than 32767. + */ + static long get_residual(const STEC_ResidualSatElement_r16* const res) { + long this_residual; + switch (res->stecResidualCorrection_r16.present) { + case STEC_ResidualSatElement_r16__stecResidualCorrection_r16_PR_b7_r16: { + this_residual = res->stecResidualCorrection_r16.choice.b7_r16; + break; + } + case STEC_ResidualSatElement_r16__stecResidualCorrection_r16_PR_b16_r16: { + this_residual = res->stecResidualCorrection_r16.choice.b16_r16; + break; + } + default: return 99999; + } + return this_residual; + } + + /** Retrieve the signal id from the `GNSS_SignalID` struct. LPP lets + * the signal ID either be stored directly in this struct or nested + * within the `ext1` attribute. + * + * @param[in] sig a reference to the struct where the signal ID is + * stored. + * + * @return the signal ID's numerical value. + */ + static uint8_t get_signalid(const GNSS_SignalID& sig) { + return sig.ext1 ? *sig.ext1->gnss_SignalID_Ext_r15 : sig.gnss_SignalID; + } + + /** It is not guaranteed that the `OCBCorrection` struct will have all + * of it's correction fields assigned, which means searching when + * trying to find the `GNSS_SystemTime`. Therefore this function + * conveniently searching for this struct. + * + * @param[in] ocb_correction an `OCBCorrection` struct where it is not + * guaranteed that all of the fields are + * assigned + * + * @return a pointer to the first system time that + * can be found. If no fields are set, a + * nullptr is returned. This is safe as if + * there is a correction assigned, LPP + * guarantees that the `GNSS_SystemTime` will + * also be defined. + */ + static const GNSS_SystemTime* get_gnss_systemtime(const OCBCorrection& ocb_corrections) { + if (ocb_corrections.orbit_corrs) { + return &ocb_corrections.orbit_corrs->epochTime_r15; + } + + if (ocb_corrections.clock_corrs) { + return &ocb_corrections.clock_corrs->epochTime_r15; + } + + if (ocb_corrections.code_bias_corrs) { + return &ocb_corrections.code_bias_corrs->epochTime_r15; + } + + if (ocb_corrections.phase_bias_corrs) { + return &ocb_corrections.phase_bias_corrs->epochTime_r16; + } + + return nullptr; + } + + static const GNSS_SystemTime* get_gnss_systemtime(const HPACCorrection& hpac_corrections) { + if (hpac_corrections.gridded_corrections) { + return &hpac_corrections.gridded_corrections->epochTime_r16; + } + + if (hpac_corrections.stec_corrections) { + return &hpac_corrections.stec_corrections->epochTime_r16; + } + + return nullptr; + } + + /* Check if the satellite reference datum is ITRF. Reference datum is + * only relevant to orbit. + * + * @param[in] orb point to orbit correction to be checked + * + * @return true if reference datum is ITRF, false if not. + */ + static inline bool check_is_itrf(const GNSS_SSR_OrbitCorrections_r15* const orb) { + static constexpr auto itrf = + GNSS_SSR_OrbitCorrections_r15__satelliteReferenceDatum_r15_itrf; + + return (bool)orb && orb->satelliteReferenceDatum_r15 == itrf; + } + + /** Add a block to it's parent. In a function because it's not a + * trivial operation. + * + * @param[out] parent block to have it's children updated. + * @param[in] child child to add to parent's children. + */ + static void add_block_to_block(const std::unique_ptr& parent, + std::unique_ptr& child) { + parent->data.push(std::move(child)); + } + + /** Create and add a field to header. + * + * @param[out] header the header to add the field to + * @param[in] id id of the new field + * @param[in] bit_count number of bits in the new field + * @param[in] bits bits in the new field + */ + static void add_field_to_header(const std::unique_ptr& header, + const uint8_t id, const uint8_t bit_count, const int64_t bits) { + std::unique_ptr new_field(new SPARTN_Field(id, bit_count, bits)); + header->fields.push(std::move(new_field)); + } + + /** Create and add a field to block. + * + * @param[out] block the block to add the field to + * @param[in] id id of the new field + * @param[in] bit_count number of bits in the new field + * @param[in] bits bits in the new field + */ + static void add_field_to_block(const std::unique_ptr& block, const uint8_t id, + const uint8_t bit_count, const int64_t bits) { + std::unique_ptr new_field(new SPARTN_Field(id, bit_count, bits)); + block->data.push(std::move(new_field)); + } + + /** Check a value is within a range defined by the minimum value, i.e. + * range of -10..+10. + * + * @param[in] value value to be checked. + * @param[in] range_min the lower bound of the range. + * + * @return true if value in range, false if not. + */ + static bool check_within_range(double value, double range_min) { + return (value > range_min) && (value < (-1 * range_min)); + } + + /** Convert a double to a long representation, using a range and + * resolution. + * + * The range and resolution can be found in the SPARTN ICD. + * + * A pretty common usage pattern for this function is: + * + * ``` + * if (SPARTN_Generator::check_within_range( + * val_dec, Constants::orbit_clock_spartn_rng_min)) + * { + * add_field_to_block( + * orbit_block, + * 20, + * 14, + * SPARTN_Generator::encode_value_spartn( + * val_dec, + * Constants::orbit_clock_spartn_rng_min, + * Constants::orbit_clock_spartn_res)); + * } + * ``` + * + * @param[in] v_original double representation of number. + * @param[in] range_min minimum allowed. + * @param[in] resolution resolution of the to be encoded value + * + * @return encoded integer representation of the double. + */ + static long encode_value_spartn(double v_original, double range_min, double resolution) { + return std::lround((v_original - range_min) / resolution); + } + + /** Convert a long representation of a double to a double, using it's + * resolution. + * + * @param[in] v_encoded encoded integer representation of the double. + * @param[in] resolution resolution of the encoded integer. + * + * @return decoded representation of double. + */ + static double decode_value_lpp(long v_encoded, double resolution) { + return (double)v_encoded * resolution; + } + + const ProvideAssistanceData_r9_IEs_t* prov_ass_data_; + + /* + * For each SV in each constellation, we must store the last known + * IOD and time at which the IOD changed, for SF022 + */ + std::map>> iods_ = {}; + + // map of signal ids and discontinuities + std::map>> + discontinuities_ = {}; + + std::map> last_generation_time_ = {}; + + int8_t qualityoveride_ = 0; + + bool ublox_clock_correction = false; + bool force_continuity = false; + + std::unique_ptr last_correction_point_ = nullptr; + + std::vector ocb_corrections_ = {}; + std::vector hpac_corrections_ = {}; + + std::map> sv_intersections_ = {}; +}; +#endif diff --git a/generator/spartn/include/generator/spartn/message.h b/generator/spartn/include/generator/spartn/message.h new file mode 100644 index 00000000..9c01efd3 --- /dev/null +++ b/generator/spartn/include/generator/spartn/message.h @@ -0,0 +1,21 @@ +#ifndef SPARTN_MESSAGE_ +#define SPARTN_MESSAGE_ + +#include +#include + +#include +#include +#include + +struct SPARTN_Message { + SPARTN_Message() : time(new SPARTN_LPP_Time) {} + + std::unique_ptr message_header; + std::queue> data = {}; + uint8_t message_type; + uint8_t message_sub_type; + std::unique_ptr time; +}; + +#endif diff --git a/generator/spartn/include/generator/spartn/message_header.h b/generator/spartn/include/generator/spartn/message_header.h new file mode 100644 index 00000000..84b109aa --- /dev/null +++ b/generator/spartn/include/generator/spartn/message_header.h @@ -0,0 +1,13 @@ +#ifndef SPARTN_MESSAGE_HEADER_ +#define SPARTN_MESSAGE_HEADER_ + +#include +#include + +#include + +struct SPARTN_Message_Header { + std::queue> fields = {}; +}; + +#endif diff --git a/generator/spartn/include/generator/spartn/time.hpp b/generator/spartn/include/generator/spartn/time.hpp new file mode 100644 index 00000000..b71c8c9b --- /dev/null +++ b/generator/spartn/include/generator/spartn/time.hpp @@ -0,0 +1,45 @@ +#ifndef SPARTN_LPP_Time_ +#define SPARTN_LPP_Time_ + +#include +#include "GNSS-ID.h" + +#include + +class SPARTN_LPP_Time { +public: + SPARTN_LPP_Time(); + SPARTN_LPP_Time(long id, long dayno, long tod); + /** Getter function for spartn_time_ + * + * @return spartn_time_ + */ + uint32_t get_spartn_time() const; + + /** Different constellations use different start days for their reference + * time, e.g. GPS starts at 1980 whereas Galileo uses 1999. This function + * normalises these start days to all start at 1970 (same as a UNIX + * timestamp). + * + * @param[in] day_number day delta to be normalised. + * @param[in] gnss_id constellation the day delta is for. + * + * @return normalised day delta to 01/01/1970 + */ + static uint64_t standard_day_number(long day_number, GNSS_ID__gnss_id gnss_id); + +private: + /** Used to generate time for the 32-bit version of TF009, who's reference + * time is 01/01/2010 00:00:00. The resulting time-tag will be stored in + * spartn_time_. + */ + void generate_spartn_time(); + + GNSS_ID__gnss_id lpp_gnss_id_; + long lpp_daynumber_; + long lpp_timeofday_; + + uint32_t spartn_time_; +}; + +#endif diff --git a/generator/spartn/include/generator/spartn/transmitter.h b/generator/spartn/include/generator/spartn/transmitter.h new file mode 100644 index 00000000..26835446 --- /dev/null +++ b/generator/spartn/include/generator/spartn/transmitter.h @@ -0,0 +1,224 @@ +#ifndef SPARTN_Transmitter_ +#define SPARTN_Transmitter_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SPARTN_Transmitter { +public: + /** Build SPARTN binary message of internal representation SPARTN message. Message can be + * hand crafted or generated from an LPP message using SPARTN_Generator::generate(). + * + * NOTE: No message authentication is used, therefore TF004, TF012, TF013 + * TF014, TF015 are all excluded. + * + * NOTE: CRC-16-CCITT is always used as the CRC algorithm, there is no + * functionality to change it implemented. + * + * @param[in] messages reference to a vector of SPARTN_Message + * objects that will be included in the payload. + */ + static std::vector build(std::unique_ptr& message); + +private: + /** Convert internal representation of SPARTN message into binary. + * + * SPARTN messages can be split up into the following three sections: + * + * +-----------------+---------+-------------+ + * | FRAME START | PAYLOAD | FRAME END | + * | TF001 -- TF0015 | TF016 | TF017 TF018 | + * +-----------------+---------+-------------+ + * + * Therefore the arguments to this function are split up into these + * sections for easier manipulation into binary. + * + * @param[in] fields_to_16 all fields that should be inserted _before_ + * the payload (TF016). + * + * @param[in] fields_from_16 all fields that should be inserted _after_ the + * payload (TF016). + * + * @param[in] payload TF016 in binary format. + * + * @param[in] payload_length As the payload is stored in a bitset that has + * the maximum possible width, it is also + * required to know how long the actual payload + * is. [units: bits] + */ + static std::vector + output(const std::vector>& fields_to_16, + const std::vector>& fields_from_16, + const std::bitset& payload, uint64_t payload_length); + + /** Takes the internal representation of a SPARTN message and returns a + * bitset of the same information, along with it's length. + * SPARTN_Transmitter::add_bits_to_bitset_from_data does a lot of the + * heavy lifting here. + * + * @param[in] message a pointer to the message to be converted to binary. + * + * @return a pair of the passed in message represented as + * binary and the length of the bitset. + */ + static std::pair, uint32_t> + generate_message_payload(std::unique_ptr& message); + + /** Depth-first recursion through tree-structure of a queue of SPARTN_Data + * structs. + * + * This function will pop the first element of the queue and check it's + * type: + * + * - SPARTN_Block: call this function again, passing in the block's queue + * of data. + * + * - SPARTN_Field: add the field's bits to the payload and return. + * + * WARN: this function modifies the `data` object, by popping off values + * from the queue and releasing the data inside of the fields. + * Therefore, any `data` passed into this function should not be + * used afterwards. + * + * WARN: this function does not truncate values that are beyond the + * specified bit count, which can lead to corruption if either field + * is set incorrectly. + * + * @param[out] payload the payload to be created. + * + * @param[in] data a reference to a queue of SPARTN_Data objects, + * typically from either a SPARTN_Message or + * SPARTN_Block. + * + * @param[out] bits_used the length of the resulting payload. + * [units: bits] + */ + template + static void add_bits_to_bitset_from_data(std::bitset& payload, + std::queue>& data, + uint64_t& bits_used); + + /** Generates the TF006 field - the frame CRC. + * + * This function does not calculate the CRC-4 directly, it sets up the data + * structure, calls SPARTN_Transmitter::generate_crc4_for_tf006() and then + * creates a SPARTN_Field to be returned. + * + * @param[in] TF002 message type field. + * @param[in] TF003 payload length. + * @param[in] TF004 encryption and authentication flag. + * @param[in] TF005 message CRC type. + * + * @return TF006 with the CRC-4 calculated. + */ + static std::unique_ptr generate_tf006(const std::unique_ptr& TF002, + const std::unique_ptr& TF003, + const std::unique_ptr& TF004, + const std::unique_ptr& TF005); + + /** Generates CRC-16-CCITT from SPARTN_Field structs and the payload. + * + * @param[in] fields_02_to_16 SPARTN fields TF002 up-to and including + * TF015. + * @param[in] payload TF016 as a bitset. + * @param[in] payload_length As the payload is stored in a bitset that has + * the maximum possible width, it is also + * required to know how long the actual payload + * is. [units: bits] + * + * @return calculated CRC-16 + */ + static uint16_t + generate_crc16(const std::vector>& fields_02_to_16, + const std::bitset& payload, + uint64_t payload_length); + + /** Opens a file descriptor to the desired path. + * + * If the path is /dev/ttyS0 then a UART connection will be set up. + * Baudrate: 460800 + * If the path is /dev/i2c-1 then a I2C connection will be setup. + * Slave address: 0x42 + * + * The serial device is expected to be accessible through the /dev/ttyS0 + * device (default on the Raspberry Pi 4). Due to the size of some of the + * messages, baudrate must be high (460800 will work, but 38400 won't for + * example). + */ + static int open_serial(const char* path); + + /** Given a payload and a list of fields, add all the bits from the fields + * to the payload. + * + * @param[out] payload a reference to the bitset to be modified. + * + * @param[in] fields a reference to the list of fields to have their + * bits appended to the payload. + * + * @param[out] bits_used number of bits used in the payload. + */ + template + static void add_bits_to_bitset(std::bitset& payload, + const std::vector>& fields, + uint64_t& bits_used) { + for (const auto& field : fields) { + payload <<= field->bit_count; + payload |= std::bitset(field->bits.to_string()); + + bits_used += field->bit_count; + } + } + + /** Calculate the CRC-4 value for the SPARTN field TF006. + * + * @param[in] bytes a reference to the three bytes used in the calculation. + * + * @return the CRC-4 value. + */ + static uint8_t generate_crc4_for_tf006(const std::array& bytes) { + uint8_t crc = 0; + for (const auto byte : bytes) { + const uint8_t iCrc = byte ^ crc; + crc = Constants::crc4_lookuptable[iCrc]; + crc &= 0xF; + } + return crc; + } + + /** Given a bitset and a position (in bytes), return the byte at that + * position. + * + * @param[in] bs reference to the bitset to a byte extracted + * from. + * + * @param[in] bits_to_shift position of where the byte should be extracted + * from. [units: bits] + * + * @return extracted byte. + */ + template + static uint8_t get_byte(const std::bitset& bs, uint64_t bits_to_shift) { + constexpr auto mask = std::bitset(0xFF); + return std::bitset((bs >> bits_to_shift) & mask).to_ulong(); + } +}; +#endif diff --git a/generator/spartn/time.cpp b/generator/spartn/time.cpp new file mode 100644 index 00000000..12ff4dd5 --- /dev/null +++ b/generator/spartn/time.cpp @@ -0,0 +1,55 @@ +#include "time.hpp" + +SPARTN_LPP_Time::SPARTN_LPP_Time() { + const std::time_t t = std::time(0); + this->spartn_time_ = t - Constants::second_delta_1970_2010; +} + +SPARTN_LPP_Time::SPARTN_LPP_Time(const long id, const long dayno, const long tod) { + this->lpp_gnss_id_ = (GNSS_ID__gnss_id)id; + this->lpp_daynumber_ = dayno; + this->lpp_timeofday_ = tod; + + this->generate_spartn_time(); +} + +uint32_t SPARTN_LPP_Time::get_spartn_time() const { + return this->spartn_time_; +} + +void SPARTN_LPP_Time::generate_spartn_time() { + const auto standard_day_number = + SPARTN_LPP_Time::standard_day_number(this->lpp_daynumber_, this->lpp_gnss_id_); + const uint32_t days_since_2010 = standard_day_number - Constants::day_delta_1970_2010; + + this->spartn_time_ = + (days_since_2010 * Constants::seconds_in_day) + (uint32_t)this->lpp_timeofday_; +} + +uint64_t SPARTN_LPP_Time::standard_day_number(const long day_number, + const GNSS_ID__gnss_id gnss_id) { + uint16_t day_delta = 0; + switch (gnss_id) { + case GNSS_ID__gnss_id_gps: { + day_delta = Constants::day_delta_1970_1980; + break; + } + case GNSS_ID__gnss_id_galileo: { + day_delta = Constants::day_delta_1970_1999; + break; + } + case GNSS_ID__gnss_id_glonass: { + day_delta = Constants::day_delta_1970_1996; + break; + } + case GNSS_ID__gnss_id_bds: { + day_delta = Constants::day_delta_1970_2006; + break; + } + default: { + day_delta = 0; + break; + } + } + return day_number + day_delta; +} diff --git a/generator/spartn/transmitter.cpp b/generator/spartn/transmitter.cpp new file mode 100644 index 00000000..8a807ef9 --- /dev/null +++ b/generator/spartn/transmitter.cpp @@ -0,0 +1,282 @@ +#include "transmitter.h" + +std::vector SPARTN_Transmitter::build(std::unique_ptr& message) { + std::cout << "Message "; + std::cout << " of type (" << (int)message->message_type << ", "; + std::cout << (int)message->message_sub_type << ")"; + + const std::pair, uint32_t> payload_size = + SPARTN_Transmitter::generate_message_payload(message); + + std::unique_ptr TF001(new SPARTN_Field{1, 8, 115}); + + std::unique_ptr TF002(new SPARTN_Field{2, 7, message->message_type}); + + std::unique_ptr TF003(new SPARTN_Field{3, 10, payload_size.second}); + + if (payload_size.second > 1024) { + std::cout << std::endl; + std::cout << "WARN: sending a payload of size than max: "; + std::cout << payload_size.second << " > " + << "1024" << std::endl; + } + + std::unique_ptr TF004(new SPARTN_Field{4, 1, 0}); + + // CRC-16-CCITT + std::unique_ptr TF005(new SPARTN_Field{5, 2, 1}); + + std::unique_ptr TF006 = + SPARTN_Transmitter::generate_tf006(TF002, TF003, TF004, TF005); + + std::unique_ptr TF007(new SPARTN_Field{7, 4, message->message_sub_type}); + + std::unique_ptr TF008(new SPARTN_Field{8, 1, 1}); + + std::unique_ptr TF009(new SPARTN_Field{9, 32, message->time->get_spartn_time()}); + + std::unique_ptr TF010(new SPARTN_Field{10, 7, 0}); + + std::unique_ptr TF011(new SPARTN_Field{11, 4, 0}); + + std::vector> to_16 = {}; + to_16.push_back(std::move(TF002)); + to_16.push_back(std::move(TF003)); + to_16.push_back(std::move(TF004)); + to_16.push_back(std::move(TF005)); + to_16.push_back(std::move(TF006)); + to_16.push_back(std::move(TF007)); + to_16.push_back(std::move(TF008)); + to_16.push_back(std::move(TF009)); + to_16.push_back(std::move(TF010)); + to_16.push_back(std::move(TF011)); + + std::unique_ptr TF018(new SPARTN_Field{ + 18, 16, + SPARTN_Transmitter::generate_crc16(to_16, payload_size.first, payload_size.second)}); + + // crc16 calc doesnt want TF001, but it is needed for bin file + // so move it in after the crc is calculated + to_16.insert(to_16.begin(), std::move(TF001)); + + std::vector> from_16 = {}; + from_16.push_back(std::move(TF018)); + + auto output = + SPARTN_Transmitter::output(to_16, from_16, payload_size.first, payload_size.second); + + std::cout << std::endl; + + return output; +} + +/* Returns: **byte-algined** payload and length of payload (in bytes) */ +std::pair, uint32_t> +SPARTN_Transmitter::generate_message_payload(std::unique_ptr& message) { + std::bitset payload; + uint64_t current_length = 0; + + SPARTN_Transmitter::add_bits_to_bitset_from_data(payload, message->message_header->fields, + current_length); + SPARTN_Transmitter::add_bits_to_bitset_from_data(payload, message->data, current_length); + + const uint64_t shift_amount = (8 - (current_length % 8)) % 8; + current_length += shift_amount; + payload <<= shift_amount; // byte align + + return std::make_pair(payload, current_length / 8); +} + +std::vector +SPARTN_Transmitter::output(const std::vector>& fields_to_16, + const std::vector>& fields_from_16, + const std::bitset& payload, + const uint64_t payload_length) { + std::bitset message(0); + uint64_t message_size = 0; + + SPARTN_Transmitter::add_bits_to_bitset(message, fields_to_16, message_size); + + // moving the payload into position + message <<= (payload_length * 8); + message |= std::bitset(payload.to_string()); + message_size += (payload_length * 8); + + SPARTN_Transmitter::add_bits_to_bitset(message, fields_from_16, message_size); + + const uint64_t shift_amount = (8 - (message_size % 8)) % 8; + const int64_t byte_aligned_message_size = (int64_t)(message_size + shift_amount); + const std::bitset byte_aligned_message = message << shift_amount; + + /* + * Seems as if i2c doesn't like one byte being stored at a time, sending + * two at once it does though. + */ + static constexpr uint8_t buff_size = 2; + char bytes_to_send[buff_size] = {}; + uint8_t byte_to_write = 0; + std::vector output; + + for (ssize_t i = (byte_aligned_message_size / 8) - 1; i >= 0; i--) { + const uint64_t byte_i = i * 8; + const uint8_t this_byte = SPARTN_Transmitter::get_byte(byte_aligned_message, byte_i); + + bytes_to_send[byte_to_write] = (char)this_byte; + if ((++byte_to_write % 2) == 0) { + byte_to_write = 0; + for (ssize_t j = 0; j < buff_size; j++) { + output.push_back(bytes_to_send[j]); + } + } + } + + if (byte_to_write == 1) { + bytes_to_send[byte_to_write] = 0x00; + ++byte_to_write; + for (ssize_t j = 0; j < buff_size; j++) { + output.push_back(bytes_to_send[j]); + } + } + + std::cout << " sent: " << (byte_aligned_message_size / 8) - 1 << "(" << output.size() + << ") bytes"; + return output; +} + +/* Do not use any values from data after this function is called */ +template +void SPARTN_Transmitter::add_bits_to_bitset_from_data( + std::bitset& payload, std::queue>& data, uint64_t& bits_used) { + // std::cout << std::endl; + while (!data.empty()) { + switch (data.front()->get_data_type()) { + case SPARTN_Data::DataType::Block: { + SPARTN_Data* d_ptr = data.front().get(); + SPARTN_Block* b_ptr = static_cast(d_ptr); + + SPARTN_Transmitter::add_bits_to_bitset_from_data(payload, b_ptr->data, bits_used); + + break; + } + case SPARTN_Data::DataType::Field: { + SPARTN_Data* d_ptr = data.front().get(); + SPARTN_Field* f_ptr = static_cast(d_ptr); + + // bear in mind that if the bits are set beyond specified bit_count + // (it is not guarrenteed that this will never be the case) then + // previous bits will be overwritten. + payload <<= f_ptr->bit_count; + payload |= std::bitset(f_ptr->bits.to_string()); + + // std::cout << "SF" << (int)f_ptr->id << "/"; + + if (f_ptr->bit_count <= 64) { + // std::cout << f_ptr->bits.to_ullong(); + if (f_ptr->bits.to_ullong() > powl(2, f_ptr->bit_count)) { + std::cout << "[WARN] SF" << (int)f_ptr->id << " has value greater than legal" + << std::endl; + } + } + // else + // { + // std::cout << f_ptr->bits.to_string(); + // } + // std::cout << " "; + + bits_used += f_ptr->bit_count; + + break; + } + } + data.pop(); + } +} + +std::unique_ptr SPARTN_Transmitter::generate_tf006( + const std::unique_ptr& TF002, const std::unique_ptr& TF003, + const std::unique_ptr& TF004, const std::unique_ptr& TF005) { + // concatenate all the bits into one variable - 24 bits long + std::bitset tf2to5 = TF002->bits; + tf2to5 <<= TF003->bit_count; + tf2to5 |= TF003->bits; + tf2to5 <<= TF004->bit_count; + tf2to5 |= TF004->bits; + tf2to5 <<= TF005->bit_count; + tf2to5 |= TF005->bits; + + /* SPARTN spec calls for an extra 4 zeros to be added for the bitstring + * to be byte aligned */ + tf2to5 <<= 4; + + static constexpr std::bitset mask = 0xFF; + // get each first 3 bytes out of the bit string + const uint8_t crc4 = SPARTN_Transmitter::generate_crc4_for_tf006( + {(uint8_t)((tf2to5 >> (size_t)(2 * 8)) & mask).to_ulong(), + (uint8_t)((tf2to5 >> (size_t)(1 * 8)) & mask).to_ulong(), + (uint8_t)((tf2to5 >> (size_t)(0 * 8)) & mask).to_ulong()}); + + std::unique_ptr TF006(new SPARTN_Field{6, 4, crc4}); + return TF006; +} + +uint16_t SPARTN_Transmitter::generate_crc16( + const std::vector>& fields_02_to_16, + const std::bitset& payload, const uint64_t payload_length) { + std::bitset message(0); + uint64_t message_size = 0; + + SPARTN_Transmitter::add_bits_to_bitset(message, fields_02_to_16, message_size); + + message <<= (payload_length * 8); + message |= std::bitset(payload.to_string()); + message_size += (payload_length * 8); + + uint16_t crc = 0; + + const uint8_t shift_amount = (8 - (message_size % 8)) % 8; + const int64_t byte_aligned_message_size = (int64_t)(message_size + shift_amount); + const std::bitset byte_aligned_message = message << shift_amount; + + for (ssize_t i = (byte_aligned_message_size / 8) - 1; i >= 0; i--) { + const uint64_t byte_i = i * 8; + const uint8_t this_byte = SPARTN_Transmitter::get_byte(byte_aligned_message, byte_i); + const uint16_t iCrc = this_byte ^ (crc >> 8); + crc = Constants::lib_crc_kCrc16qtable[iCrc] ^ (crc << 8); + } + + std::cout << " got CRC-16 of: " << (int)crc; + return crc; +} + +int SPARTN_Transmitter::open_serial(const char* path) { + int fd; + if (strcmp(path, "/dev/ttyS0") == 0) { + struct termios options; + static constexpr auto baudrate(460800); + + fd = open("/dev/ttyS0", O_WRONLY | O_NOCTTY); + if (fd == -1) return -1; + + fcntl(fd, F_SETFL, 0); + tcgetattr(fd, &options); + cfsetospeed(&options, baudrate); + cfmakeraw(&options); + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~CRTSCTS; + if (tcsetattr(fd, TCSANOW, &options) != 0) return -1; + } else if (strcmp(path, "/dev/i2c-1") == 0) { + static constexpr uint8_t i2c_slave_adrr = 0x42; + fd = open("/dev/i2c-1", O_RDWR); + if (fd == -1) { + return -1; + } + if (ioctl(fd, I2C_SLAVE, i2c_slave_adrr) < 0) { + std::cerr << "ERR: Could not open i2c interface" << std::endl; + return -1; + } + } else { + fd = open(path, O_CREAT | O_WRONLY, S_IRUSR | S_IWUSR); + } + + return fd; +} diff --git a/generator/spartn2/CMakeLists.txt b/generator/spartn2/CMakeLists.txt new file mode 100644 index 00000000..3d15976b --- /dev/null +++ b/generator/spartn2/CMakeLists.txt @@ -0,0 +1,50 @@ + +add_library(generator_spartn2 STATIC + "generator.cpp" + "ocb.cpp" + "gad.cpp" + "hpac.cpp" + "time.cpp" + "builder.cpp" + "message.cpp" +) +add_library(generator::spartn2 ALIAS generator_spartn2) + +target_include_directories(generator_spartn2 PRIVATE "./" "include/generator/spartn2/") +target_include_directories(generator_spartn2 PUBLIC "include/") +target_link_libraries(generator_spartn2 PRIVATE asn1::generated asn1::helper) +target_link_libraries(generator_spartn2 PRIVATE utility) + +if (USE_ASAN) +target_compile_options(generator_spartn2 PRIVATE -fsanitize=address,undefined,leak) +target_link_libraries(generator_spartn2 PRIVATE -fsanitize=address,undefined,leak) +endif (USE_ASAN) + +if (SPARTN_DEBUG_PRINT) +target_compile_definitions(generator_spartn2 PRIVATE SPARTN_DEBUG_PRINT) +endif (SPARTN_DEBUG_PRINT) + +target_compile_options(generator_spartn2 PRIVATE + "-Wall" + "-Wextra" + "-Wpedantic" + "-Wnon-virtual-dtor" + "-Wold-style-cast" + "-Wcast-align" + "-Woverloaded-virtual" + "-Wsign-conversion" + "-Wno-conversion" + "-Wno-old-style-cast" +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(generator_spartn2 PRIVATE + "-Wmisleading-indentation" + ) +endif() + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU") + target_compile_options(generator_spartn2 PRIVATE + "-Wno-missing-field-initializers" + ) +endif() \ No newline at end of file diff --git a/generator/spartn2/builder.cpp b/generator/spartn2/builder.cpp new file mode 100644 index 00000000..ae9f5376 --- /dev/null +++ b/generator/spartn2/builder.cpp @@ -0,0 +1,49 @@ +#include "builder.hpp" + +#include +#include + +Builder::Builder(uint32_t capacity) : mData(capacity), mBitOffset(0) {} + +void 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); +} + +void Builder::reserve(uint32_t bits) { + auto new_size = (mBitOffset + bits + 7) / 8; + if (new_size > mData.size()) { + mData.resize(new_size); + } +} + +void Builder::bits(uint64_t value, uint8_t bits) { + assert(bits <= 64); + reserve(bits); + + for (uint8_t i = 0; i < bits; ++i) { + auto value_bit_offset = bits - i - 1; + auto value_bit = static_cast((value >> value_bit_offset) & 1); + auto byte_offset = mBitOffset / 8; + auto bit_offset = 7 - (mBitOffset % 8); + auto bit = value_bit << bit_offset; + mData[byte_offset] |= bit; + mBitOffset++; + } +} + +void Builder::pad(uint8_t bits) { + reserve(bits); + mBitOffset += bits; +} + +void Builder::align(uint8_t bits) { + uint8_t remainder = mBitOffset % bits; + if (remainder) { + pad(bits - remainder); + } +} diff --git a/generator/spartn2/builder.hpp b/generator/spartn2/builder.hpp new file mode 100644 index 00000000..e09f0cc3 --- /dev/null +++ b/generator/spartn2/builder.hpp @@ -0,0 +1,46 @@ +#pragma once +#include + +#include + +/// Builds binary blobs of bits for the SPARTN format +class Builder { +public: + SPARTN_EXPLICIT Builder(uint32_t capacity); + + inline void u8(uint8_t value) { bits(value, 8); } + inline void u16(uint16_t value) { bits(value, 16); } + inline void u32(uint32_t value) { bits(value, 32); } + inline void u64(uint64_t value) { bits(value, 64); } + + inline void i8(int8_t value) { bits(value, 8); } + inline void i16(int16_t value) { bits(value, 16); } + inline void i32(int32_t value) { bits(value, 32); } + inline void i64(int64_t value) { bits(value, 64); } + + inline void b(bool value) { bits(value ? 1 : 0, 1); } + + void double_to_bits(double min_range, double max_range, double resolution, double value, + uint8_t bits); + + // TODO: float, double + void reserve(uint32_t bits); + void bits(uint64_t value, uint8_t bits); + void pad(uint8_t bits); + void align(uint8_t bits); + inline void align_byte() { align(8); } + + std::vector data() const { + auto data = std::move(mData); + data.resize((mBitOffset + 7) / 8); + return data; + } + + uint8_t* data_ptr() { return mData.data(); } + + size_t bit_length() const { return mBitOffset; } + +private: + std::vector mData; + size_t mBitOffset; +}; diff --git a/generator/spartn2/data.hpp b/generator/spartn2/data.hpp new file mode 100644 index 00000000..b800ce8f --- /dev/null +++ b/generator/spartn2/data.hpp @@ -0,0 +1,205 @@ +#pragma once +#include +#include "time.hpp" + +#include +#include +#include +#include + +#include + +struct OcbKey { + long gnss_id; + uint32_t epoch_time; +}; + +struct HpacKey { + long set_id; + long gnss_id; + uint32_t epoch_time; +}; + +namespace std { +template <> +struct hash { + size_t operator()(const OcbKey& iod_gnss) const { + return hash()(iod_gnss.gnss_id) ^ hash()(iod_gnss.epoch_time); + } +}; + +template <> +struct hash { + size_t operator()(const HpacKey& iod_gnss_set) const { + return hash()(iod_gnss_set.gnss_id) ^ hash()(iod_gnss_set.set_id) ^ + hash()(iod_gnss_set.epoch_time); + } +}; +} // namespace std + +static bool operator==(const OcbKey& lhs, const OcbKey& rhs) { + return lhs.gnss_id == rhs.gnss_id && lhs.epoch_time == rhs.epoch_time; +} + +static bool operator==(const HpacKey& lhs, const HpacKey& rhs) { + return lhs.gnss_id == rhs.gnss_id && lhs.set_id == rhs.set_id && + lhs.epoch_time == rhs.epoch_time; +} + +struct GNSS_SSR_OrbitCorrections_r15; +struct GNSS_SSR_ClockCorrections_r15; +struct GNSS_SSR_CodeBias_r15; +struct GNSS_SSR_PhaseBias_r16; +struct GNSS_SSR_URA_r16; + +struct GNSS_SSR_GriddedCorrection_r16; +struct GNSS_SSR_STEC_Correction_r16; + +struct SSR_OrbitCorrectionSatelliteElement_r15; +struct SSR_ClockCorrectionSatelliteElement_r15; +struct SSR_CodeBiasSatElement_r15; +struct SSR_PhaseBiasSatElement_r16; +struct SSR_URA_SatElement_r16; + +struct STEC_SatElement_r16; +struct STEC_ResidualSatElement_r16; + +namespace generator { +namespace spartn { + +struct CorrectionPointSet { + long set_id; + uint16_t area_id; + long grid_points; + long referencePointLatitude_r16; + long referencePointLongitude_r16; + long numberOfStepsLatitude_r16; + long numberOfStepsLongitude_r16; + long stepOfLatitude_r16; + long stepOfLongitude_r16; + uint64_t bitmask; +}; + +struct OcbSatellite { + long id; + long iod; + SSR_OrbitCorrectionSatelliteElement_r15* orbit; + SSR_ClockCorrectionSatelliteElement_r15* clock; + SSR_CodeBiasSatElement_r15* code_bias; + SSR_PhaseBiasSatElement_r16* phase_bias; + SSR_URA_SatElement_r16* ura; + + void add_correction(SSR_OrbitCorrectionSatelliteElement_r15* orbit); + void add_correction(SSR_ClockCorrectionSatelliteElement_r15* clock); + void add_correction(SSR_CodeBiasSatElement_r15* code_bias); + void add_correction(SSR_PhaseBiasSatElement_r16* phase_bias); + void add_correction(SSR_URA_SatElement_r16* ura); + + uint32_t prn() const; +}; + +struct OcbCorrections { + long gnss_id; + long iod; + SpartnTime epoch_time; + + GNSS_SSR_OrbitCorrections_r15* orbit; + GNSS_SSR_ClockCorrections_r15* clock; + GNSS_SSR_CodeBias_r15* code_bias; + GNSS_SSR_PhaseBias_r16* phase_bias; + GNSS_SSR_URA_r16* ura; + + // Generate a set of satellite ids for this correction + // this is the union of all satellite ids that have at least + // one correction type. + std::vector satellites() const; +}; + +struct OcbData { + std::unordered_map mKeyedCorrections; +}; + +struct HpacSatellite { + long id; + long iod; + STEC_SatElement_r16* stec; + std::unordered_map residuals; + + void add_correction(STEC_SatElement_r16* stec); + void add_correction(long grid_id, STEC_ResidualSatElement_r16* residual); + + uint32_t prn() const; + bool has_full_data() const { return stec && residuals.size() > 0; } +}; + +struct HpacCorrections { + long gnss_id; + long iod; + long set_id; + SpartnTime epoch_time; + + GNSS_SSR_GriddedCorrection_r16* gridded; + GNSS_SSR_STEC_Correction_r16* stec; + + // Generate a set of satellite ids for this correction + // this is the union of all satellite ids that have at least + // one correction type. + std::vector satellites() const; +}; + +struct HpacData { + std::unordered_map mKeyedCorrections; + + void set_ids(std::vector& ids) const; +}; + +struct CorrectionData { + bool group_by_epoch_time; + std::unordered_map mOcbData; + std::unordered_map mHpacData; + + CorrectionData(bool group_by_epoch_time) : group_by_epoch_time(group_by_epoch_time) {} + + std::vector iods() const; + std::vector set_ids() const; + + OcbData* ocb(long iod) { + auto it = mOcbData.find(iod); + if (it == mOcbData.end()) return nullptr; + return &it->second; + } + + HpacData* hpac(long iod) { + auto it = mHpacData.find(iod); + if (it == mHpacData.end()) return nullptr; + return &it->second; + } + + void add_correction(long gnss_id, GNSS_SSR_OrbitCorrections_r15* orbit); + void add_correction(long gnss_id, GNSS_SSR_ClockCorrections_r15* clock); + void add_correction(long gnss_id, GNSS_SSR_CodeBias_r15* code_bias); + void add_correction(long gnss_id, GNSS_SSR_PhaseBias_r16* phase_bias); + void add_correction(long gnss_id, GNSS_SSR_URA_r16* ura); + + void add_correction(long gnss_id, GNSS_SSR_GriddedCorrection_r16* gridded); + void add_correction(long gnss_id, GNSS_SSR_STEC_Correction_r16* stec); +}; + +inline uint8_t subtype_from_gnss_id(long gnss_id) { + if (gnss_id == GNSS_ID__gnss_id_gps) return 0; + if (gnss_id == GNSS_ID__gnss_id_glonass) return 1; + if (gnss_id == GNSS_ID__gnss_id_galileo) return 2; + if (gnss_id == GNSS_ID__gnss_id_bds) return 3; + if (gnss_id == GNSS_ID__gnss_id_qzss) return 4; + + SPARTN_UNREACHABLE(); +} + +} // namespace spartn +} // namespace generator + +namespace generator { +namespace spartn { +class Message; +} // namespace spartn +} // namespace generator diff --git a/generator/spartn2/decode.hpp b/generator/spartn2/decode.hpp new file mode 100644 index 00000000..fb64ca07 --- /dev/null +++ b/generator/spartn2/decode.hpp @@ -0,0 +1,176 @@ +#pragma once +#include + +#include +#include +#include +#include + +namespace decode { + +static SPARTN_CONSTEXPR double ORBIT_RADIAL_RESOLUTION = 0.0001; +static SPARTN_CONSTEXPR double ORBIT_ALONG_RESOLUTION = 0.0004; +static SPARTN_CONSTEXPR double ORBIT_CROSS_RESOLUTION = 0.0004; + +static double delta_radial_r15(long value) { + return value * ORBIT_RADIAL_RESOLUTION; +} + +static double delta_AlongTrack_r15(long value) { + return value * ORBIT_ALONG_RESOLUTION; +} + +static double delta_CrossTrack_r15(long value) { + return value * ORBIT_CROSS_RESOLUTION; +} + +static SPARTN_CONSTEXPR double CLOCK_C0_RESOLUTION = 0.0001; +static SPARTN_CONSTEXPR double CLOCK_C1_RESOLUTION = 0.000001; +static SPARTN_CONSTEXPR double CLOCK_C2_RESOLUTION = 0.00000002; + +static double delta_Clock_C0_r15(long value) { + return value * CLOCK_C0_RESOLUTION; +} + +static double delta_Clock_C1_r15(long* value) { + return value ? (*value * CLOCK_C1_RESOLUTION) : 0.0; +} + +static double delta_Clock_C2_r15(long* value) { + return value ? (*value * CLOCK_C2_RESOLUTION) : 0.0; +} + +static long signal_id(const GNSS_SignalID& signal_id) { + if (signal_id.ext1 && signal_id.ext1->gnss_SignalID_Ext_r15) { + return *signal_id.ext1->gnss_SignalID_Ext_r15; + } else { + return signal_id.gnss_SignalID; + } +} + +static SPARTN_CONSTEXPR double PHASE_BIAS_RESOLUTION = 0.001; +static SPARTN_CONSTEXPR double CODE_BIAS_RESOLUTION = 0.01; + +static double phaseBias_r16(long value) { + return value * PHASE_BIAS_RESOLUTION; +} + +static double codeBias_r15(long value) { + return value * CODE_BIAS_RESOLUTION; +} + +static double ssr_URA_r16(BIT_STRING_s ura) { + auto bits = ura.buf[0]; + auto cls = (bits >> 3) & 0x7; + auto val = bits & 0x7; + + auto cls_value = pow(3, cls); + auto val_value = static_cast(val); + auto q = (cls_value * (1 + val_value) - 1) / 100.0; + return q; +} + +static SPARTN_CONSTEXPR double REFERENCE_POINT_LATITUDE_DEG = 90.0; +static SPARTN_CONSTEXPR double REFERENCE_POINT_LATITUDE_RESOLUTION = 0.00006103515625; // 2^-14 + +static SPARTN_CONSTEXPR double REFERENCE_POINT_LONGITUDE_DEG = 180.0; +static SPARTN_CONSTEXPR double REFERENCE_POINT_LONGITUDE_RESOLUTION = 0.000030517578125; // 2^-15 + +static double referencePointLatitude_r16(long value) { + return (value * REFERENCE_POINT_LATITUDE_DEG) * REFERENCE_POINT_LATITUDE_RESOLUTION; +} + +static double referencePointLongitude_r16(long value) { + return (value * REFERENCE_POINT_LONGITUDE_DEG) * REFERENCE_POINT_LONGITUDE_RESOLUTION; +} + +static SPARTN_CONSTEXPR double STEP_OF_LONGITUDE_RESOLUTION = 0.01; +static SPARTN_CONSTEXPR double STEP_OF_LATITUDE_RESOLUTION = 0.01; + +static double stepOfLatitude_r16(long value) { + return value * STEP_OF_LONGITUDE_RESOLUTION; +} + +static double stepOfLongitude_r16(long value) { + return value * STEP_OF_LATITUDE_RESOLUTION; +} + +static SPARTN_CONSTEXPR double TROPOSPHERIC_HYDRO_STATIC_DELAY_RESOLUTION = 0.004; +static SPARTN_CONSTEXPR double TROPOSPHERIC_WET_DELAY_RESOLUTION = 0.004; + +static double tropoHydroStaticVerticalDelay_r16(long value) { + return value * TROPOSPHERIC_HYDRO_STATIC_DELAY_RESOLUTION; +} + +static double tropoWetVerticalDelay_r16(long value) { + return value * TROPOSPHERIC_WET_DELAY_RESOLUTION; +} + +struct StecQualityIndicator { + bool invalid; + double value; +}; + +static StecQualityIndicator stecQualityIndicator_r16(BIT_STRING_s& bit_string) { + static SPARTN_CONSTEXPR double QUALITY_INDICATOR[64] = { + 33.6664, 30.2992, 26.9319, 23.5647, 20.1974, 16.8301, 13.4629, 12.3405, 11.2180, + 10.0956, 8.9732, 7.8508, 6.7284, 5.6059, 4.4835, 4.1094, 3.7352, 3.3611, + 2.9870, 2.6128, 2.2387, 1.8645, 1.4904, 1.3657, 1.2410, 1.1163, 0.9915, + 0.8668, 0.7421, 0.6174, 0.4927, 0.4511, 0.4096, 0.3680, 0.3264, 0.2848, + 0.2433, 0.2017, 0.1601, 0.1463, 0.1324, 0.1186, 0.1047, 0.0908, 0.0770, + 0.0631, 0.0493, 0.0447, 0.0400, 0.0354, 0.0308, 0.0262, 0.0216, 0.0169, + 0.0123, 0.0108, 0.0092, 0.0077, 0.0062, 0.0046, 0.0031, 0.0015, 0, + }; + + auto value = bit_string.buf[0]; + + auto stec_cls = value & 0x7; + auto stec_val = (value >> 3) & 0x7; + stec_cls = ((stec_cls & 0x1) << 2) | ((stec_cls & 0x2) << 0) | ((stec_cls & 0x4) >> 2); + stec_val = ((stec_val & 0x1) << 2) | ((stec_val & 0x2) << 0) | ((stec_val & 0x4) >> 2); + auto index = (8 * stec_cls) + stec_val; + + assert(index < 64); + return StecQualityIndicator{ + index == 0, + QUALITY_INDICATOR[63 - index], + }; +} + +static SPARTN_CONSTEXPR double STEC_C00_RESOLUTION = 0.05; +static SPARTN_CONSTEXPR double STEC_C01_RESOLUTION = 0.02; +static SPARTN_CONSTEXPR double STEC_C10_RESOLUTION = 0.02; +static SPARTN_CONSTEXPR double STEC_C11_RESOLUTION = 0.02; + +static double stec_C00_r16(long value) { + return value * STEC_C00_RESOLUTION; +} + +static double stec_C01_r16(long* value) { + return (value ? *value : 0) * STEC_C01_RESOLUTION; +} + +static double stec_C10_r16(long* value) { + return (value ? *value : 0) * STEC_C10_RESOLUTION; +} + +static double stec_C11_r16(long* value) { + return (value ? *value : 0) * STEC_C11_RESOLUTION; +} + +static SPARTN_CONSTEXPR double STEC_RESIDUAL_B7_RESOLUTION = 0.04; +static SPARTN_CONSTEXPR double STEC_RESIDUAL_B16_RESOLUTION = 0.04; + +static double stecResidualCorrection_r16( + STEC_ResidualSatElement_r16::STEC_ResidualSatElement_r16__stecResidualCorrection_r16 + correction) { + switch (correction.present) { + case STEC_ResidualSatElement_r16__stecResidualCorrection_r16_PR_b7_r16: + return correction.choice.b7_r16 * STEC_RESIDUAL_B7_RESOLUTION; + case STEC_ResidualSatElement_r16__stecResidualCorrection_r16_PR_b16_r16: + return correction.choice.b16_r16 * STEC_RESIDUAL_B16_RESOLUTION; + default: SPARTN_UNREACHABLE(); + } +} + +} // namespace decode diff --git a/generator/spartn2/gad.cpp b/generator/spartn2/gad.cpp new file mode 100644 index 00000000..cc7fe0c8 --- /dev/null +++ b/generator/spartn2/gad.cpp @@ -0,0 +1,85 @@ +#include "data.hpp" +#include "decode.hpp" +#include "generator.hpp" +#include "message.hpp" + +namespace generator { +namespace spartn { + +void Generator::generate_gad(long iod, long set_id) { + auto cps_it = mCorrectionPointSets.find(set_id); + if (cps_it == mCorrectionPointSets.end()) return; + auto& correction_point_set = *(cps_it->second.get()); + +#ifdef SPARTN_DEBUG_PRINT + printf(" grid points: %ld\n", correction_point_set.grid_points); + printf(" bitmask: "); + for (auto i = 0; i < correction_point_set.grid_points; i++) { + auto bit_index = 64 - i - 1; + auto bit = (correction_point_set.bitmask >> bit_index) & 1; + printf("%d", bit ? 1 : 0); + } + printf("\n"); + printf(" ref-lat: %9.6f\n", + decode::referencePointLatitude_r16(correction_point_set.referencePointLatitude_r16)); + printf(" ref-lng: %10.6f\n", + decode::referencePointLongitude_r16(correction_point_set.referencePointLongitude_r16)); + printf(" steps-lat: %ld\n", correction_point_set.numberOfStepsLatitude_r16); + printf(" steps-lng: %ld\n", correction_point_set.numberOfStepsLongitude_r16); + printf(" delta-lat: %9.6f\n", + decode::stepOfLatitude_r16(correction_point_set.stepOfLatitude_r16)); + printf(" delta-lng: %10.6f\n", + decode::stepOfLongitude_r16(correction_point_set.stepOfLongitude_r16)); + + // print the grid in ascii + for (auto i = 0; i < correction_point_set.numberOfStepsLatitude_r16 + 1; i++) { + for (auto j = 0; j < correction_point_set.numberOfStepsLongitude_r16 + 1; j++) { + auto index = i * (correction_point_set.numberOfStepsLongitude_r16 + 1) + j; + auto bit_index = 64 - index - 1; + auto bit = (correction_point_set.bitmask >> bit_index) & 1; + if (bit == 0) { + printf("-- "); + } else { + printf("%02ld ", index); + } + } + printf("\n"); + } +#endif + + MessageBuilder builder{2 /* GAD */, 0, 0}; + builder.sf005(iod); // TODO(ewasjon): We could include AIOU in the correction point set, to + // handle overflow + builder.sf068(0); + builder.sf069(); + + // NOTE(ewasjon): 3GPP LPP can only handle one area + builder.sf030(1); + + { + builder.sf031(correction_point_set.area_id); + + auto reference_point_lat = + decode::referencePointLatitude_r16(correction_point_set.referencePointLatitude_r16); + auto reference_point_lng = + decode::referencePointLongitude_r16(correction_point_set.referencePointLongitude_r16); + builder.sf032(reference_point_lat); + builder.sf033(reference_point_lng); + + // NOTE(ewasjon): 3GPP LPP has the number of steps, not the number of points + auto grid_count_lat = correction_point_set.numberOfStepsLatitude_r16 + 1; + auto grid_count_lng = correction_point_set.numberOfStepsLongitude_r16 + 1; + builder.sf034(grid_count_lat); + builder.sf035(grid_count_lng); + + auto delta_lat = decode::stepOfLatitude_r16(correction_point_set.stepOfLatitude_r16); + auto delta_lng = decode::stepOfLongitude_r16(correction_point_set.stepOfLongitude_r16); + builder.sf036(delta_lat); + builder.sf037(delta_lng); + } + + mMessages.push_back(builder.build()); +} + +} // namespace spartn +} // namespace generator diff --git a/generator/spartn2/generator.cpp b/generator/spartn2/generator.cpp new file mode 100644 index 00000000..6fd44040 --- /dev/null +++ b/generator/spartn2/generator.cpp @@ -0,0 +1,189 @@ +#include "generator.hpp" +#include "data.hpp" +#include "decode.hpp" +#include "message.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +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), mGenerateGad(true), mGenerateOcb(true), + mGenerateHpac(true), mGpsSupported(true), mGlonassSupported(true), mGalileoSupported(true), + mBeidouSupported(false) {} + +Generator::~Generator() = default; + +std::vector Generator::generate(const LPP_Message* lpp_message) { + // Clear previous messages + mMessages.clear(); + if (!lpp_message) return mMessages; + + auto body = lpp_message->lpp_MessageBody; + if (body->present != LPP_MessageBody_PR_c1) return mMessages; + if (body->choice.c1.present != LPP_MessageBody__c1_PR_provideAssistanceData) return mMessages; + + auto& pad = body->choice.c1.choice.provideAssistanceData; + if (pad.criticalExtensions.present != ProvideAssistanceData__criticalExtensions_PR_c1) + return mMessages; + if (pad.criticalExtensions.choice.c1.present != + ProvideAssistanceData__criticalExtensions__c1_PR_provideAssistanceData_r9) + return mMessages; + + // Initialze (and clear previous) correction data + mCorrectionData = std::unique_ptr(new CorrectionData(mGroupByEpochTime)); + + auto message = &pad.criticalExtensions.choice.c1.choice.provideAssistanceData_r9; + find_correction_point_set(message); + find_ocb_corrections(message); + find_hpac_corrections(message); + + auto iods = mCorrectionData->iods(); + for (auto iod : iods) { +#ifdef SPARTN_DEBUG_PRINT + printf("-- iod=%ld\n", iod); +#endif + + auto ocb = mCorrectionData->ocb(iod); + auto hpac = mCorrectionData->hpac(iod); + + if (hpac && mGenerateGad) { + std::vector set_ids; + hpac->set_ids(set_ids); + + for (auto set_id : set_ids) { +#ifdef SPARTN_DEBUG_PRINT + printf("GAD: set=%ld, iod=%ld\n", set_id, iod); +#endif + + generate_gad(iod, set_id); + } + } + + if (ocb && mGenerateOcb) { + generate_ocb(iod); + } + + if (hpac && mGenerateHpac) { + generate_hpac(iod); + } + } + + // Increment generation index + mGenerationIndex++; + return mMessages; +} + +void Generator::find_correction_point_set(const ProvideAssistanceData_r9_IEs* message) { + if (!message->a_gnss_ProvideAssistanceData) return; + if (!message->a_gnss_ProvideAssistanceData->gnss_CommonAssistData) return; + + auto& cad = *message->a_gnss_ProvideAssistanceData->gnss_CommonAssistData; + if (!cad.ext2) return; + if (!cad.ext2->gnss_SSR_CorrectionPoints_r16) return; + + auto& ssr = *cad.ext2->gnss_SSR_CorrectionPoints_r16; + if (ssr.correctionPoints_r16.present == + GNSS_SSR_CorrectionPoints_r16__correctionPoints_r16_PR_arrayOfCorrectionPoints_r16) { + auto& array = ssr.correctionPoints_r16.choice.arrayOfCorrectionPoints_r16; + + if (mCorrectionPointSets.find(ssr.correctionPointSetID_r16) != mCorrectionPointSets.end()) { + // NOTE(ewasjon): We assume that the correction point set cannot be changed. From an + // location server point-of-view, this should never happen. The correction point set is + // only every sent on the first non-periodic message or when the UE changes cell. + return; + } else { + CorrectionPointSet correction_point_set{}; + correction_point_set.set_id = ssr.correctionPointSetID_r16; + correction_point_set.area_id = next_area_id(); + correction_point_set.grid_points = + (array.numberOfStepsLatitude_r16 + 1) * (array.numberOfStepsLongitude_r16 + 1); + correction_point_set.referencePointLatitude_r16 = array.referencePointLatitude_r16; + correction_point_set.referencePointLongitude_r16 = array.referencePointLongitude_r16; + correction_point_set.numberOfStepsLatitude_r16 = array.numberOfStepsLatitude_r16; + correction_point_set.numberOfStepsLongitude_r16 = array.numberOfStepsLongitude_r16; + correction_point_set.stepOfLatitude_r16 = array.stepOfLatitude_r16; + correction_point_set.stepOfLongitude_r16 = array.stepOfLongitude_r16; + + uint64_t bitmask = 0; + if (array.bitmaskOfGrids_r16) { + for (size_t i = 0; i < array.bitmaskOfGrids_r16->size; i++) { + bitmask <<= 8; + bitmask |= static_cast(array.bitmaskOfGrids_r16->buf[i]); + } + bitmask >>= array.bitmaskOfGrids_r16->bits_unused; + printf(" bitmask: %ld bytes, %d bits, 0x%016lX\n", array.bitmaskOfGrids_r16->size, + array.bitmaskOfGrids_r16->bits_unused, bitmask); + } else { + bitmask = 0xFFFFFFFFFFFFFFFF; + } + correction_point_set.bitmask = bitmask; + + auto correction_point_set_ptr = + std::unique_ptr(new CorrectionPointSet(correction_point_set)); + mCorrectionPointSets.insert( + std::make_pair(ssr.correctionPointSetID_r16, std::move(correction_point_set_ptr))); + } + } else { + // TODO(ewasjon): [low-priority] Support list of correction points + } +} + +void Generator::find_ocb_corrections(const ProvideAssistanceData_r9_IEs* message) { + if (!message->a_gnss_ProvideAssistanceData) return; + if (!message->a_gnss_ProvideAssistanceData->gnss_GenericAssistData) return; + + auto& gad = *message->a_gnss_ProvideAssistanceData->gnss_GenericAssistData; + for (int i = 0; i < gad.list.count; i++) { + auto element = gad.list.array[i]; + if (!element) continue; + + auto gnss_id = element->gnss_ID.gnss_id; + if (element->ext2) { + mCorrectionData->add_correction(gnss_id, element->ext2->gnss_SSR_OrbitCorrections_r15); + mCorrectionData->add_correction(gnss_id, element->ext2->gnss_SSR_ClockCorrections_r15); + mCorrectionData->add_correction(gnss_id, element->ext2->gnss_SSR_CodeBias_r15); + } + + if (element->ext3) { + mCorrectionData->add_correction(gnss_id, element->ext3->gnss_SSR_PhaseBias_r16); + mCorrectionData->add_correction(gnss_id, element->ext3->gnss_SSR_URA_r16); + } + } +} + +void Generator::find_hpac_corrections(const ProvideAssistanceData_r9_IEs* message) { + if (!message->a_gnss_ProvideAssistanceData) return; + if (!message->a_gnss_ProvideAssistanceData->gnss_GenericAssistData) return; + + auto& gad = *message->a_gnss_ProvideAssistanceData->gnss_GenericAssistData; + for (int i = 0; i < gad.list.count; i++) { + auto element = gad.list.array[i]; + if (!element) continue; + + auto gnss_id = element->gnss_ID.gnss_id; + if (element->ext3) { + mCorrectionData->add_correction(gnss_id, element->ext3->gnss_SSR_STEC_Correction_r16); + mCorrectionData->add_correction(gnss_id, element->ext3->gnss_SSR_GriddedCorrection_r16); + } + } +} + +} // namespace spartn +} // namespace generator diff --git a/generator/spartn2/hpac.cpp b/generator/spartn2/hpac.cpp new file mode 100644 index 00000000..142dce07 --- /dev/null +++ b/generator/spartn2/hpac.cpp @@ -0,0 +1,570 @@ +#include "data.hpp" +#include "decode.hpp" +#include "generator.hpp" +#include "message.hpp" + +#include +#include +#include + +#include + +namespace generator { +namespace spartn { + +uint32_t HpacSatellite::prn() const { + // NOTE(ewasjon): 3GPP LPP defines PRN starting at 0 instead of 1. + return id + 1; +} + +void HpacSatellite::add_correction(STEC_SatElement_r16* stec) { + if (!stec) return; + this->stec = stec; +} + +void HpacSatellite::add_correction(long grid_id, STEC_ResidualSatElement_r16* residual) { + if (!residual) return; + residuals[grid_id] = residual; +} + +std::vector HpacCorrections::satellites() const { + std::unordered_map satellites; + + if (stec) { + auto& list = stec->stec_SatList_r16.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + + auto id = element->svID_r16.satellite_id; + auto& satellite = satellites[id]; + satellite.id = id; + satellite.add_correction(element); + } + } + + if (gridded) { + // NOTE(ewasjon): This grid id (grid-point id) is not the absolute id. To get the absolute + // id you need to reference the correction point set bitmask. + long grid_id = 0; + auto& list = gridded->gridList_r16.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (element) { + if (element->stec_ResidualSatList_r16) { + auto& sat_list = element->stec_ResidualSatList_r16->list; + for (int j = 0; j < sat_list.count; j++) { + auto sat_element = sat_list.array[j]; + if (!sat_element) continue; + + auto id = sat_element->svID_r16.satellite_id; + auto& satellite = satellites[id]; + satellite.id = id; + satellite.add_correction(grid_id, sat_element); + } + } + } + + grid_id++; + } + } + + // Convert to vector + std::vector result; + for (auto& kv : satellites) { + result.push_back(kv.second); + } + + // Sort by satellite id + std::sort(result.begin(), result.end(), [](const HpacSatellite& a, const HpacSatellite& b) { + return a.id < b.id; + }); + + return result; +} + +void HpacData::set_ids(std::vector& ids) const { + for (auto& kvp : mKeyedCorrections) { + auto set_id = kvp.first.set_id; + if (std::find(ids.begin(), ids.end(), set_id) == ids.end()) { + ids.push_back(set_id); + } + } +} + +std::vector CorrectionData::set_ids() const { + std::vector set_ids; + + for (auto& kvp : mHpacData) { + kvp.second.set_ids(set_ids); + } + + return set_ids; +} + +std::vector CorrectionData::iods() const { + std::vector iods; + + for (auto& kvp : mOcbData) { + auto iod = kvp.first; + if (std::find(iods.begin(), iods.end(), iod) == iods.end()) { + iods.push_back(iod); + } + } + + for (auto& kvp : mHpacData) { + auto iod = kvp.first; + if (std::find(iods.begin(), iods.end(), iod) == iods.end()) { + iods.push_back(iod); + } + } + + std::sort(iods.begin(), iods.end()); + return iods; +} + +void CorrectionData::add_correction(long gnss_id, GNSS_SSR_GriddedCorrection_r16* gridded) { + if (!gridded) return; + auto iod = gridded->iod_ssr_r16; + auto& hpac = mHpacData[iod]; + + auto epoch_time = spartn_time_from(gridded->epochTime_r16); + auto set_id = gridded->correctionPointSetID_r16; + auto key = HpacKey{set_id, gnss_id, group_by_epoch_time ? epoch_time.rounded_seconds : 0}; + + auto& corrections = hpac.mKeyedCorrections[key]; + corrections.gnss_id = gnss_id; + corrections.iod = iod; + corrections.set_id = set_id; + corrections.epoch_time = epoch_time; + corrections.gridded = gridded; +} + +void CorrectionData::add_correction(long gnss_id, GNSS_SSR_STEC_Correction_r16* stec) { + if (!stec) return; + auto iod = stec->iod_ssr_r16; + auto& hpac = mHpacData[iod]; + + auto set_id = stec->correctionPointSetID_r16; + auto epoch_time = spartn_time_from(stec->epochTime_r16); + auto key = HpacKey{set_id, gnss_id, group_by_epoch_time ? epoch_time.rounded_seconds : 0}; + + auto& corrections = hpac.mKeyedCorrections[key]; + corrections.gnss_id = gnss_id; + corrections.iod = iod; + corrections.set_id = set_id; + corrections.epoch_time = epoch_time; + corrections.stec = stec; +} + +static bool within_range(double min, double max, double value) { + if (value < min) return false; + if (value > max) return false; + return true; +} + +static uint8_t compute_troposphere_block_type(const GNSS_SSR_GriddedCorrection_r16* ptr) { + // If we're missing the troposphere correction, return 0=none + if (!ptr) return 0; + + // NOTE(ewasjon): SPARTN supports three types of troposphere corrections: + // 0 = none + // 1 = polynomial + // 2 = polynomial + grid + // + // Because 3GPP LPP doesn't have a polynomial, only values for each grid point, we can only + // support type 0 and 2. Another problem is 3GPP LPP has greater controller over the grid + // point values. Where some values can be missing, this doesn't work with SPARTN. + + // TODO(ewasjon): [low-priority] Are we losing potential corrections by filtering them out due + // to missing troposphere correction for a grid point? This should be investigated. + auto& list = ptr->gridList_r16.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (element->tropospericDelayCorrection_r16) continue; + + // Missing troposphere correction for grid point, + // as we can't handle this, return 0=none + return 0; + } + + return 2; +} + +static uint8_t compute_ionosphere_block_type(const GNSS_SSR_STEC_Correction_r16* stec, + const GNSS_SSR_GriddedCorrection_r16* gridded) { + // If we're missing the ionosphere correction, return 0=none + if (!stec) return 0; + + // We have a gridded correction, so we use both the polynomial and the grid + if (gridded) { + return 2; + } + + // If we're missing the gridded correction, we only use the polynomial + // for the ionosphere correction. + return 1; +} + +static double compute_average_zentith_delay(const GNSS_SSR_GriddedCorrection_r16& data) { + double total_zenith_delay = 0.0; + double count = 0.0; + + auto& list = data.gridList_r16.list; + 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); + total_zenith_delay += residual; + count++; + } + + return total_zenith_delay / count; +} + +static void troposphere_data_block(MessageBuilder& builder, + const GNSS_SSR_GriddedCorrection_r16& data, int ura_override, + bool use_average_zenith_delay) { + // 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) { + builder.sf042_raw(ura_override); + } else if (data.troposphericDelayQualityIndicator_r16) { + // TODO(ewasjon): Refactor as function in decode namespace + auto& quality = *data.troposphericDelayQualityIndicator_r16; + auto cls = (quality.buf[0] >> 3) & 0x7; + auto val = quality.buf[0] & 0x7; + auto q = pow(3, cls) * (1 + static_cast(val) / 4.0) - 1; + 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 + } + + // NOTE(ewasjon): SPARTN have an average hydrostatic delay for all grid points. 3GPP LPP only + // have hydrostatic delay per grid point. Thus, we need to compute the average. + auto& list = data.gridList_r16.list; + double hydrostatic_delay_sum = 0; + double grid_count = 0; + 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 hydrostatic_delay = + decode::tropoHydroStaticVerticalDelay_r16(grid_point.tropoHydroStaticVerticalDelay_r16); + hydrostatic_delay_sum += hydrostatic_delay; + grid_count++; + } + + auto hydrostatic_delay_avg = hydrostatic_delay_sum / grid_count; + builder.sf043(hydrostatic_delay_avg); + +#ifdef SPARTN_DEBUG_PRINT + printf(" hydrostatic_delay_avg: %f\n", hydrostatic_delay_avg); +#endif + + // NOTE(ewasjon): 3GPP LPP doesn't include a polynomial for the wet delay (zenith delay). Thus, + // we can set this to a constant value of 0.0. We can also compute the average zenith delay for + // all grid points, and subtract this from the zenith delay for each grid point. This will + // reduce the precision needed. + + double average_zenith_delay = 0.0; + if (use_average_zenith_delay) { + average_zenith_delay = compute_average_zentith_delay(data); + } + + if (within_range(-0.252, 0.252, average_zenith_delay)) { + builder.sf044(0); // Small coefficient block + 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 + } else { + builder.sf044(0); // Small coefficient block + builder.sf045(0.0); // T_0 + average_zenith_delay = 0.0; + } + +#ifdef SPARTN_DEBUG_PRINT + 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 + + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + if (!element->tropospericDelayCorrection_r16) { +#ifdef SPARTN_DEBUG_PRINT + printf(" grid[%2d] = invalid\n", i); +#endif + 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); +#ifdef SPARTN_DEBUG_PRINT + printf(" grid[%2d] = %f\n", i, residual - average_zenith_delay); +#endif + } + } +} + +static int compute_equation_type(std::vector& satellites) { + auto final_equation_type = 0; + + for (auto& satellite : satellites) { + if (!satellite.stec) continue; + auto& stec = *satellite.stec; + + auto equation_type = 0; // C_00 + if (stec.stec_C01_r16 || stec.stec_C10_r16) { + equation_type = 1; // C_00, C_01, C_10 + } + + if (stec.stec_C11_r16) { + equation_type = 2; // C_00, C_01, C_10, C_11 + } + + if (equation_type > final_equation_type) { + final_equation_type = equation_type; + } + } + + return final_equation_type; +} + +static bool compute_coefficient_size_indicator(const STEC_SatElement_r16& satellite, + int equation_type) { + auto small_coefficient_size = true; + + // NOTE(ewasjon): There is no resolution difference between the small and large coefficent + // blocks. Thus, we only need to check that we're inside the given intervals. + + if (equation_type >= 0) { + auto c_00 = decode::stec_C00_r16(satellite.stec_C00_r16); + if (!within_range(-81.88, 81.88, c_00)) small_coefficient_size = true; + } + + if (equation_type >= 1) { + if (satellite.stec_C01_r16) { + auto c_01 = decode::stec_C01_r16(satellite.stec_C01_r16); + if (!within_range(-16.376, 16.376, c_01)) small_coefficient_size = true; + } + + if (satellite.stec_C01_r16) { + auto c_10 = decode::stec_C10_r16(satellite.stec_C10_r16); + if (!within_range(-16.376, 16.376, c_10)) small_coefficient_size = true; + } + } + + if (equation_type >= 2) { + if (satellite.stec_C11_r16) { + auto c_11 = decode::stec_C11_r16(satellite.stec_C11_r16); + if (!within_range(-8.190, 8.190, c_11)) small_coefficient_size = true; + } + } + + return !small_coefficient_size; +} + +static void ionosphere_data_block_1(MessageBuilder& builder, const HpacSatellite& satellite, + int equation_type, int sf055_override, int sf055_default) { + auto element = satellite.stec; + + if (sf055_override >= 0) { + builder.sf055_raw(sf055_override); + } else { + auto q = decode::stecQualityIndicator_r16(element->stecQualityIndicator_r16); + if (q.invalid) { + builder.sf055_raw(sf055_default); + } else { + builder.sf055(q.value); + } + } + + auto coefficient_size_indicator = compute_coefficient_size_indicator(*element, equation_type); + builder.sf056(coefficient_size_indicator); + + auto c00 = decode::stec_C00_r16(element->stec_C00_r16); + auto c01 = decode::stec_C01_r16(element->stec_C01_r16); + auto c10 = decode::stec_C10_r16(element->stec_C10_r16); + auto c11 = decode::stec_C11_r16(element->stec_C11_r16); + + if (equation_type >= 0) { + builder.ionosphere_coefficient_c00(coefficient_size_indicator, c00); + } + + if (equation_type >= 1) { + builder.ionosphere_coefficient_c10_c01(coefficient_size_indicator, c01); + builder.ionosphere_coefficient_c10_c01(coefficient_size_indicator, c10); + } + + if (equation_type >= 2) { + builder.ionosphere_coefficient_c11(coefficient_size_indicator, c11); + } +} + +static int compute_residual_field_size(const HpacSatellite& satellite) { + int residual_field_size = 0; + + for (auto& kvp : satellite.residuals) { + auto& element = *kvp.second; + auto residual = decode::stecResidualCorrection_r16(element.stecResidualCorrection_r16); + + if (within_range(-0.28, 0.28, residual)) + residual_field_size = std::max(residual_field_size, 0); + else if (within_range(-2.52, 2.52, residual)) + residual_field_size = std::max(residual_field_size, 1); + else if (within_range(-20.44, 20.44, residual)) + residual_field_size = std::max(residual_field_size, 2); + else + residual_field_size = std::max(residual_field_size, 3); + } + + return residual_field_size; +} + +static void ionosphere_data_block_2(MessageBuilder& builder, long grid_points, + HpacSatellite& satellite) { + auto residual_field_size = compute_residual_field_size(satellite); + builder.sf063(residual_field_size); + +#ifdef SPARTN_DEBUG_PRINT + printf(" residual_field_size=%d\n", residual_field_size); +#endif + + for (long i = 0; i < grid_points; i++) { + auto it = satellite.residuals.find(i); + if (it == satellite.residuals.end()) { +#ifdef SPARTN_DEBUG_PRINT + printf(" grid[%2ld] = invalid\n", i); +#endif + builder.ionosphere_residual_invalid(residual_field_size); + } else { + auto& element = *it->second; + auto residual = decode::stecResidualCorrection_r16(element.stecResidualCorrection_r16); +#ifdef SPARTN_DEBUG_PRINT + printf(" grid[%2ld] = %f\n", i, residual); +#endif + builder.ionosphere_residual(residual_field_size, residual); + } + } +} + +static void ionosphere_data_block(MessageBuilder& builder, CorrectionPointSet& correction_point_set, + HpacCorrections& corrections, long gnss_id, + int ionosphere_block_type, int sf055_override, + int sf055_default) { + auto satellites = corrections.satellites(); + + { + // NOTE(ewasjon): Remove all satellites that does not have STEC corrections. + auto it = satellites.begin(); + for (; it != satellites.end(); it++) { + if (!it->stec) { + it = satellites.erase(it); + } + } + } + + auto equation_type = compute_equation_type(satellites); + builder.sf054(equation_type); + builder.satellite_mask(gnss_id, satellites); + + for (auto& satellite : satellites) { + if (!satellite.stec) continue; + + ionosphere_data_block_1(builder, satellite, equation_type, sf055_override, sf055_default); + + if (ionosphere_block_type == 2) { + ionosphere_data_block_2(builder, correction_point_set.grid_points, satellite); + } + } +} + +void Generator::generate_hpac(long iod) { + auto hpac_data = mCorrectionData->hpac(iod); + if (!hpac_data) return; + + std::vector messages; + for (auto& kvp : hpac_data->mKeyedCorrections) { + if (!mGpsSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_gps) continue; + if (!mGlonassSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_glonass) continue; + if (!mGalileoSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_galileo) continue; + if (!mBeidouSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_bds) continue; + + messages.push_back(&kvp.second); + } + + std::sort(messages.begin(), messages.end(), + [](const HpacCorrections* a, const HpacCorrections* b) { + return subtype_from_gnss_id(a->gnss_id) < subtype_from_gnss_id(b->gnss_id); + }); + + for (size_t message_id = 0; message_id < messages.size(); message_id++) { + auto& corrections = *messages[message_id]; + auto epoch_time = corrections.epoch_time.rounded_seconds; + auto gnss_id = corrections.gnss_id; + auto set_id = corrections.set_id; + + auto cps_it = mCorrectionPointSets.find(set_id); + if (cps_it == mCorrectionPointSets.end()) continue; + auto& correction_point_set = *(cps_it->second.get()); + +#ifdef SPARTN_DEBUG_PRINT + printf("HPAC: time=%u, set=%ld, gnss=%ld, iod=%ld\n", epoch_time, set_id, gnss_id, iod); + printf(" area_id=%u\n", correction_point_set.area_id); +#endif + + auto subtype = subtype_from_gnss_id(gnss_id); + auto troposphere_block_type = compute_troposphere_block_type(corrections.gridded); + auto ionosphere_block_type = + compute_ionosphere_block_type(corrections.stec, corrections.gridded); + + MessageBuilder builder{1 /* HPAC */, subtype, epoch_time}; + builder.sf005(iod); + builder.sf068(0); // TODO(ewasjon): [low-priority] We could include AIOU in the correction point set, to + // handle overflow + builder.sf069(); + builder.sf030(1); + + // Atmosphere block + { + // Area data block + { + builder.sf031(correction_point_set.area_id); + builder.sf039(correction_point_set.grid_points); + builder.sf040(troposphere_block_type); + builder.sf040(ionosphere_block_type); + } + + // Troposphere data block + if (troposphere_block_type != 0) { + troposphere_data_block(builder, *corrections.gridded, mUraOverride, + mComputeAverageZenithDelay); + } + + // Ionosphere data block + if (ionosphere_block_type != 0) { + ionosphere_data_block(builder, correction_point_set, corrections, gnss_id, + ionosphere_block_type, mIonosphereQualityOverride, + mIonosphereQualityDefault); + } + } + + mMessages.push_back(builder.build()); + } +} + +} // namespace spartn +} // namespace generator diff --git a/generator/spartn2/include/generator/spartn2/generator.hpp b/generator/spartn2/include/generator/spartn2/generator.hpp new file mode 100644 index 00000000..42370727 --- /dev/null +++ b/generator/spartn2/include/generator/spartn2/generator.hpp @@ -0,0 +1,122 @@ +#pragma once +#include +#include +#include +#include + +struct LPP_Message; +struct ProvideAssistanceData_r9_IEs; + +namespace generator { +namespace spartn { + +/// SPARTN message +class Message { +public: + SPARTN_EXPLICIT Message(uint8_t message_type, uint8_t message_subtype, uint32_t message_time, + std::vector&& payload); + + /// Message type + uint8_t message_type() const { return mMessageType; } + /// Message subtype + uint8_t message_subtype() const { return mMessageSubtype; } + /// Message data + const std::vector& payload() const { return mPayload; } + + std::vector build(); + +private: + uint8_t mMessageType; + uint8_t mMessageSubtype; + uint32_t mMessageTime; + std::vector mPayload; +}; + +struct CorrectionPointSet; +struct CorrectionData; + +/// Generates SPARTN messages based on LPP SSR messages. +class Generator { +public: + /// Constructor. + Generator(); + + /// Destructor. + ~Generator(); + + void set_ura_override(int ura_override) { mUraOverride = ura_override; } + + void set_continuity_indicator(double continuity_indicator) { + mContinuityIndicator = continuity_indicator; + } + + void set_ublox_clock_correction(bool ublox_clock_correction) { + mUBloxClockCorrection = ublox_clock_correction; + } + + void set_compute_average_zenith_delay(bool compute_average_zenith_delay) { + mComputeAverageZenithDelay = compute_average_zenith_delay; + } + + void set_iode_shift(bool iode_shift) { mIodeShift = iode_shift; } + + void set_generate_ocb(bool generate_ocb) { mGenerateOcb = generate_ocb; } + void set_generate_hpac(bool generate_hpac) { mGenerateHpac = generate_hpac; } + void set_generate_gad(bool generate_gad) { mGenerateGad = generate_gad; } + + void set_gps_supported(bool gps_supported) { mGpsSupported = gps_supported; } + void set_glonass_supported(bool glonass_supported) { mGlonassSupported = glonass_supported; } + void set_galileo_supported(bool galileo_supported) { mGalileoSupported = galileo_supported; } + void set_beidou_supported(bool beidou_supported) { mBeidouSupported = beidou_supported; } + + /// Generate SPARTN messages based on LPP SSR messages. + /// @param[in] lpp_message The LPP SSR message. + /// @return The generated SPARTN messages. + std::vector generate(const LPP_Message* lpp_message); + +private: + void find_correction_point_set(const ProvideAssistanceData_r9_IEs* message); + void find_ocb_corrections(const ProvideAssistanceData_r9_IEs* message); + void find_hpac_corrections(const ProvideAssistanceData_r9_IEs* message); + + void generate_gad(long iod, long set_id); + void generate_ocb(long iod); + void generate_hpac(long iod); + + uint16_t next_area_id() { + auto id = mNextAreaId; + mNextAreaId = (mNextAreaId + 1) % 256; + return id; + } + +private: + uint32_t mGenerationIndex; + uint16_t mNextAreaId; + + std::unordered_map> mCorrectionPointSets; + std::unique_ptr mCorrectionData; + std::vector mMessages; + + int mUraOverride; // <0 = no override + double mContinuityIndicator; // <0 = no override + bool mUBloxClockCorrection; + + // SF055: + int mIonosphereQualityOverride; // <0 = no override + int mIonosphereQualityDefault; + + bool mComputeAverageZenithDelay; + bool mGroupByEpochTime; + bool mIodeShift; + + bool mGenerateGad; + bool mGenerateOcb; + bool mGenerateHpac; + bool mGpsSupported; + bool mGlonassSupported; + bool mGalileoSupported; + bool mBeidouSupported; +}; + +} // namespace spartn +} // namespace generator \ No newline at end of file diff --git a/generator/spartn2/include/generator/spartn2/types.hpp b/generator/spartn2/include/generator/spartn2/types.hpp new file mode 100644 index 00000000..85a1c014 --- /dev/null +++ b/generator/spartn2/include/generator/spartn2/types.hpp @@ -0,0 +1,54 @@ +#pragma once +#include +#include +#include + +#ifndef SPARTN_EXPLICIT +#define SPARTN_EXPLICIT explicit +#endif + +#ifndef SPARTN_NOEXCEPT +#define SPARTN_NOEXCEPT noexcept +#endif + +#ifndef SPARTN_CONSTEXPR +#define SPARTN_CONSTEXPR constexpr +#endif + +#ifndef SPARTN_UNUSED +#if defined(__has_cpp_attribute) +#if __has_cpp_attribute(maybe_unused) +#define SPARTN_UNUSED [[maybe_unused]] +#endif +#endif +#ifndef SPARTN_UNUSED +#define SPARTN_UNUSED +#endif +#endif + +#ifndef SPARTN_NODISCARD +#if defined(__has_cpp_attribute) +#if __has_cpp_attribute(nodiscard) +#if defined(__cplusplus) && __cplusplus >= 201703L +#define SPARTN_NODISCARD [[nodiscard]] +#endif +#endif +#endif +#ifndef SPARTN_NODISCARD +#define SPARTN_NODISCARD +#endif +#endif + +#ifndef SPARTN_UNREACHABLE +#if defined(__has_builtin) +#if __has_builtin(__builtin_unreachable) +#define SPARTN_UNREACHABLE() __builtin_unreachable() +#endif +#endif +#ifndef SPARTN_UNREACHABLE +#define SPARTN_UNREACHABLE() spartn_unreachable() +__attribute__((noreturn)) inline void spartn_unreachable() { + assert(false); +} +#endif +#endif diff --git a/generator/spartn2/message.cpp b/generator/spartn2/message.cpp new file mode 100644 index 00000000..9b487e35 --- /dev/null +++ b/generator/spartn2/message.cpp @@ -0,0 +1,248 @@ +#include "message.hpp" + +#define GNSS_ID_GPS 0 +#define GNSS_ID_GLO 4 +#define GNSS_ID_GAL 3 +#define GNSS_ID_BDS 5 +#define GNSS_ID_QZS 2 + +#include +#include +#include + +static uint16_t crc16_ccitt(uint8_t* data, size_t length) { + // CRC 16 CCITT + // polynomial = 0x1021U + // initial value = 0 + // input reflected = false + // result reflected = false + // final XOR value = 0 + + static SPARTN_CONSTEXPR const uint16_t CRC16_LOOKUP[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, + 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, + 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, 0x2462, + 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, + 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, + 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, 0x48C4, 0x58E5, + 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, + 0x9969, 0xA90A, 0xB92B, 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, 0x6CA6, 0x7C87, 0x4CE4, + 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, + 0x8D68, 0x9D49, 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, + 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, 0x9188, 0x81A9, 0xB1CA, 0xA1EB, + 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, + 0x6067, 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, + 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, + 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, + 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, + 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, 0xCB7D, + 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, + 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, + 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, 0xEF1F, 0xFF3E, + 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, + 0x3EB2, 0x0ED1, 0x1EF0, + }; + + uint16_t crc = 0; + for (size_t i = 0; i < length; i++) { + auto value = data[i]; + auto index = static_cast(value) ^ (crc >> 8); + crc = CRC16_LOOKUP[index] ^ (crc << 8); + } + + return crc; +} + +namespace generator { +namespace spartn { +Message::Message(uint8_t message_type, uint8_t message_subtype, uint32_t message_time, + std::vector&& payload) + : mMessageType(message_type), mMessageSubtype(message_subtype), mMessageTime(message_time), + mPayload(std::move(payload)) {} + +std::vector Message::build() { + if(mPayload.size() > 1023) { + return {}; + } + + TransportBuilder builder{}; + builder.tf001(); + builder.tf002(mMessageType); + builder.tf003(mPayload.size()); + builder.tf004(false); + builder.tf005(SPARTN_CRC_16_CCITT); + builder.tf006(); + builder.tf007(mMessageSubtype); + builder.tf008(true); // Full 32-bit time + builder.tf009_32bit(mMessageTime); + builder.tf010(0); + builder.tf011(0); + + builder.tf016(mPayload); + + auto tf002_to_tf016 = builder.range(8 /* skip TF001, 8 bits */, builder.bit_length() - 8); + auto crc = crc16_ccitt(tf002_to_tf016.ptr, tf002_to_tf016.size); + builder.tf018_16bit(crc); + + return builder.build(); +} + +} // namespace spartn +} // namespace generator + +TransportBuilder::TransportBuilder() : mBuilder(1228) {} + +std::vector TransportBuilder::build() { + return mBuilder.data(); +} + +ByteRange TransportBuilder::range(size_t begin_bit, size_t bits) { + assert((begin_bit % 8) == 0); + assert((bits % 8) == 0); + + auto bytes = bits / 8; + auto first_byte = begin_bit / 8; + auto ptr = mBuilder.data_ptr() + first_byte; + return ByteRange{ptr, bytes}; +} + +size_t TransportBuilder::bit_length() { + return mBuilder.bit_length(); +} + +MessageBuilder::MessageBuilder(uint8_t message_type, uint8_t message_subtype, uint32_t message_time) + : mMessageType(message_type), mMessageSubtype(message_subtype), mMessageTime(message_time), + mBuilder(1024) {} + +generator::spartn::Message MessageBuilder::build() { + auto data = mBuilder.data(); + return generator::spartn::Message{mMessageType, mMessageSubtype, mMessageTime, std::move(data)}; +} + +void MessageBuilder::satellite_mask(long gnss_id, uint64_t count, bool* bits) { + switch (gnss_id) { + case GNSS_ID_GPS: // SF011 - GPS Satellite Mask + if (count > 56) { + mBuilder.bits(3, 2); + count = 64; + } else if (count > 44) { + mBuilder.bits(2, 2); + count = 56; + } else if (count > 32) { + mBuilder.bits(1, 2); + count = 44; + } else { + mBuilder.bits(0, 2); + count = 32; + } + break; + case GNSS_ID_GLO: // SF012 - GLONASS Satellite Mask + if (count > 48) { + mBuilder.bits(3, 2); + count = 63; + } else if (count > 36) { + mBuilder.bits(2, 2); + count = 48; + } else if (count > 24) { + mBuilder.bits(1, 2); + count = 36; + } else { + mBuilder.bits(0, 2); + count = 24; + } + break; + case GNSS_ID_GAL: // SF093 - Galileo Satellite Mask + if (count > 54) { + mBuilder.bits(3, 2); + count = 64; + } else if (count > 45) { + mBuilder.bits(2, 2); + count = 54; + } else if (count > 36) { + mBuilder.bits(1, 2); + count = 45; + } else { + mBuilder.bits(0, 2); + count = 36; + } + break; + default: SPARTN_UNREACHABLE(); + } + + for (uint8_t i = 0; i < count; i++) { + mBuilder.b(bits[i]); + } +} + +void MessageBuilder::satellite_mask( + long gnss_id, const std::vector& satellites) { + uint64_t count = 0; + bool bits[64] = {false}; + for (auto& satellite : satellites) { + auto prn = satellite.prn(); + // NOTE(ewasjon): 0th bit is used for PRN 1 + auto bit = prn - 1; + bits[bit] = true; + count++; + } + + satellite_mask(gnss_id, count, bits); +} + +void MessageBuilder::satellite_mask( + long gnss_id, const std::vector& satellites) { + uint64_t count = 0; + bool bits[64] = {false}; + for (auto& satellite : satellites) { + auto prn = satellite.prn(); + // NOTE(ewasjon): 0th bit is used for PRN 1 + auto bit = prn - 1; + bits[bit] = true; + count++; + } + + satellite_mask(gnss_id, count, bits); +} + +void MessageBuilder::ephemeris_type(long gnss_id) { + switch (gnss_id) { + case GNSS_ID_GPS: // SF016 - GPS Ephemeris Type + mBuilder.bits(0, 2); // GPS L1C/A + break; + case GNSS_ID_GLO: // SF017 - GLONASS Ephemeris Type + mBuilder.bits(0, 2); // GLONASS L1C/A + break; + case GNSS_ID_GAL: // SF096 - Galileo Ephemeris Type + mBuilder.bits(1, 3); // Galileo I/NAV + break; + case GNSS_ID_BDS: // SF097 - BeiDou Ephemeris Type + mBuilder.bits(0, 4); // BeiDou D1 + break; + case GNSS_ID_QZS: // SF098 - QZSS Ephemeris Type + mBuilder.bits(0, 3); // QZSS LNAV (L1C/A) + break; + default: SPARTN_UNREACHABLE(); + } +} + +void MessageBuilder::orbit_iode(long gnss_id, BIT_STRING_s& bit_string, bool iode_shift) { + auto iode = helper::BitString::from(&bit_string)->as_int64(); + if (iode_shift) { + iode >>= 3; + } + + switch (gnss_id) { + case GNSS_ID_GPS: // SF018 - GPS IODE + mBuilder.bits(iode & 0xFF, 8); + break; + case GNSS_ID_GLO: // SF019 - GLONASS IODE + mBuilder.bits(iode & 0x7F, 7); + break; + case GNSS_ID_GAL: // SF099 - Galileo IOD + mBuilder.bits(iode & 0x3FF, 10); + break; + default: SPARTN_UNREACHABLE(); + } +} diff --git a/generator/spartn2/message.hpp b/generator/spartn2/message.hpp new file mode 100644 index 00000000..d73da8dd --- /dev/null +++ b/generator/spartn2/message.hpp @@ -0,0 +1,605 @@ +#pragma once +#include +#include + +#include "builder.hpp" +#include "data.hpp" + +#include +#include + +#define SPARTN_CRC_16_CCITT 1 + +struct BIT_STRING_s; +struct GNSS_SSR_STEC_Correction_r16; + +struct ByteRange { + uint8_t* ptr; + size_t size; +}; + +class TransportBuilder { +public: + SPARTN_EXPLICIT TransportBuilder(); + + std::vector build(); + ByteRange range(size_t begin_bit, size_t end_bit); + size_t bit_length(); + + // TF001 - Preamble + inline void tf001() { mBuilder.u8(0x73); } + + // TF002 - Message type + inline void tf002(uint8_t type) { mBuilder.bits(type, 7); } + + // TF003 - Payload length + inline void tf003(size_t length) { mBuilder.bits(static_cast(length), 10); } + + // TF004 - Encryption and authentication flag (EAF) + inline void tf004(bool use_encryption) { mBuilder.b(use_encryption); } + + // TF005 - Message CRC type + inline void tf005(uint8_t crc_type) { mBuilder.bits(crc_type, 2); } + + // TF006 - Frame CRC + inline void tf006() { + // this assumes that tf002-tf005 has been added before running. + auto length = mBuilder.data().size(); + assert(length >= 3); + + // 20 bits of data with 4 bits of padding. + uint8_t bytes[3]; + bytes[0] = mBuilder.data()[length - 3]; + bytes[1] = mBuilder.data()[length - 2]; + bytes[2] = mBuilder.data()[length - 1]; + + // CRC 4: + // polynomial = 0x09 + // initial value = 0 + // input reflected = true + // result reflected = true + // final XOR value = 0 + static SPARTN_CONSTEXPR const uint8_t CRC4_LOOKUP[256] = { + 0x00, 0x0B, 0x05, 0x0E, 0x0A, 0x01, 0x0F, 0x04, 0x07, 0x0C, 0x02, 0x09, 0x0D, 0x06, + 0x08, 0x03, 0x0E, 0x05, 0x0B, 0x00, 0x04, 0x0F, 0x01, 0x0A, 0x09, 0x02, 0x0C, 0x07, + 0x03, 0x08, 0x06, 0x0D, 0x0F, 0x04, 0x0A, 0x01, 0x05, 0x0E, 0x00, 0x0B, 0x08, 0x03, + 0x0D, 0x06, 0x02, 0x09, 0x07, 0x0C, 0x01, 0x0A, 0x04, 0x0F, 0x0B, 0x00, 0x0E, 0x05, + 0x06, 0x0D, 0x03, 0x08, 0x0C, 0x07, 0x09, 0x02, 0x0D, 0x06, 0x08, 0x03, 0x07, 0x0C, + 0x02, 0x09, 0x0A, 0x01, 0x0F, 0x04, 0x00, 0x0B, 0x05, 0x0E, 0x03, 0x08, 0x06, 0x0D, + 0x09, 0x02, 0x0C, 0x07, 0x04, 0x0F, 0x01, 0x0A, 0x0E, 0x05, 0x0B, 0x00, 0x02, 0x09, + 0x07, 0x0C, 0x08, 0x03, 0x0D, 0x06, 0x05, 0x0E, 0x00, 0x0B, 0x0F, 0x04, 0x0A, 0x01, + 0x0C, 0x07, 0x09, 0x02, 0x06, 0x0D, 0x03, 0x08, 0x0B, 0x00, 0x0E, 0x05, 0x01, 0x0A, + 0x04, 0x0F, 0x09, 0x02, 0x0C, 0x07, 0x03, 0x08, 0x06, 0x0D, 0x0E, 0x05, 0x0B, 0x00, + 0x04, 0x0F, 0x01, 0x0A, 0x07, 0x0C, 0x02, 0x09, 0x0D, 0x06, 0x08, 0x03, 0x00, 0x0B, + 0x05, 0x0E, 0x0A, 0x01, 0x0F, 0x04, 0x06, 0x0D, 0x03, 0x08, 0x0C, 0x07, 0x09, 0x02, + 0x01, 0x0A, 0x04, 0x0F, 0x0B, 0x00, 0x0E, 0x05, 0x08, 0x03, 0x0D, 0x06, 0x02, 0x09, + 0x07, 0x0C, 0x0F, 0x04, 0x0A, 0x01, 0x05, 0x0E, 0x00, 0x0B, 0x04, 0x0F, 0x01, 0x0A, + 0x0E, 0x05, 0x0B, 0x00, 0x03, 0x08, 0x06, 0x0D, 0x09, 0x02, 0x0C, 0x07, 0x0A, 0x01, + 0x0F, 0x04, 0x00, 0x0B, 0x05, 0x0E, 0x0D, 0x06, 0x08, 0x03, 0x07, 0x0C, 0x02, 0x09, + 0x0B, 0x00, 0x0E, 0x05, 0x01, 0x0A, 0x04, 0x0F, 0x0C, 0x07, 0x09, 0x02, 0x06, 0x0D, + 0x03, 0x08, 0x05, 0x0E, 0x00, 0x0B, 0x0F, 0x04, 0x0A, 0x01, 0x02, 0x09, 0x07, 0x0C, + 0x08, 0x03, 0x0D, 0x06, + }; + + uint8_t crc = 0; + for (const auto byte : bytes) { + auto index = byte ^ crc; + crc = CRC4_LOOKUP[index]; + crc &= 0xF; + } + + mBuilder.bits(crc, 4); + } + + // TF007 - Message Subtype + inline void tf007(uint8_t type) { mBuilder.bits(type, 4); } + + // TF008 - Time tag type + inline void tf008(bool full_time) { mBuilder.b(full_time); } + + // TF009 - GNSS time tag + inline void tf009_16bit(uint16_t time) { mBuilder.u16(time); } + inline void tf009_32bit(uint32_t time) { mBuilder.u32(time); } + + // TF010 - Solution ID + inline void tf010(uint8_t solution_id) { mBuilder.bits(solution_id, 7); } + + // TF011 - + inline void tf011(uint8_t solution_processor_id) { mBuilder.bits(solution_processor_id, 4); } + + // TF016 - + inline void tf016(std::vector payload) { + // TODO(ewasjon): [low-priority] This is really inefficient, we should be able to memcpy + // most of the payload data. + for (auto byte : payload) { + mBuilder.u8(byte); + } + } + + // TF018 - Message CRC + inline void tf018_16bit(uint16_t crc) { mBuilder.u16(crc); } + +private: + Builder mBuilder; +}; + +class MessageBuilder { +public: + SPARTN_EXPLICIT MessageBuilder(uint8_t message_type, uint8_t message_subtype, + uint32_t message_time); + + generator::spartn::Message build(); + + // SF005 - Solution issue of update (SIOU) + inline void sf005(uint16_t siou) { mBuilder.bits(siou, 9); } + + // SF008 - Yaw present flag + inline void sf008(bool present) { mBuilder.b(present); } + + // SF009 - Satellite reference datum + inline void sf009(uint8_t datum) { mBuilder.bits(datum, 1); } + + // SF010 - End of OCB set + inline void sf010(bool eos) { mBuilder.b(eos); } + + // Satellite Mask: (SF011, SF012, ...) + void satellite_mask(long gnss_id, + const std::vector& satellites); + void satellite_mask(long gnss_id, + const std::vector& satellites); + void satellite_mask(long gnss_id, uint64_t count, bool* bits); + + // SF013 - Do not use (DNU) + inline void sf013(bool dnu) { mBuilder.b(dnu); } + + // SF014 - OCB present flags + inline void sf014(bool orbit, bool clock, bool bias) { + mBuilder.b(orbit); + mBuilder.b(clock); + mBuilder.b(bias); + } + + // SF015 - Continuity Indicator + inline void sf015(double seconds) { + if (seconds >= 320) + mBuilder.bits(7, 3); + else if (seconds >= 120) + mBuilder.bits(6, 3); + else if (seconds >= 60) + mBuilder.bits(5, 3); + else if (seconds >= 30) + mBuilder.bits(4, 3); + else if (seconds >= 10) + mBuilder.bits(3, 3); + else if (seconds >= 5) + mBuilder.bits(2, 3); + else if (seconds >= 1) + mBuilder.bits(1, 3); + else + mBuilder.bits(0, 3); + } + + // Ephemeris Type + void ephemeris_type(long gnss_id); + + // Issue of data ephemeris + void orbit_iode(long gnss_id, BIT_STRING_s& iode, bool iode_shift); + + // SF020 - Satellite corrections + inline void sf020(double value) { mBuilder.double_to_bits(-16.382, 16.382, 0.002, value, 14); } + + // SF021 - Satellite yaw + inline void sf021(double value) { mBuilder.double_to_bits(0, 354, 6, value, 6); } + + // SF022 - IODE continuity + inline void sf022(double seconds) { + if (seconds >= 320) + mBuilder.bits(7, 3); + else if (seconds >= 120) + mBuilder.bits(6, 3); + else if (seconds >= 60) + mBuilder.bits(5, 3); + else if (seconds >= 30) + mBuilder.bits(4, 3); + else if (seconds >= 10) + mBuilder.bits(3, 3); + else if (seconds >= 5) + mBuilder.bits(2, 3); + else if (seconds >= 1) + mBuilder.bits(1, 3); + else + mBuilder.bits(0, 3); + } + + // SF023 - Fix flag (float=0, fixed=1) + inline void sf023(bool fix_or_float) { mBuilder.b(fix_or_float); } + + // SF024 - User range error (URE) + inline void sf024_raw(uint8_t ure) { mBuilder.bits(ure, 3); } + inline void sf024(double ure) { + if (ure <= 0.00) + sf024_raw(0); + else if (ure <= 0.01) + sf024_raw(1); + else if (ure <= 0.02) + sf024_raw(2); + else if (ure <= 0.05) + sf024_raw(3); + else if (ure <= 0.10) + sf024_raw(4); + else if (ure <= 0.30) + sf024_raw(5); + else if (ure <= 1.00) + sf024_raw(6); + else + sf024_raw(7); + } + + // SFXXX - General bias mask + inline void sfxxx_bias_mask_raw(uint8_t low, uint8_t high, bool extended, uint32_t mask) { + mBuilder.b(extended); + if (extended) { + mBuilder.bits(mask, high); + } else { + mBuilder.bits(mask, low); + } + } + + template + inline void sfxxx_bias_mask(uint8_t low, uint8_t high, const std::map* types) { + uint8_t size = 0; + if (types->size() > low) { + size = high; + mBuilder.b(true); + } else { + size = low; + mBuilder.b(false); + } + + for (uint8_t i = 0; i < size; ++i) { + mBuilder.b(types->count(i) > 0); + } + } + + // SF025 - GPS phase bias mask + inline void sf025_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(6, 11, extended, mask); + } + + template + inline void sf025(const std::map* types) { + sfxxx_bias_mask(6, 11, types); + } + + // SF026 - GLONASS phase bias mask + inline void sf026_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(5, 9, extended, mask); + } + + template + inline void sf026(const std::map* types) { + sfxxx_bias_mask(5, 9, types); + } + + // SF027 - GPS code bias mask + inline void sf027_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(6, 11, extended, mask); + } + + template + inline void sf027(const std::map* types) { + sfxxx_bias_mask(6, 11, types); + } + + // SF028 - GLONASS code bias mask + inline void sf028_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(5, 9, extended, mask); + } + + template + inline void sf028(const std::map* types) { + sfxxx_bias_mask(5, 9, types); + } + + // SF029 - Code bias correction + inline void sf029(double value) { mBuilder.double_to_bits(-20.46, 20.46, 0.02, value, 11); } + + // SF030 - Area Count + inline void sf030(uint8_t count) { + assert(count >= 1); + assert(count <= 32); + mBuilder.bits(count - 1, 5); + } + + // SF031 - Area ID + inline void sf031(uint8_t id) { mBuilder.bits(id, 8); } + + // SF032 - Area reference latitude + inline void sf032(double latitude) { mBuilder.double_to_bits(-90, 90, 0.1, latitude, 11); } + + // SF033 - Area reference longitude + inline void sf033(double longitude) { mBuilder.double_to_bits(-180, 180, 0.1, longitude, 12); } + + // SF034 - Area latitude grid node count + inline void sf034(uint8_t count) { + assert(count >= 1); + assert(count <= 8); + mBuilder.bits(count - 1, 3); + } + + // SF035 - Area longitude grid node count + inline void sf035(uint8_t count) { + assert(count >= 1); + assert(count <= 8); + mBuilder.bits(count - 1, 3); + } + + // SF036 - Area latitude grid node spacing + inline void sf036(double spacing) { mBuilder.double_to_bits(0.1, 3.2, 0.1, spacing, 5); } + + // SF037 - Area longitude grid node spacing + inline void sf037(double spacing) { mBuilder.double_to_bits(0.1, 3.2, 0.1, spacing, 5); } + + // SF039 - Number of grid points present + inline void sf039(uint8_t count) { + assert(count >= 0); + assert(count <= 127); + mBuilder.bits(count, 7); + } + + // SF040 - Poly/Grid block present indicator + inline void sf040(uint8_t type) { mBuilder.bits(type, 2); } + + // SF041 - Troposphere equation type + inline void sf041(uint8_t type) { mBuilder.bits(type, 3); } + + // SF042 - Troposphere quality + inline void sf042_raw(uint8_t quality) { mBuilder.bits(quality, 3); } + inline void sf042(double quality) { + if (quality <= 0.010) + sf042_raw(1); + else if (quality <= 0.020) + sf042_raw(2); + else if (quality <= 0.040) + sf042_raw(3); + else if (quality <= 0.080) + sf042_raw(4); + else if (quality <= 0.160) + sf042_raw(5); + else if (quality <= 0.320) + sf042_raw(6); + else + sf042_raw(7); + } + + // SF043 - Area average vertical hydrostatic delay + inline void sf043(double delay) { mBuilder.double_to_bits(-0.508, 0.505, 0.004, delay, 8); } + + // SF044 - Troposphere polynomial coefficient size indicator + 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); } + + // 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); } + + // SF047 - Small troposphere coefficient T_11 + inline void sf047(double value) { 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); } + + // 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); } + + // SF050 - Large troposphere coefficient T_11 + inline void sf050(double value) { 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); } + + // SF052 - Small troposphere residual zenith delay + inline void sf052_invalid() { mBuilder.bits(0x3F, 6); } + inline void sf052(double value) { mBuilder.double_to_bits(-0.124, 0.124, 0.004, value, 6); } + + // SF053 - Large troposphere residual zenith delay + inline void sf053_invalid() { mBuilder.bits(0xFF, 8); } + inline void sf053(double value) { mBuilder.double_to_bits(-0.508, 0.508, 0.004, value, 8); } + + // SF054 - Ionosphere equation type + inline void sf054(int type) { mBuilder.bits(type, 3); } + + // SF055 - Ionosphere quality + inline void sf055_raw(int value) { mBuilder.bits(value, 4); } + inline void sf055_invalid() { sf055_raw(0); } + inline void sf055(double quality) { + if (quality <= 0.03) + sf055_raw(1); + else if (quality <= 0.05) + sf055_raw(2); + else if (quality <= 0.07) + sf055_raw(3); + else if (quality <= 0.14) + sf055_raw(4); + else if (quality <= 0.28) + sf055_raw(5); + else if (quality <= 0.56) + sf055_raw(6); + else if (quality <= 1.12) + sf055_raw(7); + else if (quality <= 2.24) + sf055_raw(8); + else if (quality <= 4.48) + sf055_raw(9); + else if (quality <= 8.96) + sf055_raw(10); + else if (quality <= 17.92) + sf055_raw(11); + else if (quality <= 35.84) + sf055_raw(12); + else if (quality <= 71.68) + sf055_raw(13); + else if (quality <= 143.36) + sf055_raw(14); + else + sf055_raw(15); + } + + // SF056 - Ionosphere polynomial coefficient size indicator + inline void sf056(bool large_coefficient) { mBuilder.b(large_coefficient); } + + // SF057 - Small ionosphere coefficient C00 + inline void sf057(double value) { mBuilder.double_to_bits(-81.88, 81.88, 0.04, value, 12); } + + // SF058 - Small ionosphere coefficient C10/C01 + inline void sf058(double value) { mBuilder.double_to_bits(-16.376, 16.376, 0.008, value, 12); } + + // SF059 - Small ionosphere coefficient C11 + inline void sf059(double value) { mBuilder.double_to_bits(-8.190, 8.190, 0.002, value, 13); } + + // SF060 - Large ionosphere coefficient C00 + inline void sf060(double value) { mBuilder.double_to_bits(-327.64, 327.64, 0.04, value, 14); } + + // SF061 - Large ionosphere coefficient C10/C01 + inline void sf061(double value) { mBuilder.double_to_bits(-65.528, 65.528, 0.008, value, 14); } + + // SF062 - Large ionosphere coefficient C11 + inline void sf062(double value) { mBuilder.double_to_bits(-32.766, 32.766, 0.002, value, 15); } + + // SF0XX - Ionosphere coefficient C00 + inline void ionosphere_coefficient_c00(bool large_coefficient, double value) { + if (large_coefficient) + sf060(value); + else + sf057(value); + } + + // SF0XX - Ionosphere coefficient C10/C01 + inline void ionosphere_coefficient_c10_c01(bool large_coefficient, double value) { + if (large_coefficient) + sf061(value); + else + sf058(value); + } + + // SF0XX - Ionosphere coefficient C11 + inline void ionosphere_coefficient_c11(bool large_coefficient, double value) { + if (large_coefficient) + sf062(value); + else + sf059(value); + } + + // SF063 - Ionosphere residual field size + inline void sf063(uint8_t size) { mBuilder.bits(size, 2); } + + // SF064 - Small ionosphere residual slant delay + inline void sf064_invalid() { mBuilder.bits(0xF, 4); } + inline void sf064(double value) { mBuilder.double_to_bits(-0.28, 0.28, 0.04, value, 4); } + + // SF065 - Medium ionosphere residual slant delay + inline void sf065_invalid() { mBuilder.bits(0x7F, 7); } + inline void sf065(double value) { mBuilder.double_to_bits(-2.52, 2.52, 0.04, value, 7); } + + // SF066 - Large ionosphere residual slant delay + inline void sf066_invalid() { mBuilder.bits(0x3FF, 10); } + inline void sf066(double value) { mBuilder.double_to_bits(-20.44, 20.44, 0.04, value, 10); } + + // SF067 - Extra-large ionosphere residual slant delay + inline void sf067_invalid() { mBuilder.bits(0x3FFF, 14); } + inline void sf067(double value) { mBuilder.double_to_bits(-327.64, 327.64, 0.04, value, 14); } + + // SF0XX - Ionosphere residual slant delay + inline void ionosphere_residual(uint8_t size, double value) { + switch (size) { + case 0: sf064(value); break; + case 1: sf065(value); break; + case 2: sf066(value); break; + case 3: sf067(value); break; + default: SPARTN_UNREACHABLE(); + } + } + + inline void ionosphere_residual_invalid(uint8_t size) { + switch (size) { + case 0: sf064_invalid(); break; + case 1: sf065_invalid(); break; + case 2: sf066_invalid(); break; + case 3: sf067_invalid(); break; + default: SPARTN_UNREACHABLE(); + } + } + + // SF068 - Area Issue of Update (AIOU) + inline void sf068(uint8_t aiou) { mBuilder.bits(aiou, 4); } + + // SF069 - Reserved + inline void sf069() { mBuilder.bits(0, 1); } + + // SF102 - Galileo phase bias mask + inline void sf102_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(8, 15, extended, mask); + } + + template + inline void sf102(const std::map* types) { + sfxxx_bias_mask(8, 15, types); + } + + // SF103 - BDS phase bias mask + inline void sf103_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(8, 15, extended, mask); + } + + template + inline void sf103(const std::map* types) { + sfxxx_bias_mask(8, 15, types); + } + + // SF104 - QZSS phase bias mask + inline void sf104_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(6, 11, extended, mask); + } + + template + inline void sf104(const std::map* types) { + sfxxx_bias_mask(6, 11, types); + } + + // SF105 - Galileo code bias mask + inline void sf105_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(8, 15, extended, mask); + } + + template + inline void sf105(const std::map* types) { + sfxxx_bias_mask(8, 15, types); + } + + // SF106 - BDS code bias mask + inline void sf106_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(8, 15, extended, mask); + } + + template + inline void sf106(const std::map* types) { + sfxxx_bias_mask(8, 15, types); + } + + // SF107 - QZSS code bias mask + inline void sf107_raw(bool extended, uint32_t mask) { + sfxxx_bias_mask_raw(6, 11, extended, mask); + } + + template + inline void sf107(const std::map* types) { + sfxxx_bias_mask(6, 11, types); + } + +private: + uint8_t mMessageType; + uint8_t mMessageSubtype; + uint32_t mMessageTime; + Builder mBuilder; +}; diff --git a/generator/spartn2/ocb.cpp b/generator/spartn2/ocb.cpp new file mode 100644 index 00000000..b9278ffb --- /dev/null +++ b/generator/spartn2/ocb.cpp @@ -0,0 +1,855 @@ +#include "data.hpp" +#include "decode.hpp" +#include "generator.hpp" +#include "message.hpp" +#include "time.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace generator { +namespace spartn { + +uint32_t OcbSatellite::prn() const { + // NOTE(ewasjon): 3GPP LPP defines PRN starting at 0 instead of 1. + return id + 1; +} + +void OcbSatellite::add_correction(SSR_OrbitCorrectionSatelliteElement_r15* orbit) { + if (!orbit) return; + this->orbit = orbit; +} + +void OcbSatellite::add_correction(SSR_ClockCorrectionSatelliteElement_r15* clock) { + if (!clock) return; + this->clock = clock; +} + +void OcbSatellite::add_correction(SSR_CodeBiasSatElement_r15* code_bias) { + if (!code_bias) return; + this->code_bias = code_bias; +} + +void OcbSatellite::add_correction(SSR_PhaseBiasSatElement_r16* phase_bias) { + if (!phase_bias) return; + this->phase_bias = phase_bias; +} + +void OcbSatellite::add_correction(SSR_URA_SatElement_r16* ura) { + if (!ura) return; + this->ura = ura; +} + +std::vector OcbCorrections::satellites() const { + std::unordered_map satellites; + + // Orbit + 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; + + auto satellite_id = element->svID_r15.satellite_id; + auto& satellite = satellites[satellite_id]; + satellite.id = satellite_id; + satellite.iod = iod; + satellite.add_correction(element); + } + } + + // Clock + 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; + + auto satellite_id = element->svID_r15.satellite_id; + auto& satellite = satellites[satellite_id]; + satellite.id = satellite_id; + satellite.iod = iod; + satellite.add_correction(element); + } + } + + // Code bias + 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; + + auto satellite_id = element->svID_r15.satellite_id; + auto& satellite = satellites[satellite_id]; + satellite.id = satellite_id; + satellite.iod = iod; + satellite.add_correction(element); + } + } + + // Phase bias + 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; + + auto satellite_id = element->svID_r16.satellite_id; + auto& satellite = satellites[satellite_id]; + satellite.id = satellite_id; + satellite.iod = iod; + satellite.add_correction(element); + } + } + + // URA + 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; + + auto satellite_id = element->svID_r16.satellite_id; + auto& satellite = satellites[satellite_id]; + satellite.id = satellite_id; + satellite.iod = iod; + satellite.add_correction(element); + } + } + + // Convert to vector + std::vector result; + for (auto& kv : satellites) { + result.push_back(kv.second); + } + + // Sort by satellite id + std::sort(result.begin(), result.end(), [](const OcbSatellite& a, const OcbSatellite& b) { + return a.id < b.id; + }); + + return result; +} + +void CorrectionData::add_correction(long gnss_id, GNSS_SSR_OrbitCorrections_r15* orbit) { + if (!orbit) return; + auto iod = orbit->iod_ssr_r15; + auto& ocb = mOcbData[iod]; + + // TODO(ewasjon): [low-priority] Filter based on satellite reference datum. + auto epoch_time = spartn_time_from(orbit->epochTime_r15); + auto key = OcbKey{gnss_id, group_by_epoch_time ? epoch_time.rounded_seconds : 0}; + + auto& corrections = ocb.mKeyedCorrections[key]; + corrections.gnss_id = gnss_id; + corrections.iod = iod; + corrections.epoch_time = epoch_time; + corrections.orbit = orbit; +} + +void CorrectionData::add_correction(long gnss_id, GNSS_SSR_ClockCorrections_r15* clock) { + if (!clock) return; + auto iod = clock->iod_ssr_r15; + auto& ocb = mOcbData[iod]; + + auto epoch_time = spartn_time_from(clock->epochTime_r15); + auto key = OcbKey{gnss_id, group_by_epoch_time ? epoch_time.rounded_seconds : 0}; + + auto& corrections = ocb.mKeyedCorrections[key]; + corrections.gnss_id = gnss_id; + corrections.iod = iod; + corrections.epoch_time = epoch_time; + corrections.clock = clock; +} + +void CorrectionData::add_correction(long gnss_id, GNSS_SSR_CodeBias_r15* code_bias) { + if (!code_bias) return; + auto iod = code_bias->iod_ssr_r15; + auto& ocb = mOcbData[iod]; + + auto epoch_time = spartn_time_from(code_bias->epochTime_r15); + auto key = OcbKey{gnss_id, group_by_epoch_time ? epoch_time.rounded_seconds : 0}; + + auto& corrections = ocb.mKeyedCorrections[key]; + corrections.gnss_id = gnss_id; + corrections.iod = iod; + corrections.epoch_time = epoch_time; + corrections.code_bias = code_bias; +} + +void CorrectionData::add_correction(long gnss_id, GNSS_SSR_PhaseBias_r16* phase_bias) { + if (!phase_bias) return; + auto iod = phase_bias->iod_ssr_r16; + auto& ocb = mOcbData[iod]; + + auto epoch_time = spartn_time_from(phase_bias->epochTime_r16); + auto key = OcbKey{gnss_id, group_by_epoch_time ? epoch_time.rounded_seconds : 0}; + + auto& corrections = ocb.mKeyedCorrections[key]; + corrections.gnss_id = gnss_id; + corrections.iod = iod; + corrections.epoch_time = epoch_time; + corrections.phase_bias = phase_bias; +} + +void CorrectionData::add_correction(long gnss_id, GNSS_SSR_URA_r16* ura) { + if (!ura) return; + auto iod = ura->iod_ssr_r16; + auto& ocb = mOcbData[iod]; + + auto epoch_time = spartn_time_from(ura->epochTime_r16); + auto key = OcbKey{gnss_id, group_by_epoch_time ? epoch_time.rounded_seconds : 0}; + + auto& corrections = ocb.mKeyedCorrections[key]; + corrections.gnss_id = gnss_id; + corrections.iod = iod; + corrections.epoch_time = epoch_time; + corrections.ura = ura; +} + +struct Bias { + long signal_id; + double correction; + double continuity_indicator; + bool fix_flag; + uint8_t type; + bool mapped; + + 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) { + 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) { + 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) { + return Bias::invalid(); + } + + auto from_freq = GAL_FREQ[from_id]; + auto to_freq = GAL_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); +#endif + + return gal_bias_from_signal(to_id, shifted_correction, continuity_indicator, fix_flag); + } + + return Bias{signal_id, correction, continuity_indicator, fix_flag, type, false}; +} + +#undef X + +static bool phase_bias_fix_flag(const SSR_PhaseBiasSignalElement_r16& signal) { + if (!signal.phaseBiasIntegerIndicator_r16) { + // TODO(ewasjon): What should we do here if the fix flag information is missing? The + // original SPARTN implementation used a default value of 'true'. + return true; + } + + switch (*signal.phaseBiasIntegerIndicator_r16) { + case 0: return true; + case 1: return true; + case 2: return false; + default: return false; + } +} + +template +static std::map phase_biases(const SSR_PhaseBiasSatElement_r16& satellite, + BiasToSignal* bias_to_signal) { + std::map biases_by_type; + + auto& list = satellite.ssr_PhaseBiasSignalList_r16.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + + auto signal_id = decode::signal_id(element->signal_and_tracking_mode_ID_r16); + auto correction = decode::phaseBias_r16(element->phaseBias_r16); + auto continuity_indicator = + 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); + if (bias.signal_id == -1) { +#ifdef SPARTN_DEBUG_PRINT + printf(" unsupported bias for signal id %2ld\n", signal_id); +#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); +#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); +#endif + biases_by_type[bias.type] = bias; + } else { +#ifdef SPARTN_DEBUG_PRINT + printf(" duplicate bias type %u\n", bias.type); +#endif + } + } + + return biases_by_type; +} + +template +static std::map code_biases(const SSR_CodeBiasSatElement_r15& satellite, + BiasToSignal* bias_to_signal) { + std::map biases_by_type; + + auto& list = satellite.ssr_CodeBiasSignalList_r15.list; + for (int i = 0; i < list.count; i++) { + auto element = list.array[i]; + if (!element) continue; + + 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); + if (bias.signal_id == -1) { +#ifdef SPARTN_DEBUG_PRINT + printf(" unsupported bias for signal id %2ld\n", signal_id); +#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); +#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); +#endif + biases_by_type[bias.type] = bias; + } else { +#ifdef SPARTN_DEBUG_PRINT + printf(" duplicate bias type %u\n", bias.type); +#endif + } + } + + return biases_by_type; +} + +static void generate_gps_bias_block(MessageBuilder& builder, + const SSR_CodeBiasSatElement_r15* code_bias, + const SSR_PhaseBiasSatElement_r16* phase_bias) { + if (!phase_bias) { + builder.sf025_raw(false, 0); + } else { + auto types_of_biases = phase_biases(*phase_bias, &gps_bias_from_signal); + + builder.sf025(&types_of_biases); + for (auto& kvp : types_of_biases) { + builder.sf023(kvp.second.fix_flag); + builder.sf015(kvp.second.continuity_indicator); + builder.sf020(kvp.second.correction); + } + } + + if (!code_bias) { + builder.sf027_raw(false, 0); + } else { + auto types_of_biases = code_biases(*code_bias, &gps_bias_from_signal); + + builder.sf027(&types_of_biases); + for (auto& kvp : types_of_biases) { + builder.sf029(kvp.second.correction); + } + } +} + +static void generate_glo_bias_block(MessageBuilder& builder, + const SSR_CodeBiasSatElement_r15* code_bias, + const SSR_PhaseBiasSatElement_r16* phase_bias) { + if (!phase_bias) { + builder.sf026_raw(false, 0); + } else { + auto types_of_biases = phase_biases(*phase_bias, &glo_bias_from_signal); + + builder.sf026(&types_of_biases); + for (auto& kvp : types_of_biases) { + builder.sf023(kvp.second.fix_flag); + builder.sf015(kvp.second.continuity_indicator); + builder.sf020(kvp.second.correction); + } + } + + if (!code_bias) { + builder.sf028_raw(false, 0); + } else { + auto types_of_biases = code_biases(*code_bias, &glo_bias_from_signal); + + builder.sf028(&types_of_biases); + for (auto& kvp : types_of_biases) { + builder.sf029(kvp.second.correction); + } + } +} + +static void generate_gal_bias_block(MessageBuilder& builder, + const SSR_CodeBiasSatElement_r15* code_bias, + const SSR_PhaseBiasSatElement_r16* phase_bias) { + if (!phase_bias) { + builder.sf102_raw(false, 0); + } else { + auto types_of_biases = phase_biases(*phase_bias, &gal_bias_from_signal); + + builder.sf102(&types_of_biases); + for (auto& kvp : types_of_biases) { + builder.sf023(kvp.second.fix_flag); + builder.sf015(kvp.second.continuity_indicator); + builder.sf020(kvp.second.correction); + } + } + + if (!code_bias) { + builder.sf105_raw(false, 0); + } else { + auto types_of_biases = code_biases(*code_bias, &gal_bias_from_signal); + + builder.sf105(&types_of_biases); + for (auto& kvp : types_of_biases) { + builder.sf029(kvp.second.correction); + } + } +} + +void Generator::generate_ocb(long iod) { + auto ocb_data = mCorrectionData->ocb(iod); + if (!ocb_data) return; + + std::vector messages; + for (auto& kvp : ocb_data->mKeyedCorrections) { + if (!mGpsSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_gps) continue; + if (!mGlonassSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_glonass) continue; + if (!mGalileoSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_galileo) continue; + if (!mBeidouSupported && kvp.first.gnss_id == GNSS_ID__gnss_id_bds) continue; + + messages.push_back(&kvp.second); + } + + std::sort(messages.begin(), messages.end(), + [](const OcbCorrections* a, const OcbCorrections* b) { + return subtype_from_gnss_id(a->gnss_id) < subtype_from_gnss_id(b->gnss_id); + }); + + for (size_t message_id = 0; message_id < messages.size(); message_id++) { + auto& corrections = *messages[message_id]; + auto epoch_time = corrections.epoch_time.rounded_seconds; + auto gnss_id = corrections.gnss_id; + + auto satellites = corrections.satellites(); + +#ifdef SPARTN_DEBUG_PRINT + printf("OCB: time=%u, gnss=%ld, iod=%ld\n", epoch_time, gnss_id, iod); + for (auto& satellite : satellites) { + printf(" satellite: %4ld ", satellite.id); + if (satellite.orbit) { + printf("O"); + } else { + printf("-"); + } + if (satellite.clock) { + printf("C"); + } else { + printf("-"); + } + if (satellite.code_bias) { + printf("B"); + } else { + printf("-"); + } + if (satellite.phase_bias) { + printf("P"); + } else { + printf("-"); + } + if (satellite.ura) { + printf("U"); + } else { + printf("-"); + } + printf("\n"); + } +#endif + + auto eos = (message_id + 1) == messages.size(); + auto yaw_angle_present = false; + auto subtype = subtype_from_gnss_id(gnss_id); + + MessageBuilder builder{0 /* OCB */, subtype, epoch_time}; + builder.sf005(iod); + builder.sf010(eos); + builder.sf069(); + builder.sf008(yaw_angle_present); + builder.sf009(0 /* ITRF */); // we assume that satellite reference datum is ITRF + builder.ephemeris_type(gnss_id); + builder.satellite_mask(gnss_id, satellites); + + for (auto& satellite : satellites) { +#ifdef SPARTN_DEBUG_PRINT + printf(" SATELLITE: %4ld\n", satellite.id); +#endif + + builder.sf013(false /* do use this satellite */); + builder.sf014(satellite.orbit != nullptr, satellite.clock != nullptr, + satellite.code_bias != nullptr || satellite.phase_bias != nullptr); + if (mContinuityIndicator >= 0.0) { + builder.sf022(mContinuityIndicator); + } else { + builder.sf015(320.0); // TODO(ewasjon): compute the continuity indicator + } + + if (satellite.orbit) { + auto& orbit = *satellite.orbit; + builder.orbit_iode(gnss_id, orbit.iod_r15, mIodeShift); + + auto radial = decode::delta_radial_r15(orbit.delta_radial_r15); + auto along = decode::delta_AlongTrack_r15(orbit.delta_AlongTrack_r15); + auto cross = decode::delta_CrossTrack_r15(orbit.delta_CrossTrack_r15); + builder.sf020(radial); + builder.sf020(along); + builder.sf020(cross); + if (yaw_angle_present) { + // NOTE(ewasjon): Yaw angle is assumed to be zero, 3GPP LPP inherits this + // assumption from CLAS specification. + builder.sf021(0.0 /* 0 degrees */); + } + } + + if (satellite.clock) { + auto& clock = *satellite.clock; + if (mContinuityIndicator >= 0.0) { + builder.sf022(mContinuityIndicator); + } else { + // TODO(ewasjon): [low-priority] Compute the continuity indicator. + builder.sf022(320.0); + } + + // NOTE(ewasjon): SPARTN has a single value of the clock correction term. 3GPP + // LPP has model this as an polynomial around the middle of the ssr update rate. + // Thus, the single value at epoch time must be computed. + + auto c0 = decode::delta_Clock_C0_r15(clock.delta_Clock_C0_r15); + auto c1 = decode::delta_Clock_C2_r15(clock.delta_Clock_C1_r15); + 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 + // 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 + // possible for us to have access to that? If not, we will continue to use t=t0. + auto t = t0; + + // delta_c = c_0 + c_1 (t - t_0) + [c_2 (t - t_0)]^2 + auto dt = t - t0; + auto dc = c0 + c1 * dt + c2 * dt * dt; + + // NOTE(ewasjon): [REDACTED] observed that changing the sign of the clock + // corrections (in there correction feed) improved the result. They assumed that + // u-Blox implemented it with a flipped sign. Thus, we also need to flip the sign to + // conform to the u-Blox implementation. + if (mUBloxClockCorrection) { + dc *= -1; + } + + builder.sf020(dc); + + if (mUraOverride >= 0) { + builder.sf024_raw(mUraOverride > 7 ? 7 : mUraOverride); + } else if (satellite.ura) { + auto& ura = *satellite.ura; + auto value = decode::ssr_URA_r16(ura.ssr_URA_r16); + builder.sf024(value); + } else { + builder.sf024_raw(0); + } + } + + 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); + break; + case GNSS_ID__gnss_id_glonass: + generate_glo_bias_block(builder, satellite.code_bias, satellite.phase_bias); + break; + case GNSS_ID__gnss_id_galileo: + generate_gal_bias_block(builder, satellite.code_bias, satellite.phase_bias); + break; + default: SPARTN_UNREACHABLE(); + } + } + } + + mMessages.push_back(builder.build()); + } +} + +} // namespace spartn +} // namespace generator diff --git a/generator/spartn2/time.cpp b/generator/spartn2/time.cpp new file mode 100644 index 00000000..c71abdef --- /dev/null +++ b/generator/spartn2/time.cpp @@ -0,0 +1,41 @@ +#include "time.hpp" + +#include "GNSS-SystemTime.h" + +static SPARTN_CONSTEXPR uint32_t SECONDS_IN_DAY = 86400; +static SPARTN_CONSTEXPR uint16_t DAY_BETWEEN_1970_1980 = 3657; // Jan 6 +static SPARTN_CONSTEXPR uint16_t DAY_BETWEEN_1970_1996 = 9496; // Jan 1 +static SPARTN_CONSTEXPR uint16_t DAY_BETWEEN_1970_1999 = 10825; // Aug 2 +static SPARTN_CONSTEXPR uint16_t DAY_BETWEEN_1970_2006 = 13149; // Jan 1 +static SPARTN_CONSTEXPR uint16_t DAY_BETWEEN_1970_2010 = 14610; // Jan 1 + +uint64_t standard_day_number(const long gnss_id, const long day_number) { + switch (gnss_id) { + case GNSS_ID__gnss_id_gps: return day_number + DAY_BETWEEN_1970_1980; + case GNSS_ID__gnss_id_galileo: return day_number + DAY_BETWEEN_1970_1999; + case GNSS_ID__gnss_id_glonass: return day_number + DAY_BETWEEN_1970_1996; + case GNSS_ID__gnss_id_bds: return day_number + DAY_BETWEEN_1970_2006; + default: SPARTN_UNREACHABLE(); + } +} + +SpartnTime spartn_time_from(const GNSS_SystemTime& epoch_time) { + auto time_of_day = static_cast(epoch_time.gnss_TimeOfDay); + if (epoch_time.gnss_TimeOfDayFrac_msec) { + time_of_day += static_cast(*epoch_time.gnss_TimeOfDayFrac_msec) / 1000.0; + } + + auto day_number = epoch_time.gnss_DayNumber; + auto gnss_id = epoch_time.gnss_TimeID.gnss_id; + auto days_since_1970 = standard_day_number(gnss_id, day_number); + auto days_since_2010 = days_since_1970 - DAY_BETWEEN_1970_2010; + + auto seconds_since_2010 = days_since_2010 * SECONDS_IN_DAY; + auto rounded_time_of_day = static_cast(time_of_day + 0.5); + auto seconds = static_cast(seconds_since_2010) + rounded_time_of_day; + + return SpartnTime{ + static_cast(seconds_since_2010) + time_of_day, + seconds, + }; +} diff --git a/generator/spartn2/time.hpp b/generator/spartn2/time.hpp new file mode 100644 index 00000000..9b0bed89 --- /dev/null +++ b/generator/spartn2/time.hpp @@ -0,0 +1,10 @@ +#pragma once +#include + +struct SpartnTime { + double seconds; + uint32_t rounded_seconds; +}; + +struct GNSS_SystemTime; +SpartnTime spartn_time_from(const GNSS_SystemTime& epoch_time); diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt index b68dcfcf..44eb02a0 100644 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -3,6 +3,7 @@ add_library(dependency_interface STATIC "file.cpp" "serial.cpp" "i2c.cpp" + "stdin.cpp" "stdout.cpp" "file_descriptor.cpp" "socket.cpp" diff --git a/interface/README.md b/interface/README.md index b1b751d7..1c5ff576 100644 --- a/interface/README.md +++ b/interface/README.md @@ -5,4 +5,5 @@ This folder contains the code for the interface library. The interface library i - File (one-way communication) - TCP - UDP -- Stdout (one-way communication) \ No newline at end of file +- Stdout (one-way communication) +- Stdin (one-way communication) \ No newline at end of file diff --git a/interface/file_descriptor.cpp b/interface/file_descriptor.cpp index 9dfc1a85..93096b5c 100644 --- a/interface/file_descriptor.cpp +++ b/interface/file_descriptor.cpp @@ -6,6 +6,7 @@ #ifdef INTERFACE_FD_DEBUG #include +#include #define FD_DEBUG(...) printf(__VA_ARGS__) #else #define FD_DEBUG(...) @@ -87,7 +88,7 @@ bool FileDescriptor::select(bool read, bool write, bool except, timeval* tv) IF_ tv ? (tv->tv_sec + tv->tv_usec / 1000000.0) : 0.0, ret, tv ? " (timeout)" : ""); if (ret < 0) { - FD_DEBUG("[fd/%6d] failed=%d\n", mFileDescriptor, errno); + FD_DEBUG("[fd/%6d] failed=%d (%s)\n", mFileDescriptor, errno, strerror(errno)); if (errno == EBADF) { mError = BAD_FILE_DESCRIPTOR; close(); @@ -104,7 +105,7 @@ size_t FileDescriptor::read(void* data, size_t length) IF_NOEXCEPT { } auto bytes_read = ::read(mFileDescriptor, data, length); - FD_DEBUG("[fd/%6d] read(%zu bytes) = %d\n", mFileDescriptor, length, bytes_read); + FD_DEBUG("[fd/%6d] read(%zu bytes) = %ld\n", mFileDescriptor, length, bytes_read); if (bytes_read < 0) { FD_DEBUG("[fd/%6d] failed=%d\n", mFileDescriptor, errno); if (errno == EBADF) { @@ -123,7 +124,7 @@ size_t FileDescriptor::write(const void* data, size_t length) IF_NOEXCEPT { } auto bytes_written = ::write(mFileDescriptor, data, length); - FD_DEBUG("[fd/%6d] write(%zu bytes) = %d\n", mFileDescriptor, length, bytes_written); + FD_DEBUG("[fd/%6d] write(%zu bytes) = %ld\n", mFileDescriptor, length, bytes_written); if (bytes_written < 0) { FD_DEBUG("[fd/%6d] failed=%d\n", mFileDescriptor, errno); if (errno == EBADF) { diff --git a/interface/include/interface/interface.hpp b/interface/include/interface/interface.hpp index c2786eec..b5339011 100644 --- a/interface/include/interface/interface.hpp +++ b/interface/include/interface/interface.hpp @@ -107,6 +107,9 @@ class Interface { /// Create a stdout interface. static Interface* stdout(); + + /// Create a stdin interface. + static Interface* stdin(); }; } // namespace interface diff --git a/interface/stdin.cpp b/interface/stdin.cpp new file mode 100644 index 00000000..fa205b29 --- /dev/null +++ b/interface/stdin.cpp @@ -0,0 +1,73 @@ +#include "stdin.hpp" +#include +#include +#include "file_descriptor.hpp" + +namespace interface { + +StdinInterface::StdinInterface() IF_NOEXCEPT : mFileDescriptor(-1) {} + +StdinInterface::~StdinInterface() IF_NOEXCEPT { + close(); +} + +void StdinInterface::open() { + if (mFileDescriptor.is_open()) { + return; + } + + auto stdin_fd = STDIN_FILENO; + mFileDescriptor = FileDescriptor(stdin_fd); +} + +void StdinInterface::close() { + mFileDescriptor.close(); +} + +size_t StdinInterface::read(void* data, const size_t size) { + return mFileDescriptor.read(data, size); +} + +size_t StdinInterface::write(const void* data, const size_t size) { + return mFileDescriptor.write(data, size); +} + +bool StdinInterface::can_read() IF_NOEXCEPT { + return mFileDescriptor.can_read(); +} + +bool StdinInterface::can_write() IF_NOEXCEPT { + return mFileDescriptor.can_write(); +} + +void StdinInterface::wait_for_read() IF_NOEXCEPT { + mFileDescriptor.wait_for_read(); +} + +void StdinInterface::wait_for_write() IF_NOEXCEPT { + mFileDescriptor.wait_for_write(); +} + +bool StdinInterface::is_open() IF_NOEXCEPT { + return mFileDescriptor.is_open(); +} + +void StdinInterface::print_info() IF_NOEXCEPT { + printf("[interface]\n"); + printf(" type: stdin\n"); + if (is_open()) { + printf(" fd: %d\n", mFileDescriptor.fd()); + } else { + printf(" fd: closed\n"); + } +} + +// +// +// + +Interface* Interface::stdin() { + return new StdinInterface(); +} + +} // namespace interface diff --git a/interface/stdin.hpp b/interface/stdin.hpp new file mode 100644 index 00000000..c8dc6461 --- /dev/null +++ b/interface/stdin.hpp @@ -0,0 +1,33 @@ +#pragma once +#include +#include +#include "file_descriptor.hpp" +#include "interface.hpp" + +namespace interface { + +class StdinInterface final : public Interface { +public: + explicit StdinInterface() IF_NOEXCEPT; + ~StdinInterface() IF_NOEXCEPT override; + + void open() override; + void close() override; + + size_t read(void* data, size_t length) override; + size_t write(const void* data, size_t length) override; + + IF_NODISCARD bool can_read() IF_NOEXCEPT override; + IF_NODISCARD bool can_write() IF_NOEXCEPT override; + + void wait_for_read() IF_NOEXCEPT override; + void wait_for_write() IF_NOEXCEPT override; + + IF_NODISCARD bool is_open() IF_NOEXCEPT override; + void print_info() IF_NOEXCEPT override; + +private: + FileDescriptor mFileDescriptor; +}; + +} // namespace interface diff --git a/libs/lpplib/CMakeLists.txt b/libs/lpplib/CMakeLists.txt index a3e65d93..14909111 100644 --- a/libs/lpplib/CMakeLists.txt +++ b/libs/lpplib/CMakeLists.txt @@ -31,3 +31,7 @@ if (USE_ASAN) target_compile_options(lpplib PRIVATE -fsanitize=address,undefined,leak) target_link_libraries(lpplib PRIVATE -fsanitize=address,undefined,leak) endif (USE_ASAN) + +if (DEBUG_LPP_LIB) +target_compile_definitions(lpplib PRIVATE "DEBUG_LPP_LIB=1") +endif (DEBUG_LPP_LIB) diff --git a/libs/lpplib/include/lpp/asn_helper.h b/libs/lpplib/include/lpp/asn_helper.h index f6a61346..e116739f 100644 --- a/libs/lpplib/include/lpp/asn_helper.h +++ b/libs/lpplib/include/lpp/asn_helper.h @@ -10,63 +10,60 @@ inline T* asn1_allocate(size_t count = 1) { #include -class BitString : public BIT_STRING_s { +class BitStringBuilder { public: - explicit BitString(size_t bits); + explicit BitStringBuilder() { mBits = 0; } - void set_bit(ssize_t); - void clear_bit(ssize_t); - bool get_bit(ssize_t); - void set_integer(size_t, size_t, size_t); - - int64_t as_int64(); - - std::string as_string(); - - static BitString* allocate(size_t bits) { - auto data = asn1_allocate(); - return allocate(bits, data); + template + BitStringBuilder& set(T index) { + assert(index < 64); + mBits |= 1llu << index; + return *this; } - static BitString* allocate(size_t bits, BIT_STRING_s* inner) { - auto bit_string = reinterpret_cast(inner); - bit_string->initialize(bits); - return bit_string; + template + BitStringBuilder& clear(T index) { + assert(index < 64); + mBits &= ~(1llu << index); + return *this; } - static BitString* from(BIT_STRING_s* inner) { - auto bit_string = reinterpret_cast(inner); - return bit_string; + template + BitStringBuilder& integer(T index, T bits, uint64_t value) { + // NOTE(ewasjon): A bit string is numbered from left to right, so the + // first bit is the most significant bit. This is the opposite of how + // we usually number bits in C, so we need to reverse the order of the + // bits. + for (T i = 0; i < bits; i++) { + auto bit = bits - i - 1; + if (value & (1llu << bit)) { + set(index + i); + } else { + clear(index + i); + } + } + return *this; } - void destroy(); + BIT_STRING_s* to_bit_string(size_t bits); + BIT_STRING_s* into_bit_string(size_t bits, BIT_STRING_s* bit_string); private: - void initialize(size_t bits); - - struct Index { - size_t byte_index; - size_t local_bit; - }; - - Index bit_index(ssize_t index); + uint64_t mBits; }; -static_assert(sizeof(BitString) == sizeof(BIT_STRING_s), "BitString must be the same size as BIT_STRING_s"); - -void supl_fill_tracking_area_code(TrackingAreaCode_t* tac, int tac_value); -void supl_fill_cell_identity(CellIdentity_t*, size_t value); -MCC* supl_create_mcc(int mcc_value); -void supl_fill_mcc(MCC* mcc, int mcc_value); -void supl_fill_mnc(MNC* mnc, int mnc_value); -ECGI* ecgi_create(long mcc, long mnc, long id); +void supl_fill_tracking_area_code(TrackingAreaCode_t* tac, int tac_value); +void supl_fill_cell_identity(CellIdentity_t*, size_t value); +MCC* supl_create_mcc(int mcc_value); +void supl_fill_mcc(MCC* mcc, int mcc_value); +void supl_fill_mnc(MNC* mnc, int mnc_value); +ECGI* ecgi_create(long mcc, long mnc, long id); double long_pointer2scaled_double(long* ptr, double def, double arg); double long_pointer(long* ptr, long def); -long gnss2long(GNSS_SignalID_t gnss_id); +long gnss2long(GNSS_SignalID_t gnss_id); inline long* newLong(long value) { long* x = ALLOC_ZERO(long); *x = value; return x; } - diff --git a/libs/lpplib/include/lpp/asnlib.h b/libs/lpplib/include/lpp/asnlib.h index 09bc60fd..1c9844f6 100644 --- a/libs/lpplib/include/lpp/asnlib.h +++ b/libs/lpplib/include/lpp/asnlib.h @@ -15,6 +15,8 @@ #include #include #include +#include +#include typedef BIT_STRING_t BIT_STRING; typedef OCTET_STRING_t OCTET_STRING; diff --git a/libs/lpplib/include/lpp/location_information.h b/libs/lpplib/include/lpp/location_information.h index e9f46e28..b533e44e 100644 --- a/libs/lpplib/include/lpp/location_information.h +++ b/libs/lpplib/include/lpp/location_information.h @@ -1,12 +1,13 @@ #pragma once #include #include +#include enum class VerticalDirection { UP, DOWN }; struct LocationInformation { - // Time of the location estimate in UTC. - time_t time; + // Time of the location estimate in TAI. + TAI_Time tai_time; // Latitude represented as a decimal degree value in the interval [-90,90] with positive values // north of the equator. E.g u-blox F9P UBX-NAV-PVT attribute lat. diff --git a/libs/lpplib/include/lpp/lpp.h b/libs/lpplib/include/lpp/lpp.h index 41b7b488..9639a5f0 100644 --- a/libs/lpplib/include/lpp/lpp.h +++ b/libs/lpplib/include/lpp/lpp.h @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include #include "asn_helper.h" #include "asnlib.h" @@ -18,9 +18,9 @@ class SUPL_Client; class LPP_Client { public: typedef int32_t AD_Request; - typedef void (*AD_Callback)(LPP_Client*, LPP_Transaction*, LPP_Message*, void*); - typedef bool (*PLI_Callback)(LocationInformation&, HaGnssMetrics&, void*); - typedef bool (*PECID_Callback)(ECIDInformation&, void*); + typedef void (*AD_Callback)(LPP_Client*, LPP_Transaction*, LPP_Message*, void*); + typedef bool (*PLI_Callback)(LocationInformation&, HaGnssMetrics&, void*); + typedef bool (*PECID_Callback)(ECIDInformation&, void*); struct ProvideLI { LocationInformationType_t type; @@ -31,6 +31,8 @@ class LPP_Client { LPP_Client(bool segmentation); ~LPP_Client(); + void use_incorrect_supl_identity(); + void set_identity_msisdn(unsigned long msisdn); void set_identity_imsi(unsigned long imsi); void set_identity_ipv4(const std::string& ipv4); @@ -64,6 +66,12 @@ class LPP_Client { void provide_location_information_callback(void* userdata, PLI_Callback callback); void provide_ecid_callback(void* userdata, PECID_Callback callback); + // Force provide location information to be unsolicited sent. + void force_location_information(); + + OCTET_STRING* encode(LPP_Message* message); + LPP_Message* decode(OCTET_STRING* data); + private: bool wait_for_assistance_data_response(LPP_Transaction* transaction); bool process_message(LPP_Message*, LPP_Transaction* transaction); @@ -73,13 +81,10 @@ class LPP_Client { bool supl_start(CellID cell); bool supl_response(); bool supl_send_posinit(CellID cell); - bool supl_receive(std::vector& messages, int milliseconds); + bool supl_receive(std::vector& messages, int timeout_ms, bool blocking); bool supl_send(LPP_Message* message); bool supl_send(const std::vector& messages); - OCTET_STRING* encode(LPP_Message* message); - LPP_Message* decode(OCTET_STRING* data); - LPP_Transaction new_transaction(); private: @@ -106,7 +111,9 @@ class LPP_Client { ProvideLI provide_li; + bool mForceLocationInformation; bool mEnableSegmentation; + bool mSuplIdentityFix; }; void network_initialize(); diff --git a/libs/lpplib/include/lpp/supl.h b/libs/lpplib/include/lpp/supl.h index a50c0c63..16ac7ab3 100644 --- a/libs/lpplib/include/lpp/supl.h +++ b/libs/lpplib/include/lpp/supl.h @@ -3,13 +3,15 @@ #include "asnlib.h" #include "tcp.h" +#define SUPL_CLIENT_RECEIVER_BUFFER_SIZE (1 << 15) + using SUPL_Message = ASN_Unique; using SUPL_EncodedMessage = ASN_Unique; class SUPL_Session { public: - static std::unique_ptr msisdn(long id, unsigned long msisdn); - static std::unique_ptr imsi(long id, unsigned long imsi); + static std::unique_ptr msisdn(long id, unsigned long msisdn, bool switch_order); + static std::unique_ptr imsi(long id, unsigned long imsi, bool switch_order); static std::unique_ptr ip_address(long id, const std::string& addr); static std::unique_ptr ip_address(long id, uint32_t addr); @@ -38,6 +40,7 @@ class SUPL_Client { bool disconnect(); bool is_connected(); + SUPL_Message receive2(); SUPL_Message receive(int milliseconds); bool send(SUPL_Message& message); void harvest(SUPL_Message& message); @@ -47,56 +50,13 @@ class SUPL_Client { protected: SUPL_EncodedMessage encode(SUPL_Message& message); + SUPL_Message process(); + private: std::unique_ptr mTCP; std::unique_ptr mSession; // TODO(ewasjon): move to heap - char mReceiveBuffer[1 << 16]; -}; - -#if 0 -#include - -struct SUPLSession { - int id; - int slp_size; - char* slp_buffer; - unsigned char ip[4]; + char mReceiveBuffer[SUPL_CLIENT_RECEIVER_BUFFER_SIZE]; + size_t mReceiveLength; }; - -void supl_session_create(SUPLSession* session, uint32_t ip); - -int supl_session_harvest(SUPLSession* session, ULP_PDU_t* ulp); - -// Helper function for creating a SUPL message. -ULP_PDU_t* supl_create_message(SUPLSession* session, UlpMessage_PR present); - -void supl_free_message(ULP_PDU_t* ulp); - -// Helper function to send a SUPL message. -int supl_send_pdu(TCPClient* client, ULP_PDU_t* ulp); - -// Helper to read SUPL messages -int supl_read(TCPClient* client, ULP_PDU_t* pdu); - -ULP_PDU_t* supl_read_pdu(TCPClient* client, int milliseconds); - -// SUPL START message -int supl_start(TCPClient* client, CellID cell, SUPLSession* session); - -// Helper function for wait/receiving a SUPL response -int supl_response(TCPClient* client, SUPLSession* session); - -// Helper function to send SUPL POSINIT with LPP messages. -int supl_send_posinit(TCPClient* client, CellID cell, SUPLSession* session, - OCTET_STRING** lpp_messages, int lpp_message_count); - -// Helper function for receiving SUPL POS responses. -int supl_receive_pos(TCPClient* client, LPP_Message** messages, int* count, - int messages_size, int milliseconds); - -// Helper function for sending a SUPL POS message with LPP payloads. -int supl_send_pos(TCPClient* client, SUPLSession* session, - OCTET_STRING* lpp_message); -#endif \ No newline at end of file diff --git a/libs/lpplib/src/asn_helper.cpp b/libs/lpplib/src/asn_helper.cpp index cae8c16c..1f607cd1 100644 --- a/libs/lpplib/src/asn_helper.cpp +++ b/libs/lpplib/src/asn_helper.cpp @@ -4,175 +4,44 @@ #include #include -BitString::BitString(size_t bits) { - buf = nullptr; - size = 0; - bits_unused = 0; - _asn_ctx = {}; - initialize(bits); -} - -void BitString::destroy() { - size = 0; - bits_unused = 0; - _asn_ctx = {}; - if (buf) { - free(buf); - buf = nullptr; - } -} - -void BitString::initialize(size_t bits) { - assert(bits > 0); - destroy(); - - auto bytes = (bits + 7) / 8; - auto total_bits = bytes * 8; - auto unused_bits = total_bits - bits; - size = bytes; - assert(unused_bits >= 0); - assert(unused_bits <= 7); - bits_unused = unused_bits; - buf = reinterpret_cast(calloc(size, sizeof(uint8_t))); - memset(buf, 0, size); -} - -BitString::Index BitString::bit_index(ssize_t index) { - auto bit = static_cast(index); - assert(index >= 0); - assert(bit < size * 8 - bits_unused); - bit += bits_unused; - - auto byte = bit / 8; - auto byte_index = size - 1 - byte; - auto local_bit = (bit - byte * 8); +static void BIT_STRING_initialize(BIT_STRING_s* bit_string, size_t bits) { + BIT_STRING_free(&asn_DEF_BIT_STRING, bit_string, ASFM_FREE_UNDERLYING_AND_RESET); - assert(byte_index >= 0); - assert(byte_index < size); - assert(local_bit >= 0); - assert(local_bit < 8); - return {byte_index, local_bit}; + auto bytes = (bits + 7) / 8; + bit_string->size = bytes; + bit_string->bits_unused = 0; + bit_string->buf = reinterpret_cast(calloc(bit_string->size, sizeof(uint8_t))); } -void BitString::set_bit(ssize_t index) { - auto i = bit_index(index); - buf[i.byte_index] |= 1 << i.local_bit; +BIT_STRING_s* BitStringBuilder::to_bit_string(size_t bits) { + auto bit_string = asn1_allocate(); + return into_bit_string(bits, bit_string); } -void BitString::clear_bit(ssize_t index) { - auto i = bit_index(index); - buf[i.byte_index] &= ~(1 << i.local_bit); -} - -bool BitString::get_bit(ssize_t index) { - auto i = bit_index(index); - return ((buf[i.byte_index] >> i.local_bit) & 1) != 0; -} +BIT_STRING_s* BitStringBuilder::into_bit_string(size_t bits, BIT_STRING_s* bit_string) { + BIT_STRING_initialize(bit_string, bits); -void BitString::set_integer(size_t begin, size_t length, size_t value) { - for (size_t i = 0; i < length; i++) { - auto index = begin + i; - if (value & (1 << i)) - set_bit(index); - } -} - -int64_t BitString::as_int64() { - uint64_t value = 0; - for (size_t i = 0; i < size * 8 - bits_unused; i++) { - auto bit = get_bit(i); - size_t bit_value = bit ? 1 : 0; - value |= (bit_value << i); - } - - return *reinterpret_cast(&value); -} - -std::string BitString::as_string() { - std::stringstream stream; - - stream << "BitString("; - stream << size; - stream << ", "; - stream << bits_unused; - stream << ")\n"; - - for (size_t i = 0; i < size; i++) { - stream << " ["; - for (auto j = 0; j < 8; j++) { - if (i + 1 == size && j < bits_unused) { - stream << '*'; - continue; + assert(bits <= 64); + for (int j = 0; j < 64; j++) { + if (mBits & (1llu << j)) { + auto x = j / 8; + auto y = 7 - (j % 8); + assert(x < bit_string->size); + if (x < bit_string->size) { + bit_string->buf[x] |= 1 << y; } - - if (buf[i] & (1 << j)) - stream << '1'; - else - stream << '0'; } - stream << "]"; - } - - stream << '\n'; - - auto data = new char[size * 8]; - for (size_t i = 0; i < size * 8; i++) { - data[i] = '?'; - } - - for (size_t i = 0; i < size * 8 - bits_unused; i++) { - auto index = bit_index(i); - data[index.byte_index * 8 + index.local_bit] = - "0123456789ABCDEF"[i % 16]; } - for (size_t i = 0; i < size; i++) { - stream << " ["; - for (auto j = 0; j < 8; j++) { - stream << data[i * 8 + j]; - } - stream << "]"; - } - stream << '\n'; - - auto func = [](const void* buffer, size_t size, - void* application_specific_key) { - auto& stream = - *reinterpret_cast(application_specific_key); - stream << std::string{reinterpret_cast(buffer), size}; - return static_cast(size); - }; - - BIT_STRING_print(&asn_DEF_BIT_STRING, this, 0, func, &stream); - - std::string buffer; - buffer.reserve(4096); - xer_encode( - &asn_DEF_BIT_STRING, this, XER_F_BASIC, - [](const void* data, size_t size, void* userdata) { - auto buffer = static_cast(userdata); - auto begin = static_cast(data); - auto end = begin + size; - buffer->insert(buffer->end(), begin, end); - return 0; - }, - &buffer); - - stream << '\n'; - stream << buffer; - - delete[] data; - return stream.str(); + return bit_string; } void supl_fill_tracking_area_code(TrackingAreaCode_t* tac, int tac_value) { - auto bit_string = BitString::allocate(16, tac); - bit_string->set_integer(0, 16, tac_value); + BitStringBuilder{}.integer(0, 16, tac_value).into_bit_string(16, tac); } void supl_fill_cell_identity(CellIdentity_t* identity, size_t value) { - auto bit_string = BitString::allocate(28, identity); - bit_string->set_integer(0, 28, value); + BitStringBuilder{}.integer(0, 28, value).into_bit_string(28, identity); } MCC* supl_create_mcc(int mcc_value) { @@ -241,14 +110,12 @@ ECGI* ecgi_create(long mcc, long mnc, long id) { } double long_pointer2scaled_double(long* ptr, double def, double arg) { - if (!ptr) - return def; + if (!ptr) return def; return *ptr / arg; } double long_pointer(long* ptr, long def) { - if (!ptr) - return def; + if (!ptr) return def; return *ptr; } @@ -259,4 +126,3 @@ long gnss2long(GNSS_SignalID_t gnss_id) { return gnss_id.gnss_SignalID; } } - diff --git a/libs/lpplib/src/assistance_data.cpp b/libs/lpplib/src/assistance_data.cpp index f9ffbe57..4d1fcb17 100644 --- a/libs/lpplib/src/assistance_data.cpp +++ b/libs/lpplib/src/assistance_data.cpp @@ -16,16 +16,14 @@ static GNSS_ReferenceTimeReq_t* request_reference_time(long gnss_id) { static GNSS_IonosphericModelReq_t* request_ionospheric_model(long gnss_id) { if (gnss_id == GNSS_ID__gnss_id_gps || gnss_id == GNSS_ID__gnss_id_glonass) { - auto element = ALLOC_ZERO(GNSS_IonosphericModelReq_t); - element->klobucharModelReq = ALLOC_ZERO(BIT_STRING_t); - auto bit_string = BitString::allocate(2, element->klobucharModelReq); - bit_string->set_integer(0, 2, 0); + auto element = ALLOC_ZERO(GNSS_IonosphericModelReq_t); + // Request '00' Klobuchar model + element->klobucharModelReq = BitStringBuilder{}.clear(0).clear(1).to_bit_string(2); return element; } else if (gnss_id == GNSS_ID__gnss_id_bds) { - auto element = ALLOC_ZERO(GNSS_IonosphericModelReq_t); - element->klobucharModelReq = ALLOC_ZERO(BIT_STRING_t); - auto bit_string = BitString::allocate(2, element->klobucharModelReq); - bit_string->set_integer(0, 2, 1); + auto element = ALLOC_ZERO(GNSS_IonosphericModelReq_t); + // Request '01' BDS Klobuchar model + element->klobucharModelReq = BitStringBuilder{}.clear(0).set(1).to_bit_string(2); return element; } else if (gnss_id == GNSS_ID__gnss_id_galileo) { auto element = ALLOC_ZERO(GNSS_IonosphericModelReq_t); @@ -76,23 +74,13 @@ static GNSS_RTK_ObservationsReq_r15_t* request_observations() { element->gnss_RTK_PhaseRangeRateReq_r15 = true; element->gnss_RTK_CNR_Req_r15 = true; - auto signals = BitString::allocate(8, &element->gnss_RTK_SignalsReq_r15.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - - auto ext_signals = BitString::allocate(16); - for (size_t i = 0; i < 16; i++) { - ext_signals->set_bit(i); - } + BitStringBuilder{} + .integer(0, 8, 0xFF) + .into_bit_string(8, &element->gnss_RTK_SignalsReq_r15.gnss_SignalIDs); element->gnss_RTK_SignalsReq_r15.ext1 = ALLOC_ZERO(GNSS_SignalIDs::GNSS_SignalIDs__ext1); - element->gnss_RTK_SignalsReq_r15.ext1->gnss_SignalIDs_Ext_r15 = ext_signals; + element->gnss_RTK_SignalsReq_r15.ext1->gnss_SignalIDs_Ext_r15 = + BitStringBuilder{}.integer(0, 16, 0xFFFF).to_bit_string(16); return element; } @@ -108,46 +96,29 @@ static GNSS_SSR_ClockCorrectionsReq_r15_t* request_clocks() { static GNSS_SSR_CodeBiasReq_r15_t* request_codebias() { auto element = ALLOC_ZERO(GNSS_SSR_CodeBiasReq_r15_t); - auto signals = - BitString::allocate(8, &element->signal_and_tracking_mode_ID_Map_r15.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - - auto ext_signals = BitString::allocate(16); - for (size_t i = 0; i < 16; i++) { - ext_signals->set_bit(i); - } - element->signal_and_tracking_mode_ID_Map_r15.ext1 = ALLOC_ZERO(GNSS_SignalIDs::GNSS_SignalIDs__ext1); - element->signal_and_tracking_mode_ID_Map_r15.ext1->gnss_SignalIDs_Ext_r15 = ext_signals; + BitStringBuilder{} + .integer(0, 8, 0xFF) + .into_bit_string(8, &element->signal_and_tracking_mode_ID_Map_r15.gnss_SignalIDs); + + element->signal_and_tracking_mode_ID_Map_r15.ext1 = + ALLOC_ZERO(GNSS_SignalIDs::GNSS_SignalIDs__ext1); + element->signal_and_tracking_mode_ID_Map_r15.ext1->gnss_SignalIDs_Ext_r15 = + BitStringBuilder{}.integer(0, 16, 0xFFFF).to_bit_string(16); return element; } static GNSS_SSR_PhaseBiasReq_r16_t* request_phasebias() { auto element = ALLOC_ZERO(GNSS_SSR_PhaseBiasReq_r16_t); - auto signals = - BitString::allocate(8, &element->signal_and_tracking_mode_ID_Map_r16.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - auto ext_signals = BitString::allocate(16); - for (size_t i = 0; i < 16; i++) { - ext_signals->set_bit(i); - } - element->signal_and_tracking_mode_ID_Map_r16.ext1 = ALLOC_ZERO(GNSS_SignalIDs::GNSS_SignalIDs__ext1); - element->signal_and_tracking_mode_ID_Map_r16.ext1->gnss_SignalIDs_Ext_r15 = ext_signals; + BitStringBuilder{} + .integer(0, 8, 0xFF) + .into_bit_string(8, &element->signal_and_tracking_mode_ID_Map_r16.gnss_SignalIDs); + + element->signal_and_tracking_mode_ID_Map_r16.ext1 = + ALLOC_ZERO(GNSS_SignalIDs::GNSS_SignalIDs__ext1); + element->signal_and_tracking_mode_ID_Map_r16.ext1->gnss_SignalIDs_Ext_r15 = + BitStringBuilder{}.integer(0, 16, 0xFFFF).to_bit_string(16); return element; } @@ -273,8 +244,7 @@ LPP_Message* lpp_request_assistance_data(LPP_Transaction* transaction, CellID ce return message; } -LPP_Message* lpp_request_agnss(LPP_Transaction* transaction, CellID cell, - long gnss_id) { +LPP_Message* lpp_request_agnss(LPP_Transaction* transaction, CellID cell, long gnss_id) { auto message = lpp_create(transaction, LPP_MessageBody__c1_PR_requestAssistanceData); auto body = message->lpp_MessageBody; @@ -327,7 +297,7 @@ LPP_Message* lpp_request_assistance_data_ssr(LPP_Transaction* transaction, CellI cad->ext2 = cad2; auto car2_ssr = ALLOC_ZERO(GNSS_SSR_CorrectionPointsReq_r16); - auto car2 = ALLOC_ZERO(GNSS_CommonAssistDataReq::GNSS_CommonAssistDataReq__ext2); + auto car2 = ALLOC_ZERO(GNSS_CommonAssistDataReq::GNSS_CommonAssistDataReq__ext2); car2->gnss_SSR_CorrectionPointsReq_r16 = car2_ssr; auto car = ALLOC_ZERO(GNSS_CommonAssistDataReq_t); diff --git a/libs/lpplib/src/capabilities.cpp b/libs/lpplib/src/capabilities.cpp index b538f93c..5eb03d6f 100644 --- a/libs/lpplib/src/capabilities.cpp +++ b/libs/lpplib/src/capabilities.cpp @@ -1,103 +1,40 @@ -#include "lpp.h" #include "internal_lpp.h" +#include "lpp.h" -static GNSS_SupportList* build_support_list() { - auto gnss_list = ALLOC_ZERO(GNSS_SupportList); - asn_sequence_empty(&gnss_list->list); +static GNSS_SupportElement* build_support_element(long gnss_id) { + auto element = ALLOC_ZERO(GNSS_SupportElement); + element->gnss_ID.gnss_id = gnss_id; + element->adr_Support = false; + element->velocityMeasurementSupport = true; - { - auto element = ALLOC_ZERO(GNSS_SupportElement); - element->gnss_ID.gnss_id = GNSS_ID__gnss_id_gps; - element->adr_Support = false; - element->velocityMeasurementSupport = true; - - auto posModes = BitString::allocate(8, &element->agnss_Modes.posModes); - posModes->set_bit(PositioningModes__posModes_ue_based); - posModes->set_bit(PositioningModes__posModes_ue_assisted); - - auto ext = ALLOC_ZERO(GNSS_SupportElement::GNSS_SupportElement__ext1); - ext->ha_gnss_Modes_r15 = ALLOC_ZERO(PositioningModes); - element->ext1 = ext; - - auto posModes2 = - BitString::allocate(8, &ext->ha_gnss_Modes_r15->posModes); - posModes2->set_bit(PositioningModes__posModes_ue_based); - - auto signals = - BitString::allocate(8, &element->gnss_Signals.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - asn_sequence_add(&gnss_list->list, element); - } + BitStringBuilder{} + .set(PositioningModes__posModes_ue_based) + .set(PositioningModes__posModes_ue_assisted) + .into_bit_string(8, &element->agnss_Modes.posModes); - { - auto element = ALLOC_ZERO(GNSS_SupportElement); - element->gnss_ID.gnss_id = GNSS_ID__gnss_id_glonass; - element->adr_Support = false; - element->velocityMeasurementSupport = true; - - auto posModes = BitString::allocate(8, &element->agnss_Modes.posModes); - posModes->set_bit(PositioningModes__posModes_ue_based); - posModes->set_bit(PositioningModes__posModes_ue_assisted); - - auto ext = ALLOC_ZERO(GNSS_SupportElement::GNSS_SupportElement__ext1); - ext->ha_gnss_Modes_r15 = ALLOC_ZERO(PositioningModes); - element->ext1 = ext; - - auto posModes2 = - BitString::allocate(8, &ext->ha_gnss_Modes_r15->posModes); - posModes2->set_bit(PositioningModes__posModes_ue_based); - - auto signals = - BitString::allocate(8, &element->gnss_Signals.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - asn_sequence_add(&gnss_list->list, element); - } + auto ha_gnss_modes_r15 = ALLOC_ZERO(PositioningModes); + BitStringBuilder{} + .set(PositioningModes__posModes_ue_based) + .into_bit_string(8, &ha_gnss_modes_r15->posModes); - { - auto element = ALLOC_ZERO(GNSS_SupportElement); - element->gnss_ID.gnss_id = GNSS_ID__gnss_id_galileo; - element->adr_Support = false; - element->velocityMeasurementSupport = true; - - auto posModes = BitString::allocate(8, &element->agnss_Modes.posModes); - posModes->set_bit(PositioningModes__posModes_ue_based); - posModes->set_bit(PositioningModes__posModes_ue_assisted); - - auto ext = ALLOC_ZERO(GNSS_SupportElement::GNSS_SupportElement__ext1); - ext->ha_gnss_Modes_r15 = ALLOC_ZERO(PositioningModes); - element->ext1 = ext; - - auto posModes2 = - BitString::allocate(8, &ext->ha_gnss_Modes_r15->posModes); - posModes2->set_bit(PositioningModes__posModes_ue_based); - - auto signals = - BitString::allocate(8, &element->gnss_Signals.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - asn_sequence_add(&gnss_list->list, element); - } + auto ext = ALLOC_ZERO(GNSS_SupportElement::GNSS_SupportElement__ext1); + ext->ha_gnss_Modes_r15 = ha_gnss_modes_r15; + element->ext1 = ext; + + BitStringBuilder{} + .integer(0, 8, 0xFF) + .into_bit_string(8, &element->gnss_Signals.gnss_SignalIDs); + return element; +} + +static GNSS_SupportList* build_support_list() { + auto gnss_list = ALLOC_ZERO(GNSS_SupportList); + asn_sequence_empty(&gnss_list->list); + asn_sequence_add(&gnss_list->list, build_support_element(GNSS_ID__gnss_id_gps)); + asn_sequence_add(&gnss_list->list, build_support_element(GNSS_ID__gnss_id_glonass)); + asn_sequence_add(&gnss_list->list, build_support_element(GNSS_ID__gnss_id_galileo)); + asn_sequence_add(&gnss_list->list, build_support_element(GNSS_ID__gnss_id_bds)); return gnss_list; } @@ -105,57 +42,44 @@ static AssistanceDataSupportList* build_assistance_support() { auto as = ALLOC_ZERO(AssistanceDataSupportList); // GNSS Common AD Support - auto as_ext1 = ALLOC_ZERO(GNSS_CommonAssistanceDataSupport:: - GNSS_CommonAssistanceDataSupport__ext1); - - auto rtk_reference_station_support = - ALLOC_ZERO(GNSS_RTK_ReferenceStationInfoSupport_r15); - auto rtk_auxiliary_station_support = - ALLOC_ZERO(GNSS_RTK_AuxiliaryStationDataSupport_r15); - as_ext1->gnss_RTK_ReferenceStationInfoSupport_r15 = - rtk_reference_station_support; - as_ext1->gnss_RTK_AuxiliaryStationDataSupport_r15 = - rtk_auxiliary_station_support; - as->gnss_CommonAssistanceDataSupport.ext1 = as_ext1; + auto as_ext1 = + ALLOC_ZERO(GNSS_CommonAssistanceDataSupport::GNSS_CommonAssistanceDataSupport__ext1); + + auto rtk_reference_station_support = ALLOC_ZERO(GNSS_RTK_ReferenceStationInfoSupport_r15); + auto rtk_auxiliary_station_support = ALLOC_ZERO(GNSS_RTK_AuxiliaryStationDataSupport_r15); + as_ext1->gnss_RTK_ReferenceStationInfoSupport_r15 = rtk_reference_station_support; + as_ext1->gnss_RTK_AuxiliaryStationDataSupport_r15 = rtk_auxiliary_station_support; + as->gnss_CommonAssistanceDataSupport.ext1 = as_ext1; // GNSS Generic AD Support auto generic_assistance = &as->gnss_GenericAssistanceDataSupport; asn_sequence_empty(&generic_assistance->list); { - auto gps_assistance = ALLOC_ZERO(GNSS_GenericAssistDataSupportElement); + auto gps_assistance = ALLOC_ZERO(GNSS_GenericAssistDataSupportElement); gps_assistance->gnss_ID.gnss_id = GNSS_ID__gnss_id_gps; gps_assistance->gnss_AcquisitionAssistanceSupport = ALLOC_ZERO(GNSS_AcquisitionAssistanceSupport); - auto ext = ALLOC_ZERO(GNSS_GenericAssistDataSupportElement:: - GNSS_GenericAssistDataSupportElement__ext2); - gps_assistance->ext2 = ext; - ext->gnss_RTK_ObservationsSupport_r15 = - ALLOC_ZERO(GNSS_RTK_ObservationsSupport_r15_t); - auto signals = - BitString::allocate(8, &ext->gnss_RTK_ObservationsSupport_r15 - ->gnssSignalIDs_r15.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - - ext->gnss_RTK_ResidualsSupport_r15 = - ALLOC_ZERO(GNSS_RTK_ResidualsSupport_r15_t); + auto ext = ALLOC_ZERO( + GNSS_GenericAssistDataSupportElement::GNSS_GenericAssistDataSupportElement__ext2); + gps_assistance->ext2 = ext; + ext->gnss_RTK_ObservationsSupport_r15 = ALLOC_ZERO(GNSS_RTK_ObservationsSupport_r15_t); + + BitStringBuilder{} + .integer(0, 8, 0xFF) + .into_bit_string( + 8, &ext->gnss_RTK_ObservationsSupport_r15->gnssSignalIDs_r15.gnss_SignalIDs); + + ext->gnss_RTK_ResidualsSupport_r15 = ALLOC_ZERO(GNSS_RTK_ResidualsSupport_r15_t); GNSS_Link_CombinationsList_r15_t* link_list = &ext->gnss_RTK_ResidualsSupport_r15->link_combinations_support_r15; asn_sequence_empty(&link_list->list); { - GNSS_Link_Combinations_r15_t* link = - ALLOC_ZERO(GNSS_Link_Combinations_r15_t); - link->l1_r15.gnss_FrequencyID_r15 = 0; - link->l2_r15.gnss_FrequencyID_r15 = 1; + GNSS_Link_Combinations_r15_t* link = ALLOC_ZERO(GNSS_Link_Combinations_r15_t); + link->l1_r15.gnss_FrequencyID_r15 = 0; + link->l2_r15.gnss_FrequencyID_r15 = 1; asn_sequence_add(&link_list->list, link); } @@ -163,39 +87,28 @@ static AssistanceDataSupportList* build_assistance_support() { } { - auto glonass_assistance = - ALLOC_ZERO(GNSS_GenericAssistDataSupportElement_t); + auto glonass_assistance = ALLOC_ZERO(GNSS_GenericAssistDataSupportElement_t); glonass_assistance->gnss_ID.gnss_id = GNSS_ID__gnss_id_glonass; glonass_assistance->gnss_AcquisitionAssistanceSupport = ALLOC_ZERO(GNSS_AcquisitionAssistanceSupport_t); - auto ext = ALLOC_ZERO(GNSS_GenericAssistDataSupportElement:: - GNSS_GenericAssistDataSupportElement__ext2); - glonass_assistance->ext2 = ext; - ext->gnss_RTK_ObservationsSupport_r15 = - ALLOC_ZERO(GNSS_RTK_ObservationsSupport_r15_t); - auto signals = - BitString::allocate(8, &ext->gnss_RTK_ObservationsSupport_r15 - ->gnssSignalIDs_r15.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - - ext->glo_RTK_BiasInformationSupport_r15 = - ALLOC_ZERO(GLO_RTK_BiasInformationSupport_r15_t); - ext->gnss_RTK_ResidualsSupport_r15 = - ALLOC_ZERO(GNSS_RTK_ResidualsSupport_r15_t); - auto link_list = - &ext->gnss_RTK_ResidualsSupport_r15->link_combinations_support_r15; + auto ext = ALLOC_ZERO( + GNSS_GenericAssistDataSupportElement::GNSS_GenericAssistDataSupportElement__ext2); + glonass_assistance->ext2 = ext; + ext->gnss_RTK_ObservationsSupport_r15 = ALLOC_ZERO(GNSS_RTK_ObservationsSupport_r15_t); + + BitStringBuilder{} + .integer(0, 8, 0xFF) + .into_bit_string( + 8, &ext->gnss_RTK_ObservationsSupport_r15->gnssSignalIDs_r15.gnss_SignalIDs); + + ext->glo_RTK_BiasInformationSupport_r15 = ALLOC_ZERO(GLO_RTK_BiasInformationSupport_r15_t); + ext->gnss_RTK_ResidualsSupport_r15 = ALLOC_ZERO(GNSS_RTK_ResidualsSupport_r15_t); + auto link_list = &ext->gnss_RTK_ResidualsSupport_r15->link_combinations_support_r15; asn_sequence_empty(&link_list->list); { - auto link = ALLOC_ZERO(GNSS_Link_Combinations_r15_t); + auto link = ALLOC_ZERO(GNSS_Link_Combinations_r15_t); link->l1_r15.gnss_FrequencyID_r15 = 0; link->l2_r15.gnss_FrequencyID_r15 = 1; asn_sequence_add(&link_list->list, link); @@ -205,40 +118,29 @@ static AssistanceDataSupportList* build_assistance_support() { } { - auto galileo_assistance = - ALLOC_ZERO(GNSS_GenericAssistDataSupportElement); + auto galileo_assistance = ALLOC_ZERO(GNSS_GenericAssistDataSupportElement); galileo_assistance->gnss_ID.gnss_id = GNSS_ID__gnss_id_galileo; galileo_assistance->gnss_AcquisitionAssistanceSupport = ALLOC_ZERO(GNSS_AcquisitionAssistanceSupport); - auto ext = ALLOC_ZERO(GNSS_GenericAssistDataSupportElement:: - GNSS_GenericAssistDataSupportElement__ext2); - galileo_assistance->ext2 = ext; - ext->gnss_RTK_ObservationsSupport_r15 = - ALLOC_ZERO(GNSS_RTK_ObservationsSupport_r15_t); - auto signals = - BitString::allocate(8, &ext->gnss_RTK_ObservationsSupport_r15 - ->gnssSignalIDs_r15.gnss_SignalIDs); - signals->set_bit(0); - signals->set_bit(1); - signals->set_bit(2); - signals->set_bit(3); - signals->set_bit(4); - signals->set_bit(5); - signals->set_bit(6); - signals->set_bit(7); - - ext->gnss_RTK_ResidualsSupport_r15 = - ALLOC_ZERO(GNSS_RTK_ResidualsSupport_r15_t); - auto link_list = - &ext->gnss_RTK_ResidualsSupport_r15->link_combinations_support_r15; + auto ext = ALLOC_ZERO( + GNSS_GenericAssistDataSupportElement::GNSS_GenericAssistDataSupportElement__ext2); + galileo_assistance->ext2 = ext; + ext->gnss_RTK_ObservationsSupport_r15 = ALLOC_ZERO(GNSS_RTK_ObservationsSupport_r15_t); + + BitStringBuilder{} + .integer(0, 8, 0xFF) + .into_bit_string( + 8, &ext->gnss_RTK_ObservationsSupport_r15->gnssSignalIDs_r15.gnss_SignalIDs); + + ext->gnss_RTK_ResidualsSupport_r15 = ALLOC_ZERO(GNSS_RTK_ResidualsSupport_r15_t); + auto link_list = &ext->gnss_RTK_ResidualsSupport_r15->link_combinations_support_r15; asn_sequence_empty(&link_list->list); { - GNSS_Link_Combinations_r15_t* link = - ALLOC_ZERO(GNSS_Link_Combinations_r15_t); - link->l1_r15.gnss_FrequencyID_r15 = 0; - link->l2_r15.gnss_FrequencyID_r15 = 1; + GNSS_Link_Combinations_r15_t* link = ALLOC_ZERO(GNSS_Link_Combinations_r15_t); + link->l1_r15.gnss_FrequencyID_r15 = 0; + link->l2_r15.gnss_FrequencyID_r15 = 1; asn_sequence_add(&link_list->list, link); } @@ -249,52 +151,52 @@ static AssistanceDataSupportList* build_assistance_support() { } LPP_Message* lpp_provide_capabilities(LPP_Transaction* transaction, bool segmentation) { - auto message = - lpp_create(transaction, LPP_MessageBody__c1_PR_provideCapabilities); - auto body = message->lpp_MessageBody; + auto message = lpp_create(transaction, LPP_MessageBody__c1_PR_provideCapabilities); + auto body = message->lpp_MessageBody; - auto cext = &body->choice.c1.choice.provideCapabilities.criticalExtensions; - cext->present = ProvideCapabilities__criticalExtensions_PR_c1; - cext->choice.c1.present = - ProvideCapabilities__criticalExtensions__c1_PR_provideCapabilities_r9; + auto cext = &body->choice.c1.choice.provideCapabilities.criticalExtensions; + cext->present = ProvideCapabilities__criticalExtensions_PR_c1; + cext->choice.c1.present = ProvideCapabilities__criticalExtensions__c1_PR_provideCapabilities_r9; - auto ext2 = ALLOC_ZERO( - A_GNSS_ProvideCapabilities::A_GNSS_ProvideCapabilities__ext2); + auto ext2 = ALLOC_ZERO(A_GNSS_ProvideCapabilities::A_GNSS_ProvideCapabilities__ext2); - auto pad = BitString::allocate(8); + auto pad = BitStringBuilder{} + .set(A_GNSS_ProvideCapabilities__ext2__periodicAssistanceData_r15_solicited) + .set(A_GNSS_ProvideCapabilities__ext2__periodicAssistanceData_r15_unsolicited) + .to_bit_string(8); ext2->periodicAssistanceData_r15 = pad; - pad->set_bit( - A_GNSS_ProvideCapabilities__ext2__periodicAssistanceData_r15_solicited); - pad->set_bit( - A_GNSS_ProvideCapabilities__ext2__periodicAssistanceData_r15_unsolicited); - auto gnss_capabilities = ALLOC_ZERO(A_GNSS_ProvideCapabilities); + auto gnss_capabilities = ALLOC_ZERO(A_GNSS_ProvideCapabilities); gnss_capabilities->assistanceDataSupportList = build_assistance_support(); gnss_capabilities->gnss_SupportList = build_support_list(); gnss_capabilities->ext2 = ext2; auto ecid_capabilities = ALLOC_ZERO(ECID_ProvideCapabilities); - auto ecid_MeasSupported = - BitString::allocate(8, &ecid_capabilities->ecid_MeasSupported); - ecid_MeasSupported->set_bit( - ECID_ProvideCapabilities__ecid_MeasSupported_rsrpSup); - ecid_MeasSupported->set_bit( - ECID_ProvideCapabilities__ecid_MeasSupported_rsrqSup); + + BitStringBuilder{} + .set(ECID_ProvideCapabilities__ecid_MeasSupported_rsrpSup) + .set(ECID_ProvideCapabilities__ecid_MeasSupported_rsrqSup) + .set(ECID_ProvideCapabilities__ecid_MeasSupported_ueRxTxSup) + .set(ECID_ProvideCapabilities__ecid_MeasSupported_nrsrpSup_r14) + .set(ECID_ProvideCapabilities__ecid_MeasSupported_nrsrqSup_r14) + .into_bit_string(8, &ecid_capabilities->ecid_MeasSupported); auto common_capabilities = ALLOC_ZERO(CommonIEsProvideCapabilities); auto cc_ext1 = ALLOC_ZERO(CommonIEsProvideCapabilities::CommonIEsProvideCapabilities__ext1); - auto segmentation_support = BitString::allocate(2); - if(segmentation) { - segmentation_support->set_bit(0); // Support receiving segmented messages + auto segmentation_support = BitStringBuilder{}; + if (segmentation) { + segmentation_support.set( + CommonIEsProvideCapabilities__ext1__lpp_message_segmentation_r14_serverToTarget); + segmentation_support.set( + CommonIEsProvideCapabilities__ext1__lpp_message_segmentation_r14_targetToServer); } - cc_ext1->lpp_message_segmentation_r14 = segmentation_support; - common_capabilities->ext1 = cc_ext1; - // segmentation + cc_ext1->lpp_message_segmentation_r14 = segmentation_support.to_bit_string(2); + common_capabilities->ext1 = cc_ext1; - auto pcr9 = &cext->choice.c1.choice.provideCapabilities_r9; + auto pcr9 = &cext->choice.c1.choice.provideCapabilities_r9; pcr9->commonIEsProvideCapabilities = common_capabilities; - pcr9->a_gnss_ProvideCapabilities = gnss_capabilities; - pcr9->ecid_ProvideCapabilities = ecid_capabilities; + pcr9->a_gnss_ProvideCapabilities = gnss_capabilities; + pcr9->ecid_ProvideCapabilities = ecid_capabilities; return message; } \ No newline at end of file diff --git a/libs/lpplib/src/internal_lpp.cpp b/libs/lpplib/src/internal_lpp.cpp index 39903336..e44d5a00 100644 --- a/libs/lpplib/src/internal_lpp.cpp +++ b/libs/lpplib/src/internal_lpp.cpp @@ -2,8 +2,10 @@ #include "location_information.h" #include "lpp.h" +#include #include #include +#include bool lpp_harvest_transaction(LPP_Transaction* transaction, LPP_Message* lpp) { if (!lpp) return false; @@ -35,6 +37,8 @@ OCTET_STRING* lpp_encode(LPP_Message* lpp) { asn_enc_rval_t ret = uper_encode_to_buffer(&asn_DEF_LPP_Message, NULL, lpp, buffer, sizeof(buffer)); if (ret.encoded == -1) { + printf("ERROR: Encoding failed: %s\n", + ret.failed_type ? ret.failed_type->name : ""); return NULL; } @@ -263,7 +267,7 @@ static void lpp_HWVVAU(HorizontalWithVerticalVelocityAndUncertainty_t* HWVVAU, static Velocity_t* lpp_Velocity(const LocationInformation& location) { auto V = ALLOC_ZERO(Velocity_t); V->present = Velocity_PR_horizontalWithVerticalVelocityAndUncertainty; - lpp_HWVVAU(&V->choice.horizontalWithVerticalVelocityAndUncertainty, location); + lpp_HWVVAU(&V->choice.horizontalWithVerticalVelocityAndUncertainty, location); // TODO: return V; } @@ -271,12 +275,12 @@ static CommonIEsProvideLocationInformation_t::CommonIEsProvideLocationInformatio lpp_PLI_CIE_ext2(const LocationInformation& location) { auto ext2 = ALLOC_ZERO( CommonIEsProvideLocationInformation_t::CommonIEsProvideLocationInformation__ext2); - auto locationSource = BitString::allocate(6); - ext2->locationSource_r13 = locationSource; - locationSource->set_bit(LocationSource_r13_ha_gnss_v1510); + ext2->locationSource_r13 = + BitStringBuilder{}.set(LocationSource_r13_ha_gnss_v1510).to_bit_string(6); struct tm tm {}; - auto current_time = location.time; + auto seconds = UTC_Time{location.tai_time}.timestamp().seconds(); + auto current_time = static_cast(seconds); auto ptm = gmtime_r(¤t_time, &tm); ext2->locationTimestamp_r13 = asn_time2UT(NULL, ptm, 1); @@ -302,6 +306,41 @@ lpp_PLI_CommonIEsProvideLocationInformation(const LocationInformation& location, return CIE_PLI; } +static GNSS_LocationInformation* lpp_LocationInformation(const LocationInformation& location) { + auto location_information = ALLOC_ZERO(GNSS_LocationInformation); + auto& mrt = location_information->measurementReferenceTime; + + auto time = GPS_Time{location.tai_time}; + auto tod = time.time_of_day(); + // time of day in milliseconds + auto msec = static_cast(tod.full_seconds() * 1000); + // time of day in 250 nanoseconds + auto nfrac = tod.full_seconds() * 1000.0 - static_cast(msec); + nfrac *= 1000.0 * 4.0; + + mrt.gnss_TimeID.gnss_id = GNSS_ID__gnss_id_gps; + // only take the first 3600 * 1000 milliseconds of the day + mrt.gnss_TOD_msec = msec % (3600 * 1000); + mrt.gnss_TOD_frac = newLong((long)nfrac); + + BitStringBuilder{} + .set(GNSS_ID_Bitmap__gnss_ids_gps) + .into_bit_string(6, &location_information->agnss_List.gnss_ids); + + return location_information; +} + +static A_GNSS_ProvideLocationInformation* +lpp_PLI_A_GNSS_ProvideLocationInformation(const LocationInformation& location, + bool has_information) { + if (!has_information) return nullptr; + + auto provide_location_information = ALLOC_ZERO(A_GNSS_ProvideLocationInformation); + provide_location_information->gnss_LocationInformation = lpp_LocationInformation(location); + + return provide_location_information; +} + LPP_Message* lpp_PLI_location_estimate(LPP_Transaction* transaction, LocationInformation* li, bool has_information) { // @@ -319,6 +358,8 @@ LPP_Message* lpp_PLI_location_estimate(LPP_Transaction* transaction, LocationInf auto CIE_PLI = lpp_PLI_CommonIEsProvideLocationInformation(*li, has_information); PLI->commonIEsProvideLocationInformation = CIE_PLI; + PLI->a_gnss_ProvideLocationInformation = + lpp_PLI_A_GNSS_ProvideLocationInformation(*li, has_information); return lpp; } @@ -368,8 +409,9 @@ lpp_PLI_get_ECID_ProvideLocationInformation(ECIDInformation* ecid, bool has_info CellGlobalIdEUTRA_AndUTRA_t* CGI = ALLOC_ZERO(CellGlobalIdEUTRA_AndUTRA_t); CGI->cellIdentity.present = CellGlobalIdEUTRA_AndUTRA__cellIdentity_PR_eutra; - auto eutra = BitString::allocate(28, &CGI->cellIdentity.choice.eutra); - eutra->set_integer(0, 28, ecid->cell.cell); + BitStringBuilder{} + .integer(0, 28, ecid->cell.cell) + .into_bit_string(28, &CGI->cellIdentity.choice.eutra); int mcc = ecid->cell.mcc; int temp = (int)(mcc - mcc % 100) / 100; diff --git a/libs/lpplib/src/lpp.cpp b/libs/lpplib/src/lpp.cpp index 4a1b160e..e7e5993c 100644 --- a/libs/lpplib/src/lpp.cpp +++ b/libs/lpplib/src/lpp.cpp @@ -2,16 +2,19 @@ #include "internal_lpp.h" #include "supl.h" +#include #include LPP_Client::LPP_Client(bool segmentation) { - connected = false; - main_request = AD_REQUEST_INVALID; - provide_li.type = -1; - transaction_counter = 1; - client_id = 0xC0DEC0DE; - mEnableSegmentation = segmentation; - mSUPL = std::make_unique(); + connected = false; + main_request = AD_REQUEST_INVALID; + provide_li.type = -1; + transaction_counter = 1; + client_id = 0xC0DEC0DE; + mEnableSegmentation = segmentation; + mForceLocationInformation = false; + mSUPL = std::make_unique(); + mSuplIdentityFix = true; main_request_callback = nullptr; main_request_userdata = nullptr; @@ -21,14 +24,18 @@ LPP_Client::LPP_Client(bool segmentation) { LPP_Client::~LPP_Client() {} +void LPP_Client::use_incorrect_supl_identity() { + mSuplIdentityFix = false; +} + void LPP_Client::set_identity_msisdn(unsigned long msisdn) { if (connected) return; - mSUPL->set_session(SUPL_Session::msisdn(0, msisdn)); + mSUPL->set_session(SUPL_Session::msisdn(0, msisdn, mSuplIdentityFix)); } void LPP_Client::set_identity_imsi(unsigned long imsi) { if (connected) return; - mSUPL->set_session(SUPL_Session::imsi(0, imsi)); + mSUPL->set_session(SUPL_Session::imsi(0, imsi, mSuplIdentityFix)); } void LPP_Client::set_identity_ipv4(const std::string& ipv4) { @@ -59,6 +66,28 @@ bool LPP_Client::supl_start(CellID cell) { start->sETCapabilities.posProtocol.ver2_PosProtocol_extension = pos_protocol_ext; } + { + // Application Id + std::string client_name = "supl-3gpp-lpp-client"; + std::string client_provider = "ericsson"; + std::string client_version = CLIENT_VERSION; + + if (mSuplIdentityFix) { + client_name += "/sif"; + } + + auto application_id = ALLOC_ZERO(ApplicationID); + OCTET_STRING_fromString(&application_id->appName, client_name.c_str()); + OCTET_STRING_fromString(&application_id->appProvider, client_provider.c_str()); + + application_id->appVersion = ALLOC_ZERO(IA5String_t); + OCTET_STRING_fromString(application_id->appVersion, client_version.c_str()); + + auto ext = ALLOC_ZERO(Ver2_SUPL_START_extension); + ext->applicationID = application_id; + start->ver2_SUPL_START_extension = ext; + } + { auto cell_info = &start->locationId.cellInfo; cell_info->present = CellInfo_PR_ver2_CellInfo_extension; @@ -142,8 +171,14 @@ bool LPP_Client::supl_send_posinit(CellID cell) { return mSUPL->send(message); } -bool LPP_Client::supl_receive(std::vector& messages, int milliseconds) { - auto message = mSUPL->receive(milliseconds); +bool LPP_Client::supl_receive(std::vector& messages, int milliseconds, + bool blocking) { + SUPL_Message message{}; + if (blocking) { + message = mSUPL->receive(milliseconds); + } else { + message = mSUPL->receive2(); + } if (!message) { return false; } @@ -186,6 +221,10 @@ bool LPP_Client::supl_send(const std::vector& messages) { auto lpp = encode(data); if (lpp) { asn_sequence_add(&payload->list, lpp); + } else { +#if DEBUG_LPP_LIB + printf("ERROR: Failed to encode LPP message\n"); +#endif } } @@ -206,29 +245,41 @@ LPP_Message* LPP_Client::decode(OCTET_STRING* data) { } bool LPP_Client::connect(const std::string& host, int port, bool use_ssl, CellID supl_cell) { +#if DEBUG_LPP_LIB printf("DEBUG: Connecting to SUPL server %s:%d\n", host.c_str(), port); +#endif + // Initialize and connect to the location server if (!mSUPL->connect(host, port, use_ssl)) { +#if DEBUG_LPP_LIB printf("ERROR: Failed to connect to SUPL server\n"); +#endif return false; } // Send SUPL-START request if (!supl_start(supl_cell)) { +#if DEBUG_LPP_LIB printf("ERROR: Failed to send SUPL-START\n"); +#endif mSUPL->disconnect(); return false; } // Wait for SUPL-RESPONSE with slp session if (!supl_response()) { +#if DEBUG_LPP_LIB + printf("ERROR: Failed to receive SUPL-RESPONSE\n"); +#endif mSUPL->disconnect(); return false; } // Send SUPL-POSINIT if (!supl_send_posinit(supl_cell)) { +#if DEBUG_LPP_LIB printf("ERROR: Failed to send SUPL-POSINIT\n"); +#endif mSUPL->disconnect(); return false; } @@ -279,6 +330,9 @@ bool LPP_Client::process_message(LPP_Message* message, LPP_Transaction* transact auto message = lpp_provide_capabilities(transaction, mEnableSegmentation); if (!supl_send(message)) { +#if DEBUG_LPP_LIB + printf("ERROR: Failed to send LPP Provide Capabilities\n"); +#endif lpp_destroy(message); disconnect(); return false; @@ -293,24 +347,25 @@ bool LPP_Client::process_message(LPP_Message* message, LPP_Transaction* transact } bool LPP_Client::process() { + using namespace std::chrono; if (!mSUPL->is_connected()) { return false; } + int timeout = -1; if (provide_li.type >= 0) { - using namespace std::chrono; auto current = system_clock::now(); auto duration = current - provide_li.last; if (duration > std::chrono::seconds(1)) { handle_provide_location_information(&provide_li); - provide_li.last = current - (duration - std::chrono::seconds(1)); + provide_li.last = current; + } else { + timeout = duration_cast(duration).count(); } } - int timeout = -1; - std::vector messages; - if (!supl_receive(messages, timeout)) { + if (!supl_receive(messages, timeout, false)) { if (!mSUPL->is_connected()) { return false; } @@ -338,7 +393,7 @@ bool LPP_Client::wait_for_assistance_data_response(LPP_Transaction* transaction) auto last = time(NULL); while (time(NULL) - last < timeout_seconds && !ok) { std::vector messages; - if (!supl_receive(messages, timeout_seconds * 1000)) { + if (!supl_receive(messages, timeout_seconds * 1000, true)) { disconnect(); return false; } @@ -545,8 +600,18 @@ bool LPP_Client::handle_provide_location_information(LPP_Client::ProvideLI* pli) assert(message); if (!supl_send(message)) { + printf("ERROR: Failed to send LPP message\n"); return false; } return false; -} \ No newline at end of file +} + +void LPP_Client::force_location_information() { + mForceLocationInformation = true; + provide_li.type = LocationInformationType_locationEstimateRequired; + provide_li.last = std::chrono::system_clock::now(); + provide_li.transaction.id = 200; + provide_li.transaction.end = 0; + provide_li.transaction.initiator = 0; +} diff --git a/libs/lpplib/src/supl.cpp b/libs/lpplib/src/supl.cpp index 0cd85bbe..127a27b5 100644 --- a/libs/lpplib/src/supl.cpp +++ b/libs/lpplib/src/supl.cpp @@ -20,7 +20,9 @@ void ASN_Deleter::operator()(OCTET_STRING* ptr) { // // -static void binary_coded_decimal(unsigned long value, unsigned char* buf) { +static void binary_coded_decimal(unsigned long value, unsigned char* buf, bool switch_order) { + // `switch_order` = false, this will encode it in the wrong order + // msisdn, mnd and imsi are a BCD (Binary Coded Decimal) string // represent digits from 0 through 9, // two digits per octet, each digit encoded 0000 to 1001 (0 to 9) @@ -30,7 +32,7 @@ static void binary_coded_decimal(unsigned long value, unsigned char* buf) { unsigned char digits[16]; int i = 0; while (value > 0) { - if(i >= 16) { + if (i >= 16) { break; } digits[i] = value % 10; @@ -43,8 +45,8 @@ static void binary_coded_decimal(unsigned long value, unsigned char* buf) { } int j = 0; - while(i > 0) { - if(j % 2 == 0) { + while (i > 0) { + if (j % 2 == (switch_order ? 1 : 0)) { buf[j / 2] |= digits[i - 1] << 4; } else { buf[j / 2] |= digits[i - 1]; @@ -53,8 +55,8 @@ static void binary_coded_decimal(unsigned long value, unsigned char* buf) { j++; } - while(j < 16) { - if(j % 2 == 0) { + while (j < 16) { + if (j % 2 == 0) { buf[j / 2] |= 0xf0; } else { buf[j / 2] |= 0xf; @@ -63,25 +65,26 @@ static void binary_coded_decimal(unsigned long value, unsigned char* buf) { } } -std::unique_ptr SUPL_Session::msisdn(long id, unsigned long msisdn) { +std::unique_ptr SUPL_Session::msisdn(long id, unsigned long msisdn, + bool switch_order) { auto set = ALLOC_ZERO(SetSessionID); set->sessionId = id; set->setId.present = SETId_PR_msisdn; unsigned char buf[8]; - binary_coded_decimal(msisdn, buf); + binary_coded_decimal(msisdn, buf, switch_order); OCTET_STRING_fromBuf(&set->setId.choice.msisdn, reinterpret_cast(buf), sizeof(buf)); return std::unique_ptr(new SUPL_Session(set, nullptr)); } -std::unique_ptr SUPL_Session::imsi(long id, unsigned long imsi) { +std::unique_ptr SUPL_Session::imsi(long id, unsigned long imsi, bool switch_order) { auto set = ALLOC_ZERO(SetSessionID); set->sessionId = id; set->setId.present = SETId_PR_imsi; unsigned char buf[8]; - binary_coded_decimal(imsi, buf); + binary_coded_decimal(imsi, buf, switch_order); OCTET_STRING_fromBuf(&set->setId.choice.imsi, reinterpret_cast(buf), sizeof(buf)); return std::unique_ptr(new SUPL_Session(set, nullptr)); @@ -164,7 +167,8 @@ SUPL_Session::~SUPL_Session() { // // -SUPL_Client::SUPL_Client() : mTCP(std::make_unique()), mSession(nullptr) {} +SUPL_Client::SUPL_Client() + : mTCP(std::make_unique()), mSession(nullptr), mReceiveLength(0) {} SUPL_Client::~SUPL_Client() {} @@ -189,6 +193,95 @@ bool SUPL_Client::is_connected() { return mTCP->is_connected(); } +SUPL_Message SUPL_Client::process() { + if (mReceiveLength < 16) { + return nullptr; + } + + auto pdu = (ULP_PDU*)nullptr; + long expected_size = 0; + + // NOTE: Some SUPL messages are very big, e.g. supl.google.com SUPLPOS with + // ephemeris. This means that sometimes the 'buffer' will not contain the + // whole message but only apart of it. This means we need to wait for the + // rest of the data. Try to decode the message and if it failed with + // RC_WMORE wait for more data and try again. + auto result = + uper_decode_complete(0, &asn_DEF_ULP_PDU, (void**)&pdu, mReceiveBuffer, mReceiveLength); + if (result.code == RC_FAIL) { + mReceiveLength = 0; + return nullptr; + } else if (result.code == RC_WMORE) { + expected_size = pdu->length; + if (expected_size > sizeof(mReceiveBuffer)) { + // Unable to handle such big messages + mReceiveLength = 0; + return nullptr; + } else if (expected_size > mReceiveLength) { + // Not enough data + return nullptr; + } + + result = + uper_decode_complete(0, &asn_DEF_ULP_PDU, (void**)&pdu, mReceiveBuffer, expected_size); + if (result.code != RC_OK) { + mReceiveLength = 0; + return nullptr; + } + } else { + expected_size = pdu->length; + } + + // Remove the message from the buffer + if(expected_size > mReceiveLength) { + mReceiveLength = 0; + } + + memmove(mReceiveBuffer, mReceiveBuffer + expected_size, mReceiveLength - expected_size); + mReceiveLength -= expected_size; + + // xer_fprint(stdout, &asn_DEF_ULP_PDU, pdu); + return ASN_Unique(pdu, {}); +} + +SUPL_Message SUPL_Client::receive2() { + if (!mTCP->is_connected()) { + return nullptr; + } + + // Try to parse the message from the buffer + auto message = process(); + if (message) { + return message; + } + + // If the buffer is full, clear it + if (mReceiveLength > SUPL_CLIENT_RECEIVER_BUFFER_SIZE - 64) { + mReceiveLength = 0; + } + + // Receive more data + auto bytes = mTCP->receive(mReceiveBuffer + mReceiveLength, + SUPL_CLIENT_RECEIVER_BUFFER_SIZE - mReceiveLength, 0); + if (bytes <= 0) { + return nullptr; + } + + mReceiveLength += bytes; + if(mReceiveLength >= SUPL_CLIENT_RECEIVER_BUFFER_SIZE) { + mReceiveLength = 0; + return nullptr; + } + + // Try again to parse the message from the buffer + message = process(); + if (message) { + return message; + } + + return nullptr; +} + SUPL_Message SUPL_Client::receive(int milliseconds) { if (!mTCP->is_connected()) { return nullptr; @@ -233,7 +326,7 @@ SUPL_Message SUPL_Client::receive(int milliseconds) { } } - //xer_fprint(stdout, &asn_DEF_ULP_PDU, pdu); + // xer_fprint(stdout, &asn_DEF_ULP_PDU, pdu); return ASN_Unique(pdu, {}); } @@ -255,6 +348,8 @@ SUPL_EncodedMessage SUPL_Client::encode(SUPL_Message& message) { // Determine the PDU length auto result = uper_encode_to_length(&asn_DEF_ULP_PDU, NULL, pdu); if (result.encoded < 0) { + printf("ERROR: Failed to determine PDU length\n"); + printf("ERROR: %s\n", result.failed_type->name); return nullptr; } @@ -271,11 +366,13 @@ SUPL_EncodedMessage SUPL_Client::encode(SUPL_Message& message) { // Encode PDU as UPER result = uper_encode_to_buffer(&asn_DEF_ULP_PDU, NULL, pdu, buffer, length); if (result.encoded < 0) { + printf("ERROR: Failed to encode PDU\n"); return nullptr; } // Make sure that the PDU stayed intact and that length wasn't adjusted. if (length != (result.encoded + 7) / 8) { + printf("ERROR: PDU length mismatch\n"); return nullptr; } @@ -289,11 +386,13 @@ bool SUPL_Client::send(SUPL_Message& message) { auto encoded_message = encode(message); if (!encoded_message) { + printf("ERROR: Failed to encode message\n"); return false; } auto bytes = mTCP->send(encoded_message->buf, encoded_message->size); if (bytes != encoded_message->size) { + printf("ERROR: Failed to send message\n"); return false; } diff --git a/libs/lpplib/src/tcp.cpp b/libs/lpplib/src/tcp.cpp index 611dffef..e6209aac 100644 --- a/libs/lpplib/src/tcp.cpp +++ b/libs/lpplib/src/tcp.cpp @@ -180,9 +180,9 @@ int TCP_Client::receive(void* buffer, int size, int milliseconds) { timeout.tv_usec = (milliseconds % 1000) * 1000; auto timeout_ptr = &timeout; - if (milliseconds == 0) { - timeout_ptr = NULL; - } + // if (milliseconds == 0) { + // timeout_ptr = NULL; + // } fd_set sock; FD_ZERO(&sock); @@ -217,15 +217,6 @@ int TCP_Client::receive(void* buffer, int size, int milliseconds) { } int TCP_Client::send(void* buffer, int size) { -#if 0 - // hexdump - for (int i = 0; i < size; i++) { - printf("%02x ", ((uint8_t*)buffer)[i]); - if (i % 16 == 15) { - printf("\n"); - } - } -#endif #if USE_OPENSSL if (mUseSSL) return SSL_write(mSSL, buffer, size); diff --git a/libs/utility/include/utility/time.h b/libs/utility/include/utility/time.h index 17e4f0dc..0ff616b7 100644 --- a/libs/utility/include/utility/time.h +++ b/libs/utility/include/utility/time.h @@ -1,4 +1,5 @@ #pragma once +#include #include constexpr s64 DAYS_PER_WEEK = 7LL; diff --git a/libs/utility/include/utility/time/tai_time.h b/libs/utility/include/utility/time/tai_time.h index 5257bb3d..4d0f07f1 100644 --- a/libs/utility/include/utility/time/tai_time.h +++ b/libs/utility/include/utility/time/tai_time.h @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include @@ -21,7 +21,7 @@ class TAI_Time { explicit TAI_Time(const BDT_Time& time); NO_DISCARD Timestamp timestamp() const { return tm; } - NO_DISCARD std::string rtklib_time_string(); + NO_DISCARD std::string rtklib_time_string() const; NO_DISCARD static TAI_Time now(); diff --git a/libs/utility/include/utility/time/utc_time.h b/libs/utility/include/utility/time/utc_time.h index 14aade63..f6088dd8 100644 --- a/libs/utility/include/utility/time/utc_time.h +++ b/libs/utility/include/utility/time/utc_time.h @@ -1,6 +1,6 @@ #pragma once -#include #include +#include #include @@ -8,14 +8,17 @@ class UTC_Time { public: UTC_Time() = default; explicit UTC_Time(const Timestamp& timestamp) : tm{timestamp} {} + explicit UTC_Time(s64 days, f64 tod); explicit UTC_Time(const TAI_Time& time); explicit UTC_Time(const GPS_Time& time); explicit UTC_Time(const GLO_Time& time); explicit UTC_Time(const GST_Time& time); explicit UTC_Time(const BDT_Time& time); + NO_DISCARD s64 days() const; + NO_DISCARD Timestamp timestamp() const { return tm; } - NO_DISCARD std::string rtklib_time_string(); + NO_DISCARD std::string rtklib_time_string() const; UTC_Time& add(s64 seconds) { tm.add(seconds); diff --git a/libs/utility/include/utility/types.h b/libs/utility/include/utility/types.h index 24214bdb..9b9ad5d4 100644 --- a/libs/utility/include/utility/types.h +++ b/libs/utility/include/utility/types.h @@ -1,12 +1,12 @@ #pragma once +#include #include #include #include #include #include #include -#include typedef uint8_t u8; typedef int8_t s8; @@ -22,6 +22,7 @@ typedef double f64; #if defined(__has_cpp_attribute) #if __has_cpp_attribute(nodiscard) +#if defined(__cplusplus) && __cplusplus >= 201703L #define NO_DISCARD [[nodiscard]] #else #define NO_DISCARD @@ -29,6 +30,9 @@ typedef double f64; #else #define NO_DISCARD #endif +#else +#define NO_DISCARD +#endif #ifndef UNUSED #if defined(__has_cpp_attribute) diff --git a/libs/utility/src/time/tai_time.cpp b/libs/utility/src/time/tai_time.cpp index 2c92fead..6e454f64 100644 --- a/libs/utility/src/time/tai_time.cpp +++ b/libs/utility/src/time/tai_time.cpp @@ -16,7 +16,7 @@ TAI_Time::TAI_Time(const GST_Time& time) : TAI_Time(UTC_Time(time)) {} TAI_Time::TAI_Time(const BDT_Time& time) : TAI_Time(UTC_Time(time)) {} TAI_Time::TAI_Time(const UTC_Time& time) : tm(utc_2_tai(time.timestamp())) {} -std::string TAI_Time::rtklib_time_string() { +std::string TAI_Time::rtklib_time_string() const { return UTC_Time{*this}.rtklib_time_string(); } diff --git a/libs/utility/src/time/utc_time.cpp b/libs/utility/src/time/utc_time.cpp index e7c6fa23..0b078348 100644 --- a/libs/utility/src/time/utc_time.cpp +++ b/libs/utility/src/time/utc_time.cpp @@ -1,8 +1,7 @@ #include - -#include - #include +#include +#include // NOTE: The day each month of the year starts with. constexpr static std::array day_of_year = { @@ -108,22 +107,28 @@ static TimeEpoch date_from_utc(Timestamp time) { }; } +UTC_Time::UTC_Time(s64 days, f64 tod) : tm(static_cast(days * DAY_IN_SECONDS) + tod) {} UTC_Time::UTC_Time(const TAI_Time& time) : tm(time.utc_timestamp()) {} UTC_Time::UTC_Time(const GPS_Time& time) : tm(time.utc_timestamp()) {} UTC_Time::UTC_Time(const GLO_Time& time) : tm(time.utc_timestamp()) {} UTC_Time::UTC_Time(const GST_Time& time) : tm(time.utc_timestamp()) {} UTC_Time::UTC_Time(const BDT_Time& time) : tm(time.utc_timestamp()) {} -std::string UTC_Time::rtklib_time_string() { +s64 UTC_Time::days() const { + return tm.seconds() / DAY_IN_SECONDS; +} + +std::string UTC_Time::rtklib_time_string() const { constexpr int fraction_digits = 12; auto ts = timestamp(); auto epoch = date_from_utc(ts); std::array buffer; - snprintf(buffer.data(), buffer.size(), "%04" PRId64 "/%02" PRId64 "/%02" PRId64 " %02" PRId64 ":%02" PRId64 ":%0*.*f", epoch.year, - epoch.month + 1, epoch.day + 1, epoch.hour, epoch.minutes, fraction_digits + 3, - fraction_digits, epoch.seconds); + snprintf(buffer.data(), buffer.size(), + "%04" PRId64 "/%02" PRId64 "/%02" PRId64 " %02" PRId64 ":%02" PRId64 ":%0*.*f", + epoch.year, epoch.month + 1, epoch.day + 1, epoch.hour, epoch.minutes, + fraction_digits + 3, fraction_digits, epoch.seconds); return std::string{buffer.data()}; } diff --git a/receiver/nmea/CMakeLists.txt b/receiver/nmea/CMakeLists.txt new file mode 100644 index 00000000..af466c17 --- /dev/null +++ b/receiver/nmea/CMakeLists.txt @@ -0,0 +1,53 @@ + +add_library(receiver_nmea STATIC + "message.cpp" + "parser.cpp" + "receiver.cpp" + "threaded_receiver.cpp" + "gga.cpp" + "vtg.cpp" + "gst.cpp" +) +add_library(receiver::nmea ALIAS receiver_nmea) + +target_include_directories(receiver_nmea PRIVATE "./" "include/receiver/nmea/") +target_include_directories(receiver_nmea PUBLIC "include/") +target_link_libraries(receiver_nmea PRIVATE dependency::interface) +target_link_libraries(receiver_nmea PRIVATE utility) + +if (USE_ASAN) +target_compile_options(receiver_nmea PRIVATE -fsanitize=address,undefined,leak) +target_link_libraries(receiver_nmea PRIVATE -fsanitize=address,undefined,leak) +endif (USE_ASAN) + +target_compile_options(receiver_nmea PRIVATE + "-Wall" + "-Wextra" + "-Wpedantic" + "-Wnon-virtual-dtor" + "-Wold-style-cast" + "-Wcast-align" + "-Woverloaded-virtual" + "-Wsign-conversion" + "-Wno-conversion" +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options(receiver_nmea PRIVATE + "-Wmisleading-indentation" + ) +endif() + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU") + target_compile_options(receiver_nmea PRIVATE + "-Wno-missing-field-initializers" + ) +endif() + +if(${RECEIVER_NMEA_THREADED}) + target_compile_definitions(receiver_nmea PUBLIC RECEIVER_NMEA_THREADED=1) +endif() + +if(${RECEIVER_NMEA_DEBUG}) + target_compile_definitions(receiver_nmea PUBLIC RECEIVER_NMEA_DEBUG=1) +endif() diff --git a/receiver/nmea/gga.cpp b/receiver/nmea/gga.cpp new file mode 100644 index 00000000..9eef1d23 --- /dev/null +++ b/receiver/nmea/gga.cpp @@ -0,0 +1,184 @@ +#include +#include "helper.hpp" + +namespace receiver { +namespace nmea { + +// parse UTC time of day from string "hhmmss.sss" +static bool parse_utc(const std::string& utc, TAI_Time& time_of_day) { + try { + auto tokens = split(utc, '.'); + if (tokens.size() != 2) { + return false; + } + + auto hours = std::stoi(tokens[0].substr(0, 2)); + auto minutes = std::stoi(tokens[0].substr(2, 2)); + auto seconds = std::stoi(tokens[0].substr(4, 2)); + auto milliseconds = std::stoi(tokens[1]); + + auto tod = + hours * HOUR_IN_SECONDS + minutes * MINUTE_IN_SECONDS + seconds + milliseconds * 1e-3; + auto utc_now = UTC_Time::now(); + auto utc_then = UTC_Time{utc_now.days(), tod}; + time_of_day = TAI_Time{utc_then}; + return true; + } catch (...) { + return false; + } +} + +// parse latitude from string "ddmm.mmmm*" +static bool parse_latitude(const std::string& latitude, const std::string& nw_indicator, + double& lat) { + try { + auto degrees = std::stod(latitude.substr(0, 2)); + auto minutes = std::stod(latitude.substr(2)); + auto value = degrees + minutes / 60.0; + + if (nw_indicator == "S") { + lat = -value; + return true; + } else if (nw_indicator == "N") { + lat = value; + return true; + } else { + return false; + } + } catch (...) { + return false; + } +} + +// parse longitude from string "dddmm.mmmm*" +static bool parse_longitude(const std::string& longitude, const std::string& ew_indicator, + double& lon) { + try { + auto degrees = std::stod(longitude.substr(0, 3)); + auto minutes = std::stod(longitude.substr(3)); + auto value = degrees + minutes / 60.0; + + if (ew_indicator == "W") { + lon = -value; + return true; + } else if (ew_indicator == "E") { + lon = value; + return true; + } else { + return false; + } + } catch (...) { + return false; + } +} + +static bool parse_fix_quality(const std::string& fix_quality, GgaFixQuality& quality) { + try { + auto value = std::stoi(fix_quality); + switch (value) { + case 0: quality = GgaFixQuality::Invalid; break; + case 1: quality = GgaFixQuality::GpsFix; break; + case 2: quality = GgaFixQuality::DgpsFix; break; + case 3: quality = GgaFixQuality::PpsFix; break; + case 4: quality = GgaFixQuality::RtkFixed; break; + case 5: quality = GgaFixQuality::RtkFloat; break; + case 6: quality = GgaFixQuality::DeadReckoning; break; + default: return false; + } + + return true; + } catch (...) { + return false; + } +} + +static bool parse_satellites_in_view(const std::string& satellites_in_view, int& satellites) { + try { + satellites = std::stoi(satellites_in_view); + return true; + } catch (...) { + return false; + } +} + +static bool parse_hdop(const std::string& hdop, double& value) { + try { + value = std::stod(hdop); + return true; + } catch (...) { + return false; + } +} + +static bool parse_altitude(const std::string& altitude, const std::string& units, double& value) { + try { + value = std::stod(altitude); + if (units == "M") { + return true; + } else { + return false; + } + } catch (...) { + return false; + } +} + +GgaMessage::GgaMessage(std::string prefix) NMEA_NOEXCEPT : Message{prefix}, + mTimeOfDay{TAI_Time::now()}, + mLatitude{0.0}, + mLongitude{0.0}, + mFixQuality{GgaFixQuality::Invalid}, + mSatellitesInView{0} {} + +void GgaMessage::print() const NMEA_NOEXCEPT { + printf("[%5s]\n", prefix().c_str()); + printf(" time of day: %s\n", time_of_day().rtklib_time_string().c_str()); + printf(" latitude: %.8f\n", latitude()); + printf(" longitude: %.8f\n", longitude()); + printf(" fix quality: "); + switch (fix_quality()) { + case GgaFixQuality::Invalid: printf("invalid"); break; + case GgaFixQuality::GpsFix: printf("gps fix"); break; + case GgaFixQuality::DgpsFix: printf("dgps fix"); break; + case GgaFixQuality::PpsFix: printf("pps fix"); break; + case GgaFixQuality::RtkFixed: printf("rtk fixed"); break; + case GgaFixQuality::RtkFloat: printf("rtk float"); break; + case GgaFixQuality::DeadReckoning: printf("dead reckoning"); break; + } + printf(" (%d)\n", static_cast(fix_quality())); + printf(" satellites: %d\n", satellites_in_view()); + printf(" hdop: %.4f\n", h_dop()); + printf(" altitude: %.2f\n", altitude()); +} + +std::unique_ptr GgaMessage::parse(std::string prefix, const std::string& payload) { + // split payload by ',' + auto tokens = split(payload, ','); + + // check number of tokens + if (tokens.size() < 13) { + return nullptr; + } + + // parse + auto message = new GgaMessage(prefix); + auto success = true; + success &= parse_utc(tokens[0], message->mTimeOfDay); + success &= parse_latitude(tokens[1], tokens[2], message->mLatitude); + success &= parse_longitude(tokens[3], tokens[4], message->mLongitude); + success &= parse_fix_quality(tokens[5], message->mFixQuality); + success &= parse_satellites_in_view(tokens[6], message->mSatellitesInView); + success &= parse_hdop(tokens[7], message->mHdop); + success &= parse_altitude(tokens[8], tokens[9], message->mMsl); + success &= parse_altitude(tokens[10], tokens[11], message->mGeoidSeparation); + + if (success) { + return std::unique_ptr(message); + } else { + delete message; + return nullptr; + } +} + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/gst.cpp b/receiver/nmea/gst.cpp new file mode 100644 index 00000000..bd056317 --- /dev/null +++ b/receiver/nmea/gst.cpp @@ -0,0 +1,83 @@ +#include +#include "helper.hpp" + +namespace receiver { +namespace nmea { + +static bool parse_double(const std::string& token, double& value) { + try { + value = std::stod(token); + return true; + } catch (...) { +#if RECEIVER_NMEA_DEBUG + printf("[--GST] failed to parse double: \"%s\"\n", token.c_str()); +#endif + return false; + } +} + +static bool parse_double_opt(const std::string& token, double& value) { + try { + value = std::stod(token); + return true; + } catch (...) { + value = 0; + return true; + } +} + +GstMessage::GstMessage(std::string prefix) NMEA_NOEXCEPT : Message{prefix}, + mRmsValue{0.0}, + mSemiMajorError{0.0}, + mSemiMinorError{0.0}, + mOrientationOfSemiMajorError{0.0}, + mLatitudeError{0.0}, + mLongitudeError{0.0}, + mAltitudeError{0.0} {} +void GstMessage::print() const NMEA_NOEXCEPT { + printf("[%5s]\n", prefix().c_str()); + printf(" rms value: %f\n", mRmsValue); + printf(" semi-major error: %f\n", mSemiMajorError); + printf(" semi-minor error: %f\n", mSemiMinorError); + printf(" orientation of semi-major error: %f\n", mOrientationOfSemiMajorError); + printf(" latitude error: %f\n", mLatitudeError); + printf(" longitude error: %f\n", mLongitudeError); + printf(" altitude error: %f\n", mAltitudeError); +} + +std::unique_ptr GstMessage::parse(std::string prefix, const std::string& payload) { + // split payload by ',' + auto tokens = split(payload, ','); + + // check number of tokens + if (tokens.size() < 8) { +#if RECEIVER_NMEA_DEBUG + printf("[--GST] invalid number of tokens: %zu\n", tokens.size()); +#endif + return nullptr; + } + + // parse + auto message = new GstMessage(prefix); + auto success = true; + success &= parse_double_opt(tokens[1], message->mRmsValue); + success &= parse_double_opt(tokens[2], message->mSemiMajorError); + success &= parse_double_opt(tokens[3], message->mSemiMinorError); + success &= parse_double_opt(tokens[4], message->mOrientationOfSemiMajorError); + success &= parse_double_opt(tokens[5], message->mLatitudeError); + success &= parse_double_opt(tokens[6], message->mLongitudeError); + success &= parse_double_opt(tokens[7], message->mAltitudeError); + + if (success) { + return std::unique_ptr(message); + } else { +#if RECEIVER_NMEA_DEBUG + printf("[--GST] failed to parse message\n"); +#endif + delete message; + return nullptr; + } +} + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/helper.hpp b/receiver/nmea/helper.hpp new file mode 100644 index 00000000..2b52fd22 --- /dev/null +++ b/receiver/nmea/helper.hpp @@ -0,0 +1,14 @@ +#pragma once +#include +#include +#include + +static std::vector split(const std::string& str, char delim) { + std::vector tokens; + std::string token; + std::istringstream token_stream(str); + while (std::getline(token_stream, token, delim)) { + tokens.push_back(token); + } + return tokens; +} diff --git a/receiver/nmea/include/receiver/nmea/gga.hpp b/receiver/nmea/include/receiver/nmea/gga.hpp new file mode 100644 index 00000000..cca01af0 --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/gga.hpp @@ -0,0 +1,74 @@ +#pragma once +#include +#include + +#include +#include + +namespace receiver { +namespace nmea { + +enum class GgaFixQuality { + Invalid = 0, + GpsFix = 1, + DgpsFix = 2, + PpsFix = 3, + RtkFixed = 4, + RtkFloat = 5, + DeadReckoning = 6, +}; + +class GgaMessage final : public Message { +public: + ~GgaMessage() override = default; + + GgaMessage(const GgaMessage& other) + : Message(other), mTimeOfDay(other.mTimeOfDay), mLatitude(other.mLatitude), + mLongitude(other.mLongitude), mFixQuality(other.mFixQuality), + mSatellitesInView(other.mSatellitesInView), mHdop(other.mHdop), mMsl(other.mMsl), + mGeoidSeparation(other.mGeoidSeparation) {} + GgaMessage(GgaMessage&&) = delete; + GgaMessage& operator=(const GgaMessage&) = delete; + GgaMessage& operator=(GgaMessage&&) = delete; + + void print() const NMEA_NOEXCEPT override; + + /// Get the time of day. + NMEA_NODISCARD const TAI_Time& time_of_day() const NMEA_NOEXCEPT { return mTimeOfDay; } + + /// Get the latitude. + NMEA_NODISCARD double latitude() const NMEA_NOEXCEPT { return mLatitude; } + + /// Get the longitude. + NMEA_NODISCARD double longitude() const NMEA_NOEXCEPT { return mLongitude; } + + /// Get the fix quality. + NMEA_NODISCARD GgaFixQuality fix_quality() const NMEA_NOEXCEPT { return mFixQuality; } + + /// Get the number of satellites in view. + NMEA_NODISCARD int satellites_in_view() const NMEA_NOEXCEPT { return mSatellitesInView; } + + /// Get the horizontal dilution of precision. + NMEA_NODISCARD double h_dop() const NMEA_NOEXCEPT { return mHdop; } + + /// Get the altitude in meters. + NMEA_NODISCARD double altitude() const NMEA_NOEXCEPT { return mMsl + mGeoidSeparation; } + + NMEA_NODISCARD static std::unique_ptr parse(std::string prefix, + const std::string& payload); + +private: + NMEA_EXPLICIT GgaMessage(std::string prefix) NMEA_NOEXCEPT; + + TAI_Time mTimeOfDay; + double mLatitude; + double mLongitude; + GgaFixQuality mFixQuality; + int mSatellitesInView; + double mHdop; + double mMsl; + double mGeoidSeparation; +}; + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/include/receiver/nmea/gst.hpp b/receiver/nmea/include/receiver/nmea/gst.hpp new file mode 100644 index 00000000..77ef3a8c --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/gst.hpp @@ -0,0 +1,53 @@ +#pragma once +#include +#include + +#include +#include +#include + +namespace receiver { +namespace nmea { + +class GstMessage final : public Message { +public: + ~GstMessage() override = default; + + GstMessage(const GstMessage& other) + : Message(other), mRmsValue(other.mRmsValue), mSemiMajorError(other.mSemiMajorError), + mSemiMinorError(other.mSemiMinorError), + mOrientationOfSemiMajorError(other.mOrientationOfSemiMajorError), + mLatitudeError(other.mLatitudeError), mLongitudeError(other.mLongitudeError), + mAltitudeError(other.mAltitudeError) {} + GstMessage(GstMessage&&) = delete; + GstMessage& operator=(const GstMessage&) = delete; + GstMessage& operator=(GstMessage&&) = delete; + + void print() const NMEA_NOEXCEPT override; + + /// Get the horizontal position error. + NMEA_NODISCARD double horizontal_position_error() const NMEA_NOEXCEPT { + // HPE = sqrt(semiMajorError^2 + semiMinorError^2) + return sqrt(mSemiMajorError * mSemiMajorError + mSemiMinorError * mSemiMinorError); + } + + /// Get the vertical position error. + NMEA_NODISCARD double vertical_position_error() const NMEA_NOEXCEPT { return mAltitudeError; } + + NMEA_NODISCARD static std::unique_ptr parse(std::string prefix, + const std::string& payload); + +private: + NMEA_EXPLICIT GstMessage(std::string prefix) NMEA_NOEXCEPT; + + double mRmsValue; + double mSemiMajorError; + double mSemiMinorError; + double mOrientationOfSemiMajorError; + double mLatitudeError; + double mLongitudeError; + double mAltitudeError; +}; + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/include/receiver/nmea/message.hpp b/receiver/nmea/include/receiver/nmea/message.hpp new file mode 100644 index 00000000..e84af483 --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/message.hpp @@ -0,0 +1,72 @@ +#pragma once +#include + +#include + +namespace receiver { +namespace nmea { + +/// Base class for all messages. +class Message { +public: + NMEA_EXPLICIT Message(std::string prefix) NMEA_NOEXCEPT; + virtual ~Message() = default; + + Message(const Message& other) : mPrefix(other.mPrefix) {} + Message(Message&&) = delete; + Message& operator=(const Message&) = delete; + Message& operator=(Message&&) = delete; + + /// Get the message prefix, e.g. "$GPGGA". + NMEA_NODISCARD const std::string& prefix() const NMEA_NOEXCEPT { return mPrefix; } + + /// Print the message to stdout. + virtual void print() const NMEA_NOEXCEPT = 0; + +private: + std::string mPrefix; +}; + +/// Unsupported or unknown message. +class UnsupportedMessage final : public Message { +public: + NMEA_EXPLICIT UnsupportedMessage(std::string prefix, std::string payload) NMEA_NOEXCEPT; + ~UnsupportedMessage() override = default; + + UnsupportedMessage(const UnsupportedMessage& other) + : Message(other), mPayload(other.mPayload) {} + UnsupportedMessage(UnsupportedMessage&&) = delete; + UnsupportedMessage& operator=(const UnsupportedMessage&) = delete; + UnsupportedMessage& operator=(UnsupportedMessage&&) = delete; + + void print() const NMEA_NOEXCEPT override; + + /// Get the message payload. + NMEA_NODISCARD const std::string& payload() const NMEA_NOEXCEPT { return mPayload; } + +private: + std::string mPayload; +}; + +/// Error message. This is used to indicate that the message could not be parsed. +class ErrorMessage final : public Message { +public: + NMEA_EXPLICIT ErrorMessage(std::string prefix, std::string payload) NMEA_NOEXCEPT; + ~ErrorMessage() override = default; + + ErrorMessage(const ErrorMessage& other) : Message(other), mPayload(other.mPayload) {} + ErrorMessage(ErrorMessage&&) = delete; + ErrorMessage& operator=(const ErrorMessage&) = delete; + ErrorMessage& operator=(ErrorMessage&&) = delete; + + void print() const NMEA_NOEXCEPT override; + + /// Get the message payload. + NMEA_NODISCARD const std::string& payload() const NMEA_NOEXCEPT { return mPayload; } + +private: + std::string mPayload; +}; + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/include/receiver/nmea/parser.hpp b/receiver/nmea/include/receiver/nmea/parser.hpp new file mode 100644 index 00000000..9dccd034 --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/parser.hpp @@ -0,0 +1,46 @@ +#pragma once +#include +#include + +namespace receiver { +namespace nmea { + +enum class ChecksumResult { + OK = 0, + INVALID_STRING_NOSTAR, + INVALID_STRING_LENGTH, + INVALID_VALUE, +}; + +class Message; +class Parser { +public: + NMEA_EXPLICIT Parser() NMEA_NOEXCEPT; + ~Parser() NMEA_NOEXCEPT; + + bool append(uint8_t* data, uint16_t length) NMEA_NOEXCEPT; + void clear() NMEA_NOEXCEPT; + + NMEA_NODISCARD std::unique_ptr try_parse() NMEA_NOEXCEPT; + NMEA_NODISCARD uint32_t buffer_length() const NMEA_NOEXCEPT; + NMEA_NODISCARD uint32_t available_space() const NMEA_NOEXCEPT; + + NMEA_NODISCARD static ChecksumResult checksum(const std::string& buffer); + +protected: + NMEA_NODISCARD uint8_t peek(uint32_t index) const NMEA_NOEXCEPT; + void skip(uint32_t length) NMEA_NOEXCEPT; + void copy_to_buffer(uint8_t* data, uint32_t length) NMEA_NOEXCEPT; + + NMEA_NODISCARD std::string parse_prefix(const uint8_t* data, + uint32_t length) const NMEA_NOEXCEPT; + +private: + uint8_t* mBuffer; + uint32_t mBufferCapacity; + uint32_t mBufferRead; + uint32_t mBufferWrite; +}; + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/include/receiver/nmea/receiver.hpp b/receiver/nmea/include/receiver/nmea/receiver.hpp new file mode 100644 index 00000000..423c5e2c --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/receiver.hpp @@ -0,0 +1,47 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace interface { +class Interface; +} + +namespace receiver { +namespace nmea { + +class Message; +class Parser; +class NmeaReceiver { +public: + /// Construct a receiver. This will block until the receiver configuration has been + /// acquired. + NMEA_EXPLICIT NmeaReceiver(std::unique_ptr interface) NMEA_NOEXCEPT; + ~NmeaReceiver() NMEA_NOEXCEPT; + + /// Read bytes from the interface and append them to the parse buffer. This will not + /// block. + void process(); + + /// Block until a message is available. + /// @return A pointer to the message, or nullptr if the interface is closed. + NMEA_NODISCARD std::unique_ptr wait_for_message(); + + /// Try to parse a message in the parse buffer. This will not block. + /// @return A pointer to the message, or nullptr if no message is available. + NMEA_NODISCARD std::unique_ptr try_parse(); + + /// The receiver interface. + /// @return The receiver interface. + NMEA_NODISCARD interface::Interface& interface() const NMEA_NOEXCEPT { return *mInterface; } + +protected: +private: + std::unique_ptr mInterface; + Parser* mParser; +}; + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/include/receiver/nmea/threaded_receiver.hpp b/receiver/nmea/include/receiver/nmea/threaded_receiver.hpp new file mode 100644 index 00000000..109a9090 --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/threaded_receiver.hpp @@ -0,0 +1,64 @@ +#pragma once +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace receiver { +namespace nmea { + +/// A receiver that run in a separate thread. +class ThreadedReceiver { +public: + /// The receiver will be created on the thread, thus this will _not_ block. + NMEA_EXPLICIT ThreadedReceiver(std::unique_ptr interface, + bool print_messages) NMEA_NOEXCEPT; + ~ThreadedReceiver() NMEA_NOEXCEPT; + + /// Start the receiver thread. + void start(); + + /// Stop the receiver thread. + void stop(); + + /// Interface of the receiver. + /// @return A pointer to the interface, or nullptr if the receiver is not running. + NMEA_NODISCARD interface::Interface* interface() NMEA_NOEXCEPT; + + /// Get the last received GGA message. + /// @return A unique pointer to the message, or nullptr if no message has been received. + NMEA_NODISCARD std::unique_ptr gga() NMEA_NOEXCEPT; + + /// Get the last received VTG message. + /// @return A unique pointer to the message, or nullptr if no message has been received. + NMEA_NODISCARD std::unique_ptr vtg() NMEA_NOEXCEPT; + + /// Get the last received GST message. + /// @return A unique pointer to the message, or nullptr if no message has been received. + NMEA_NODISCARD std::unique_ptr gst() NMEA_NOEXCEPT; + +protected: + /// This function is called at the start of the receiver thread. It handles the blocking + /// communication with the receiver. + void run(); + +private: + std::unique_ptr mInterface; + std::unique_ptr mReceiver; + std::unique_ptr mThread; + std::atomic mRunning; + std::mutex mMutex; + bool mPrintMessages; + + std::unique_ptr mGga; + std::unique_ptr mVtg; + std::unique_ptr mGst; +}; + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/include/receiver/nmea/types.hpp b/receiver/nmea/include/receiver/nmea/types.hpp new file mode 100644 index 00000000..66cbc493 --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/types.hpp @@ -0,0 +1,55 @@ +#pragma once +#include +#include +#include + +#ifndef NMEA_EXPLICIT +#define NMEA_EXPLICIT explicit +#endif + +#ifndef NMEA_NOEXCEPT +#define NMEA_NOEXCEPT noexcept +#endif + +#ifndef NMEA_CONSTEXPR +#define NMEA_CONSTEXPR constexpr +#endif + +#ifndef NMEA_UNUSED +#if defined(__has_cpp_attribute) +#if __has_cpp_attribute(maybe_unused) +#define NMEA_UNUSED [[maybe_unused]] +#endif +#endif +#ifndef NMEA_UNUSED +#define NMEA_UNUSED +#endif +#endif + +#ifndef NMEA_NODISCARD +#if defined(__has_cpp_attribute) +#if __has_cpp_attribute(nodiscard) +#if defined(__cplusplus) && __cplusplus >= 201703L +#define NMEA_NODISCARD [[nodiscard]] +#endif +#endif +#endif +#ifndef NMEA_NODISCARD +#define NMEA_NODISCARD +#endif +#endif + +#ifndef NMEA_UNREACHABLE +#if defined(__has_builtin) +#if __has_builtin(__builtin_unreachable) +#define NMEA_UNREACHABLE() __builtin_unreachable() +#endif +#endif +#ifndef NMEA_UNREACHABLE +#define NMEA_UNREACHABLE() nmea_unreachable() +__attribute__((noreturn)) inline void nmea_unreachable() { + assert(false); +} +#endif +#endif + diff --git a/receiver/nmea/include/receiver/nmea/vtg.hpp b/receiver/nmea/include/receiver/nmea/vtg.hpp new file mode 100644 index 00000000..fb749876 --- /dev/null +++ b/receiver/nmea/include/receiver/nmea/vtg.hpp @@ -0,0 +1,56 @@ +#pragma once +#include +#include + +#include +#include + +namespace receiver { +namespace nmea { + +enum class ModeIndicator { + Unknown, + Autonomous = 'A', + Differential = 'D', +}; + +class VtgMessage final : public Message { +public: + ~VtgMessage() override = default; + + VtgMessage(const VtgMessage& other) + : Message(other), mTrueCourseOverGround(other.mTrueCourseOverGround), + mMagneticCourseOverGround(other.mMagneticCourseOverGround), + mSpeedOverGroundKnots(other.mSpeedOverGroundKnots), + mSpeedOverGroundKmh(other.mSpeedOverGroundKmh), mModeIndicator(other.mModeIndicator) {} + VtgMessage(VtgMessage&&) = delete; + VtgMessage& operator=(const VtgMessage&) = delete; + VtgMessage& operator=(VtgMessage&&) = delete; + + void print() const NMEA_NOEXCEPT override; + + /// Get the true course over ground in degrees from true north. + NMEA_NODISCARD double true_course_over_ground() const NMEA_NOEXCEPT { + return mTrueCourseOverGround; + } + + /// Get the speed over ground in meters per second. + NMEA_NODISCARD double speed_over_ground() const NMEA_NOEXCEPT { + return mSpeedOverGroundKmh / 3.6; + } + + NMEA_NODISCARD static std::unique_ptr parse(std::string prefix, + const std::string& payload); + +private: + NMEA_EXPLICIT VtgMessage(std::string prefix) NMEA_NOEXCEPT; + + double mTrueCourseOverGround; + double mMagneticCourseOverGround; + double mSpeedOverGroundKnots; + double mSpeedOverGroundKmh; + ModeIndicator mModeIndicator; +}; + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/message.cpp b/receiver/nmea/message.cpp new file mode 100644 index 00000000..3a129c40 --- /dev/null +++ b/receiver/nmea/message.cpp @@ -0,0 +1,35 @@ +#include "message.hpp" + +#include + +namespace receiver { +namespace nmea { + +Message::Message(std::string prefix) NMEA_NOEXCEPT : mPrefix(prefix) {} + +// +// +// + +UnsupportedMessage::UnsupportedMessage(std::string prefix, std::string payload) NMEA_NOEXCEPT + : Message(prefix), + mPayload(payload) {} + +void UnsupportedMessage::print() const NMEA_NOEXCEPT { + printf("[%5s] UNSUPPORTED %s\n", prefix().c_str(), payload().c_str()); +} + +// +// +// + +ErrorMessage::ErrorMessage(std::string prefix, std::string payload) NMEA_NOEXCEPT + : Message(prefix), + mPayload(payload) {} + +void ErrorMessage::print() const NMEA_NOEXCEPT { + printf("[%5s] ERROR %s\n", prefix().c_str(), payload().c_str()); +} + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/parser.cpp b/receiver/nmea/parser.cpp new file mode 100644 index 00000000..8c5da147 --- /dev/null +++ b/receiver/nmea/parser.cpp @@ -0,0 +1,233 @@ +#include "parser.hpp" +#include "gga.hpp" +#include "gst.hpp" +#include "message.hpp" +#include "vtg.hpp" + +#include + +namespace receiver { +namespace nmea { + +static NMEA_CONSTEXPR uint32_t NMEA_PARSER_BUFFER_SIZE = 16 * 1024; + +Parser::Parser() NMEA_NOEXCEPT : mBuffer(nullptr), + mBufferCapacity(0), + mBufferRead(0), + mBufferWrite(0) { + mBuffer = new uint8_t[NMEA_PARSER_BUFFER_SIZE]; + mBufferCapacity = NMEA_PARSER_BUFFER_SIZE; +} + +Parser::~Parser() NMEA_NOEXCEPT { + if (mBuffer != nullptr) { + delete[] mBuffer; + } +} + +bool Parser::append(uint8_t* data, uint16_t length) NMEA_NOEXCEPT { + if (length > mBufferCapacity) { + // TODO(ewasjon): report error + return false; + } + + // copy data to buffer + for (uint16_t i = 0; i < length; i++) { + mBuffer[mBufferWrite] = data[i]; + mBufferWrite = (mBufferWrite + 1) % mBufferCapacity; + if (mBufferWrite == mBufferRead) { + // buffer overflow + mBufferRead = (mBufferRead + 1) % mBufferCapacity; + } + } + + return true; +} + +void Parser::clear() NMEA_NOEXCEPT { + mBufferRead = 0; + mBufferWrite = 0; +} + +std::unique_ptr Parser::try_parse() NMEA_NOEXCEPT { + // search for '$' + for (;;) { + if (buffer_length() < 1) { + // not enough data to search for '$' + return nullptr; + } + + if (peek(0) == '$') { + // found '$' + break; + } + + // skip one byte and try again + skip(1); + } + + // search for '\r\n' + auto length = 1u; + for (;;) { + if (buffer_length() < length + 2) { + // not enough data to search for '\r\n' + return nullptr; + } + + if (peek(length + 0) == '\r' && peek(length + 1) == '\n') { + // found '\r\n' + break; + } + + // skip one byte and try again + length++; + } + + // copy message to buffer + std::string payload; + payload.resize(length + 2); + copy_to_buffer(reinterpret_cast(&payload[0]), length + 2); + skip(length + 2); + + // check checksum + auto result = checksum(payload); + if (result != ChecksumResult::OK) { + // checksum failed + return nullptr; + } + + auto length_with_clrf = length + 2; + auto prefix = parse_prefix(reinterpret_cast(payload.data()), length_with_clrf); + if (prefix.empty()) { + // invalid prefix + return nullptr; + } + + // '$XXXXX,' [data] '*XY\r\n' + auto data_start = prefix.size() + 1 /* $ */ + 1 /* , */; + auto data_end = length_with_clrf - 5; + if (data_start >= data_end) { + // no data + return nullptr; + } + + auto data_length = data_end - data_start; + auto data_payload = payload.substr(data_start, data_length); + + // parse message + if (prefix == "GPGGA" || prefix == "GLGGA" || prefix == "GAGGA" || prefix == "GNGGA") { + auto message = GgaMessage::parse(prefix, data_payload); + if (message) { + return message; + } else { + return std::unique_ptr(new ErrorMessage(prefix, data_payload)); + } + } else if (prefix == "GPVTG" || prefix == "GLVTG" || prefix == "GAVTG" || prefix == "GNVTG") { + auto message = VtgMessage::parse(prefix, data_payload); + if (message) { + return message; + } else { + return std::unique_ptr(new ErrorMessage(prefix, data_payload)); + } + } else if (prefix == "GPGST" || prefix == "GLGST" || prefix == "GAGST" || prefix == "GNGST") { + auto message = GstMessage::parse(prefix, data_payload); + if (message) { + return message; + } else { + return std::unique_ptr(new ErrorMessage(prefix, data_payload)); + } + } else { + return std::unique_ptr(new UnsupportedMessage(prefix, data_payload)); + } +} + +uint32_t Parser::buffer_length() const NMEA_NOEXCEPT { + if (mBufferWrite >= mBufferRead) { + return mBufferWrite - mBufferRead; + } else { + return mBufferCapacity - mBufferRead + mBufferWrite; + } +} + +uint32_t Parser::available_space() const NMEA_NOEXCEPT { + return mBufferCapacity - buffer_length() - 1; +} + +uint8_t Parser::peek(uint32_t index) const NMEA_NOEXCEPT { + if (index >= buffer_length()) { + // NOTE(ewasjon): the caller should check buffer_length() before calling peek + return 0; + } + + return mBuffer[(mBufferRead + index) % mBufferCapacity]; +} + +void Parser::skip(uint32_t length) NMEA_NOEXCEPT { + auto available = buffer_length(); + if (length > available) { + length = available; + } + + mBufferRead = (mBufferRead + length) % mBufferCapacity; +} + +void Parser::copy_to_buffer(uint8_t* data, uint32_t length) NMEA_NOEXCEPT { + auto available = buffer_length(); + if (length > available) { + length = available; + } + + for (uint32_t i = 0; i < length; i++) { + data[i] = mBuffer[(mBufferRead + i) % mBufferCapacity]; + } +} + +ChecksumResult Parser::checksum(const std::string& buffer) { + auto nmea_end = buffer.find_last_of('*'); + if (nmea_end == std::string::npos) { + return ChecksumResult::INVALID_STRING_NOSTAR; + } + + if (nmea_end + 3 /* *XY */ + 2 /* \r\n */ != buffer.size()) { + return ChecksumResult::INVALID_STRING_LENGTH; + } + + auto nmea_string = buffer.substr(1, nmea_end - 1); + + auto expected_checksum_hex = buffer.substr(nmea_end + 1, nmea_end + 3); + auto expected_checksum = std::stoull(std::string{expected_checksum_hex}, nullptr, 16); + + auto calculated_checksum = 0ULL; + for (auto nmea_char : nmea_string) { + calculated_checksum = calculated_checksum ^ static_cast(nmea_char); + } + + if (expected_checksum == calculated_checksum) { + return ChecksumResult::OK; + } else { + return ChecksumResult::INVALID_VALUE; + } +} + +std::string Parser::parse_prefix(const uint8_t* data, uint32_t length) const NMEA_NOEXCEPT { + // parse '$XXXXX' until first ',' or '*' + std::string prefix; + for (uint32_t i = 0; i < length; i++) { + auto c = data[i]; + if (c == ',' || c == '*') { + break; + } + + prefix += c; + } + + if (prefix.size() != 6) { + return ""; + } + + // remove $ + return prefix.substr(1); +} + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/receiver.cpp b/receiver/nmea/receiver.cpp new file mode 100644 index 00000000..78a02acd --- /dev/null +++ b/receiver/nmea/receiver.cpp @@ -0,0 +1,68 @@ +#include "receiver.hpp" +#include "message.hpp" +#include "parser.hpp" + +#include +#include +#include + +namespace receiver { +namespace nmea { + +NmeaReceiver::NmeaReceiver(std::unique_ptr interface) NMEA_NOEXCEPT + : mInterface(std::move(interface)) { + mParser = new Parser(); +} + +NmeaReceiver::~NmeaReceiver() NMEA_NOEXCEPT { + if (mParser != nullptr) { + delete mParser; + } +} + +void NmeaReceiver::process() { + if (!mInterface->is_open()) { + mInterface->open(); + } + + if (mInterface->can_read()) { + uint8_t buffer[1024]; + auto available = mParser->available_space(); + if (available > 0) { + auto read_length = (available > sizeof(buffer)) ? sizeof(buffer) : available; + auto length = mInterface->read(buffer, read_length); + if (length <= 0) { + // This will only happen if the interface is closed or disconnected. In this case, + // we will try to re-open the interface in the next iteration of the main loop. + mInterface->close(); + return; + } + + // Ignore the return value, this should never fail if the parser buffer is >= 1024 bytes + mParser->append(buffer, length); + } + } +} + +std::unique_ptr NmeaReceiver::wait_for_message() { + for (;;) { + process(); + + auto message = try_parse(); + if (message) { + return message; + } + + mInterface->wait_for_read(); + if (!mInterface->is_open() && mParser->buffer_length() == 0) { + return nullptr; + } + } +} + +std::unique_ptr NmeaReceiver::try_parse() { + return mParser->try_parse(); +} + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/threaded_receiver.cpp b/receiver/nmea/threaded_receiver.cpp new file mode 100644 index 00000000..509ffd27 --- /dev/null +++ b/receiver/nmea/threaded_receiver.cpp @@ -0,0 +1,148 @@ +#include "threaded_receiver.hpp" +#include +#include +#include "message.hpp" + +#ifdef RECEIVER_NMEA_THREADED +#include +#define RNT_DEBUG(...) printf(__VA_ARGS__) +#else +#define RNT_DEBUG(...) +#endif + +namespace receiver { +namespace nmea { + +ThreadedReceiver::ThreadedReceiver(std::unique_ptr interface, + bool print_messages) NMEA_NOEXCEPT + : mInterface(std::move(interface)), + mRunning(false), + mPrintMessages(print_messages), + mGga(nullptr), + mVtg(nullptr), + mGst(nullptr) { + RNT_DEBUG("[rnt] created\n"); +} + +ThreadedReceiver::~ThreadedReceiver() NMEA_NOEXCEPT { + stop(); + RNT_DEBUG("[rnt] destroyed\n"); +} + +void ThreadedReceiver::start() { + if (mRunning) return; + RNT_DEBUG("[rnt] starting\n"); + mRunning = true; + mThread = std::unique_ptr(new std::thread(&ThreadedReceiver::run, this)); +} + +void ThreadedReceiver::stop() { + if (mRunning) { + RNT_DEBUG("[rnt] stopping\n"); + mRunning = false; + if (mThread && mThread->joinable()) { + RNT_DEBUG("[rnt] joining\n"); + mThread->join(); + } + } +} + +void ThreadedReceiver::run() { + RNT_DEBUG("[rnt] running\n"); + + { + RNT_DEBUG("[rnt] lock (run)\n"); + std::lock_guard lock(mMutex); + RNT_DEBUG("[rnt] create receiver\n"); + mReceiver = std::unique_ptr(new NmeaReceiver(std::move(mInterface))); + RNT_DEBUG("[rnt] unlock (run)\n"); + } + + while (mRunning) { + { + RNT_DEBUG("[rnt] lock (run)\n"); + std::lock_guard lock(mMutex); + + if (mReceiver) { + RNT_DEBUG("[rnt] process\n"); + mReceiver->process(); + + for (;;) { + RNT_DEBUG("[rnt] check\n"); + auto message = mReceiver->try_parse(); + if (message) { + if (mPrintMessages) { + message->print(); + } + if (dynamic_cast(message.get())) { + mGga = std::unique_ptr( + static_cast(message.release())); + } else if (dynamic_cast(message.get())) { + mVtg = std::unique_ptr( + static_cast(message.release())); + } else if (dynamic_cast(message.get())) { + mGst = std::unique_ptr( + static_cast(message.release())); + } + } else { + break; + } + } + } + RNT_DEBUG("[rnt] unlock (run)\n"); + } + + if (mReceiver) { + RNT_DEBUG("[rnt] wait\n"); + mReceiver->interface().wait_for_read(); + } + + RNT_DEBUG("[rnt] sleep\n"); + usleep(10 * 1000); + } +} + +interface::Interface* ThreadedReceiver::interface() NMEA_NOEXCEPT { + if (!mReceiver) return nullptr; + RNT_DEBUG("[rnt] lock (interface)\n"); + std::lock_guard lock(mMutex); + auto interface = &mReceiver->interface(); + RNT_DEBUG("[rnt] unlock (interface)\n"); + return interface; +} + +std::unique_ptr ThreadedReceiver::gga() NMEA_NOEXCEPT { + if (!mReceiver) return nullptr; + RNT_DEBUG("[rnt] lock (gga)\n"); + std::lock_guard lock(mMutex); + + auto gga = std::unique_ptr(new GgaMessage{*mGga.get()}); + // auto gga = std::move(mGga); + RNT_DEBUG("[rnt] unlock (gga)\n"); + return gga; +} + +std::unique_ptr ThreadedReceiver::vtg() NMEA_NOEXCEPT { + if (!mReceiver) return nullptr; + RNT_DEBUG("[rnt] lock (vtg)\n"); + std::lock_guard lock(mMutex); + + auto vtg = std::unique_ptr(new VtgMessage{*mVtg.get()}); + // auto vtg = std::move(mVtg); + RNT_DEBUG("[rnt] unlock (vtg)\n"); + return vtg; +} + +std::unique_ptr ThreadedReceiver::gst() NMEA_NOEXCEPT { + if (!mReceiver) return nullptr; + RNT_DEBUG("[rnt] lock (gst)\n"); + std::lock_guard lock(mMutex); + + auto gst = std::unique_ptr(new GstMessage{*mGst.get()}); + // auto gst = std::move(mGst); + RNT_DEBUG("[rnt] unlock (gst)\n"); + return gst; +} + +} // namespace nmea +} // namespace receiver diff --git a/receiver/nmea/vtg.cpp b/receiver/nmea/vtg.cpp new file mode 100644 index 00000000..f1b17b5f --- /dev/null +++ b/receiver/nmea/vtg.cpp @@ -0,0 +1,100 @@ +#include +#include "helper.hpp" + +namespace receiver { +namespace nmea { + +static bool parse_double(const std::string& token, double& value) { + try { + value = std::stod(token); + return true; + } catch (...) { +#if RECEIVER_NMEA_DEBUG + printf("[--VTG] failed to parse double: \"%s\"\n", token.c_str()); +#endif + return false; + } +} + +static bool parse_double_opt(const std::string& token, double& value) { + try { + value = std::stod(token); + return true; + } catch (...) { + value = 0; + return true; + } +} + +static bool parse_mode_indicator(const std::string& token, ModeIndicator& mode_indicator) { + if (token.size() != 1) { +#if RECEIVER_NMEA_DEBUG + printf("[--VTG] invalid mode indicator: \"%s\"\n", token.c_str()); +#endif + return false; + } + + switch (token[0]) { + case 'A': mode_indicator = ModeIndicator::Autonomous; return true; + case 'D': mode_indicator = ModeIndicator::Differential; return true; + default: mode_indicator = ModeIndicator::Unknown; return true; + } +} + +VtgMessage::VtgMessage(std::string prefix) NMEA_NOEXCEPT : Message{prefix}, + mTrueCourseOverGround{0.0}, + mMagneticCourseOverGround{0.0}, + mSpeedOverGroundKnots{0.0}, + mSpeedOverGroundKmh{0.0}, + mModeIndicator{ModeIndicator::Unknown} {} + +void VtgMessage::print() const NMEA_NOEXCEPT { + printf("[%5s]\n", prefix().c_str()); + printf(" course over ground (true): %.2f\n", mTrueCourseOverGround); + printf(" course over ground (magnetic): %.2f\n", mMagneticCourseOverGround); + printf(" speed over ground (knots): %.2f\n", mSpeedOverGroundKnots); + printf(" speed over ground (km/h): %.2f\n", mSpeedOverGroundKmh); + printf(" mode indicator: "); + switch (mModeIndicator) { + case ModeIndicator::Unknown: printf("unknown\n"); break; + case ModeIndicator::Autonomous: printf("autonomous\n"); break; + case ModeIndicator::Differential: printf("differential\n"); break; + default: printf("invalid\n"); break; + } +} + +std::unique_ptr VtgMessage::parse(std::string prefix, const std::string& payload) { + // split payload by ',' + auto tokens = split(payload, ','); + + // check number of tokens + if (tokens.size() < 9) { +#if RECEIVER_NMEA_DEBUG + printf("[--VTG] invalid number of tokens: %zu\n", tokens.size()); +#endif + return nullptr; + } + + // parse + auto message = new VtgMessage(prefix); + auto success = true; + success &= parse_double_opt(tokens[0], message->mTrueCourseOverGround); + success &= parse_double_opt(tokens[2], message->mMagneticCourseOverGround); + success &= parse_double(tokens[4], message->mSpeedOverGroundKnots); + success &= parse_double(tokens[6], message->mSpeedOverGroundKmh); + success &= tokens[7] == "K"; + success &= parse_mode_indicator(tokens[8], message->mModeIndicator); + + if (success) { + return std::unique_ptr(message); + } else { +#if RECEIVER_NMEA_DEBUG + printf("[--VTG] failed to parse message\n"); +#endif + delete message; + return nullptr; + } +} + +} // namespace nmea +} // namespace receiver diff --git a/receiver/ublox/CMakeLists.txt b/receiver/ublox/CMakeLists.txt index 3350698e..fda915bf 100644 --- a/receiver/ublox/CMakeLists.txt +++ b/receiver/ublox/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(receiver::ublox ALIAS receiver_ublox) target_include_directories(receiver_ublox PRIVATE "./" "include/receiver/ublox/") target_include_directories(receiver_ublox PUBLIC "include/") target_link_libraries(receiver_ublox PRIVATE dependency::interface) +target_link_libraries(receiver_ublox PRIVATE utility) if (USE_ASAN) target_compile_options(receiver_ublox PRIVATE -fsanitize=address,undefined,leak) diff --git a/receiver/ublox/include/receiver/ublox/message.hpp b/receiver/ublox/include/receiver/ublox/message.hpp index 144dedf2..904c7005 100644 --- a/receiver/ublox/include/receiver/ublox/message.hpp +++ b/receiver/ublox/include/receiver/ublox/message.hpp @@ -10,6 +10,11 @@ class Message { UBLOX_EXPLICIT Message(uint8_t message_class, uint8_t message_id) UBLOX_NOEXCEPT; virtual ~Message() = default; + Message(const Message& other) : mClass(other.mClass), mId(other.mId) {} + Message(Message&&) = delete; + Message& operator=(const Message&) = delete; + Message& operator=(Message&&) = delete; + /// Get the message class. UBLOX_NODISCARD uint8_t message_class() const UBLOX_NOEXCEPT { return mClass; } @@ -30,6 +35,11 @@ class UnsupportedMessage final : public Message { UBLOX_EXPLICIT UnsupportedMessage(uint8_t message_class, uint8_t message_id) UBLOX_NOEXCEPT; ~UnsupportedMessage() override = default; + UnsupportedMessage(const UnsupportedMessage& other) : Message(other) {} + UnsupportedMessage(UnsupportedMessage&&) = delete; + UnsupportedMessage& operator=(const UnsupportedMessage&) = delete; + UnsupportedMessage& operator=(UnsupportedMessage&&) = delete; + void print() const UBLOX_NOEXCEPT override; }; diff --git a/receiver/ublox/include/receiver/ublox/threaded_receiver.hpp b/receiver/ublox/include/receiver/ublox/threaded_receiver.hpp index bc1e8997..235e7bf1 100644 --- a/receiver/ublox/include/receiver/ublox/threaded_receiver.hpp +++ b/receiver/ublox/include/receiver/ublox/threaded_receiver.hpp @@ -13,8 +13,8 @@ namespace ublox { class ThreadedReceiver { public: /// The receiver will be created on the thread, thus this will _not_ block. - UBLOX_EXPLICIT ThreadedReceiver(Port port, - std::unique_ptr interface) UBLOX_NOEXCEPT; + UBLOX_EXPLICIT ThreadedReceiver(Port port, std::unique_ptr interface, + bool print_messages) UBLOX_NOEXCEPT; ~ThreadedReceiver() UBLOX_NOEXCEPT; /// Start the receiver thread. @@ -43,6 +43,7 @@ class ThreadedReceiver { std::unique_ptr mThread; std::atomic mRunning; std::mutex mMutex; + bool mPrintMessages; std::unique_ptr mNavPvt; }; diff --git a/receiver/ublox/include/receiver/ublox/ubx_ack_ack.hpp b/receiver/ublox/include/receiver/ublox/ubx_ack_ack.hpp index f270bebf..cf815523 100644 --- a/receiver/ublox/include/receiver/ublox/ubx_ack_ack.hpp +++ b/receiver/ublox/include/receiver/ublox/ubx_ack_ack.hpp @@ -1,8 +1,8 @@ #pragma once +#include #include #include #include -#include namespace receiver { namespace ublox { @@ -23,6 +23,11 @@ class UbxAckAck : public Message { UBLOX_EXPLICIT UbxAckAck(raw::AckAck payload) UBLOX_NOEXCEPT; ~UbxAckAck() override = default; + UbxAckAck(const UbxAckAck& other) : Message(other), mPayload(other.mPayload) {} + UbxAckAck(UbxAckAck&&) = delete; + UbxAckAck& operator=(const UbxAckAck&) = delete; + UbxAckAck& operator=(UbxAckAck&&) = delete; + UBLOX_NODISCARD const raw::AckAck& payload() const UBLOX_NOEXCEPT { return mPayload; } UBLOX_NODISCARD uint8_t cls_id() const UBLOX_NOEXCEPT { return mPayload.cls_id; } diff --git a/receiver/ublox/include/receiver/ublox/ubx_ack_nak.hpp b/receiver/ublox/include/receiver/ublox/ubx_ack_nak.hpp index cdbfab4c..55e2bcdd 100644 --- a/receiver/ublox/include/receiver/ublox/ubx_ack_nak.hpp +++ b/receiver/ublox/include/receiver/ublox/ubx_ack_nak.hpp @@ -1,8 +1,8 @@ #pragma once +#include #include #include #include -#include namespace receiver { namespace ublox { @@ -23,6 +23,11 @@ class UbxAckNak : public Message { UBLOX_EXPLICIT UbxAckNak(raw::AckNak payload) UBLOX_NOEXCEPT; ~UbxAckNak() override = default; + UbxAckNak(const UbxAckNak& other) : Message(other), mPayload(other.mPayload) {} + UbxAckNak(UbxAckNak&&) = delete; + UbxAckNak& operator=(const UbxAckNak&) = delete; + UbxAckNak& operator=(UbxAckNak&&) = delete; + UBLOX_NODISCARD const raw::AckNak& payload() const UBLOX_NOEXCEPT { return mPayload; } UBLOX_NODISCARD uint8_t cls_id() const UBLOX_NOEXCEPT { return mPayload.cls_id; } diff --git a/receiver/ublox/include/receiver/ublox/ubx_cfg_valget.hpp b/receiver/ublox/include/receiver/ublox/ubx_cfg_valget.hpp index 408bb87a..9558665b 100644 --- a/receiver/ublox/include/receiver/ublox/ubx_cfg_valget.hpp +++ b/receiver/ublox/include/receiver/ublox/ubx_cfg_valget.hpp @@ -28,6 +28,11 @@ class UbxCfgValget : public Message { UBLOX_EXPLICIT UbxCfgValget(raw::CfgValget payload) UBLOX_NOEXCEPT; ~UbxCfgValget() override = default; + UbxCfgValget(const UbxCfgValget& other) : Message(other), mPayload(other.mPayload) {} + UbxCfgValget(UbxCfgValget&&) = delete; + UbxCfgValget& operator=(const UbxCfgValget&) = delete; + UbxCfgValget& operator=(UbxCfgValget&&) = delete; + void print() const UBLOX_NOEXCEPT override; /// Get the value of a configuration key. If the key is not present, diff --git a/receiver/ublox/include/receiver/ublox/ubx_mon_ver.hpp b/receiver/ublox/include/receiver/ublox/ubx_mon_ver.hpp index d66986de..7e5ed973 100644 --- a/receiver/ublox/include/receiver/ublox/ubx_mon_ver.hpp +++ b/receiver/ublox/include/receiver/ublox/ubx_mon_ver.hpp @@ -1,8 +1,8 @@ #pragma once +#include #include #include #include -#include namespace receiver { namespace ublox { @@ -25,6 +25,11 @@ class UbxMonVer : public Message { UBLOX_EXPLICIT UbxMonVer(raw::MonVer payload) UBLOX_NOEXCEPT; ~UbxMonVer() override = default; + UbxMonVer(const UbxMonVer& other) : Message(other), mPayload(other.mPayload) {} + UbxMonVer(UbxMonVer&&) = delete; + UbxMonVer& operator=(const UbxMonVer&) = delete; + UbxMonVer& operator=(UbxMonVer&&) = delete; + UBLOX_NODISCARD const raw::MonVer& payload() const UBLOX_NOEXCEPT { return mPayload; } UBLOX_NODISCARD const std::string& sw_version() const UBLOX_NOEXCEPT { @@ -40,7 +45,7 @@ class UbxMonVer : public Message { void print() const UBLOX_NOEXCEPT override; UBLOX_NODISCARD static std::unique_ptr parse(Decoder& decoder) UBLOX_NOEXCEPT; - UBLOX_NODISCARD static uint32_t poll(Encoder& encoder) UBLOX_NOEXCEPT; + UBLOX_NODISCARD static uint32_t poll(Encoder& encoder) UBLOX_NOEXCEPT; private: raw::MonVer mPayload; diff --git a/receiver/ublox/include/receiver/ublox/ubx_nav_pvt.hpp b/receiver/ublox/include/receiver/ublox/ubx_nav_pvt.hpp index 0a241be0..8d70910c 100644 --- a/receiver/ublox/include/receiver/ublox/ubx_nav_pvt.hpp +++ b/receiver/ublox/include/receiver/ublox/ubx_nav_pvt.hpp @@ -1,6 +1,7 @@ #pragma once #include #include +#include namespace receiver { namespace ublox { @@ -70,6 +71,11 @@ class UbxNavPvt : public Message { UBLOX_EXPLICIT UbxNavPvt(raw::NavPvt payload) UBLOX_NOEXCEPT; ~UbxNavPvt() override = default; + UbxNavPvt(const UbxNavPvt& other) : Message(other), mPayload(other.mPayload) {} + UbxNavPvt(UbxNavPvt&&) = delete; + UbxNavPvt& operator=(const UbxNavPvt&) = delete; + UbxNavPvt& operator=(UbxNavPvt&&) = delete; + /// Returns the unprocessed raw payload. UBLOX_NODISCARD const raw::NavPvt& payload() const UBLOX_NOEXCEPT { return mPayload; } @@ -105,6 +111,14 @@ class UbxNavPvt : public Message { /// Position dilution of precision. UBLOX_NODISCARD double p_dop() const UBLOX_NOEXCEPT; + /// UTC timestamp of the navigation epoch. (as a UNIX timestamp) + UBLOX_NODISCARD time_t timestamp() const UBLOX_NOEXCEPT; + /// TAI timestamp of the navigation epoch. + UBLOX_NODISCARD TAI_Time tai_time() const UBLOX_NOEXCEPT; + + /// UTC timestamp is valid. + UBLOX_NODISCARD bool valid_time() const UBLOX_NOEXCEPT; + void print() const UBLOX_NOEXCEPT override; UBLOX_NODISCARD static std::unique_ptr parse(Decoder& decoder) UBLOX_NOEXCEPT; diff --git a/receiver/ublox/threaded_receiver.cpp b/receiver/ublox/threaded_receiver.cpp index 6552eca7..d9b45246 100644 --- a/receiver/ublox/threaded_receiver.cpp +++ b/receiver/ublox/threaded_receiver.cpp @@ -13,11 +13,12 @@ namespace receiver { namespace ublox { -ThreadedReceiver::ThreadedReceiver(Port port, - std::unique_ptr interface) UBLOX_NOEXCEPT +ThreadedReceiver::ThreadedReceiver(Port port, std::unique_ptr interface, + bool print_messages) UBLOX_NOEXCEPT : mPort(port), mInterface(std::move(interface)), - mRunning(false) { + mRunning(false), + mPrintMessages(print_messages) { RUT_DEBUG("[rut] created\n"); } @@ -57,26 +58,33 @@ void ThreadedReceiver::run() { while (mRunning) { { - RUT_DEBUG("[rut] lock (run)\n"); - std::lock_guard lock(mMutex); - if (mReceiver) { RUT_DEBUG("[rut] process\n"); mReceiver->process(); - RUT_DEBUG("[rut] check\n"); - auto message = mReceiver->try_parse(); - if (message) { - if (message->message_class() == UbxNavPvt::CLASS_ID && - message->message_id() == UbxNavPvt::MESSAGE_ID) { - RUT_DEBUG("[rut] save navpvt\n"); - // Cast unique_ptr to unique_ptr. - mNavPvt = - std::unique_ptr(static_cast(message.release())); + for (;;) { + RUT_DEBUG("[rut] check\n"); + auto message = mReceiver->try_parse(); + if (message) { + RUT_DEBUG("[rut] lock (run)\n"); + std::lock_guard lock(mMutex); + + if (mPrintMessages) { + message->print(); + } + if (message->message_class() == UbxNavPvt::CLASS_ID && + message->message_id() == UbxNavPvt::MESSAGE_ID) { + RUT_DEBUG("[rut] save navpvt\n"); + // Cast unique_ptr to unique_ptr. + mNavPvt = std::unique_ptr( + static_cast(message.release())); + } + RUT_DEBUG("[rut] unlock (run)\n"); + } else { + break; } } } - RUT_DEBUG("[rut] unlock (run)\n"); } if (mReceiver) { diff --git a/receiver/ublox/ubx_nav_pvt.cpp b/receiver/ublox/ubx_nav_pvt.cpp index 3d1c01a1..20a313f1 100644 --- a/receiver/ublox/ubx_nav_pvt.cpp +++ b/receiver/ublox/ubx_nav_pvt.cpp @@ -64,6 +64,42 @@ double UbxNavPvt::p_dop() const UBLOX_NOEXCEPT { return static_cast(mPayload.p_dop) * 0.01; } +time_t UbxNavPvt::timestamp() const UBLOX_NOEXCEPT { + struct tm tm {}; + tm.tm_year = mPayload.year - 1900; + tm.tm_mon = mPayload.month - 1; + tm.tm_mday = mPayload.day; + tm.tm_hour = mPayload.hour; + tm.tm_min = mPayload.min; + tm.tm_sec = mPayload.sec; + tm.tm_isdst = 0; + + auto time = mktime(&tm); + return time; +} + +TAI_Time UbxNavPvt::tai_time() const UBLOX_NOEXCEPT { + auto time = timestamp(); + auto seconds = static_cast(time); + auto fraction = static_cast(mPayload.nano) * 1.0e-9; + if (fraction < 0.0) { + seconds -= 1; + fraction += 1.0; + } + + if (seconds < 0) { + seconds = 0; + fraction = 0.0; + } + + auto ts = Timestamp{seconds, fraction}; + return TAI_Time{UTC_Time{ts}}; +} + +bool UbxNavPvt::valid_time() const UBLOX_NOEXCEPT { + return mPayload.valid.valid_date != 0 && mPayload.valid.valid_time != 0; +} + void UbxNavPvt::print() const UBLOX_NOEXCEPT { printf("[%02X %02X] UBX-NAV-PVT:\n", message_class(), message_id()); printf("[.....] i_tow: %u\n", mPayload.i_tow); @@ -129,26 +165,26 @@ std::unique_ptr UbxNavPvt::parse(Decoder& decoder) UBLOX_NOEXCEPT { payload.sec = decoder.U1(); auto valid = decoder.X1(); - payload.valid.valid_date = valid & 0x01; - payload.valid.valid_time = valid & 0x02; - payload.valid.fully_resolved = valid & 0x04; - payload.valid.valid_mag = valid & 0x08; + payload.valid.valid_date = (valid >> 0) & 0x01; + payload.valid.valid_time = (valid >> 1) & 0x02; + payload.valid.fully_resolved = (valid >> 2) & 0x04; + payload.valid.valid_mag = (valid >> 3) & 0x08; payload.t_acc = decoder.U4(); payload.nano = decoder.I4(); payload.fix_type = decoder.U1(); auto flags = decoder.X1(); - payload.flags.gnss_fix_ok = flags & 0x01; - payload.flags.diff_soln = flags & 0x02; + payload.flags.gnss_fix_ok = (flags >> 0) & 0x01; + payload.flags.diff_soln = (flags >> 1) & 0x02; payload.flags.psm_state = (flags >> 2) & 0x07; - payload.flags.head_veh_valid = flags & 0x20; + payload.flags.head_veh_valid = (flags >> 5) & 0x20; payload.flags.carr_soln = (flags >> 6) & 0x03; auto flags2 = decoder.X1(); - payload.flags2.confirmed_avai = flags2 & 0x20; - payload.flags2.confirmed_date = flags2 & 0x40; - payload.flags2.confirmed_time = flags2 & 0x80; + payload.flags2.confirmed_avai = (flags2 >> 5) & 0x20; + payload.flags2.confirmed_date = (flags2 >> 6) & 0x40; + payload.flags2.confirmed_time = (flags2 >> 7) & 0x80; payload.num_sv = decoder.U1(); payload.lon = decoder.I4(); @@ -167,7 +203,7 @@ std::unique_ptr UbxNavPvt::parse(Decoder& decoder) UBLOX_NOEXCEPT { payload.p_dop = decoder.U2(); auto flags3 = decoder.X1(); - payload.flags3.invalid_llh = flags3 & 0x01; + payload.flags3.invalid_llh = (flags3 >> 0) & 0x01; payload.flags3.last_correction_arg = (flags3 >> 1) & 0x0F; decoder.skip(4);