From 2ccac0400365adfa1ece82be7725dcdbb657410f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Sep 2021 10:24:56 +0200 Subject: [PATCH 1/8] Add Advanced Navigation Protocol printer --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 25 ++ .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 20 ++ .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 4 + src/algorithms/PVT/libs/CMakeLists.txt | 2 + src/algorithms/PVT/libs/an_packet_printer.cc | 291 ++++++++++++++++++ src/algorithms/PVT/libs/an_packet_printer.h | 128 ++++++++ src/algorithms/PVT/libs/pvt_conf.h | 3 + 7 files changed, 473 insertions(+) create mode 100644 src/algorithms/PVT/libs/an_packet_printer.cc create mode 100644 src/algorithms/PVT/libs/an_packet_printer.h diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index aa97d8de7..748ce5264 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -122,10 +122,35 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; } + // Advanced Nativation Protocol Printer settings + pvt_output_parameters.an_output_enabled = configuration->property(role + ".an_output_enabled", pvt_output_parameters.an_output_enabled); + pvt_output_parameters.an_dump_devname = configuration->property(role + ".an_dump_devname", default_nmea_dump_devname); + if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_nmea_tty_port) + { + if (pvt_output_parameters.nmea_dump_devname == pvt_output_parameters.an_dump_devname) + { + std::cerr << "Warning: NMEA an Advanced Nativation printers set to write to the same serial port.\n" + << "Please make sure that PVT.an_dump_devname and PVT.an_dump_devname are different.\n" + << "Shutting down the NMEA serial output.\n"; + pvt_output_parameters.flag_nmea_tty_port = false; + } + } + if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_rtcm_tty_port) + { + if (pvt_output_parameters.rtcm_dump_devname == pvt_output_parameters.an_dump_devname) + { + std::cerr << "Warning: RTCM an Advanced Nativation printers set to write to the same serial port.\n" + << "Please make sure that PVT.an_dump_devname and .rtcm_dump_devname are different.\n" + << "Shutting down the RTCM serial output.\n"; + pvt_output_parameters.flag_rtcm_tty_port = false; + } + } + pvt_output_parameters.kml_rate_ms = bc::lcm(configuration->property(role + ".kml_rate_ms", pvt_output_parameters.kml_rate_ms), pvt_output_parameters.output_rate_ms); pvt_output_parameters.gpx_rate_ms = bc::lcm(configuration->property(role + ".gpx_rate_ms", pvt_output_parameters.gpx_rate_ms), pvt_output_parameters.output_rate_ms); pvt_output_parameters.geojson_rate_ms = bc::lcm(configuration->property(role + ".geojson_rate_ms", pvt_output_parameters.geojson_rate_ms), pvt_output_parameters.output_rate_ms); pvt_output_parameters.nmea_rate_ms = bc::lcm(configuration->property(role + ".nmea_rate_ms", pvt_output_parameters.nmea_rate_ms), pvt_output_parameters.output_rate_ms); + pvt_output_parameters.an_rate_ms = bc::lcm(configuration->property(role + ".an_rate_ms", pvt_output_parameters.an_rate_ms), pvt_output_parameters.output_rate_ms); // Infer the type of receiver /* diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index aacc65fe4..e716449e6 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -16,6 +16,7 @@ #include "rtklib_pvt_gs.h" #include "MATH_CONSTANTS.h" +#include "an_packet_printer.h" #include "beidou_dnav_almanac.h" #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" @@ -411,6 +412,18 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_rx_time = 0.0; d_last_status_print_seg = 0; + // Initialize AN printer + d_an_printer_enabled = conf_.an_output_enabled; + d_an_rate_ms = conf_.an_rate_ms; + if (d_an_printer_enabled) + { + d_an_printer = std::make_unique(conf_.an_dump_devname); + } + else + { + d_an_printer = nullptr; + } + // PVT MONITOR d_flag_monitor_pvt_enabled = conf_.monitor_enabled; if (d_flag_monitor_pvt_enabled) @@ -2236,6 +2249,13 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item flag_write_RTCM_1045_output, d_enable_rx_clock_correction); } + if (d_an_printer_enabled) + { + if (current_RX_time_ms % d_an_rate_ms == 0) + { + d_an_printer->print_packet(d_user_pvt_solver.get()); + } + } } } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 40d6e065e..3240d50ac 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -57,6 +57,7 @@ class Nmea_Printer; class Pvt_Conf; class Rinex_Printer; class Rtcm_Printer; +class An_Packet_Printer; class Has_Simple_Printer; class Rtklib_Solver; class rtklib_pvt_gs; @@ -174,6 +175,7 @@ private: std::unique_ptr d_udp_sink_ptr; std::unique_ptr d_eph_udp_sink_ptr; std::unique_ptr d_has_simple_printer; + std::unique_ptr d_an_printer; std::chrono::time_point d_start; std::chrono::time_point d_end; @@ -247,6 +249,7 @@ private: int32_t d_gpx_rate_ms; int32_t d_geojson_rate_ms; int32_t d_nmea_rate_ms; + int32_t d_an_rate_ms; int32_t d_last_status_print_seg; // for status printer int32_t d_output_rate_ms; int32_t d_display_rate_ms; @@ -273,6 +276,7 @@ private: bool d_waiting_obs_block_rx_clock_offset_correction_msg; bool d_enable_rx_clock_correction; bool d_enable_has_messages; + bool d_an_printer_enabled; }; diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 3047b1e95..e450a1b8e 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -9,6 +9,7 @@ protobuf_generate_cpp(PROTO_SRCS2 PROTO_HDRS2 ${CMAKE_SOURCE_DIR}/docs/protobuf/ protobuf_generate_cpp(PROTO_SRCS3 PROTO_HDRS3 ${CMAKE_SOURCE_DIR}/docs/protobuf/galileo_ephemeris.proto) set(PVT_LIB_SOURCES + an_packet_printer.cc pvt_solution.cc geojson_printer.cc gpx_printer.cc @@ -24,6 +25,7 @@ set(PVT_LIB_SOURCES ) set(PVT_LIB_HEADERS + an_packet_printer.h pvt_conf.h pvt_solution.h geojson_printer.h diff --git a/src/algorithms/PVT/libs/an_packet_printer.cc b/src/algorithms/PVT/libs/an_packet_printer.cc new file mode 100644 index 000000000..4c422bab0 --- /dev/null +++ b/src/algorithms/PVT/libs/an_packet_printer.cc @@ -0,0 +1,291 @@ +/*! + * \file an_packet_printer.cc + * \brief Implementation of a class that prints PVT solutions in a serial device + * following a custom version of the Advanced Navigation Packet Protocol + * \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es + * \author Miguel Angel Gomez Lopez, 2021. gomezlma(at)inta.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "an_packet_printer.h" +#include "rtklib_solver.h" // for Rtklib_Solver +#include +#include // for DLOG +#include // for std::sqrt, M_PI +#include // for fcntl +#include // for std::cerr +#include // values for termios +#include // for write(), read(), close() + + +An_Packet_Printer::An_Packet_Printer(const std::string& an_dump_devname) +{ + d_an_devname = an_dump_devname; + + d_an_dev_descriptor = init_serial(d_an_devname); + if (d_an_dev_descriptor != -1) + { + DLOG(INFO) << "AN Printer writing on " << d_an_devname; + } +} + + +An_Packet_Printer::~An_Packet_Printer() +{ + try + { + close_serial(); + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + } +} + + +bool An_Packet_Printer::print_packet(const Rtklib_Solver* const pvt_data) +{ + an_packet_t an_packet{}; + raw_gnss_packet_t raw_packet{}; + + update_raw_gnss_packet(&raw_packet, pvt_data); + encode_raw_gnss_packet(&raw_packet, &an_packet); + + if (d_an_dev_descriptor != -1) + { + if (write(d_an_dev_descriptor, &an_packet, sizeof(an_packet)) == -1) + { + LOG(ERROR) << "Advanced Navigation printer cannot write on serial device " << d_an_devname; + return false; + } + } + return true; +} + + +void An_Packet_Printer::close_serial() const +{ + if (d_an_dev_descriptor != -1) + { + close(d_an_dev_descriptor); + } +} + +/* + * @brief update_raw_gnss_packet + * @param raw_gnss_packet_t* Pointer to a structure that contains + * the output information. + * @param NavData_t* pointer to input packet with all the information + * @reval None + */ +void An_Packet_Printer::update_raw_gnss_packet(raw_gnss_packet_t* _packet, const Rtklib_Solver* const pvt) const +{ + const boost::posix_time::ptime time_origin(boost::gregorian::date(1970, 1, 1)); + boost::date_time::time_duration unix_t = pvt->get_position_UTC_time() - time_origin; + + _packet->unix_time_stamp = unix_t.total_seconds(); + _packet->microseconds = unix_t.total_microseconds() - unix_t.total_seconds() * 1e6; + _packet->latitude = static_cast(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.0); + _packet->longitude = static_cast(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.0); + _packet->height = static_cast(pvt->get_height()) / 100.0; + _packet->velocity[0] = static_cast(pvt->pvt_sol.rr[3]) / 100.0; + _packet->velocity[1] = static_cast(pvt->pvt_sol.rr[4]) / 100.0; + _packet->velocity[2] = static_cast(pvt->pvt_sol.rr[5]) / 100.0; + _packet->position_standard_deviation[0] = std::sqrt(static_cast(pvt->pvt_sol.qr[0])); + _packet->position_standard_deviation[1] = std::sqrt(static_cast(pvt->pvt_sol.qr[1])); + _packet->position_standard_deviation[2] = std::sqrt(static_cast(pvt->pvt_sol.qr[2])); + + _packet->status.b.fix_type = 2; //!< 2: 3D fix + _packet->status.b.velocity_valid = 0; + _packet->status.b.time_valid = 0; + _packet->status.b.external_gnss = 0; +} + + +/* + * @brief encode_raw_gnss_packet + * @param raw_gnss_packet_t* Pointer to a structure that contains + * the information. + * @param an_packet_t* pointer to output packet + * @reval None + */ +void An_Packet_Printer::encode_raw_gnss_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const +{ + _packet->id = 201; + _packet->length = RAW_GNSS_PACKET_LENGTH; + if (_packet != NULL) + { + uint8_t offset = 0; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->unix_time_stamp), offset, _packet->data, sizeof(raw_packet->unix_time_stamp)); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->microseconds), offset, _packet->data, sizeof(raw_packet->microseconds)); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->latitude), offset, _packet->data, sizeof(raw_packet->latitude)); + offset += 8; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->longitude), offset, _packet->data, sizeof(raw_packet->longitude)); + offset += 8; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->height), offset, _packet->data, sizeof(raw_packet->height)); + offset += 8; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->velocity[0]), offset, _packet->data, sizeof(raw_packet->velocity[0])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->velocity[1]), offset, _packet->data, sizeof(raw_packet->velocity[1])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->velocity[2]), offset, _packet->data, sizeof(raw_packet->velocity[2])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->position_standard_deviation[0]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[0])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->position_standard_deviation[1]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[1])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->position_standard_deviation[2]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[2])); + offset += 4; + // This could be optimized to add just zeros + LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[0]), offset, _packet->data, sizeof(raw_packet->reserved[0])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[1]), offset, _packet->data, sizeof(raw_packet->reserved[1])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[2]), offset, _packet->data, sizeof(raw_packet->reserved[2])); + offset += 4; + LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[3]), offset, _packet->data, sizeof(raw_packet->reserved[3])); + offset += 4; + + LSB_bytes_to_array(reinterpret_cast(&raw_packet->status.r), offset, _packet->data, sizeof(raw_packet->status)); + offset += 2; + } + an_packet_encode(_packet); +} + + +/* + * Function to encode an an_packet + */ +void An_Packet_Printer::an_packet_encode(an_packet_t* an_packet) const +{ + uint16_t crc; + an_packet->header[1] = an_packet->id; + an_packet->header[2] = an_packet->length; + crc = calculate_crc16(an_packet->data, an_packet->length); + memcpy(&an_packet->header[3], &crc, sizeof(uint16_t)); + an_packet->header[0] = calculate_header_lrc(&an_packet->header[1]); +} + + +/* + * Function to calculate a 4 byte LRC + */ +uint8_t An_Packet_Printer::calculate_header_lrc(const uint8_t* data) const +{ + return ((data[0] + data[1] + data[2] + data[3]) ^ 0xFF) + 1; +} + + +void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const +{ + switch (var_size) + { + case 1: + { + uint8_t* tmp = reinterpret_cast(_in); + for (int i = 0; i < var_size; i++) + { + _out[offset + i] = (*tmp >> 8 * i) & 0xFF; + } + break; + } + case 2: + { + uint16_t* tmp = reinterpret_cast(_in); + for (int i = 0; i < var_size; i++) + { + _out[offset + i] = (*tmp >> 8 * i) & 0xFF; + } + break; + } + case 4: + { + uint32_t* tmp = reinterpret_cast(_in); + for (int i = 0; i < var_size; i++) + { + _out[offset + i] = (*tmp >> 8 * i) & 0xFF; + } + break; + } + case 8: + { + uint64_t* tmp = reinterpret_cast(_in); + for (int i = 0; i < var_size; i++) + { + _out[offset + i] = (*tmp >> 8 * i) & 0xFF; + } + break; + } + default: + break; + } +} + + +/* + * Function to calculate the CRC16 of data + * CRC16-CCITT + * Initial value = 0xFFFF + * Polynomial = x^16 + x^12 + x^5 + x^0 + */ +uint16_t An_Packet_Printer::calculate_crc16(const void* data, uint16_t length) const +{ + const uint8_t* bytes = reinterpret_cast(data); + uint16_t crc = 0xFFFF; + for (uint16_t i = 0; i < length; i++) + { + crc = static_cast((crc << 8) ^ d_crc16_table[(crc >> 8) ^ bytes[i]]); + } + return crc; +} + + +/* + * Opens the serial device and sets the default baud rate for a NMEA transmission (9600,8,N,1) + */ +int An_Packet_Printer::init_serial(const std::string& serial_device) +{ + int fd = 0; + // clang-format off + struct termios options{}; + // clang-format on + const int64_t BAUD = B9600; // BAUD = B38400; + const int64_t DATABITS = CS8; + const int64_t STOPBITS = 0; + const int64_t PARITYON = 0; + const int64_t PARITY = 0; + + fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC); + if (fd == -1) + { + return fd; // failed to open TTY port + } + + if (fcntl(fd, F_SETFL, 0) == -1) + { + LOG(INFO) << "Error enabling direct I/O"; // clear all flags on descriptor, enable direct I/O + } + tcgetattr(fd, &options); // read serial port options + + options.c_cflag = BAUD | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD; + // enable receiver, set 8 bit data, ignore control lines + // options.c_cflag |= (CLOCAL | CREAD | CS8); + options.c_iflag = IGNPAR; + + // set the new port options + tcsetattr(fd, TCSANOW, &options); + return fd; +} diff --git a/src/algorithms/PVT/libs/an_packet_printer.h b/src/algorithms/PVT/libs/an_packet_printer.h new file mode 100644 index 000000000..2d7d53177 --- /dev/null +++ b/src/algorithms/PVT/libs/an_packet_printer.h @@ -0,0 +1,128 @@ +/*! + * \file an_packet_printer.h + * \brief Interface of a class that prints PVT solutions in a serial device + * following a custom version of the Advanced Navigation Packet Protocol + * \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es + * \author Miguel Angel Gomez Lopez, 2021. gomezlma(at)inta.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_AN_PACKET_PRINTER_H +#define GNSS_SDR_AN_PACKET_PRINTER_H + +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + +class Rtklib_Solver; + +typedef struct +{ + uint32_t unix_time_stamp; + uint32_t microseconds; + double latitude; + double longitude; + double height; + float velocity[3]; + float position_standard_deviation[3]; + float reserved[4]; + union + { + uint16_t r; + struct + { + uint32_t fix_type : 3; + uint32_t velocity_valid : 1; + uint32_t time_valid : 1; + uint32_t external_gnss : 1; + } b; + } status; +} raw_gnss_packet_t; + + +typedef struct +{ + uint8_t id; + uint8_t length; + uint8_t header[5]; // AN_PACKET_HEADER_SIZE + uint8_t data[126]; // AN_MAXIMUM_PACKET_SIZE +} an_packet_t; + + +/*! + * \brief class that prints PVT solutions in a serial device following a custom + * version of the Advanced Navigation Packet Protocol. + */ +class An_Packet_Printer +{ +public: + /*! + * \brief Default constructor. + */ + explicit An_Packet_Printer(const std::string& an_dump_devname); + + /*! + * \brief Default destructor. + */ + ~An_Packet_Printer(); + + /*! + * \brief Print AN packet to the initialized device. + */ + bool print_packet(const Rtklib_Solver* const pvt_data); + + /*! + * \brief Close serial port. Also done in the destructor, this is only + * for testing. + */ + void close_serial() const; + +private: + const std::array d_crc16_table = + { + 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}; + + const size_t RAW_GNSS_PACKET_LENGTH = sizeof(raw_gnss_packet_t{}); + + int init_serial(const std::string& serial_device); + void update_raw_gnss_packet(raw_gnss_packet_t* _packet, const Rtklib_Solver* const pvt) const; + void encode_gnss_cttc_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const; + uint16_t calculate_crc16(const void* data, uint16_t length) const; + uint8_t calculate_header_lrc(const uint8_t* data) const; + void an_packet_encode(an_packet_t* an_packet) const; + void encode_raw_gnss_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const; + void LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const; + + std::string d_an_devname; + int d_an_dev_descriptor; // serial device descriptor (i.e. COM port) +}; + +/** \} */ +/** \} */ +#endif // GNSS_SDR_AN_PACKET_PRINTER_H diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index b0224b85a..2bc89adde 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -37,6 +37,7 @@ public: std::string nmea_dump_filename; std::string nmea_dump_devname; std::string rtcm_dump_devname; + std::string an_dump_devname; std::string output_path = std::string("."); std::string rinex_output_path = std::string("."); std::string gpx_output_path = std::string("."); @@ -59,6 +60,7 @@ public: int32_t nmea_rate_ms = 1000; int32_t rinex_version = 0; int32_t rinexobs_rate_ms = 0; + int32_t an_rate_ms = 1000; int32_t max_obs_block_rx_clock_offset_ms = 40; int udp_port = 0; int udp_eph_port = 0; @@ -75,6 +77,7 @@ public: bool gpx_output_enabled = true; bool geojson_output_enabled = true; bool nmea_output_file_enabled = true; + bool an_output_enabled = false; bool kml_output_enabled = true; bool xml_output_enabled = true; bool rtcm_output_file_enabled = true; From 387d9ad1b18cf048e2fdee7f2a97d9d30d36e62d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Sep 2021 11:08:13 +0200 Subject: [PATCH 2/8] Fix serial port speed to 115200 bauds for the AN printer --- src/algorithms/PVT/libs/an_packet_printer.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/an_packet_printer.cc b/src/algorithms/PVT/libs/an_packet_printer.cc index 4c422bab0..db0536581 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.cc +++ b/src/algorithms/PVT/libs/an_packet_printer.cc @@ -262,7 +262,7 @@ int An_Packet_Printer::init_serial(const std::string& serial_device) // clang-format off struct termios options{}; // clang-format on - const int64_t BAUD = B9600; // BAUD = B38400; + const int64_t BAUD = B115200; const int64_t DATABITS = CS8; const int64_t STOPBITS = 0; const int64_t PARITYON = 0; From d01f4cb86b23d54c15990179f6fcae6c1dab4ee7 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Sep 2021 15:01:58 +0200 Subject: [PATCH 3/8] Add work on AN printer --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 2 +- src/algorithms/PVT/libs/an_packet_printer.cc | 142 +++++++++++------- src/algorithms/PVT/libs/an_packet_printer.h | 50 +++--- 3 files changed, 112 insertions(+), 82 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index e716449e6..60c749af9 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2253,7 +2253,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { if (current_RX_time_ms % d_an_rate_ms == 0) { - d_an_printer->print_packet(d_user_pvt_solver.get()); + d_an_printer->print_packet(d_user_pvt_solver.get(), d_gnss_observables_map); } } } diff --git a/src/algorithms/PVT/libs/an_packet_printer.cc b/src/algorithms/PVT/libs/an_packet_printer.cc index db0536581..bd1b74ebc 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.cc +++ b/src/algorithms/PVT/libs/an_packet_printer.cc @@ -54,13 +54,13 @@ An_Packet_Printer::~An_Packet_Printer() } -bool An_Packet_Printer::print_packet(const Rtklib_Solver* const pvt_data) +bool An_Packet_Printer::print_packet(const Rtklib_Solver* const pvt_data, const std::map& gnss_observables_map) { an_packet_t an_packet{}; - raw_gnss_packet_t raw_packet{}; + sdr_gnss_packet_t sdr_gnss_packet{}; - update_raw_gnss_packet(&raw_packet, pvt_data); - encode_raw_gnss_packet(&raw_packet, &an_packet); + update_sdr_gnss_packet(&sdr_gnss_packet, pvt_data, gnss_observables_map); + encode_sdr_gnss_packet(&sdr_gnss_packet, &an_packet); if (d_an_dev_descriptor != -1) { @@ -83,84 +83,112 @@ void An_Packet_Printer::close_serial() const } /* - * @brief update_raw_gnss_packet - * @param raw_gnss_packet_t* Pointer to a structure that contains + * @brief update_sdr_gnss_packet + * @param sdr_gnss_packet_t* Pointer to a structure that contains * the output information. * @param NavData_t* pointer to input packet with all the information * @reval None */ -void An_Packet_Printer::update_raw_gnss_packet(raw_gnss_packet_t* _packet, const Rtklib_Solver* const pvt) const +void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const Rtklib_Solver* const pvt, const std::map& gnss_observables_map) const { const boost::posix_time::ptime time_origin(boost::gregorian::date(1970, 1, 1)); boost::date_time::time_duration unix_t = pvt->get_position_UTC_time() - time_origin; - _packet->unix_time_stamp = unix_t.total_seconds(); - _packet->microseconds = unix_t.total_microseconds() - unix_t.total_seconds() * 1e6; - _packet->latitude = static_cast(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.0); - _packet->longitude = static_cast(pvt->get_latitude()) / 1.0e-7 * (M_PI / 180.0); - _packet->height = static_cast(pvt->get_height()) / 100.0; - _packet->velocity[0] = static_cast(pvt->pvt_sol.rr[3]) / 100.0; - _packet->velocity[1] = static_cast(pvt->pvt_sol.rr[4]) / 100.0; - _packet->velocity[2] = static_cast(pvt->pvt_sol.rr[5]) / 100.0; - _packet->position_standard_deviation[0] = std::sqrt(static_cast(pvt->pvt_sol.qr[0])); - _packet->position_standard_deviation[1] = std::sqrt(static_cast(pvt->pvt_sol.qr[1])); - _packet->position_standard_deviation[2] = std::sqrt(static_cast(pvt->pvt_sol.qr[2])); + std::map::const_iterator gnss_observables_iter; + uint8_t num_gps_sats = 0; + uint8_t num_gal_sats = 0; + int index = 0; - _packet->status.b.fix_type = 2; //!< 2: 3D fix - _packet->status.b.velocity_valid = 0; - _packet->status.b.time_valid = 0; - _packet->status.b.external_gnss = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); + gnss_observables_iter != gnss_observables_map.cend(); + ++gnss_observables_iter) + { + if (gnss_observables_iter->second.Flag_valid_pseudorange) + { + switch (gnss_observables_iter->second.System) + { + case 'G': + num_gps_sats++; + if (index < 6) + { + _packet->sats[index].prn = static_cast(gnss_observables_iter->second.PRN); + _packet->sats[index].snr = static_cast(gnss_observables_iter->second.CN0_dB_hz); + index++; + } + break; + case 'E': + num_gal_sats++; + if (index < 6) + { + _packet->sats[index].prn = static_cast(gnss_observables_iter->second.PRN) + 100; + _packet->sats[index].snr = static_cast(gnss_observables_iter->second.CN0_dB_hz); + index++; + } + break; + default: + break; + } + } + } + _packet->nsvfix = static_cast(pvt->get_num_valid_observations()); + _packet->gps_satellites = num_gps_sats; + _packet->galileo_satellites = num_gal_sats; + _packet->microseconds = static_cast(unix_t.total_microseconds()); + _packet->latitude = static_cast(pvt->get_latitude()) * (M_PI / 180.0); + _packet->longitude = static_cast(pvt->get_longitude()) * (M_PI / 180.0); + _packet->height = static_cast(pvt->get_height()); + _packet->velocity[0] = static_cast(pvt->get_rx_vel()[1]); + _packet->velocity[1] = static_cast(pvt->get_rx_vel()[0]); + _packet->velocity[2] = static_cast(-pvt->get_rx_vel()[2]); + + uint16_t status = 0; + // Clarify table + _packet->status = status; } /* - * @brief encode_raw_gnss_packet - * @param raw_gnss_packet_t* Pointer to a structure that contains - * the information. + * @brief encode_sdr_gnss_packet + * @param sdr_gnss_packet_t* Pointer to a structure that contains the data. * @param an_packet_t* pointer to output packet * @reval None */ -void An_Packet_Printer::encode_raw_gnss_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const +void An_Packet_Printer::encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const { - _packet->id = 201; - _packet->length = RAW_GNSS_PACKET_LENGTH; - if (_packet != NULL) + _packet->id = SDR_GNSS_PACKET_ID; + _packet->length = SDR_GNSS_PACKET_LENGTH; + if (_packet != nullptr) { uint8_t offset = 0; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->unix_time_stamp), offset, _packet->data, sizeof(raw_packet->unix_time_stamp)); + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->nsvfix), offset, _packet->data, sizeof(sdr_gnss_packet->nsvfix)); + offset += 1; + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->gps_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->gps_satellites)); + offset += 1; + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->galileo_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->galileo_satellites)); + offset += 1; + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->microseconds), offset, _packet->data, sizeof(sdr_gnss_packet->microseconds)); offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->microseconds), offset, _packet->data, sizeof(raw_packet->microseconds)); - offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->latitude), offset, _packet->data, sizeof(raw_packet->latitude)); + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->latitude), offset, _packet->data, sizeof(sdr_gnss_packet->latitude)); offset += 8; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->longitude), offset, _packet->data, sizeof(raw_packet->longitude)); + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->longitude), offset, _packet->data, sizeof(sdr_gnss_packet->longitude)); offset += 8; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->height), offset, _packet->data, sizeof(raw_packet->height)); + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->height), offset, _packet->data, sizeof(sdr_gnss_packet->height)); offset += 8; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->velocity[0]), offset, _packet->data, sizeof(raw_packet->velocity[0])); + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->velocity[0]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[0])); offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->velocity[1]), offset, _packet->data, sizeof(raw_packet->velocity[1])); + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->velocity[1]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[1])); offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->velocity[2]), offset, _packet->data, sizeof(raw_packet->velocity[2])); + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->velocity[2]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[2])); offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->position_standard_deviation[0]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[0])); - offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->position_standard_deviation[1]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[1])); - offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->position_standard_deviation[2]), offset, _packet->data, sizeof(raw_packet->position_standard_deviation[2])); - offset += 4; - // This could be optimized to add just zeros - LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[0]), offset, _packet->data, sizeof(raw_packet->reserved[0])); - offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[1]), offset, _packet->data, sizeof(raw_packet->reserved[1])); - offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[2]), offset, _packet->data, sizeof(raw_packet->reserved[2])); - offset += 4; - LSB_bytes_to_array(reinterpret_cast(&raw_packet->reserved[3]), offset, _packet->data, sizeof(raw_packet->reserved[3])); - offset += 4; - - LSB_bytes_to_array(reinterpret_cast(&raw_packet->status.r), offset, _packet->data, sizeof(raw_packet->status)); - offset += 2; + for (int i = 0; i < 6; i++) + { + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->sats[i].prn), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].prn)); + offset += 1; + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->sats[i].snr), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].snr)); + offset += 1; + } + offset += 16; + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->status), offset, _packet->data, sizeof(sdr_gnss_packet->status)); } an_packet_encode(_packet); } @@ -254,7 +282,7 @@ uint16_t An_Packet_Printer::calculate_crc16(const void* data, uint16_t length) c /* - * Opens the serial device and sets the default baud rate for a NMEA transmission (9600,8,N,1) + * Opens the serial device and sets the default baud rate for a transmission (115200,8,N,1) */ int An_Packet_Printer::init_serial(const std::string& serial_device) { diff --git a/src/algorithms/PVT/libs/an_packet_printer.h b/src/algorithms/PVT/libs/an_packet_printer.h index 2d7d53177..7e16c5c1e 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.h +++ b/src/algorithms/PVT/libs/an_packet_printer.h @@ -21,8 +21,10 @@ #ifndef GNSS_SDR_AN_PACKET_PRINTER_H #define GNSS_SDR_AN_PACKET_PRINTER_H +#include "gnss_synchro.h" #include #include +#include #include /** \addtogroup PVT @@ -34,26 +36,25 @@ class Rtklib_Solver; typedef struct { - uint32_t unix_time_stamp; - uint32_t microseconds; - double latitude; - double longitude; - double height; - float velocity[3]; - float position_standard_deviation[3]; - float reserved[4]; - union + uint8_t nsvfix; // number of sats used in PVT fix + uint8_t gps_satellites; // number of tracked GPS satellites + uint8_t galileo_satellites; // number of tracked Galileo satellites + uint32_t microseconds; // ?? + double latitude; // in [rad] + double longitude; // in [rad] + double height; // in [m] + float velocity[3]; // North, East, Down, in [m/s] + + struct { - uint16_t r; - struct - { - uint32_t fix_type : 3; - uint32_t velocity_valid : 1; - uint32_t time_valid : 1; - uint32_t external_gnss : 1; - } b; - } status; -} raw_gnss_packet_t; + uint8_t prn; // Galileo sats expressed as PRN + 100 + uint8_t snr; // in [dB-Hz] + } sats[6]; + + float reserved[4]; + uint16_t status; + +} sdr_gnss_packet_t; typedef struct @@ -85,7 +86,7 @@ public: /*! * \brief Print AN packet to the initialized device. */ - bool print_packet(const Rtklib_Solver* const pvt_data); + bool print_packet(const Rtklib_Solver* const pvt_data, const std::map& gnss_observables_map); /*! * \brief Close serial port. Also done in the destructor, this is only @@ -108,15 +109,16 @@ private: 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}; - const size_t RAW_GNSS_PACKET_LENGTH = sizeof(raw_gnss_packet_t{}); + const size_t SDR_GNSS_PACKET_LENGTH = 73; + const uint8_t SDR_GNSS_PACKET_ID = 201; int init_serial(const std::string& serial_device); - void update_raw_gnss_packet(raw_gnss_packet_t* _packet, const Rtklib_Solver* const pvt) const; - void encode_gnss_cttc_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const; + void update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const Rtklib_Solver* const pvt, const std::map& gnss_observables_map) const; + void encode_gnss_cttc_packet(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const; uint16_t calculate_crc16(const void* data, uint16_t length) const; uint8_t calculate_header_lrc(const uint8_t* data) const; void an_packet_encode(an_packet_t* an_packet) const; - void encode_raw_gnss_packet(raw_gnss_packet_t* raw_packet, an_packet_t* _packet) const; + void encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const; void LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const; std::string d_an_devname; From 5e0c99faeed6d8261876c79188282681b53eb891 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Sep 2021 18:35:34 +0200 Subject: [PATCH 4/8] Add work on AN printer --- src/algorithms/PVT/libs/an_packet_printer.cc | 67 ++++++++++++++++---- src/algorithms/PVT/libs/an_packet_printer.h | 9 +-- 2 files changed, 59 insertions(+), 17 deletions(-) diff --git a/src/algorithms/PVT/libs/an_packet_printer.cc b/src/algorithms/PVT/libs/an_packet_printer.cc index bd1b74ebc..726ab50ab 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.cc +++ b/src/algorithms/PVT/libs/an_packet_printer.cc @@ -20,13 +20,13 @@ #include "an_packet_printer.h" #include "rtklib_solver.h" // for Rtklib_Solver -#include -#include // for DLOG -#include // for std::sqrt, M_PI -#include // for fcntl -#include // for std::cerr -#include // values for termios -#include // for write(), read(), close() +#include // for DLOG +#include // for std::sqrt, M_PI +#include // for fcntl +#include // for std::cerr +#include // std::numeric_limits +#include // values for termios +#include // for write(), read(), close() An_Packet_Printer::An_Packet_Printer(const std::string& an_dump_devname) @@ -82,6 +82,7 @@ void An_Packet_Printer::close_serial() const } } + /* * @brief update_sdr_gnss_packet * @param sdr_gnss_packet_t* Pointer to a structure that contains @@ -91,12 +92,10 @@ void An_Packet_Printer::close_serial() const */ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const Rtklib_Solver* const pvt, const std::map& gnss_observables_map) const { - const boost::posix_time::ptime time_origin(boost::gregorian::date(1970, 1, 1)); - boost::date_time::time_duration unix_t = pvt->get_position_UTC_time() - time_origin; - std::map::const_iterator gnss_observables_iter; uint8_t num_gps_sats = 0; uint8_t num_gal_sats = 0; + uint32_t microseconds = 0; int index = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); @@ -113,6 +112,23 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const { _packet->sats[index].prn = static_cast(gnss_observables_iter->second.PRN); _packet->sats[index].snr = static_cast(gnss_observables_iter->second.CN0_dB_hz); + int16_t doppler = 0; + double Carrier_Doppler_hz = gnss_observables_iter->second.CN0_dB_hz; + if (Carrier_Doppler_hz > static_cast(std::numeric_limits::max())) + { + doppler = std::numeric_limits::max(); + } + else if (Carrier_Doppler_hz < static_cast(std::numeric_limits::min())) + { + doppler = std::numeric_limits::min(); + } + else + { + doppler = static_cast(Carrier_Doppler_hz); + } + + _packet->sats[index].doppler = doppler; + microseconds = static_cast(static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs)) * 1e6; index++; } break; @@ -122,6 +138,23 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const { _packet->sats[index].prn = static_cast(gnss_observables_iter->second.PRN) + 100; _packet->sats[index].snr = static_cast(gnss_observables_iter->second.CN0_dB_hz); + int16_t doppler = 0; + double Carrier_Doppler_hz = gnss_observables_iter->second.CN0_dB_hz; + if (Carrier_Doppler_hz > static_cast(std::numeric_limits::max())) + { + doppler = std::numeric_limits::max(); + } + else if (Carrier_Doppler_hz < static_cast(std::numeric_limits::min())) + { + doppler = std::numeric_limits::min(); + } + else + { + doppler = static_cast(Carrier_Doppler_hz); + } + + _packet->sats[index].doppler = doppler; + microseconds = static_cast(static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs)) * 1e6; index++; } break; @@ -130,10 +163,11 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const } } } + _packet->nsvfix = static_cast(pvt->get_num_valid_observations()); _packet->gps_satellites = num_gps_sats; _packet->galileo_satellites = num_gal_sats; - _packet->microseconds = static_cast(unix_t.total_microseconds()); + _packet->microseconds = microseconds; _packet->latitude = static_cast(pvt->get_latitude()) * (M_PI / 180.0); _packet->longitude = static_cast(pvt->get_longitude()) * (M_PI / 180.0); _packet->height = static_cast(pvt->get_height()); @@ -142,7 +176,12 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const _packet->velocity[2] = static_cast(-pvt->get_rx_vel()[2]); uint16_t status = 0; - // Clarify table + // Set 3D fix + status = status & 0b00000011; // set bit 0 and 1 + // Set Doppler velocity valid + status = status & 0b00000100; // set bit 2 + // Set Time valid + status = status & 0b00001000; // set bit 3 _packet->status = status; } @@ -186,8 +225,10 @@ void An_Packet_Printer::encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packe offset += 1; LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->sats[i].snr), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].snr)); offset += 1; + LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->sats[i].doppler), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].doppler)); + offset += 2; } - offset += 16; + offset += 4; // reserved LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->status), offset, _packet->data, sizeof(sdr_gnss_packet->status)); } an_packet_encode(_packet); diff --git a/src/algorithms/PVT/libs/an_packet_printer.h b/src/algorithms/PVT/libs/an_packet_printer.h index 7e16c5c1e..19544306c 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.h +++ b/src/algorithms/PVT/libs/an_packet_printer.h @@ -39,7 +39,7 @@ typedef struct uint8_t nsvfix; // number of sats used in PVT fix uint8_t gps_satellites; // number of tracked GPS satellites uint8_t galileo_satellites; // number of tracked Galileo satellites - uint32_t microseconds; // ?? + uint32_t microseconds; // from start of receiver operation double latitude; // in [rad] double longitude; // in [rad] double height; // in [m] @@ -47,11 +47,12 @@ typedef struct struct { - uint8_t prn; // Galileo sats expressed as PRN + 100 - uint8_t snr; // in [dB-Hz] + uint8_t prn; // PRN ID. Galileo sats expressed as PRN + 100 + uint8_t snr; // in [dB-Hz] + int16_t doppler; // in [Hz], saturates at +/- 32767 Hz } sats[6]; - float reserved[4]; + uint32_t reserved = 0; uint16_t status; } sdr_gnss_packet_t; From 87fa29f3ab6880589f1e2d588f007b34d492f74f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 1 Oct 2021 08:49:15 +0200 Subject: [PATCH 5/8] CI: Fix cpplint job --- src/algorithms/PVT/libs/an_packet_printer.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/algorithms/PVT/libs/an_packet_printer.h b/src/algorithms/PVT/libs/an_packet_printer.h index 19544306c..f4a68ba45 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.h +++ b/src/algorithms/PVT/libs/an_packet_printer.h @@ -54,7 +54,6 @@ typedef struct uint32_t reserved = 0; uint16_t status; - } sdr_gnss_packet_t; From 1f7eefba8193e28da5c6ae8adf5f872257e054c1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 1 Oct 2021 08:50:33 +0200 Subject: [PATCH 6/8] CI: fix building in macOS --- .github/workflows/main.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 3d065dfa7..84ead2408 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -26,7 +26,7 @@ jobs: steps: - uses: actions/checkout@v1 - name: install dependencies - run: brew update && brew install ninja pkg-config hdf5 automake armadillo lapack gflags glog gnuradio log4cpp pugixml protobuf && pip3 install mako + run: brew update && brew install ninja pkg-config hdf5 automake armadillo lapack gflags glog gnuradio log4cpp openssl pugixml protobuf && pip3 install mako - name: configure run: cd build && cmake -GNinja .. - name: build @@ -54,7 +54,7 @@ jobs: steps: - uses: actions/checkout@v1 - name: install dependencies - run: brew update && brew install llvm pkg-config hdf5 armadillo lapack gflags glog gnuradio libmatio log4cpp pugixml protobuf && ln -s /usr/local/opt/llvm/bin/clang-tidy /usr/local/bin && ln -s /usr/local/Cellar/llvm/12.*/bin/clang-apply-replacements /usr/local/bin && cp /usr/local/Cellar/llvm/12.*/share/clang/run-clang-tidy.py /usr/local/bin && pip3 install mako + run: brew update && brew install llvm pkg-config hdf5 armadillo lapack gflags glog gnuradio libmatio log4cpp openssl pugixml protobuf && ln -s /usr/local/opt/llvm/bin/clang-tidy /usr/local/bin && ln -s /usr/local/Cellar/llvm/12.*/bin/clang-apply-replacements /usr/local/bin && cp /usr/local/Cellar/llvm/12.*/share/clang/run-clang-tidy.py /usr/local/bin && pip3 install mako - name: Prepare run run: cd build && cmake .. && make volk_gnsssdr_module gtest-1.11.0 core_monitor core_libs pvt_libs - name: run clang-tidy From 8298062f0e537a8907bdc53685e095de27ab6a3f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 1 Oct 2021 14:25:03 +0200 Subject: [PATCH 7/8] Improve code robustness --- src/algorithms/PVT/libs/an_packet_printer.cc | 64 ++++++++++---------- src/algorithms/PVT/libs/an_packet_printer.h | 3 +- 2 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/algorithms/PVT/libs/an_packet_printer.cc b/src/algorithms/PVT/libs/an_packet_printer.cc index 726ab50ab..1f7440c8f 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.cc +++ b/src/algorithms/PVT/libs/an_packet_printer.cc @@ -97,6 +97,7 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const uint8_t num_gal_sats = 0; uint32_t microseconds = 0; int index = 0; + const int max_reported_sats = *(&_packet->sats + 1) - _packet->sats; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); @@ -108,12 +109,12 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const { case 'G': num_gps_sats++; - if (index < 6) + if (index < max_reported_sats) { _packet->sats[index].prn = static_cast(gnss_observables_iter->second.PRN); _packet->sats[index].snr = static_cast(gnss_observables_iter->second.CN0_dB_hz); int16_t doppler = 0; - double Carrier_Doppler_hz = gnss_observables_iter->second.CN0_dB_hz; + double Carrier_Doppler_hz = gnss_observables_iter->second.Carrier_Doppler_hz; if (Carrier_Doppler_hz > static_cast(std::numeric_limits::max())) { doppler = std::numeric_limits::max(); @@ -134,12 +135,12 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const break; case 'E': num_gal_sats++; - if (index < 6) + if (index < max_reported_sats) { _packet->sats[index].prn = static_cast(gnss_observables_iter->second.PRN) + 100; _packet->sats[index].snr = static_cast(gnss_observables_iter->second.CN0_dB_hz); int16_t doppler = 0; - double Carrier_Doppler_hz = gnss_observables_iter->second.CN0_dB_hz; + double Carrier_Doppler_hz = gnss_observables_iter->second.Carrier_Doppler_hz; if (Carrier_Doppler_hz > static_cast(std::numeric_limits::max())) { doppler = std::numeric_limits::max(); @@ -176,12 +177,8 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const _packet->velocity[2] = static_cast(-pvt->get_rx_vel()[2]); uint16_t status = 0; - // Set 3D fix - status = status & 0b00000011; // set bit 0 and 1 - // Set Doppler velocity valid - status = status & 0b00000100; // set bit 2 - // Set Time valid - status = status & 0b00001000; // set bit 3 + // Set 3D fix (bit 0 and 1) / Set Doppler velocity valid (bit 2) / Set Time valid (bit 3) + status = status & 0b00001111; _packet->status = status; } @@ -200,35 +197,36 @@ void An_Packet_Printer::encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packe { uint8_t offset = 0; LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->nsvfix), offset, _packet->data, sizeof(sdr_gnss_packet->nsvfix)); - offset += 1; + offset += sizeof(sdr_gnss_packet->nsvfix); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->gps_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->gps_satellites)); - offset += 1; + offset += sizeof(sdr_gnss_packet->gps_satellites); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->galileo_satellites), offset, _packet->data, sizeof(sdr_gnss_packet->galileo_satellites)); - offset += 1; + offset += sizeof(sdr_gnss_packet->galileo_satellites); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->microseconds), offset, _packet->data, sizeof(sdr_gnss_packet->microseconds)); - offset += 4; + offset += sizeof(sdr_gnss_packet->microseconds); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->latitude), offset, _packet->data, sizeof(sdr_gnss_packet->latitude)); - offset += 8; + offset += sizeof(sdr_gnss_packet->latitude); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->longitude), offset, _packet->data, sizeof(sdr_gnss_packet->longitude)); - offset += 8; + offset += sizeof(sdr_gnss_packet->longitude); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->height), offset, _packet->data, sizeof(sdr_gnss_packet->height)); - offset += 8; + offset += sizeof(sdr_gnss_packet->height); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->velocity[0]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[0])); - offset += 4; + offset += sizeof(sdr_gnss_packet->velocity[0]); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->velocity[1]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[1])); - offset += 4; + offset += sizeof(sdr_gnss_packet->velocity[1]); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->velocity[2]), offset, _packet->data, sizeof(sdr_gnss_packet->velocity[2])); - offset += 4; - for (int i = 0; i < 6; i++) + offset += sizeof(sdr_gnss_packet->velocity[2]); + for (auto& sat : sdr_gnss_packet->sats) { - LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->sats[i].prn), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].prn)); - offset += 1; - LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->sats[i].snr), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].snr)); - offset += 1; - LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->sats[i].doppler), offset, _packet->data, sizeof(sdr_gnss_packet->sats[i].doppler)); - offset += 2; + LSB_bytes_to_array(reinterpret_cast(&sat.prn), offset, _packet->data, sizeof(sat.prn)); + offset += sizeof(sat.prn); + LSB_bytes_to_array(reinterpret_cast(&sat.snr), offset, _packet->data, sizeof(sat.snr)); + offset += sizeof(sat.snr); + LSB_bytes_to_array(reinterpret_cast(&sat.doppler), offset, _packet->data, sizeof(sat.doppler)); + offset += sizeof(sat.doppler); } - offset += 4; // reserved + + offset = static_cast(SDR_GNSS_PACKET_LENGTH - sizeof(sdr_gnss_packet->status)); LSB_bytes_to_array(reinterpret_cast(&sdr_gnss_packet->status), offset, _packet->data, sizeof(sdr_gnss_packet->status)); } an_packet_encode(_packet); @@ -264,7 +262,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, { case 1: { - uint8_t* tmp = reinterpret_cast(_in); + auto* tmp = reinterpret_cast(_in); for (int i = 0; i < var_size; i++) { _out[offset + i] = (*tmp >> 8 * i) & 0xFF; @@ -273,7 +271,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, } case 2: { - uint16_t* tmp = reinterpret_cast(_in); + auto* tmp = reinterpret_cast(_in); for (int i = 0; i < var_size; i++) { _out[offset + i] = (*tmp >> 8 * i) & 0xFF; @@ -282,7 +280,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, } case 4: { - uint32_t* tmp = reinterpret_cast(_in); + auto* tmp = reinterpret_cast(_in); for (int i = 0; i < var_size; i++) { _out[offset + i] = (*tmp >> 8 * i) & 0xFF; @@ -291,7 +289,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, } case 8: { - uint64_t* tmp = reinterpret_cast(_in); + auto* tmp = reinterpret_cast(_in); for (int i = 0; i < var_size; i++) { _out[offset + i] = (*tmp >> 8 * i) & 0xFF; @@ -312,7 +310,7 @@ void An_Packet_Printer::LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, */ uint16_t An_Packet_Printer::calculate_crc16(const void* data, uint16_t length) const { - const uint8_t* bytes = reinterpret_cast(data); + const auto* bytes = reinterpret_cast(data); uint16_t crc = 0xFFFF; for (uint16_t i = 0; i < length; i++) { diff --git a/src/algorithms/PVT/libs/an_packet_printer.h b/src/algorithms/PVT/libs/an_packet_printer.h index f4a68ba45..a85b72364 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.h +++ b/src/algorithms/PVT/libs/an_packet_printer.h @@ -23,6 +23,7 @@ #include "gnss_synchro.h" #include +#include #include #include #include @@ -49,7 +50,7 @@ typedef struct { uint8_t prn; // PRN ID. Galileo sats expressed as PRN + 100 uint8_t snr; // in [dB-Hz] - int16_t doppler; // in [Hz], saturates at +/- 32767 Hz + int16_t doppler; // in [Hz], saturates at +32767 / -32768 Hz } sats[6]; uint32_t reserved = 0; From 4bb3bab2d32e691c02a131fbe00c63b745a06dbc Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 1 Oct 2021 14:36:57 +0200 Subject: [PATCH 8/8] Fix comment in include --- src/algorithms/PVT/libs/an_packet_printer.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/an_packet_printer.cc b/src/algorithms/PVT/libs/an_packet_printer.cc index 1f7440c8f..a840334fd 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.cc +++ b/src/algorithms/PVT/libs/an_packet_printer.cc @@ -21,7 +21,7 @@ #include "an_packet_printer.h" #include "rtklib_solver.h" // for Rtklib_Solver #include // for DLOG -#include // for std::sqrt, M_PI +#include // for M_PI #include // for fcntl #include // for std::cerr #include // std::numeric_limits