From 4fd4c24747607cd68a65431fdfd41964fc0d65d5 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 30 Apr 2016 15:20:56 +0200 Subject: [PATCH] Print RTCM messages in binary format --- src/core/system_parameters/rtcm.cc | 57 +++++++++++++++++++++-- src/core/system_parameters/rtcm.h | 3 ++ src/tests/formats/rtcm_test.cc | 30 ++++++++---- src/tests/gnss_block/rtcm_printer_test.cc | 19 ++++++-- 4 files changed, 91 insertions(+), 18 deletions(-) diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 905a82492..cd2aa8821 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -148,7 +148,7 @@ std::string Rtcm::add_CRC (const std::string & message_without_crc) const // 3) Builds the complete message std::string complete_message = message_without_crc + crc_frame.to_string(); - return bin_to_hex(complete_message); + return bin_to_binary_data(complete_message); } @@ -156,7 +156,7 @@ bool Rtcm::check_CRC(const std::string & message) const { boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> CRC_RTCM_CHECK; // Convert message to binary - std::string message_bin = Rtcm::hex_to_bin(message); + std::string message_bin = Rtcm::binary_data_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); @@ -180,6 +180,53 @@ bool Rtcm::check_CRC(const std::string & message) const } +std::string Rtcm::bin_to_binary_data(const std::string& s) const +{ + std::string s_aux; + int remainder = static_cast(std::fmod(s.length(), 4)); + unsigned char c[s.length()]; + unsigned int k = 0; + if (remainder != 0) + { + s_aux.assign(s, 0 , remainder); + boost::dynamic_bitset<> rembits(s_aux); + unsigned long int n = rembits.to_ulong(); + c[0] = (unsigned char)n; + k++; + } + + unsigned int start = std::max(remainder, 0); + for(unsigned int i = start; i < s.length() - 1; i = i + 4) + { + s_aux.assign(s, i, 4); + std::bitset<4> bs(s_aux); + unsigned n = bs.to_ulong(); + c[k] = (unsigned char)n; + k++; + } + + std::string ret(c, c + k / sizeof(c[0])); + return ret; +} + + +std::string Rtcm::binary_data_to_bin(const std::string& s) const +{ + std::string s_aux; + std::stringstream ss; + + for(unsigned int i = 0; i < s.length(); i++) + { + unsigned char val = static_cast(s.at(i)); + std::bitset<4> bs(val); + ss << bs; + } + + s_aux = ss.str(); + return s_aux; +} + + std::string Rtcm::bin_to_hex(const std::string& s) const { std::string s_aux; @@ -842,7 +889,7 @@ std::string Rtcm::print_MT1005( unsigned int ref_id, double ecef_x, double ecef_ int Rtcm::read_MT1005(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); + std::string message_bin = Rtcm::binary_data_to_bin(message); if(!Rtcm::check_CRC(message) ) { @@ -1118,7 +1165,7 @@ std::string Rtcm::print_MT1019(const Gps_Ephemeris & gps_eph) int Rtcm::read_MT1019(const std::string & message, Gps_Ephemeris & gps_eph) { // Convert message to binary - std::string message_bin = Rtcm::hex_to_bin(message); + std::string message_bin = Rtcm::binary_data_to_bin(message); if(!Rtcm::check_CRC(message) ) { @@ -1400,7 +1447,7 @@ std::string Rtcm::print_MT1045(const Galileo_Ephemeris & gal_eph) int Rtcm::read_MT1045(const std::string & message, Galileo_Ephemeris & gal_eph) { // Convert message to binary - std::string message_bin = Rtcm::hex_to_bin(message); + std::string message_bin = Rtcm::binary_data_to_bin(message); if(!Rtcm::check_CRC(message) ) { diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index 2e7bd4e66..2460e6cf8 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -266,6 +266,9 @@ public: std::string bin_to_hex(const std::string & s) const; //(); + std::string bin_str("01011010"); + std::string data_str = rtcm->bin_to_binary_data(bin_str); + std::string recovered_str = rtcm->binary_data_to_bin(data_str); + EXPECT_EQ(0, recovered_str.compare(bin_str)); +} + + TEST(Rtcm_Test, Check_CRC) { auto rtcm = std::make_shared(); bool expected_true = true; bool expected_false = false; - EXPECT_EQ(expected_true, rtcm->check_CRC("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B98")); - EXPECT_EQ(expected_false, rtcm->check_CRC("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B99")); + std::string good_crc = rtcm->bin_to_binary_data(rtcm->hex_to_bin("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B98")); + std::string bad_crc = rtcm->bin_to_binary_data(rtcm->hex_to_bin("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B99")); + EXPECT_EQ(expected_true, rtcm->check_CRC(good_crc)); + EXPECT_EQ(expected_false, rtcm->check_CRC(bad_crc)); EXPECT_EQ(expected_true, rtcm->check_CRC(rtcm->print_MT1005_test())); EXPECT_EQ(expected_true, rtcm->check_CRC(rtcm->print_MT1005_test())); // Run twice to check that CRC has no memory @@ -264,10 +276,10 @@ TEST(Rtcm_Test, MT1029) Gps_Ephemeris gps_eph = Gps_Ephemeris(); std::string m1029 = rtcm->print_MT1029(ref_id, gps_eph, obs_time, s_test); std::string encoded_text = m1029.substr(24, 60); - std::string expected_encoded_text("5554462D3820D0BFD180D0BED0B2D0B5D180D0BAD0B02077C3B672746572"); + std::string expected_encoded_text(rtcm->bin_to_binary_data(rtcm->hex_to_bin("5554462D3820D0BFD180D0BED0B2D0B5D180D0BAD0B02077C3B672746572"))); EXPECT_EQ(0, expected_encoded_text.compare(encoded_text)); - std::string characters_to_follow = m1029.substr(22, 2); + std::string characters_to_follow = rtcm->bin_to_hex(rtcm->binary_data_to_bin(m1029.substr(22, 2))); std::string expected_characters_to_follow("1E"); EXPECT_EQ(0, expected_characters_to_follow.compare(characters_to_follow)); } @@ -368,7 +380,7 @@ TEST(Rtcm_Test, MSMCell) divergence_free, more_messages); - std::string MSM1_bin = rtcm->hex_to_bin(MSM1); + std::string MSM1_bin = rtcm->binary_data_to_bin(MSM1); unsigned int Nsat = 4; unsigned int Nsig = 3; unsigned int size_header = 14; @@ -392,7 +404,7 @@ TEST(Rtcm_Test, MSMCell) smooth_int, divergence_free, more_messages); - std::string MSM1_bin_2 = rtcm->hex_to_bin(MSM1_2); + std::string MSM1_bin_2 = rtcm->binary_data_to_bin(MSM1_2); EXPECT_EQ(0, MSM1_bin_2.substr(size_header + size_msg_length + 169, Nsat * Nsig).compare("001010101100")); // check cell mask Gnss_Synchro gnss_synchro6; @@ -419,7 +431,7 @@ TEST(Rtcm_Test, MSMCell) smooth_int, divergence_free, more_messages); - std::string MSM1_bin_3 = rtcm->hex_to_bin(MSM1_3); + std::string MSM1_bin_3 = rtcm->binary_data_to_bin(MSM1_3); EXPECT_EQ(0, MSM1_bin_3.substr(size_header + size_msg_length + 169, (Nsat-1) * Nsig).compare("001010111")); // check cell mask } @@ -489,7 +501,7 @@ TEST(Rtcm_Test, MSM1) EXPECT_EQ(expected_true, rtcm->check_CRC(MSM1)); - std::string MSM1_bin = rtcm->hex_to_bin(MSM1); + std::string MSM1_bin = rtcm->binary_data_to_bin(MSM1); unsigned int Nsat = 3; unsigned int Nsig = 2; unsigned int size_header = 14; @@ -533,7 +545,7 @@ TEST(Rtcm_Test, MSM1) smooth_int, divergence_free, more_messages); - std::string MSM1_bin2 = rtcm->hex_to_bin(MSM1_2); + std::string MSM1_bin2 = rtcm->binary_data_to_bin(MSM1_2); int read_psrng4_s_2 = rtcm->bin_to_int( MSM1_bin2.substr(size_header + size_msg_length + 169 + (Nsat * Nsig) + 30 + 15 * 3, 15)); EXPECT_EQ(psrng4_s, read_psrng4_s_2); } diff --git a/src/tests/gnss_block/rtcm_printer_test.cc b/src/tests/gnss_block/rtcm_printer_test.cc index 74eed31b3..33e99cc39 100644 --- a/src/tests/gnss_block/rtcm_printer_test.cc +++ b/src/tests/gnss_block/rtcm_printer_test.cc @@ -37,9 +37,6 @@ //#include "gps_ephemeris.h" - - - TEST(Rtcm_Printer_Test, Instantiate) { std::string filename = "hello.rtcm"; @@ -49,6 +46,7 @@ TEST(Rtcm_Printer_Test, Instantiate) std::unique_ptr RTCM_printer(new Rtcm_Printer(filename, flag_rtcm_server, flag_rtcm_tty_port, rtcm_dump_devname)); } + TEST(Rtcm_Printer_Test, Run) { // std::string file_name = "./gps_ephemeris_rx.xml"; @@ -75,9 +73,22 @@ TEST(Rtcm_Printer_Test, Run) std::unique_ptr RTCM_printer(new Rtcm_Printer(filename, flag_rtcm_server, flag_rtcm_tty_port, rtcm_dump_devname)); std::string reference_msg = "D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B98"; + + /* Convert the reference message to binary data */ + std::string reference_msg_binary; + unsigned char c[1]; + for(unsigned int i = 0; i < reference_msg.length(); i++) + { + unsigned long int n; + std::istringstream(reference_msg.substr(i,1)) >> std::hex >> n; + c[0] = (unsigned char)n; + std::string ret(c, c+1); + reference_msg_binary += ret; + } + std::string testing_msg = RTCM_printer->print_MT1005_test(); - EXPECT_EQ(reference_msg, testing_msg); + EXPECT_EQ(0, reference_msg_binary.compare(testing_msg)); }