From d8a0275c7238e58f3b3aea0893b137d5a0ff1c9b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 4 Dec 2015 13:06:05 +0100 Subject: [PATCH] Now the Rtcm class prints MSM1 messages --- src/core/system_parameters/rtcm.cc | 341 +++++++++++++++++------------ src/core/system_parameters/rtcm.h | 19 ++ src/tests/formats/rtcm_test.cc | 39 +++- 3 files changed, 261 insertions(+), 138 deletions(-) diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index f7b05c3e6..eb5e569cb 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -1033,13 +1033,9 @@ std::string Rtcm::print_MSM_1( const Gps_Ephemeris & gps_eph, divergence_free, more_messages); - //std::cout << "MSM1 header: " << header << std::endl; - std::string sat_data = Rtcm::get_MSM_1_content_sat_data(pseudoranges); - //std::cout << "MSM1 sat data: " << sat_data << std::endl; std::string signal_data = Rtcm::get_MSM_1_content_signal_data(pseudoranges); - //std::cout << "MSM1 signal data: " << signal_data << std::endl; std::string message = build_message(header + sat_data + signal_data); return message; @@ -1076,9 +1072,7 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & Rtcm::set_DF418(smooth_int); Rtcm::set_DF394(pseudoranges); - //std::cout << "Satellite mask DF394: " << DF394.to_string() << std::endl; Rtcm::set_DF395(pseudoranges); - //std::cout << "Signal mask DF395: " << DF395.to_string() << std::endl; std::string header = DF002.to_string() + DF003.to_string(); if(gps_eph.i_satellite_PRN != 0) @@ -1093,6 +1087,7 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & DF409.to_string() + DF001_.to_string() + DF411.to_string() + + DF417.to_string() + DF412.to_string() + DF418.to_string() + DF394.to_string() + @@ -1111,8 +1106,7 @@ std::string Rtcm::get_MSM_1_content_sat_data(const std::map & Rtcm::set_DF394(pseudoranges); unsigned int num_satellites = DF394.count(); - std::map pseudoranges_ordered; - + std::vector > pseudoranges_vector; std::map::const_iterator gnss_synchro_iter; std::vector pos; std::vector::iterator it; @@ -1121,23 +1115,19 @@ std::string Rtcm::get_MSM_1_content_sat_data(const std::map & gnss_synchro_iter != pseudoranges.end(); gnss_synchro_iter++) { - pseudoranges_ordered.insert(std::pair(65 - gnss_synchro_iter->second.PRN, gnss_synchro_iter->second)); - it = std::find (pos.begin(), pos.end(), 65 - gnss_synchro_iter->second.PRN); - if (it == pos.end()) + if(it == pos.end()) { pos.push_back(65 - gnss_synchro_iter->second.PRN); - //std::cout << 65 - gnss_synchro_iter->second.PRN << std::endl; + pseudoranges_vector.push_back(*gnss_synchro_iter); } } - std::sort(pos.begin(), pos.end()); - std::reverse(pos.begin(), pos.end()); + std::vector > ordered_by_PRN_pos = Rtcm::sort_by_PRN_mask(pseudoranges_vector); - for(unsigned int Nsat = 1; Nsat < num_satellites+1; Nsat++) + for(unsigned int nsat = 0; nsat < num_satellites; nsat++) { - //std::cout << "pos:" << pos.at(Nsat-1) << std::endl; - Rtcm::set_DF398(pseudoranges_ordered.at( pos.at(Nsat-1) )); + Rtcm::set_DF398( ordered_by_PRN_pos.at(nsat).second ); sat_data += DF398.to_string(); } @@ -1149,39 +1139,25 @@ std::string Rtcm::get_MSM_1_content_signal_data(const std::map db(Rtcm::set_DF396(pseudoranges)); - unsigned int Ncells = db.count(); - //std::cout << "Ncells: " << Ncells << std::endl; - - std::map pseudoranges_ordered; + unsigned int Ncells = pseudoranges.size(); + std::vector > pseudoranges_vector; std::map::const_iterator gnss_synchro_iter; - std::vector pos; - std::vector::iterator it; for(gnss_synchro_iter = pseudoranges.begin(); gnss_synchro_iter != pseudoranges.end(); gnss_synchro_iter++) { - pseudoranges_ordered.insert(std::pair(65 - gnss_synchro_iter->second.PRN, gnss_synchro_iter->second)); - it = std::find (pos.begin(), pos.end(), 65 - gnss_synchro_iter->second.PRN); - if (it == pos.end()) - { - pos.push_back(65 - gnss_synchro_iter->second.PRN); - } + pseudoranges_vector.push_back(*gnss_synchro_iter); } - std::sort(pos.begin(), pos.end()); - std::reverse(pos.begin(), pos.end()); + std::vector > ordered_by_PRN_pos = Rtcm::sort_by_PRN_mask(pseudoranges_vector); + std::vector > ordered_by_signal = Rtcm::sort_by_signal(ordered_by_PRN_pos); - - - - - for(unsigned int cell = 1; cell < Ncells+1; cell++) + for(unsigned int cell = 0; cell < Ncells ; cell++) { - Rtcm::set_DF400(pseudoranges_ordered.at( pos.at(cell - 1) )); // GET THE RIGHT SYSTEM ORDER!! + Rtcm::set_DF400(ordered_by_PRN_pos.at( cell ).second); signal_data += DF400.to_string(); } @@ -1197,6 +1173,140 @@ std::string Rtcm::get_MSM_4_content_sat_data(const std::map & } +// ***************************************************************************************************** +// Some utilities +// ***************************************************************************************************** + +std::vector > Rtcm::sort_by_PRN_mask(const std::vector > & synchro_map) +{ + std::vector >::const_iterator synchro_map_iter; + std::vector > my_vec; + struct { + bool operator()(const std::pair & a, const std::pair & b) + { + unsigned int value_a = 64 - a.second.PRN; + unsigned int value_b = 64 - b.second.PRN; + return value_a < value_b; + } + } has_lower_pos; + + for(synchro_map_iter = synchro_map.begin(); + synchro_map_iter != synchro_map.end(); + synchro_map_iter++) + + { + std::pair p(synchro_map_iter->first, synchro_map_iter->second); + my_vec.push_back(p); + } + + std::sort(my_vec.begin(), my_vec.end(), has_lower_pos); + std::reverse(my_vec.begin(), my_vec.end()); + return my_vec; +} + + +std::vector > Rtcm::sort_by_signal(const std::vector > & synchro_map) +{ + std::vector >::const_iterator synchro_map_iter; + std::vector > my_vec; + + struct { + bool operator()(const std::pair & a, const std::pair & b) + { + unsigned int value_a = 0; + unsigned int value_b = 0; + std::string system_a(&a.second.System, 1); + std::string system_b(&b.second.System, 1); + std::string sig_a_(a.second.Signal); + std::string sig_a = sig_a_.substr(0,2); + std::string sig_b_(b.second.Signal); + std::string sig_b = sig_b_.substr(0,2); + + if(system_a.compare("G") == 0) + { + value_a = gps_signal_map.at(sig_a); + } + + if(system_a.compare("E") == 0) + { + value_a = galileo_signal_map.at(sig_a); + } + + if(system_b.compare("G") == 0) + { + value_b = gps_signal_map.at(sig_b); + } + + if(system_b.compare("E") == 0) + { + value_b = galileo_signal_map.at(sig_b); + } + + return value_a < value_b; + } + } has_lower_signalID; + + + for(synchro_map_iter = synchro_map.begin(); + synchro_map_iter != synchro_map.end(); + synchro_map_iter++) + + { + std::pair p(synchro_map_iter->first, synchro_map_iter->second); + my_vec.push_back(p); + } + + std::sort(my_vec.begin(), my_vec.end(), has_lower_signalID); + return my_vec; +} + + +auto Rtcm::gps_signal_map = [] +{ + std::map gps_signal_map_; + // Table 3.5-91 + gps_signal_map_["1C"] = 2; + gps_signal_map_["1P"] = 3; + gps_signal_map_["1W"] = 4; + gps_signal_map_["2C"] = 8; + gps_signal_map_["2P"] = 9; + gps_signal_map_["2W"] = 10; + gps_signal_map_["2S"] = 15; + gps_signal_map_["2L"] = 16; + gps_signal_map_["2X"] = 17; + gps_signal_map_["5I"] = 22; + gps_signal_map_["5Q"] = 23; + gps_signal_map_["5X"] = 24; + return gps_signal_map_; +}(); + +auto Rtcm::galileo_signal_map = [] +{ + std::map galileo_signal_map_; + // Table 3.5-100 + galileo_signal_map_["1C"] = 2; + galileo_signal_map_["1A"] = 3; + galileo_signal_map_["1B"] = 4; + galileo_signal_map_["1X"] = 5; + galileo_signal_map_["1Z"] = 6; + galileo_signal_map_["6C"] = 8; + galileo_signal_map_["6A"] = 9; + galileo_signal_map_["6B"] = 10; + galileo_signal_map_["6X"] = 11; + galileo_signal_map_["6Z"] = 12; + galileo_signal_map_["7I"] = 14; + galileo_signal_map_["7Q"] = 15; + galileo_signal_map_["7X"] = 16; + galileo_signal_map_["8I"] = 18; + galileo_signal_map_["8Q"] = 19; + galileo_signal_map_["8X"] = 20; + galileo_signal_map_["5I"] = 22; + galileo_signal_map_["5Q"] = 23; + galileo_signal_map_["5X"] = 24; + return galileo_signal_map_; +}(); + + // ***************************************************************************************************** @@ -1303,6 +1413,7 @@ int Rtcm::reset_data_fields() DF412.reset(); DF417.reset(); DF418.reset(); + DF420.reset(); return 0; } @@ -2153,20 +2264,21 @@ std::string Rtcm::set_DF396(const std::map & pseudoranges) } } - std::sort( list_of_sats.begin(), list_of_sats.end() ); list_of_sats.erase( std::unique( list_of_sats.begin(), list_of_sats.end() ), list_of_sats.end() ); std::sort( list_of_signals.begin(), list_of_signals.end() ); std::reverse(list_of_signals.begin(), list_of_signals.end()); list_of_signals.erase( std::unique( list_of_signals.begin(), list_of_signals.end() ), list_of_signals.end() ); - //std::cout << "List of signals: " << list_of_signals.at(0) << " " << list_of_signals.at(1) << std::endl; // fill the matrix + bool value; + for(unsigned int row = 0; row < num_signals; row++) { for(unsigned int sat = 0; sat < num_satellites; sat++) { + value = false; for(pseudoranges_iter = pseudoranges.begin(); pseudoranges_iter != pseudoranges.end(); pseudoranges_iter++) @@ -2175,93 +2287,37 @@ std::string Rtcm::set_DF396(const std::map & pseudoranges) sig = sig_.substr(0,2); std::string sys(&pseudoranges_iter->second.System, 1); - if ((sig.compare("1C") == 0) && (sys.compare("G") == 0 ) && (list_of_signals.at(row) == 32 - 2)) + if ((sig.compare("1C") == 0) && (sys.compare("G") == 0 ) && (list_of_signals.at(row) == 32 - 2) && (pseudoranges_iter->second.PRN == list_of_sats.at(sat) ) ) { - if(pseudoranges_iter->second.PRN == list_of_sats.at(sat)) - { - matrix[row].push_back(true); - } - } - else - { - if((pseudoranges_iter->second.PRN == list_of_sats.at(sat)) && (list_of_signals.at(row) == 32 - 2) ) - { - matrix[row].push_back(false); - } - } - if ((sig.compare("2S") == 0) && (sys.compare("G") == 0 ) && (list_of_signals.at(row) == 32 - 15)) - { - if(pseudoranges_iter->second.PRN == list_of_sats.at(sat)) - { - matrix[row].push_back(true); - } - } - else - { - if((pseudoranges_iter->second.PRN == list_of_sats.at(sat)) && (list_of_signals.at(row) == 32 - 15) ) - { - matrix[row].push_back(false); - } - } - if ((sig.compare("5X") == 0) && (sys.compare("G") == 0 ) && (list_of_signals.at(row) == 32 - 24)) - { - if(pseudoranges_iter->second.PRN == list_of_sats.at(sat)) - { - matrix[row].push_back(true); - } - - } - else - { - if((pseudoranges_iter->second.PRN == list_of_sats.at(sat)) && (list_of_signals.at(row) == 32 - 24) ) - { - matrix[row].push_back(false); - } + value = true; } - if ((sig.compare("1B") == 0) && (sys.compare("E") == 0 ) && (list_of_signals.at(row) == 32 - 4)) + if ((sig.compare("2S") == 0) && (sys.compare("G") == 0 ) && (list_of_signals.at(row) == 32 - 15) && (pseudoranges_iter->second.PRN == list_of_sats.at(sat) ) ) { - if(pseudoranges_iter->second.PRN == list_of_sats.at(sat)) - { - matrix[row].push_back(true); - } + value = true; } - else + + if ((sig.compare("5X") == 0) && (sys.compare("G") == 0 ) && (list_of_signals.at(row) == 32 - 24) && (pseudoranges_iter->second.PRN == list_of_sats.at(sat) ) ) { - if((pseudoranges_iter->second.PRN == list_of_sats.at(sat)) && (list_of_signals.at(row) == 32 - 4) ) - { - matrix[row].push_back(false); - } + value = true; } - if ((sig.compare("5X") == 0) && (sys.compare("E") == 0 ) && (list_of_signals.at(row) == 32 - 24)) + + if ((sig.compare("1B") == 0) && (sys.compare("E") == 0 ) && (list_of_signals.at(row) == 32 - 4) && (pseudoranges_iter->second.PRN == list_of_sats.at(sat) ) ) { - if(pseudoranges_iter->second.PRN == list_of_sats.at(sat)) - { - matrix[row].push_back(true); - } + value = true; } - else + + if ((sig.compare("5X") == 0) && (sys.compare("E") == 0 ) && (list_of_signals.at(row) == 32 - 24) && (pseudoranges_iter->second.PRN == list_of_sats.at(sat) ) ) { - if((pseudoranges_iter->second.PRN == list_of_sats.at(sat)) && (list_of_signals.at(row) == 32 - 24) ) - { - matrix[row].push_back(false); - } + value = true; } - if ((sig.compare("7X") == 0) && (sys.compare("E") == 0 ) && (list_of_signals.at(row) == 32 - 16)) + + if ((sig.compare("7X") == 0) && (sys.compare("E") == 0 ) && (list_of_signals.at(row) == 32 - 16) && (pseudoranges_iter->second.PRN == list_of_sats.at(sat) ) ) { - if(pseudoranges_iter->second.PRN == list_of_sats.at(sat)) - { - matrix[row].push_back(true); - } - } - else - { - if((pseudoranges_iter->second.PRN == list_of_sats.at(sat)) && (list_of_signals.at(row) == 32 - 16) ) - { - matrix[row].push_back(false); - } + value = true; } } + matrix[row].push_back(value); } } @@ -2283,7 +2339,6 @@ std::string Rtcm::set_DF396(const std::map & pseudoranges) DF396 += ss; } } - //std::cout << "DF396: " << DF396 << std::endl; return DF396; } @@ -2318,11 +2373,21 @@ int Rtcm::set_DF397(const Gnss_Synchro & gnss_synchro) int Rtcm::set_DF398(const Gnss_Synchro & gnss_synchro) { double meters_to_miliseconds = GPS_C_m_s * 0.001; - double rough_range_ms = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10; - DF398 = std::bitset<10>(static_cast(rough_range_ms)); + double rough_range_m = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10; + unsigned int rr_mod_ms; + if((rough_range_m <= 0.0) || (rough_range_m > meters_to_miliseconds * 255.0)) + { + rr_mod_ms = 0; + } + else + { + rr_mod_ms = static_cast(std::floor(rough_range_m / meters_to_miliseconds / TWO_N10) + 0.5) & 0x3FFu; + } + DF398 = std::bitset<10>(rr_mod_ms); return 0; } + int Rtcm::set_DF399(const Gnss_Synchro & gnss_synchro) { double lambda = 0.0; @@ -2356,33 +2421,26 @@ int Rtcm::set_DF399(const Gnss_Synchro & gnss_synchro) int Rtcm::set_DF400(const Gnss_Synchro & gnss_synchro) { double meters_to_miliseconds = GPS_C_m_s * 0.001; - double rough_range_s = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10; + double rough_range_m = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10; double psrng_s; - double lambda = 0.0; - std::string sig_(gnss_synchro.Signal); - std::string sig = sig_.substr(0,2); + int fine_pseudorange; - if (sig.compare("1C") == 0 ) + psrng_s = gnss_synchro.Pseudorange_m - rough_range_m; + + if(psrng_s == 0) { - lambda = GPS_C_m_s / GPS_L1_FREQ_HZ; + fine_pseudorange = - 16384; } - if (sig.compare("2S") == 0 ) + else if(std::fabs(psrng_s) > 292.7) { - lambda = GPS_C_m_s / GPS_L2_FREQ_HZ; + fine_pseudorange = - 16384; + } + else + { + fine_pseudorange = static_cast(std::round(psrng_s / meters_to_miliseconds / TWO_N24)); } - if (sig.compare("5X") == 0 ) - { - lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ; - } - if (sig.compare("1B") == 0 ) - { - lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ; - } - psrng_s = (gnss_synchro.Carrier_phase_rads / GPS_TWO_PI) * lambda - rough_range_s; - - - DF400 = std::bitset<15>(static_cast( psrng_s)); // Units!! + DF400 = std::bitset<15>(fine_pseudorange); return 0; } @@ -2393,12 +2451,14 @@ int Rtcm::set_DF409(unsigned int iods) return 0; } + int Rtcm::set_DF411(unsigned int clock_steering_indicator) { DF411 = std::bitset<2>(clock_steering_indicator); return 0; } + int Rtcm::set_DF412(unsigned int external_clock_indicator) { DF412 = std::bitset<2>(external_clock_indicator); @@ -2412,6 +2472,7 @@ int Rtcm::set_DF417(bool using_divergence_free_smoothing) return 0; } + int Rtcm::set_DF418(int carrier_smoothing_interval_s) { unsigned int smoothing_int = abs(carrier_smoothing_interval_s); @@ -2453,3 +2514,11 @@ int Rtcm::set_DF418(int carrier_smoothing_interval_s) return 0; } + +int Rtcm::set_DF420(const Gnss_Synchro & gnss_synchro) +{ + // todo: read the value from gnss_synchro + bool half_cycle_ambiguity_indicator = true; + DF420 = std::bitset<1>(half_cycle_ambiguity_indicator); + return 0; +} diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index 5296fcf59..af1e06464 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include "gnss_synchro.h" @@ -52,8 +53,14 @@ class Rtcm public: Rtcm(); // & pseudoranges); + /*! + * \brief Prints message type 1002 (Extended L1-Only GPS RTK Observables) + */ std::string print_MT1002(const Gps_Ephemeris & gps_eph, double obs_time, const std::map & pseudoranges); /*! @@ -87,6 +94,9 @@ public: */ int read_MT1045(const std::string & message, Galileo_Ephemeris & gal_eph); + /*! + * \brief Prints messages of type MSM1 (Compact GNSS pseudoranges) + */ std::string print_MSM_1( const Gps_Ephemeris & gps_eph, const Galileo_Ephemeris & gal_eph, double obs_time, @@ -158,6 +168,12 @@ private: std::string get_MSM_4_content_sat_data(const std::map & pseudoranges); + // Utilities + static std::map galileo_signal_map; + static std::map gps_signal_map; + std::vector > sort_by_signal(const std::vector > & synchro_map); + std::vector > sort_by_PRN_mask(const std::vector > & synchro_map); + // // Transport Layer // @@ -464,6 +480,9 @@ private: std::bitset<3> DF418; int set_DF418(int carrier_smoothing_interval_s); + + std::bitset<1> DF420; + int set_DF420(const Gnss_Synchro & gnss_synchro); }; #endif diff --git a/src/tests/formats/rtcm_test.cc b/src/tests/formats/rtcm_test.cc index aae9469ed..0639db1af 100644 --- a/src/tests/formats/rtcm_test.cc +++ b/src/tests/formats/rtcm_test.cc @@ -279,10 +279,12 @@ TEST(Rtcm_Test, MSM1) Gnss_Synchro gnss_synchro; Gnss_Synchro gnss_synchro2; Gnss_Synchro gnss_synchro3; + Gnss_Synchro gnss_synchro4; gnss_synchro.PRN = 2; gnss_synchro2.PRN = 4; gnss_synchro3.PRN = 32; + gnss_synchro4.PRN = 4; std::string sys = "G"; @@ -292,18 +294,22 @@ TEST(Rtcm_Test, MSM1) gnss_synchro.System = *sys.c_str(); gnss_synchro2.System = *sys.c_str(); gnss_synchro3.System = *sys.c_str(); + gnss_synchro4.System = *sys.c_str(); std::memcpy((void*)gnss_synchro.Signal, sig.c_str(), 3); std::memcpy((void*)gnss_synchro2.Signal, sig.c_str(), 3); std::memcpy((void*)gnss_synchro3.Signal, sig2.c_str(), 3); + std::memcpy((void*)gnss_synchro4.Signal, sig2.c_str(), 3); gnss_synchro.Pseudorange_m = 20000000.0; - gnss_synchro2.Pseudorange_m = 20000010.0; - gnss_synchro3.Pseudorange_m = 20000020.0; + gnss_synchro2.Pseudorange_m = 20001010.0; + gnss_synchro3.Pseudorange_m = 24002020.0; + gnss_synchro4.Pseudorange_m = 20003010.1; pseudoranges.insert(std::pair(1, gnss_synchro)); pseudoranges.insert(std::pair(2, gnss_synchro2)); pseudoranges.insert(std::pair(3, gnss_synchro3)); + pseudoranges.insert(std::pair(4, gnss_synchro4)); unsigned int ref_id = 1234; unsigned int clock_steering_indicator = 0; @@ -329,5 +335,34 @@ TEST(Rtcm_Test, MSM1) more_messages); EXPECT_EQ(true, rtcm->check_CRC(MSM1)); + + std::string MSM1_bin = rtcm->hex_to_bin(MSM1); + unsigned int Nsat = 3; + unsigned int Nsig = 2; + unsigned int size_header = 14; + unsigned int size_crc = 24; + unsigned int size_msg_length = 10; + unsigned int upper_bound = 169 + Nsat * 10 + 43 * Nsig; + unsigned int data_size = MSM1_bin.length() - size_header - size_msg_length - size_crc; + EXPECT_EQ(true, upper_bound >= data_size); + EXPECT_EQ(0, MSM1_bin.substr(0, size_header).compare("11010011000000")); + EXPECT_EQ(ref_id, rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 12, 12))); + EXPECT_EQ(0, MSM1_bin.substr(size_header + size_msg_length + 169, Nsat * Nsig).compare("101101")); // check cell mask + + double meters_to_miliseconds = GPS_C_m_s * 0.001; + unsigned int rough_range_1 = static_cast(std::floor(std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu; + unsigned int rough_range_2 = static_cast(std::floor(std::round(gnss_synchro2.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu; + unsigned int rough_range_4 = static_cast(std::floor(std::round(gnss_synchro3.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu; + unsigned int read_pseudorange_1 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig , 10)); + unsigned int read_pseudorange_2 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig + 10, 10)); + unsigned int read_pseudorange_4 = rtcm->bin_to_uint( MSM1_bin.substr(size_header + size_msg_length + 169 + Nsat * Nsig + 20, 10)); + + EXPECT_EQ(rough_range_1, read_pseudorange_1); + EXPECT_EQ(rough_range_2, read_pseudorange_2); + EXPECT_EQ(rough_range_4, read_pseudorange_4); + + int psrng4_s = static_cast(std::round( (gnss_synchro3.Pseudorange_m - std::round(gnss_synchro3.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10)/ meters_to_miliseconds / TWO_N24)); + int read_psrng4_s = rtcm->bin_to_int( MSM1_bin.substr(size_header + size_msg_length + 169 + (Nsat * Nsig) + 30 + 15 * 3, 15)); + EXPECT_EQ(psrng4_s, read_psrng4_s); }