From 60dd9b4f282de06a2d7c1df4c88fbc79287bb277 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 21 Nov 2015 13:01:50 +0100 Subject: [PATCH] working on the RTCM printer --- src/algorithms/PVT/libs/rtcm_printer.cc | 304 +--- src/algorithms/PVT/libs/rtcm_printer.h | 166 +- src/core/system_parameters/CMakeLists.txt | 1 + .../system_parameters/galileo_ephemeris.cc | 2 + .../system_parameters/galileo_ephemeris.h | 2 + .../system_parameters/galileo_fnav_message.cc | 8 +- .../system_parameters/galileo_fnav_message.h | 4 +- src/core/system_parameters/rtcm.cc | 1577 +++++++++++++++++ src/core/system_parameters/rtcm.h | 416 +++++ src/tests/gnss_block/rtcm_printer_test.cc | 83 + 10 files changed, 2148 insertions(+), 415 deletions(-) create mode 100644 src/core/system_parameters/rtcm.cc create mode 100644 src/core/system_parameters/rtcm.h diff --git a/src/algorithms/PVT/libs/rtcm_printer.cc b/src/algorithms/PVT/libs/rtcm_printer.cc index 1c84239e1..518314446 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.cc +++ b/src/algorithms/PVT/libs/rtcm_printer.cc @@ -34,18 +34,11 @@ #include "rtcm_printer.h" #include // for O_RDWR #include // for tcgetattr -#include // for std::reverse -#include // for std::stringstream -#include // for to_upper_copy -#include -#include #include #include - using google::LogMessage; -//DEFINE_string(RTCM_version, "3.2", "Specifies the RTCM Version"); Rtcm_Printer::Rtcm_Printer(std::string filename, bool flag_rtcm_tty_port, std::string rtcm_dump_devname) { @@ -69,9 +62,7 @@ Rtcm_Printer::Rtcm_Printer(std::string filename, bool flag_rtcm_tty_port, std::s { rtcm_dev_descriptor = -1; } - Rtcm_Printer::reset_data_fields(); - preamble = std::bitset<8>("11010011"); - reserved_field = std::bitset<6>("000000"); + rtcm = std::make_shared(); } @@ -93,6 +84,27 @@ Rtcm_Printer::~Rtcm_Printer() } +bool Rtcm_Printer::Print_Rtcm_M1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map & pseudoranges) +{ + std::string m1001 = rtcm->print_M1001( gps_eph, obs_time, pseudoranges); + Rtcm_Printer::Print_Message(m1001); + return true; +} + +bool Rtcm_Printer::Print_Rtcm_M1019(const Gps_Ephemeris & gps_eph) +{ + std::string m1019 = rtcm->print_M1019(gps_eph); + Rtcm_Printer::Print_Message(m1019); + return true; +} + +bool Rtcm_Printer::Print_Rtcm_M1045(const Galileo_Ephemeris & gal_eph) +{ + std::string m1045 = rtcm->print_M1045(gal_eph); + Rtcm_Printer::Print_Message(m1045); + return true; +} + int Rtcm_Printer::init_serial(std::string serial_device) { @@ -140,255 +152,39 @@ void Rtcm_Printer::close_serial() } } -void Rtcm_Printer::reset_data_fields() + + +bool Rtcm_Printer::Print_Message(std::string message) { - //DF001.reset(); - DF002.reset(); - DF003.reset(); - DF004.reset(); - DF005.reset(); - DF006.reset(); - DF007.reset(); - DF008.reset(); - DF009.reset(); - DF010.reset(); - DF011.reset(); - DF012.reset(); - DF013.reset(); - DF014.reset(); - DF015.reset(); + try + { + rtcm_file_descriptor << message << std::endl; - // Contents of GPS Satellite Ephemeris Data, Message Type 1019 - DF071.reset(); - DF076.reset(); - DF077.reset(); - DF078.reset(); - DF079.reset(); - DF081.reset(); - DF082.reset(); - DF083.reset(); - DF084.reset(); - DF085.reset(); - DF086.reset(); - DF087.reset(); + } + catch(std::exception ex) + { + DLOG(INFO) << "RTCM printer can not write on output file" << rtcm_filename.c_str(); + } - DF088.reset(); - DF089.reset(); - DF090.reset(); - DF091.reset(); - DF092.reset(); - DF093.reset(); - DF094.reset(); - DF095.reset(); - DF096.reset(); - DF097.reset(); - DF098.reset(); - DF099.reset(); - DF100.reset(); - DF101.reset(); - DF102.reset(); - DF103.reset(); - DF137.reset(); - - // Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045 - DF252.reset(); - DF289.reset(); - DF290.reset(); - DF291.reset(); - DF292.reset(); - DF293.reset(); - DF294.reset(); - DF295.reset(); - DF296.reset(); - DF297.reset(); - DF298.reset(); - DF299.reset(); - DF300.reset(); - DF301.reset(); - DF302.reset(); - DF303.reset(); - DF304.reset(); - DF305.reset(); - DF306.reset(); - DF307.reset(); - DF308.reset(); - DF309.reset(); - DF310.reset(); - DF311.reset(); - DF312.reset(); - DF314.reset(); - DF315.reset(); -} - - - -/* Stationary Antenna Reference Point, No Height Information - * Reference Station Id = 2003 - GPS Service supported, but not GLONASS or Galileo - ARP ECEF-X = 1114104.5999 meters - ARP ECEF-Y = -4850729.7108 meters - ARP ECEF-Z = 3975521.4643 meters - Expected output: D3 00 13 3E D7 D3 02 02 98 0E DE EF 34 B4 BD 62 - AC 09 41 98 6F 33 36 0B 98 - */ -std::bitset<152> Rtcm_Printer::get_M1005_test () -{ - unsigned int m1005 = 1005; - unsigned int reference_station_id = 2003; // Max: 4095 - long long int ECEF_X = 11141045999; // Resolution 0.0001 m - long long int ECEF_Y = -48507297108; // Resolution 0.0001 m - long long int ECEF_Z = 39755214643; // Resolution 0.0001 m - unsigned int itrf_realization_year = 0; // Reserved - std::bitset<1> DF001; - - DF002 = std::bitset<12>(m1005); - DF003 = std::bitset<12>(reference_station_id); - DF021 = std::bitset<6>(itrf_realization_year); - DF022 = std::bitset<1>("1"); // GPS - DF023 = std::bitset<1>("0"); // Glonass - DF024 = std::bitset<1>("0"); // Galileo - DF141 = std::bitset<1>("0"); // 0: Real, physical reference station - DF001 = std::bitset<1>("0"); // Reserved, set to 0 - DF025 = std::bitset<38>(ECEF_X); // ECEF-X in 0.0001 m - DF142 = std::bitset<1>("0"); // Single Receiver Oscillator Indicator - DF026 = std::bitset<38>(ECEF_Y); // ECEF-Y in 0.0001 m - DF364 = std::bitset<2>("00"); // Quarter Cycle Indicator - DF027 = std::bitset<38>(ECEF_Z); // ECEF-Z in 0.0001 m - - std::string message = DF002.to_string() + - DF003.to_string() + - DF021.to_string() + - DF022.to_string() + - DF023.to_string() + - DF024.to_string() + - DF141.to_string() + - DF025.to_string() + - DF142.to_string() + - DF001.to_string() + - DF026.to_string() + - DF364.to_string() + - DF027.to_string() ; - - std::bitset<152> test_msg(message); - return test_msg; -} - - - -std::string Rtcm_Printer::print_M1005_test () -{ - std::bitset<152> m1005 = get_M1005_test(); - unsigned int msg_length_bits = m1005.to_string().length(); - unsigned int msg_length_bytes = std::ceil(static_cast(msg_length_bits) / 8.0); - message_length = std::bitset<10>(msg_length_bytes); - unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits; - std::string b(zeros_to_fill, '0'); - std::string msg_content = m1005.to_string() + b; - std::string msg_without_crc = preamble.to_string() + - reserved_field.to_string() + - message_length.to_string() + - msg_content; - return Rtcm_Printer::add_CRC(msg_without_crc); -} - - - -std::bitset<122> Rtcm_Printer::get_M1001() -{ - unsigned int m1001 = 1001; - unsigned int reference_station_id = 1234; // Max: 4095 - DF002 = std::bitset<12>(m1001); - DF003 = std::bitset<12>(reference_station_id); - //DF004 = std::bitset<30> - //DF005 = std::bitset<1> - //DF006 = std::bitset<5> - DF007 = std::bitset<1>("0"); - //DF008 = std::bitset<3> - std::bitset<122> fake_msg; - fake_msg.reset(); - return fake_msg; -} - - - -void Rtcm_Printer::print_M1001 () -{ - std::bitset<122> m1001 = get_M1001(); - unsigned int msg_length_bits = m1001.to_string().length(); - unsigned int msg_length_bytes = std::ceil(static_cast(msg_length_bits) / 8.0); - message_length = std::bitset<10>(msg_length_bytes); - unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits; - std::string b(zeros_to_fill, '0'); - message_length = std::bitset<10>(static_cast(msg_length_bytes)); - std::string msg_content = m1001.to_string() + b; - std::string msg_without_crc = preamble.to_string() + - reserved_field.to_string() + - message_length.to_string() + - msg_content; - std::string message = Rtcm_Printer::add_CRC(msg_without_crc); -} - - - -std::bitset<138> Rtcm_Printer::get_M1002 () -{ - std::bitset<138> fake_msg; - fake_msg.reset(); - return fake_msg; -} - -std::bitset<488> Rtcm_Printer::get_M1019 () -{ - std::bitset<488> fake_msg; - fake_msg.reset(); - return fake_msg; -} - - - -std::bitset<496> Rtcm_Printer::get_M1045 () -{ - std::bitset<496> fake_msg; - fake_msg.reset(); - return fake_msg; -} - - - - - -std::string Rtcm_Printer::add_CRC (const std::string& message_without_crc) -{ - // ****** Computes Qualcomm CRC-24Q ****** - // 1) Converts the string to a vector of unsigned char: - boost::dynamic_bitset frame_bits(message_without_crc); - std::vector bytes; - boost::to_block_range(frame_bits, std::back_inserter(bytes)); - std::reverse(bytes.begin(),bytes.end()); - - // 2) Computes CRC - CRC_RTCM.process_bytes(bytes.data(), bytes.size()); - crc_frame = std::bitset<24>(CRC_RTCM.checksum()); - - // 3) Builds the complete message - std::string complete_message = message_without_crc + crc_frame.to_string(); - return bin_to_hex(complete_message); -} - - -std::string Rtcm_Printer::bin_to_hex(const std::string& s) -{ - std::string s_aux; - std::stringstream ss; - for(int i = 0; i < s.length() - 1; i = i + 32) + //write to serial device + if (rtcm_dev_descriptor!=-1) { - s_aux.assign(s, i, 32); - std::bitset<32> bs(s_aux); - unsigned n = bs.to_ulong(); - ss << std::hex << n; + try + { + int n_bytes_written; + n_bytes_written = write(rtcm_dev_descriptor, message.c_str(), message.length()); + } + catch(std::exception ex) + { + DLOG(INFO) << "RTCM printer can not write on serial device" << rtcm_filename.c_str();; + } } - //return ss.str(); - return boost::to_upper_copy(ss.str()); + return true; } +std::string Rtcm_Printer::print_M1005_test() +{ + std::string test = rtcm->print_M1005_test(); + return test; +} diff --git a/src/algorithms/PVT/libs/rtcm_printer.h b/src/algorithms/PVT/libs/rtcm_printer.h index 2ce08dcad..bc0a540f9 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.h +++ b/src/algorithms/PVT/libs/rtcm_printer.h @@ -34,13 +34,9 @@ #ifndef GNSS_SDR_RTCM_PRINTER_H_ #define GNSS_SDR_RTCM_PRINTER_H_ -#include // std::bitset #include // std::ofstream #include // std::cout -#include // std::string -#include -#include - +#include "rtcm.h" /*! * \brief This class provides a implementation of a subset of the RTCM Standard 10403.2 messages @@ -53,170 +49,26 @@ public: */ Rtcm_Printer(std::string filename, bool flag_rtcm_tty_port, std::string rtcm_dump_filename); - /*! - * \brief Print RTCM 3.2 messages to the initialized device - */ - //bool Print_Nmea_Line(gps_l1_ca_ls_pvt* position, bool print_average_values); - /*! * \brief Default destructor. */ ~Rtcm_Printer(); - void print_M1001(); - std::string print_M1005_test(); + bool Print_Rtcm_M1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map & pseudoranges); + bool Print_Rtcm_M1019(const Gps_Ephemeris & gps_eph); // DF001; - std::bitset<12> DF002; - std::bitset<12> DF003; - std::bitset<30> DF004; - std::bitset<1> DF005; - std::bitset<5> DF006; - std::bitset<1> DF007; - std::bitset<3> DF008; - std::bitset<6> DF009; - std::bitset<1> DF010; - std::bitset<24> DF011; - std::bitset<20> DF012; - std::bitset<7> DF013; - std::bitset<8> DF014; - std::bitset<8> DF015; - - - std::bitset<6> DF021; - std::bitset<1> DF022; - std::bitset<1> DF023; - std::bitset<1> DF024; - std::bitset<38> DF025; - std::bitset<38> DF026; - std::bitset<38> DF027; - - // Contents of GPS Satellite Ephemeris Data, Message Type 1019 - std::bitset<8> DF071; - std::bitset<10> DF076; - std::bitset<4> DF077; - std::bitset<2> DF078; - std::bitset<14> DF079; - std::bitset<16> DF081; - std::bitset<8> DF082; - std::bitset<16> DF083; - std::bitset<22> DF084; - std::bitset<10> DF085; - std::bitset<16> DF086; - std::bitset<16> DF087; - - std::bitset<32> DF088; - std::bitset<16> DF089; - std::bitset<32> DF090; - std::bitset<16> DF091; - std::bitset<32> DF092; - std::bitset<16> DF093; - std::bitset<16> DF094; - std::bitset<32> DF095; - std::bitset<16> DF096; - std::bitset<32> DF097; - std::bitset<16> DF098; - std::bitset<32> DF099; - std::bitset<24> DF100; - std::bitset<8> DF101; - std::bitset<6> DF102; - std::bitset<1> DF103; - std::bitset<1> DF137; - - - std::bitset<1> DF141; - std::bitset<1> DF142; - - // Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045 - std::bitset<6> DF252; - std::bitset<12> DF289; - std::bitset<10> DF290; - std::bitset<8> DF291; - std::bitset<14> DF292; - std::bitset<14> DF293; - std::bitset<6> DF294; - std::bitset<21> DF295; - std::bitset<31> DF296; - std::bitset<16> DF297; - std::bitset<32> DF298; - std::bitset<14> DF299; - std::bitset<16> DF300; - std::bitset<32> DF301; - std::bitset<16> DF302; - std::bitset<32> DF303; - std::bitset<14> DF304; - std::bitset<16> DF305; - std::bitset<32> DF306; - std::bitset<16> DF307; - std::bitset<32> DF308; - std::bitset<16> DF309; - std::bitset<32> DF310; - std::bitset<24> DF311; - std::bitset<10> DF312; - std::bitset<2> DF314; - std::bitset<1> DF315; - - std::bitset<2> DF364; - - // Content of message header for MSM1, MSM2, MSM3, MSM4, MSM5, MSM6 and MSM7 - std::bitset<1> DF393; - std::bitset<1> DF394; - std::bitset<1> DF395; - std::bitset<1> DF396; //variable - std::bitset<1> DF409; - std::bitset<1> DF411; - std::bitset<1> DF412; - std::bitset<1> DF417; - std::bitset<1> DF418; - - // Content of Satellite data for MSM4 and MSM6 - std::vector > DF397; // 8*NSAT - std::vector > DF398; // 10*NSAT - - // Content of Satellite data for MSM5 and MSM7 - std::vector > DF399; // 14*NSAT - - // Messages - std::bitset<64> message1001_header; - std::bitset<58> message1001_content; - std::bitset<64> message1002_header; - std::bitset<74> message1002_content; - - std::bitset<488> message1019_content; - - std::bitset<496> message1045_content; - - std::bitset<169> MSM_header; // 169+X - - std::vector > MSM4_content; // 18 * Nsat - - std::vector > MSM5_content; // 36 * Nsat - - std::bitset<122> get_M1001(); - std::bitset<138> get_M1002(); // GPS observables - std::bitset<488> get_M1019(); // GPS ephemeris - std::bitset<496> get_M1045(); // Galileo ephemeris - std::bitset<152> get_M1005_test(); - - void reset_data_fields (); - - // Transport Layer - std::bitset<8> preamble; - std::bitset<6> reserved_field; - std::bitset<10> message_length; - std::bitset<24> crc_frame; - typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type; - crc_24_q_type CRC_RTCM; - std::string add_CRC(const std::string& m); - std::string bin_to_hex(const std::string& s); + std::shared_ptr rtcm; + bool Print_Message(std::string message); }; #endif diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index bc6949dff..e46e1ad39 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -41,6 +41,7 @@ set(SYSTEM_PARAMETERS_SOURCES gps_cnav_navigation_message.cc gps_cnav_iono.cc gps_cnav_utc_model.cc + rtcm.cc ) diff --git a/src/core/system_parameters/galileo_ephemeris.cc b/src/core/system_parameters/galileo_ephemeris.cc index b69a40d35..bff3dedff 100644 --- a/src/core/system_parameters/galileo_ephemeris.cc +++ b/src/core/system_parameters/galileo_ephemeris.cc @@ -63,8 +63,10 @@ Galileo_Ephemeris::Galileo_Ephemeris() TOW_5 = 0; // SV status SISA_3 = 0; + E5a_HS = 0; E5b_HS_5 = 0; E1B_HS_5 = 0; + E5a_DVS = false; E5b_DVS_5 = 0; E1B_DVS_5 = 0; BGD_E1E5a_5 = 0; // E1-E5a Broadcast Group Delay [s] diff --git a/src/core/system_parameters/galileo_ephemeris.h b/src/core/system_parameters/galileo_ephemeris.h index c206973d8..61b6afc46 100644 --- a/src/core/system_parameters/galileo_ephemeris.h +++ b/src/core/system_parameters/galileo_ephemeris.h @@ -85,8 +85,10 @@ public: // SV status double SISA_3; + unsigned int E5a_HS; //!< E5a Signal Health Status double E5b_HS_5; //!< E5b Signal Health Status double E1B_HS_5; //!< E1B Signal Health Status + bool E5a_DVS; //!< E5a Data Validity Status double E5b_DVS_5; //!< E5b Data Validity Status double E1B_DVS_5; //!< E1B Data Validity Status diff --git a/src/core/system_parameters/galileo_fnav_message.cc b/src/core/system_parameters/galileo_fnav_message.cc index 776e4d544..e8a34ad44 100644 --- a/src/core/system_parameters/galileo_fnav_message.cc +++ b/src/core/system_parameters/galileo_fnav_message.cc @@ -270,10 +270,10 @@ void Galileo_Fnav_Message::decode_page(std::string data) FNAV_region5_1 = static_cast(read_navigation_unsigned(data_bits, FNAV_region5_1_bit)); FNAV_BGD_1 = static_cast(read_navigation_signed(data_bits, FNAV_BGD_1_bit)); FNAV_BGD_1 *= FNAV_BGD_1_LSB; - FNAV_E5ahs_1 = static_cast(read_navigation_unsigned(data_bits, FNAV_E5ahs_1_bit)); + FNAV_E5ahs_1 = static_cast(read_navigation_unsigned(data_bits, FNAV_E5ahs_1_bit)); FNAV_WN_1 = static_cast(read_navigation_unsigned(data_bits, FNAV_WN_1_bit)); FNAV_TOW_1 = static_cast(read_navigation_unsigned(data_bits, FNAV_TOW_1_bit)); - FNAV_E5advs_1 = static_cast(read_navigation_unsigned(data_bits, FNAV_E5advs_1_bit)); + FNAV_E5advs_1 = static_cast(read_navigation_unsigned(data_bits, FNAV_E5advs_1_bit)); flag_TOW_1 = true; flag_TOW_set = true; @@ -628,6 +628,10 @@ Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris() /*GST*/ ephemeris.WN_5 = FNAV_WN_3; // Week number ephemeris.TOW_5 = FNAV_TOW_3; // Time of Week + + /* Health status */ + ephemeris.E5a_HS = FNAV_E5ahs_1; + ephemeris.E5a_DVS = FNAV_E5advs_1; return ephemeris; } diff --git a/src/core/system_parameters/galileo_fnav_message.h b/src/core/system_parameters/galileo_fnav_message.h index 6e398090f..453473568 100644 --- a/src/core/system_parameters/galileo_fnav_message.h +++ b/src/core/system_parameters/galileo_fnav_message.h @@ -127,7 +127,7 @@ public: double FNAV_E5ahs_1; double FNAV_WN_1; double FNAV_TOW_1; - double FNAV_E5advs_1; + bool FNAV_E5advs_1; // WORD 2 Ephemeris (1/3) and GST int FNAV_IODnav_2; @@ -186,7 +186,7 @@ public: double FNAV_M0_1_5; double FNAV_af0_1_5; double FNAV_af1_1_5; - double FNAV_E5ahs_1_5; + unsigned int FNAV_E5ahs_1_5; int FNAV_SVID2_5; double FNAV_Deltaa12_2_5; double FNAV_e_2_5; diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc new file mode 100644 index 000000000..6a0a2c31f --- /dev/null +++ b/src/core/system_parameters/rtcm.cc @@ -0,0 +1,1577 @@ +/*! + * \file rtcm.cc + * \brief Implementation of RTCM 3.2 Standard + * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "rtcm.h" +#include // for std::reverse +#include // for strtol +#include // for std::stringstream +#include // for to_upper_copy +#include +#include +#include +#include +#include "GPS_L1_CA.h" + + + +using google::LogMessage; + +DEFINE_int32(RTCM_Ref_Station_ID, 1234, "Reference Station ID in RTCM messages"); + + + +Rtcm::Rtcm() +{ + Rtcm::reset_data_fields(); + preamble = std::bitset<8>("11010011"); + reserved_field = std::bitset<6>("000000"); +} + + + + +// ***************************************************************************************************** +// +// TRANSPORT LAYER AS DEFINED AT RTCM STANDARD 10403.2 +// +// ***************************************************************************************************** + +std::string Rtcm::add_CRC (const std::string& message_without_crc) +{ + // ****** Computes Qualcomm CRC-24Q ****** + // 1) Converts the string to a vector of unsigned char: + boost::dynamic_bitset frame_bits(message_without_crc); + std::vector bytes; + boost::to_block_range(frame_bits, std::back_inserter(bytes)); + std::reverse(bytes.begin(),bytes.end()); + + // 2) Computes CRC + CRC_RTCM.process_bytes(bytes.data(), bytes.size()); + crc_frame = std::bitset<24>(CRC_RTCM.checksum()); + + // 3) Builds the complete message + std::string complete_message = message_without_crc + crc_frame.to_string(); + return bin_to_hex(complete_message); +} + + +bool Rtcm::check_CRC(const std::string & message) +{ + // Convert message to binary + std::string message_bin = Rtcm::hex_to_bin(message); + // Check CRC + std::string crc = message_bin.substr(message_bin.length() - 24, 24); + std::bitset<24> read_crc = std::bitset<24>(crc); + std::string msg_without_crc = message_bin.substr(0, message_bin.length() - 24); + + boost::dynamic_bitset frame_bits(msg_without_crc); + std::vector bytes; + boost::to_block_range(frame_bits, std::back_inserter(bytes)); + std::reverse(bytes.begin(),bytes.end()); + + CRC_RTCM.process_bytes(bytes.data(), bytes.size()); + std::bitset<24> computed_crc = std::bitset<24>(CRC_RTCM.checksum()); + if(read_crc == computed_crc) + { + return true; + } + else + { + return false; + } +} + + +std::string Rtcm::bin_to_hex(const std::string& s) +{ + std::string s_aux; + std::stringstream ss; + for(int i = 0; i < s.length() - 1; i = i + 32) + { + s_aux.assign(s, i, 32); + std::bitset<32> bs(s_aux); + unsigned n = bs.to_ulong(); + ss << std::hex << n; + } + return boost::to_upper_copy(ss.str()); +} + +std::string Rtcm::hex_to_bin(const std::string& s) +{ + std::string s_aux; + s_aux.clear(); + std::stringstream ss; + ss << s; + std::string s_lower = boost::to_upper_copy(ss.str()); + for(int i = 0; i < s.length(); i++) + { + unsigned long int n; + std::istringstream(s_lower.substr(i,1)) >> std::hex >> n; + std::bitset<4> bs(n); + s_aux += bs.to_string(); + } + return s_aux; +} + +unsigned long int Rtcm::bin_to_uint(const std::string& s) +{ + if(s.length() > 32) + { + LOG(WARNING) << "Cannot convert to a unsigned long int"; + return 0; + } + unsigned long int reading = strtoul(s.c_str(), NULL, 2); + return reading; +} + +long int Rtcm::bin_to_int(const std::string& s) +{ + if(s.length() > 32) + { + LOG(WARNING) << "Cannot convert to a long int"; + return 0; + } + long int reading = strtol(s.c_str(), NULL, 2); + return reading; +} + + +double Rtcm::bin_to_double(const std::string& s) +{ + double reading; + if(s.length() > 64) + { + LOG(WARNING) << "Cannot convert to a double"; + return 0; + } + + long long int reading_int = strtoll(s.c_str(), NULL, 2); + + // Handle negative numbers + if(s.substr(0,1).compare("0")) + { + // Computing two's complement + boost::dynamic_bitset<> original_bitset(s); + original_bitset.flip(); + reading_int = - (original_bitset.to_ulong() + 1); + } + + reading = static_cast(reading_int); + return reading; +} + + +unsigned long int Rtcm::hex_to_uint(const std::string& s) +{ + if(s.length() > 32) + { + LOG(WARNING) << "Cannot convert to a unsigned long int"; + return 0; + } + unsigned long int reading = strtoul(s.c_str(), NULL, 16); + return reading; +} + + +long int Rtcm::hex_to_int(const std::string& s) +{ + if(s.length() > 32) + { + LOG(WARNING) << "Cannot convert to a long int"; + return 0; + } + long int reading = strtol(s.c_str(), NULL, 16); + return reading; +} + + +std::string Rtcm::build_message(std::string data) +{ + unsigned int msg_length_bits = data.length(); + unsigned int msg_length_bytes = std::ceil(static_cast(msg_length_bits) / 8.0); + message_length = std::bitset<10>(msg_length_bytes); + unsigned int zeros_to_fill = 8 * msg_length_bytes - msg_length_bits; + std::string b(zeros_to_fill, '0'); + std::string msg_content = data + b; + std::string msg_without_crc = preamble.to_string() + + reserved_field.to_string() + + message_length.to_string() + + msg_content; + return Rtcm::add_CRC(msg_without_crc); +} + + + +// ***************************************************************************************************** +// +// MESSAGES AS DEFINED AT RTCM STANDARD 10403.2 +// +// ***************************************************************************************************** + + +/* Stationary Antenna Reference Point, No Height Information + * Reference Station Id = 2003 + GPS Service supported, but not GLONASS or Galileo + ARP ECEF-X = 1114104.5999 meters + ARP ECEF-Y = -4850729.7108 meters + ARP ECEF-Z = 3975521.4643 meters + Expected output: D3 00 13 3E D7 D3 02 02 98 0E DE EF 34 B4 BD 62 + AC 09 41 98 6F 33 36 0B 98 + */ +std::bitset<152> Rtcm::get_M1005_test () +{ + unsigned int m1005 = 1005; + unsigned int reference_station_id = 2003; // Max: 4095 + double ECEF_X = 1114104.5999; // units: m + double ECEF_Y = -4850729.7108; // units: m + double ECEF_Z = 3975521.4643; // units: m + + std::bitset<1> DF001_; + + Rtcm::set_DF002(m1005); + Rtcm::set_DF003(reference_station_id); + Rtcm::set_DF021(); + Rtcm::set_DF022(true); // GPS + Rtcm::set_DF023(false); // Glonass + Rtcm::set_DF024(false); // Galileo + DF141 = std::bitset<1>("0"); // 0: Real, physical reference station + DF001_ = std::bitset<1>("0"); // Reserved, set to 0 + Rtcm::set_DF025(ECEF_X); + DF142 = std::bitset<1>("0"); // Single Receiver Oscillator Indicator + Rtcm::set_DF026(ECEF_Y); + DF364 = std::bitset<2>("00"); // Quarter Cycle Indicator + Rtcm::set_DF027(ECEF_Z); + + std::string message = DF002.to_string() + + DF003.to_string() + + DF021.to_string() + + DF022.to_string() + + DF023.to_string() + + DF024.to_string() + + DF141.to_string() + + DF025.to_string() + + DF142.to_string() + + DF001_.to_string() + + DF026.to_string() + + DF364.to_string() + + DF027.to_string() ; + + std::bitset<152> test_msg(message); + return test_msg; +} + + +int Rtcm::read_M1005(const std::string & message, unsigned int & ref_id, double & ecef_x, double & ecef_y, double & ecef_z, bool & gps, bool & glonass, bool & galileo) +{ + // Convert message to binary + std::string message_bin = Rtcm::hex_to_bin(message); + + if(!Rtcm::check_CRC(message) ) + { + LOG(WARNING) << " Bad CRC detected in RTCM message M1005"; + std::cout << " ----- Bad CRC detected in RTCM message M1005 " << std::endl; + return 1; + } + + // Check than the message number is correct + unsigned int preamble_length = 8; + unsigned int reserved_field_length = 6; + unsigned int index = preamble_length + reserved_field_length; + + unsigned int read_message_length = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 10))); + index += 10; + if (read_message_length != 19) + { + LOG(WARNING) << " Message M1005 seems too long (19 bytes expected, " << read_message_length << " received)"; + std::cout << " -----Message M1005 seems too long (19 bytes expected, " << read_message_length << " received)" << std::endl; + return 1; + } + + unsigned int msg_number = 1005; + Rtcm::set_DF002(msg_number); + std::bitset<12> read_msg_number(message_bin.substr(index, 12)); + index += 12; + + if (DF002 != read_msg_number) + { + LOG(WARNING) << " This is not a M1005 message"; + std::cout << " ----- This is not a M1005 message"<< std::endl; + return 1; + } + + + ref_id = Rtcm::bin_to_uint(message_bin.substr(index, 12)); + index += 12; + + index += 6; // ITRF year + gps = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + glonass = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + galileo = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); + index += 1; + + index += 1; // ref_sattion_indicator + + ecef_x = Rtcm::bin_to_double(message_bin.substr(index, 38)) / 10000.0; + index += 38; + + index += 1; // single rx oscillator + index += 1; // reserved + + ecef_y = Rtcm::bin_to_double(message_bin.substr(index, 38)) / 10000.0; + index += 38; + + index += 2; // quarter cycle indicator + ecef_z = Rtcm::bin_to_double(message_bin.substr(index, 38)) / 10000.0; + + return 0; +} + + +std::string Rtcm::print_M1005_test() +{ + std::bitset<152> m1005 = get_M1005_test(); + return Rtcm::build_message(m1005.to_string()); +} + + +std::bitset<64> Rtcm::get_M1001_header(const Gps_Ephemeris & gps_eph, double obs_time, const std::map & pseudoranges, + unsigned int ref_id, unsigned int smooth_int, bool sync_flag, bool divergence_free) +{ + unsigned int m1001 = 1001; + unsigned int reference_station_id = ref_id; // Max: 4095 + const std::map pseudoranges_ = pseudoranges; + bool synchronous_GNSS_flag = sync_flag; + bool divergence_free_smoothing_indicator = divergence_free; + unsigned int smoothing_interval = smooth_int; + Rtcm::set_DF002(m1001); + Rtcm::set_DF003(reference_station_id); + Rtcm::set_DF004(gps_eph, obs_time); + Rtcm::set_DF005(synchronous_GNSS_flag); + Rtcm::set_DF006(pseudoranges_); + Rtcm::set_DF007(divergence_free_smoothing_indicator); + Rtcm::set_DF008(smoothing_interval); + + std::string header = DF002.to_string() + + DF003.to_string() + + DF004.to_string() + + DF005.to_string() + + DF006.to_string() + + DF007.to_string() + + DF008.to_string(); + + std::bitset<64> header_msg(header); + return header_msg; +} + + +std::bitset<58> Rtcm::get_M1001_sat_content(const Gnss_Synchro & gnss_synchro) +{ + Gnss_Synchro gnss_synchro_ = gnss_synchro; + bool code_indicator = false; // code indicator 0: C/A code 1: P(Y) code direct + Rtcm::set_DF009(gnss_synchro_); + Rtcm::set_DF010(code_indicator); // code indicator 0: C/A code 1: P(Y) code direct + Rtcm::set_DF011(gnss_synchro_); + + long int gps_L1_phaserange_minus_L1_pseudorange; + long int phaserange_m = (gnss_synchro.Carrier_phase_rads * GPS_C_m_s) / (GPS_TWO_PI * GPS_L1_FREQ_HZ); + gps_L1_phaserange_minus_L1_pseudorange = phaserange_m; // TODO + DF012 = std::bitset<20>(gps_L1_phaserange_minus_L1_pseudorange); + + unsigned int lock_time_indicator = 0; // TODO + DF013 = std::bitset<7>(lock_time_indicator); + + std::string content = DF009.to_string() + + DF010.to_string() + + DF011.to_string() + + DF012.to_string() + + DF013.to_string(); + + std::bitset<58> content_msg(content); + return content_msg; +} + + + +std::string Rtcm::print_M1001(const Gps_Ephemeris & gps_eph, double obs_time, const std::map & pseudoranges) +{ + unsigned int ref_id = static_cast(FLAGS_RTCM_Ref_Station_ID); + unsigned int smooth_int = 0; + bool sync_flag = false; + bool divergence_free = false; + + std::bitset<64> header = Rtcm::get_M1001_header(gps_eph, obs_time, pseudoranges, ref_id, smooth_int, sync_flag, divergence_free); + std::string data = header.to_string(); + + std::map::const_iterator pseudoranges_iter; + for(pseudoranges_iter = pseudoranges.begin(); + pseudoranges_iter != pseudoranges.end(); + pseudoranges_iter++) + { + + std::bitset<58> content = Rtcm::get_M1001_sat_content(pseudoranges_iter->second); + data += content.to_string(); + } + + return Rtcm::build_message(data); +} + + + +std::string Rtcm::print_M1019(const Gps_Ephemeris & gps_eph) +{ + unsigned int msg_number = 1019; + + Rtcm::set_DF002(msg_number); + Rtcm::set_DF009(gps_eph); + Rtcm::set_DF076(gps_eph); + Rtcm::set_DF077(gps_eph); + Rtcm::set_DF078(gps_eph); + Rtcm::set_DF079(gps_eph); + Rtcm::set_DF071(gps_eph); + Rtcm::set_DF081(gps_eph); + Rtcm::set_DF082(gps_eph); + Rtcm::set_DF083(gps_eph); + Rtcm::set_DF084(gps_eph); + Rtcm::set_DF085(gps_eph); + Rtcm::set_DF086(gps_eph); + Rtcm::set_DF087(gps_eph); + Rtcm::set_DF088(gps_eph); + Rtcm::set_DF089(gps_eph); + Rtcm::set_DF090(gps_eph); + Rtcm::set_DF091(gps_eph); + Rtcm::set_DF092(gps_eph); + Rtcm::set_DF093(gps_eph); + Rtcm::set_DF094(gps_eph); + Rtcm::set_DF095(gps_eph); + Rtcm::set_DF096(gps_eph); + Rtcm::set_DF097(gps_eph); + Rtcm::set_DF098(gps_eph); + Rtcm::set_DF099(gps_eph); + Rtcm::set_DF100(gps_eph); + Rtcm::set_DF101(gps_eph); + Rtcm::set_DF102(gps_eph); + Rtcm::set_DF103(gps_eph); + Rtcm::set_DF137(gps_eph); + + std::string data; + data.clear(); + data = DF002.to_string() + + DF009.to_string() + + DF076.to_string() + + DF077.to_string() + + DF078.to_string() + + DF079.to_string() + + DF071.to_string() + + DF081.to_string() + + DF082.to_string() + + DF083.to_string() + + DF084.to_string() + + DF085.to_string() + + DF086.to_string() + + DF087.to_string() + + DF088.to_string() + + DF089.to_string() + + DF090.to_string() + + DF091.to_string() + + DF092.to_string() + + DF093.to_string() + + DF094.to_string() + + DF095.to_string() + + DF096.to_string() + + DF097.to_string() + + DF098.to_string() + + DF099.to_string() + + DF100.to_string() + + DF101.to_string() + + DF102.to_string() + + DF103.to_string() + + DF137.to_string(); + + if (data.length() != 488) + { + LOG(WARNING) << "Bad-formatted RTCM M1019 (488 bits expected, found " << data.length() << ")"; + } + + message1019_content = std::bitset<488>(data); + std::string message = build_message(data); + return message; +} + + +int Rtcm::read_M1019(const std::string & message, Gps_Ephemeris & gps_eph) +{ + // Convert message to binary + std::string message_bin = Rtcm::hex_to_bin(message); + + if(!Rtcm::check_CRC(message) ) + { + LOG(WARNING) << " Bad CRC detected in RTCM message M1019"; + std::cout << " ----- Bad CRC detected in RTCM message M1019 " << std::endl; + return 1; + } + + unsigned int preamble_length = 8; + unsigned int reserved_field_length = 6; + unsigned int index = preamble_length + reserved_field_length - 1; + + unsigned int read_message_length = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 10))); + index += 10; + + if (read_message_length != 61) + { + LOG(WARNING) << " Message M1019 seems too long (61 bytes expected, " << read_message_length << " received)"; + return 1; + } + + // Check than the message number is correct + unsigned int msg_number = 1019; + Rtcm::set_DF002(msg_number); + std::bitset<12> read_msg_number(message_bin.substr(index, 12)); + index += 12; + + if (DF002 != read_msg_number) + { + LOG(WARNING) << " This is not a M1019 message"; + return 1; + } + + gps_eph.i_satellite_PRN = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 6))); + index += 6; + + // idea: define get_DFXXX? + +// Rtcm::set_DF002(msg_number); +// Rtcm::set_DF009(gps_eph); +// Rtcm::set_DF076(gps_eph); +// Rtcm::set_DF077(gps_eph); +// Rtcm::set_DF078(gps_eph); +// Rtcm::set_DF079(gps_eph); +// Rtcm::set_DF071(gps_eph); +// Rtcm::set_DF081(gps_eph); +// Rtcm::set_DF082(gps_eph); +// Rtcm::set_DF083(gps_eph); +// Rtcm::set_DF084(gps_eph); +// Rtcm::set_DF085(gps_eph); +// Rtcm::set_DF086(gps_eph); +// Rtcm::set_DF087(gps_eph); +// Rtcm::set_DF088(gps_eph); +// Rtcm::set_DF089(gps_eph); +// Rtcm::set_DF090(gps_eph); +// Rtcm::set_DF091(gps_eph); +// Rtcm::set_DF092(gps_eph); +// Rtcm::set_DF093(gps_eph); +// Rtcm::set_DF094(gps_eph); +// Rtcm::set_DF095(gps_eph); +// Rtcm::set_DF096(gps_eph); +// Rtcm::set_DF097(gps_eph); +// Rtcm::set_DF098(gps_eph); +// Rtcm::set_DF099(gps_eph); +// Rtcm::set_DF100(gps_eph); +// Rtcm::set_DF101(gps_eph); +// Rtcm::set_DF102(gps_eph); +// Rtcm::set_DF103(gps_eph); +// Rtcm::set_DF137(gps_eph); + + + return 0; +} + + +std::string Rtcm::print_M1045(const Galileo_Ephemeris & gal_eph) +{ + unsigned int msg_number = 1045; + + Rtcm::set_DF002(msg_number); + Rtcm::set_DF252(gal_eph); + Rtcm::set_DF289(gal_eph); + Rtcm::set_DF290(gal_eph); + Rtcm::set_DF291(gal_eph); + Rtcm::set_DF293(gal_eph); + Rtcm::set_DF294(gal_eph); + Rtcm::set_DF295(gal_eph); + Rtcm::set_DF296(gal_eph); + Rtcm::set_DF297(gal_eph); + Rtcm::set_DF298(gal_eph); + Rtcm::set_DF299(gal_eph); + Rtcm::set_DF300(gal_eph); + Rtcm::set_DF301(gal_eph); + Rtcm::set_DF302(gal_eph); + Rtcm::set_DF303(gal_eph); + Rtcm::set_DF304(gal_eph); + Rtcm::set_DF305(gal_eph); + Rtcm::set_DF306(gal_eph); + Rtcm::set_DF307(gal_eph); + Rtcm::set_DF308(gal_eph); + Rtcm::set_DF309(gal_eph); + Rtcm::set_DF310(gal_eph); + Rtcm::set_DF311(gal_eph); + Rtcm::set_DF312(gal_eph); + Rtcm::set_DF314(gal_eph); + Rtcm::set_DF315(gal_eph); + unsigned int seven_zero = 0; + std::bitset<7> DF001_ = std::bitset<7>(seven_zero); + + std::string data; + data.clear(); + data = DF002.to_string() + + DF252.to_string() + + DF289.to_string() + + DF290.to_string() + + DF291.to_string() + + DF292.to_string() + + DF293.to_string() + + DF294.to_string() + + DF295.to_string() + + DF296.to_string() + + DF297.to_string() + + DF298.to_string() + + DF299.to_string() + + DF300.to_string() + + DF301.to_string() + + DF302.to_string() + + DF303.to_string() + + DF304.to_string() + + DF305.to_string() + + DF306.to_string() + + DF307.to_string() + + DF308.to_string() + + DF309.to_string() + + DF310.to_string() + + DF311.to_string() + + DF312.to_string() + + DF314.to_string() + + DF315.to_string() + + DF001_.to_string(); + + if (data.length() != 496) + { + LOG(WARNING) << "Bad-formatted RTCM M1045 (496 bits expected, found " << data.length() << ")"; + } + message1045_content = std::bitset<496>(data); + std::string message = build_message(data); + return message; +} + + + +std::bitset<138> Rtcm::get_M1002 () +{ + std::bitset<138> fake_msg; + fake_msg.reset(); + return fake_msg; +} + + + + + +// ***************************************************************************************************** +// +// DATA FIELDS AS DEFINED AT RTCM STANDARD 10403.2 +// +// ***************************************************************************************************** + +int Rtcm::reset_data_fields() +{ + //DF001.reset(); + DF002.reset(); + DF003.reset(); + DF004.reset(); + DF005.reset(); + DF006.reset(); + DF007.reset(); + DF008.reset(); + DF009.reset(); + DF010.reset(); + DF011.reset(); + DF012.reset(); + DF013.reset(); + DF014.reset(); + DF015.reset(); + + // Contents of GPS Satellite Ephemeris Data, Message Type 1019 + DF071.reset(); + DF076.reset(); + DF077.reset(); + DF078.reset(); + DF079.reset(); + DF081.reset(); + DF082.reset(); + DF083.reset(); + DF084.reset(); + DF085.reset(); + DF086.reset(); + DF087.reset(); + + DF088.reset(); + DF089.reset(); + DF090.reset(); + DF091.reset(); + DF092.reset(); + DF093.reset(); + DF094.reset(); + DF095.reset(); + DF096.reset(); + DF097.reset(); + DF098.reset(); + DF099.reset(); + DF100.reset(); + DF101.reset(); + DF102.reset(); + DF103.reset(); + DF137.reset(); + + // Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045 + DF252.reset(); + DF289.reset(); + DF290.reset(); + DF291.reset(); + DF292.reset(); + DF293.reset(); + DF294.reset(); + DF295.reset(); + DF296.reset(); + DF297.reset(); + DF298.reset(); + DF299.reset(); + DF300.reset(); + DF301.reset(); + DF302.reset(); + DF303.reset(); + DF304.reset(); + DF305.reset(); + DF306.reset(); + DF307.reset(); + DF308.reset(); + DF309.reset(); + DF310.reset(); + DF311.reset(); + DF312.reset(); + DF314.reset(); + DF315.reset(); + + DF364.reset(); + + DF393.reset(); + DF394.reset(); + DF395.reset(); + + DF409.reset(); + + DF411.reset(); + DF412.reset(); + DF417.reset(); + DF418.reset(); + + return 0; +} + + +int Rtcm::set_DF002(unsigned int message_number) +{ + if (message_number > 4095) + { + LOG(WARNING) << "RTCM message number must be between 0 and 4095, but it has been set to " << message_number; + } + DF002 = std::bitset<12>(message_number); + return 0; +} + + +int Rtcm::set_DF003(unsigned int ref_station_ID) +{ + //unsigned int station_ID = ref_station_ID; + if (ref_station_ID > 4095) + { + LOG(WARNING) << "RTCM reference station ID must be between 0 and 4095, but it has been set to " << ref_station_ID; + } + DF003 = std::bitset<12>(ref_station_ID); + return 0; +} + + +int Rtcm::set_DF004(const Gps_Ephemeris & gps_eph, double obs_time) +{ + // TOW in milliseconds from the beginning of the GPS week, measured in GPS time + unsigned long int tow = static_cast(std::round((obs_time + 604800 * static_cast(gps_eph.i_GPS_week % 1024)) * 1000)); + if(tow > 604799999) + { + LOG(WARNING) << "To large TOW! Set to the last millisecond of the week"; + tow = 604799999; + } + DF004 = std::bitset<30>(tow); + return 0; +} + + +int Rtcm::set_DF005(bool sync_flag) +{ + // 0 - No further GNSS observables referenced to the same Epoch Time will be transmitted. This enables the receiver to begin processing + // the data immediately after decoding the message. + // 1 - The next message will contain observables of another GNSS source referenced to the same Epoch Time. + DF005 = std::bitset<1>(sync_flag); + return 0; +} + + +int Rtcm::set_DF006(const std::map & pseudoranges) +{ + //Number of satellites observed in current epoch + unsigned short int nsats = 0; + std::map::const_iterator pseudoranges_iter; + for(pseudoranges_iter = pseudoranges.begin(); + pseudoranges_iter != pseudoranges.end(); + pseudoranges_iter++) + { + nsats++; + } + if (nsats > 31) + { + LOG(WARNING) << "The number of processed GPS satellites must be between 0 and 31, but it seems that you are processing " << nsats; + nsats = 31; + } + DF006 = std::bitset<5>(nsats); + return 0; +} + + +int Rtcm::set_DF007(bool divergence_free_smoothing_indicator) +{ + // 0 - Divergence-free smoothing not used 1 - Divergence-free smoothing used + DF007 = std::bitset<1>(divergence_free_smoothing_indicator); + return 0; +} + + +int Rtcm::set_DF008(short int smoothing_interval) +{ + std::bitset<3> DF008_ = std::bitset<3>(smoothing_interval); + return 0; +} + + +int Rtcm::set_DF009(const Gnss_Synchro & gnss_synchro) +{ + unsigned int prn_ = gnss_synchro.PRN; + if(prn_ > 31) + { + LOG(WARNING) << "GPS satellite ID must be between 0 and 31, but PRN " << prn_ << " was found"; + } + DF009 = std::bitset<6>(prn_); + return 0; +} + + +int Rtcm::set_DF009(const Gps_Ephemeris & gps_eph) +{ + unsigned int prn_ = gps_eph.i_satellite_PRN; + if(prn_ > 31) + { + LOG(WARNING) << "GPS satellite ID must be between 0 and 31, but PRN " << prn_ << " was found"; + } + DF009 = std::bitset<6>(prn_); + return 0; +} + + +int Rtcm::set_DF010(bool code_indicator) +{ + DF010 = std::bitset<1>(code_indicator); + return 0; +} + + +int Rtcm::set_DF011(const Gnss_Synchro & gnss_synchro) +{ + unsigned long int gps_L1_pseudorange = static_cast(std::round(std::fmod(gnss_synchro.Pseudorange_m, 299792.458) / 0.02 )); + DF011 = std::bitset<24>(gps_L1_pseudorange); + return 0; +} + + +int Rtcm::set_DF012(const Gnss_Synchro & gnss_synchro) +{ + double L1_pseudorange = gnss_synchro.Pseudorange_m; + double L1_pseudorange_integers = std::floor(L1_pseudorange / 299792.458); + double L1_pseudorange_field = std::fmod(L1_pseudorange, 299792.458); + + long int gps_L1_phaserange_minus_L1_pseudorange = 0; /////////////////////// + DF012 = std::bitset<20>(gps_L1_phaserange_minus_L1_pseudorange); + return 0; +} + + +int Rtcm::set_DF014(const Gnss_Synchro & gnss_synchro) +{ + unsigned int gps_L1_pseudorange_ambiguity = static_cast(std::floor(gnss_synchro.Pseudorange_m / 299792.458)); + DF014 = std::bitset<8>(gps_L1_pseudorange_ambiguity); + return 0; +} + + +int Rtcm::set_DF015(const Gnss_Synchro & gnss_synchro) +{ + double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz; + if (CN0_dB_Hz_est > 63.75) + { + CN0_dB_Hz_est = 63.75; + } + unsigned int CN0_dB_Hz = static_cast(std::round(CN0_dB_Hz_est / 0.25 )); + DF015 = std::bitset<8>(CN0_dB_Hz); + return 0; +} + + +int Rtcm::set_DF021() +{ + unsigned short int itfr_year = 0; + DF021 = std::bitset<6>(itfr_year); + return 0; +} + + +int Rtcm::set_DF022(bool gps_indicator) +{ + DF022 = std::bitset<1>(gps_indicator); + return 0; +} + + +int Rtcm::set_DF023(bool glonass_indicator) +{ + DF023 = std::bitset<1>(glonass_indicator); + return 0; +} + + +int Rtcm::set_DF024(bool galileo_indicator) +{ + DF024 = std::bitset<1>(galileo_indicator); + return 0; +} + + +int Rtcm::set_DF025(double antenna_ECEF_X_m) +{ + long long int ant_ref_x = static_cast(std::round( antenna_ECEF_X_m * 10000)); + DF025 = std::bitset<38>(ant_ref_x); + return 0; +} + + +int Rtcm::set_DF026(double antenna_ECEF_Y_m) +{ + long long int ant_ref_y = static_cast(std::round( antenna_ECEF_Y_m * 10000)); + DF026 = std::bitset<38>(ant_ref_y); + return 0; +} + + +int Rtcm::set_DF027(double antenna_ECEF_Z_m) +{ + long long int ant_ref_z = static_cast(std::round( antenna_ECEF_Z_m * 10000)); + DF027 = std::bitset<38>(ant_ref_z); + return 0; +} + + +int Rtcm::set_DF071(const Gps_Ephemeris & gps_eph) +{ + unsigned int iode = static_cast(gps_eph.d_IODE_SF2); + DF071 = std::bitset<8>(iode); + return 0; +} + + +int Rtcm::set_DF076(const Gps_Ephemeris & gps_eph) +{ + unsigned int week_number = static_cast(gps_eph.i_GPS_week); + DF076 = std::bitset<10>(week_number); + return 0; +} + + +int Rtcm::set_DF077(const Gps_Ephemeris & gps_eph) +{ + unsigned short int ura = static_cast(gps_eph.i_SV_accuracy); + DF077 = std::bitset<4>(ura); + return 0; +} + + +int Rtcm::set_DF078(const Gps_Ephemeris & gps_eph) +{ + unsigned short int code_on_L2 = static_cast(gps_eph.i_code_on_L2); + DF078 = std::bitset<2>(code_on_L2); + return 0; +} + + +int Rtcm::set_DF079(const Gps_Ephemeris & gps_eph) +{ + unsigned int idot = static_cast(std::round(gps_eph.d_IDOT / I_DOT_LSB )); + DF079 = std::bitset<14>(idot); + return 0; +} + + +int Rtcm::set_DF080(const Gps_Ephemeris & gps_eph) +{ + unsigned short int iode = static_cast(gps_eph.d_IODE_SF2); + DF080 = std::bitset<8>(iode); + return 0; +} + + +int Rtcm::set_DF081(const Gps_Ephemeris & gps_eph) +{ + unsigned int toc = static_cast(std::round(gps_eph.d_Toc / T_OC_LSB )); + DF081 = std::bitset<16>(toc); + return 0; +} + + +int Rtcm::set_DF082(const Gps_Ephemeris & gps_eph) +{ + short int af2 = static_cast(std::round(gps_eph.d_A_f2 / A_F2_LSB )); + DF082 = std::bitset<8>(af2); + return 0; +} + + +int Rtcm::set_DF083(const Gps_Ephemeris & gps_eph) +{ + int af1 = static_cast(std::round(gps_eph.d_A_f1 / A_F1_LSB )); + DF083 = std::bitset<16>(af1); + return 0; +} + + +int Rtcm::set_DF084(const Gps_Ephemeris & gps_eph) +{ + long int af0 = static_cast(std::round(gps_eph.d_A_f0 / A_F0_LSB )); + DF084 = std::bitset<22>(af0); + return 0; +} + + +int Rtcm::set_DF085(const Gps_Ephemeris & gps_eph) +{ + unsigned int iodc = static_cast(gps_eph.d_IODC); + DF085 = std::bitset<10>(iodc); + return 0; +} + + +int Rtcm::set_DF086(const Gps_Ephemeris & gps_eph) +{ + int crs = static_cast(std::round(gps_eph.d_Crs / C_RS_LSB )); + DF086 = std::bitset<16>(crs); + return 0; +} + + +int Rtcm::set_DF087(const Gps_Ephemeris & gps_eph) +{ + int delta_n = static_cast(std::round(gps_eph.d_Delta_n / DELTA_N_LSB )); + DF087 = std::bitset<16>(delta_n); + return 0; +} + + +int Rtcm::set_DF088(const Gps_Ephemeris & gps_eph) +{ + long int m0 = static_cast(std::round(gps_eph.d_M_0 / M_0_LSB )); + DF088 = std::bitset<32>(m0); + return 0; +} + + +int Rtcm::set_DF089(const Gps_Ephemeris & gps_eph) +{ + int cuc = static_cast(std::round(gps_eph.d_Cuc / C_UC_LSB )); + DF089 = std::bitset<16>(cuc); + return 0; +} + +int Rtcm::set_DF090(const Gps_Ephemeris & gps_eph) +{ + unsigned long int ecc = static_cast(std::round(gps_eph.d_e_eccentricity / E_LSB )); + DF090 = std::bitset<32>(ecc); + return 0; +} + + +int Rtcm::set_DF091(const Gps_Ephemeris & gps_eph) +{ + int cus = static_cast(std::round(gps_eph.d_Cus / C_US_LSB )); + DF091 = std::bitset<16>(cus); + return 0; +} + + +int Rtcm::set_DF092(const Gps_Ephemeris & gps_eph) +{ + unsigned long int sqr_a = static_cast(std::round(gps_eph.d_sqrt_A / SQRT_A_LSB )); + DF092 = std::bitset<32>(sqr_a); + return 0; +} + + +int Rtcm::set_DF093(const Gps_Ephemeris & gps_eph) +{ + unsigned int toe = static_cast(std::round(gps_eph.d_Toe / T_OE_LSB )); + DF093 = std::bitset<16>(toe); + return 0; +} + + +int Rtcm::set_DF094(const Gps_Ephemeris & gps_eph) +{ + int cic = static_cast(std::round(gps_eph.d_Cic / C_IC_LSB )); + DF094 = std::bitset<16>(cic); + return 0; +} + + +int Rtcm::set_DF095(const Gps_Ephemeris & gps_eph) +{ + long int Omega0 = static_cast(std::round(gps_eph.d_OMEGA0 / OMEGA_0_LSB )); + DF095 = std::bitset<32>(Omega0); + return 0; +} + + +int Rtcm::set_DF096(const Gps_Ephemeris & gps_eph) +{ + int cis = static_cast(std::round(gps_eph.d_Cis / C_IS_LSB )); + DF096 = std::bitset<16>(cis); + return 0; +} + + +int Rtcm::set_DF097(const Gps_Ephemeris & gps_eph) +{ + long int i0 = static_cast(std::round(gps_eph.d_i_0 / I_0_LSB )); + DF097 = std::bitset<32>(i0); + return 0; +} + + +int Rtcm::set_DF098(const Gps_Ephemeris & gps_eph) +{ + int crc = static_cast(std::round(gps_eph.d_Crc / C_RC_LSB )); + DF098 = std::bitset<16>(crc); + return 0; +} + + +int Rtcm::set_DF099(const Gps_Ephemeris & gps_eph) +{ + long int omega = static_cast(std::round(gps_eph.d_OMEGA / OMEGA_LSB )); + DF099 = std::bitset<32>(omega); + return 0; +} + + +int Rtcm::set_DF100(const Gps_Ephemeris & gps_eph) +{ + long int omegadot = static_cast(std::round(gps_eph.d_OMEGA_DOT / OMEGA_DOT_LSB )); + DF100 = std::bitset<24>(omegadot); + return 0; +} + + +int Rtcm::set_DF101(const Gps_Ephemeris & gps_eph) +{ + short int tgd = static_cast(std::round(gps_eph.d_TGD / T_GD_LSB )); + DF101 = std::bitset<8>(tgd); + return 0; +} + + +int Rtcm::set_DF102(const Gps_Ephemeris & gps_eph) +{ + unsigned short int sv_heath = static_cast(gps_eph.i_SV_health); + DF102 = std::bitset<6>(sv_heath); + return 0; +} + + +int Rtcm::set_DF103(const Gps_Ephemeris & gps_eph) +{ + DF103 = std::bitset<1>(gps_eph.b_L2_P_data_flag); + return 0; +} + + + + +int Rtcm::set_DF137(const Gps_Ephemeris & gps_eph) +{ + DF137 = std::bitset<1>(gps_eph.b_fit_interval_flag); + return 0; +} + + + +int Rtcm::set_DF252(const Galileo_Ephemeris & gal_eph) +{ + unsigned int prn_ = gal_eph.i_satellite_PRN; + if(prn_ > 63) + { + LOG(WARNING) << "Galileo satellite ID must be between 0 and 63, but PRN " << prn_ << " was found"; + } + DF252 = std::bitset<6>(prn_); + return 0; +} + + +int Rtcm::set_DF289(const Galileo_Ephemeris & gal_eph) +{ + unsigned int galileo_week_number = static_cast(gal_eph.WN_5); + if(galileo_week_number > 4095) + { + LOG(WARNING) << "Error decoding Galileo week number (it has a 4096 roll-off, but " << galileo_week_number << " was detected)"; + } + DF289 = std::bitset<12>(galileo_week_number); + return 0; +} + + +int Rtcm::set_DF290(const Galileo_Ephemeris & gal_eph) +{ + unsigned int iod_nav = static_cast(gal_eph.IOD_nav_1); + if(iod_nav > 1023) + { + LOG(WARNING) << "Error decoding Galileo IODnav (it has a max of 1023, but " << iod_nav << " was detected)"; + } + DF290 = std::bitset<10>(iod_nav); + return 0; +} + + +int Rtcm::set_DF291(const Galileo_Ephemeris & gal_eph) +{ + unsigned short int SISA = static_cast(gal_eph.SISA_3); + //SISA = 0; // SIS Accuracy, data content definition not given in Galileo OS SIS ICD, Issue 1.1, Sept 2010 + DF291 = std::bitset<8>(SISA); + return 0; +} + + +int Rtcm::set_DF292(const Galileo_Ephemeris & gal_eph) +{ + + int idot = static_cast(std::round(gal_eph.iDot_2 / FNAV_idot_2_LSB)); + DF292 = std::bitset<14>(idot); + return 0; +} + +int Rtcm::set_DF293(const Galileo_Ephemeris & gal_eph) +{ + + unsigned int toc = static_cast(gal_eph.t0c_4); + if(toc > 604740) + { + LOG(WARNING) << "Error decoding Galileo ephemeris time (max of 604740, but " << toc << " was detected)"; + } + DF293 = std::bitset<14>(toc); + return 0; +} + + +int Rtcm::set_DF294(const Galileo_Ephemeris & gal_eph) +{ + short int af2 = static_cast(std::round(gal_eph.af2_4 / FNAV_af2_1_LSB)); + DF294 = std::bitset<6>(af2); + return 0; +} + + +int Rtcm::set_DF295(const Galileo_Ephemeris & gal_eph) +{ + long int af1 = static_cast(std::round(gal_eph.af1_4 / FNAV_af1_1_LSB)); + DF295 = std::bitset<21>(af1); + return 0; +} + + +int Rtcm::set_DF296(const Galileo_Ephemeris & gal_eph) +{ + long int af0 = static_cast(std::round(gal_eph.af0_4 / FNAV_af0_1_LSB)); + DF296 = std::bitset<31>(af0); + return 0; +} + + +int Rtcm::set_DF297(const Galileo_Ephemeris & gal_eph) +{ + int crs = static_cast(std::round(gal_eph.C_rs_3 / FNAV_Crs_3_LSB)); + DF297 = std::bitset<16>(crs); + return 0; +} + + +int Rtcm::set_DF298(const Galileo_Ephemeris & gal_eph) +{ + int delta_n = static_cast(std::round(gal_eph.delta_n_3 / FNAV_deltan_3_LSB)); + DF298 = std::bitset<16>(delta_n); + return 0; +} + + +int Rtcm::set_DF299(const Galileo_Ephemeris & gal_eph) +{ + long int m0 = static_cast(std::round(gal_eph.M0_1 / FNAV_M0_2_LSB)); + DF299 = std::bitset<32>(m0); + return 0; +} + + +int Rtcm::set_DF300(const Galileo_Ephemeris & gal_eph) +{ + int cuc = static_cast(std::round(gal_eph.C_uc_3 / FNAV_Cuc_3_LSB)); + DF300 = std::bitset<16>(cuc); + return 0; +} + +int Rtcm::set_DF301(const Galileo_Ephemeris & gal_eph) +{ + unsigned long int ecc = static_cast(std::round(gal_eph.e_1 / FNAV_e_2_LSB)); + DF301 = std::bitset<32>(ecc); + return 0; +} + + +int Rtcm::set_DF302(const Galileo_Ephemeris & gal_eph) +{ + int cus = static_cast(std::round(gal_eph.C_us_3 / FNAV_Cus_3_LSB)); + DF302 = std::bitset<16>(cus); + return 0; +} + + +int Rtcm::set_DF303(const Galileo_Ephemeris & gal_eph) +{ + unsigned long int sqr_a = static_cast(std::round(gal_eph.A_1 / FNAV_a12_2_LSB)); + DF303 = std::bitset<32>(sqr_a); + return 0; +} + + + +int Rtcm::set_DF304(const Galileo_Ephemeris & gal_eph) +{ + unsigned int toe = static_cast(std::round(gal_eph.t0e_1 / FNAV_t0e_3_LSB)); + DF304 = std::bitset<14>(toe); + return 0; +} + + +int Rtcm::set_DF305(const Galileo_Ephemeris & gal_eph) +{ + int cic = static_cast(std::round(gal_eph.C_ic_4 / FNAV_Cic_4_LSB)); + DF305 = std::bitset<16>(cic); + return 0; +} + + +int Rtcm::set_DF306(const Galileo_Ephemeris & gal_eph) +{ + long int Omega0 = static_cast(std::round(gal_eph.OMEGA_0_2 / FNAV_omega0_2_LSB)); + DF306 = std::bitset<32>(Omega0); + return 0; +} + + +int Rtcm::set_DF307(const Galileo_Ephemeris & gal_eph) +{ + int cis = static_cast(std::round(gal_eph.C_is_4 / FNAV_Cis_4_LSB)); + DF307 = std::bitset<16>(cis); + return 0; +} + + +int Rtcm::set_DF308(const Galileo_Ephemeris & gal_eph) +{ + long int i0 = static_cast(std::round(gal_eph.i_0_2 / FNAV_i0_3_LSB)); + DF308 = std::bitset<32>(i0); + return 0; +} + + +int Rtcm::set_DF309(const Galileo_Ephemeris & gal_eph) +{ + int crc = static_cast(std::round(gal_eph.C_rc_3 / FNAV_Crc_3_LSB)); + DF309 = std::bitset<16>(crc); + return 0; +} + + +int Rtcm::set_DF310(const Galileo_Ephemeris & gal_eph) +{ + int omega = static_cast(std::round(gal_eph.omega_2 / FNAV_omega0_2_LSB)); + DF310 = std::bitset<32>(omega); + return 0; +} + + +int Rtcm::set_DF311(const Galileo_Ephemeris & gal_eph) +{ + long int Omegadot = static_cast(std::round(gal_eph.OMEGA_dot_3 / FNAV_omegadot_2_LSB)); + DF311 = std::bitset<24>(Omegadot); + return 0; +} + + +int Rtcm::set_DF312(const Galileo_Ephemeris & gal_eph) +{ + int bdg_E1_E5a = static_cast(std::round(gal_eph.BGD_E1E5a_5 / FNAV_BGD_1_LSB)); + DF312 = std::bitset<10>(bdg_E1_E5a); + return 0; +} + + +int Rtcm::set_DF313(const Galileo_Ephemeris & gal_eph) +{ + unsigned int bdg_E5b_E1 = static_cast(std::round(gal_eph.BGD_E1E5b_5 )); + //bdg_E5b_E1 = 0; //reserved + DF313 = std::bitset<10>(bdg_E5b_E1); + return 0; +} + + +int Rtcm::set_DF314(const Galileo_Ephemeris & gal_eph) +{ + DF314 = std::bitset<2>(gal_eph.E5a_HS); + return 0; +} + + +int Rtcm::set_DF315(const Galileo_Ephemeris & gal_eph) +{ + DF315 = std::bitset<1>(gal_eph.E5a_DVS); + return 0; +} + + + +int Rtcm::set_DF393(bool more_messages) +{ + DF393 = std::bitset<1>(more_messages); + return 0; +} + + +int Rtcm::set_DF394(const std::map & gnss_synchro) +{ + DF394.reset(); + std::map::const_iterator gnss_synchro_iter; + unsigned int mask_position; + for(gnss_synchro_iter = gnss_synchro.begin(); + gnss_synchro_iter != gnss_synchro.end(); + gnss_synchro_iter++) + { + mask_position = 65 - gnss_synchro_iter->second.PRN; + DF394.set(mask_position, true); + } + return 0; +} + + +int Rtcm::set_DF395(const std::map & gnss_synchro) +{ + DF395.reset(); + std::map::const_iterator gnss_synchro_iter; + std::string sig; + unsigned int mask_position; + for(gnss_synchro_iter = gnss_synchro.begin(); + gnss_synchro_iter != gnss_synchro.end(); + gnss_synchro_iter++) + { + std::string sig_(gnss_synchro_iter->second.Signal); + sig = sig_.substr(0,2); + + std::string sys(gnss_synchro_iter->second.System, 1); + + if ((sig.compare("1C") == 0) && (sys.compare("G") == 0 ) ) + { + mask_position = 33 - 2; + DF395.set(mask_position, true); + } + if ((sig.compare("2S") == 0) && (sys.compare("G") == 0 ) ) + { + mask_position = 33 - 15; + DF395.set(mask_position, true); + } + + if ((sig.compare("5X") == 0) && (sys.compare("G") == 0 ) ) + { + mask_position = 33 - 24; + DF395.set(mask_position, true); + } + if ((sig.compare("1B") == 0) && (sys.compare("E") == 0 ) ) + { + mask_position = 33 - 4; + DF395.set(mask_position, true); + } + + if ((sig.compare("5X") == 0) && (sys.compare("E") == 0 ) ) + { + mask_position = 33 - 24; + DF395.set(mask_position, true); + } + if ((sig.compare("7X") == 0) && (sys.compare("E") == 0 ) ) + { + mask_position = 33 - 16; + DF395.set(mask_position, true); + } + } + return 0; +} + + +int Rtcm::set_DF409(unsigned int iods) +{ + DF409 = std::bitset<3>(iods); + return 0; +} + + +int Rtcm::set_DF417(bool using_divergence_free_smoothing) +{ + DF417 = std::bitset<1>(using_divergence_free_smoothing); + return 0; +} diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h new file mode 100644 index 000000000..dc3d03e96 --- /dev/null +++ b/src/core/system_parameters/rtcm.h @@ -0,0 +1,416 @@ +/*! + * \file rtcm.h + * \brief Interface for the RTCM 3.2 Standard + * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_RTCM_H_ +#define GNSS_SDR_RTCM_H_ + + +#include // std::bitset +#include +#include // std::string +#include +#include +#include "gnss_synchro.h" +#include "galileo_fnav_message.h" +#include "gps_navigation_message.h" + +/*! + * \brief This class implements the RTCM 3.2 Stardard + * + */ +class Rtcm +{ +public: + Rtcm(); + + std::string print_M1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map & pseudoranges); + + std::string print_M1045(const Galileo_Ephemeris & gal_eph); // message1001_header; + std::bitset<58> message1001_content; + std::bitset<64> message1002_header; + std::bitset<74> message1002_content; + + std::bitset<488> message1019_content; + + std::bitset<496> message1045_content; + + std::bitset<169> MSM_header; // 169+X + + std::vector > MSM4_content; // 18 * Nsat + + std::vector > MSM5_content; // 36 * Nsat + + + std::bitset<64> get_M1001_header(const Gps_Ephemeris & gps_eph, + double obs_time, + const std::map & pseudoranges, + unsigned int ref_id, + unsigned int smooth_int, + bool sync_flag, + bool divergence_free); + + std::bitset<58> get_M1001_sat_content(const Gnss_Synchro & gnss_synchro); + + std::bitset<138> get_M1002(); // GPS observables + //std::bitset<488> get_M1019(); // GPS ephemeris + //std::bitset<496> get_M1045(); // Galileo ephemeris + std::bitset<152> get_M1005_test(); + + + + // + // Transport Layer + // + std::bitset<8> preamble; + std::bitset<6> reserved_field; + std::bitset<10> message_length; + std::bitset<24> crc_frame; + typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type; + crc_24_q_type CRC_RTCM; + std::string add_CRC(const std::string& m); + bool check_CRC(const std::string & message); + + std::string build_message(std::string data); // adds 0s to complete a byte and adds the CRC + + + // + // Data Fields + // + int reset_data_fields(); + + std::bitset<12> DF002; + int set_DF002(unsigned int message_number); + + std::bitset<12> DF003; + int set_DF003(unsigned int ref_station_ID); + + std::bitset<30> DF004; + int set_DF004(const Gps_Ephemeris & gps_eph, double obs_time); + + std::bitset<1> DF005; + int set_DF005(bool sync_flag); + + std::bitset<5> DF006; + int set_DF006(const std::map & pseudoranges); + + std::bitset<1> DF007; + int set_DF007(bool divergence_free_smoothing_indicator); // 0 - Divergence-free smoothing not used 1 - Divergence-free smoothing used + + std::bitset<3> DF008; + int set_DF008(short int smoothing_interval); + + std::bitset<6> DF009; + int set_DF009(const Gps_Ephemeris & gps_eph); + int set_DF009(const Gnss_Synchro & gnss_synchro); + + std::bitset<1> DF010; + int set_DF010(bool code_indicator); + + std::bitset<24> DF011; + int set_DF011(const Gnss_Synchro & gnss_synchro); + + std::bitset<20> DF012; + int set_DF012(const Gnss_Synchro & gnss_synchro); + + std::bitset<7> DF013; + std::bitset<8> DF014; + int set_DF014(const Gnss_Synchro & gnss_synchro); + + std::bitset<8> DF015; + int set_DF015(const Gnss_Synchro & gnss_synchro); + + + std::bitset<6> DF021; + int set_DF021(); + + std::bitset<1> DF022; + int set_DF022(bool gps_indicator); + + std::bitset<1> DF023; + int set_DF023(bool glonass_indicator); + + std::bitset<1> DF024; + int set_DF024(bool galileo_indicator); + + std::bitset<38> DF025; + int set_DF025(double antenna_ECEF_X_m); + + std::bitset<38> DF026; + int set_DF026(double antenna_ECEF_Y_m); + + std::bitset<38> DF027; + int set_DF027(double antenna_ECEF_Z_m); + + // Contents of GPS Satellite Ephemeris Data, Message Type 1019 + std::bitset<8> DF071; + int set_DF071(const Gps_Ephemeris & gps_eph); + + std::bitset<10> DF076; + int set_DF076(const Gps_Ephemeris & gps_eph); + + std::bitset<4> DF077; + int set_DF077(const Gps_Ephemeris & gps_eph); + + std::bitset<2> DF078; + int set_DF078(const Gps_Ephemeris & gps_eph); + + std::bitset<14> DF079; + int set_DF079(const Gps_Ephemeris & gps_eph); + + std::bitset<8> DF080; + int set_DF080(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF081; + int set_DF081(const Gps_Ephemeris & gps_eph); + + std::bitset<8> DF082; + int set_DF082(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF083; + int set_DF083(const Gps_Ephemeris & gps_eph); + + std::bitset<22> DF084; + int set_DF084(const Gps_Ephemeris & gps_eph); + + std::bitset<10> DF085; + int set_DF085(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF086; + int set_DF086(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF087; + int set_DF087(const Gps_Ephemeris & gps_eph); + + std::bitset<32> DF088; + int set_DF088(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF089; + int set_DF089(const Gps_Ephemeris & gps_eph); + + std::bitset<32> DF090; + int set_DF090(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF091; + int set_DF091(const Gps_Ephemeris & gps_eph); + + std::bitset<32> DF092; + int set_DF092(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF093; + int set_DF093(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF094; + int set_DF094(const Gps_Ephemeris & gps_eph); + + std::bitset<32> DF095; + int set_DF095(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF096; + int set_DF096(const Gps_Ephemeris & gps_eph); + + std::bitset<32> DF097; + int set_DF097(const Gps_Ephemeris & gps_eph); + + std::bitset<16> DF098; + int set_DF098(const Gps_Ephemeris & gps_eph); + + std::bitset<32> DF099; + int set_DF099(const Gps_Ephemeris & gps_eph); + + std::bitset<24> DF100; + int set_DF100(const Gps_Ephemeris & gps_eph); + + std::bitset<8> DF101; + int set_DF101(const Gps_Ephemeris & gps_eph); + + std::bitset<6> DF102; + int set_DF102(const Gps_Ephemeris & gps_eph); + + std::bitset<1> DF103; + int set_DF103(const Gps_Ephemeris & gps_eph); + + std::bitset<1> DF137; + int set_DF137(const Gps_Ephemeris & gps_eph); + + + + std::bitset<1> DF141; + int set_DF141(const Gps_Ephemeris & gps_eph); + + std::bitset<1> DF142; + int set_DF142(const Gps_Ephemeris & gps_eph); + + // Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045 + std::bitset<6> DF252; + int set_DF252(const Galileo_Ephemeris & gal_eph); + + std::bitset<12> DF289; + int set_DF289(const Galileo_Ephemeris & gal_eph); + + std::bitset<10> DF290; + int set_DF290(const Galileo_Ephemeris & gal_eph); + + std::bitset<8> DF291; + int set_DF291(const Galileo_Ephemeris & gal_eph); + + std::bitset<14> DF292; + int set_DF292(const Galileo_Ephemeris & gal_eph); + + std::bitset<14> DF293; + int set_DF293(const Galileo_Ephemeris & gal_eph); + + std::bitset<6> DF294; + int set_DF294(const Galileo_Ephemeris & gal_eph); + + std::bitset<21> DF295; + int set_DF295(const Galileo_Ephemeris & gal_eph); + + std::bitset<31> DF296; + int set_DF296(const Galileo_Ephemeris & gal_eph); + + std::bitset<16> DF297; + int set_DF297(const Galileo_Ephemeris & gal_eph); + + std::bitset<16> DF298; + int set_DF298(const Galileo_Ephemeris & gal_eph); + + std::bitset<32> DF299; + int set_DF299(const Galileo_Ephemeris & gal_eph); + + std::bitset<16> DF300; + int set_DF300(const Galileo_Ephemeris & gal_eph); + + std::bitset<32> DF301; + int set_DF301(const Galileo_Ephemeris & gal_eph); + + std::bitset<16> DF302; + int set_DF302(const Galileo_Ephemeris & gal_eph); + + std::bitset<32> DF303; + int set_DF303(const Galileo_Ephemeris & gal_eph); + + std::bitset<14> DF304; + int set_DF304(const Galileo_Ephemeris & gal_eph); + + std::bitset<16> DF305; + int set_DF305(const Galileo_Ephemeris & gal_eph); + + std::bitset<32> DF306; + int set_DF306(const Galileo_Ephemeris & gal_eph); + + std::bitset<16> DF307; + int set_DF307(const Galileo_Ephemeris & gal_eph); + + std::bitset<32> DF308; + int set_DF308(const Galileo_Ephemeris & gal_eph); + + std::bitset<16> DF309; + int set_DF309(const Galileo_Ephemeris & gal_eph); + + std::bitset<32> DF310; + int set_DF310(const Galileo_Ephemeris & gal_eph); + + std::bitset<24> DF311; + int set_DF311(const Galileo_Ephemeris & gal_eph); + + std::bitset<10> DF312; + int set_DF312(const Galileo_Ephemeris & gal_eph); + + std::bitset<10> DF313; + int set_DF313(const Galileo_Ephemeris & gal_eph); + + std::bitset<2> DF314; + int set_DF314(const Galileo_Ephemeris & gal_eph); + + std::bitset<1> DF315; + int set_DF315(const Galileo_Ephemeris & gal_eph); + + std::bitset<2> DF364; + + // Content of message header for MSM1, MSM2, MSM3, MSM4, MSM5, MSM6 and MSM7 + std::bitset<1> DF393; + int set_DF393(bool more_messages); //1 indicates that more MSMs follow for given physical time and reference station ID + + std::bitset<64> DF394; + int set_DF394(const std::map & gnss_synchro); + + std::bitset<32> DF395; + int set_DF395(const std::map & gnss_synchro); + + //std::bitset<1> DF396; //variable + std::bitset<3> DF409; + int set_DF409(unsigned int iods); + + std::bitset<2> DF411; + std::bitset<2> DF412; + std::bitset<1> DF417; + int set_DF417(bool using_divergence_free_smoothing); + + std::bitset<3> DF418; + + // Content of Satellite data for MSM4 and MSM6 + std::vector > DF397; // 8*NSAT + std::vector > DF398; // 10*NSAT + + // Content of Satellite data for MSM5 and MSM7 + std::vector > DF399; // 14*NSAT +}; + +#endif diff --git a/src/tests/gnss_block/rtcm_printer_test.cc b/src/tests/gnss_block/rtcm_printer_test.cc index 7e0d52013..bd9f5f2d2 100644 --- a/src/tests/gnss_block/rtcm_printer_test.cc +++ b/src/tests/gnss_block/rtcm_printer_test.cc @@ -37,6 +37,9 @@ #include "gps_ephemeris.h" + + + TEST(Rtcm_Printer_Test, Instantiate) { std::string filename = "hello.rtcm"; @@ -73,3 +76,83 @@ TEST(Rtcm_Printer_Test, Instantiate_and_Run) EXPECT_EQ(reference_msg, testing_msg); } + +TEST(Rtcm_Printer_Test, Bin_to_hex) +{ + auto rtcm = std::make_shared(); + + std::string test1 = "2A"; + std::string test1_bin = rtcm->hex_to_bin(test1); + EXPECT_EQ(0, test1_bin.compare("00101010")); + + std::string test2 = "FF"; + std::string test2_bin = rtcm->hex_to_bin(test2); + EXPECT_EQ(0, test2_bin.compare("11111111")); + + std::string test3 = "ff"; + std::string test3_bin = rtcm->hex_to_bin(test3); + EXPECT_EQ(0, test3_bin.compare("11111111")); +} + + + +TEST(Rtcm_Printer_Test, Hex_to_int) +{ + auto rtcm = std::make_shared(); + + std::string test1 = "2A"; + long int test1_int = rtcm->hex_to_int(test1); + long int expected1 = 42; + EXPECT_EQ(expected1, test1_int); +} + +TEST(Rtcm_Printer_Test, Bin_to_double) +{ + auto rtcm = std::make_shared(); + + std::bitset<4> test1(5); + long int test1_int = static_cast(rtcm->bin_to_double(test1.to_string())); + long int expected1 = 5; + EXPECT_EQ(expected1, test1_int); + + std::bitset<4> test2(-5); + long int test2_int = static_cast(rtcm->bin_to_double(test2.to_string())); + long int expected2 = -5; + EXPECT_EQ(expected2, test2_int); + + std::bitset<65> test3(-5); + long int test3_int = static_cast(rtcm->bin_to_double(test3.to_string())); + long int expected3 = 0; + EXPECT_EQ(expected3, test3_int); +} + + +TEST(Rtcm_Printer_Test, Read_M1005) +{ + std::string filename = "hello.rtcm"; + bool flag_rtcm_tty_port = false; + std::string rtcm_dump_devname = "/dev/pts/4"; + + auto rtcm = std::make_shared(); + auto rtcm_printer = std::make_shared(filename, flag_rtcm_tty_port, rtcm_dump_devname); + std::string reference_msg = rtcm_printer->print_M1005_test(); + + unsigned int ref_id; + double ecef_x; + double ecef_y; + double ecef_z; + bool gps; + bool glonass; + bool galileo; + + rtcm->read_M1005(reference_msg, ref_id, ecef_x, ecef_y, ecef_z, gps, glonass, galileo); + + EXPECT_EQ(true, gps); + EXPECT_EQ(false, glonass); + EXPECT_EQ(false, galileo); + + EXPECT_EQ(2003, ref_id); + EXPECT_DOUBLE_EQ(1114104.5999, ecef_x); + EXPECT_DOUBLE_EQ(-4850729.7108, ecef_y); + EXPECT_DOUBLE_EQ(3975521.4643, ecef_z); +}