recovering from an accident

src/core/system_parameters/rtcm.cc

@@ -30,6 +30,7 @@
 
 #include "rtcm.h"
 #include <algorithm>  // for std::reverse
+#include <cmath>      // for std::fmod
 #include <cstdlib>    // for strtol
 #include <sstream>    // for std::stringstream
 #include <boost/algorithm/string.hpp>  // for to_upper_copy
@@ -86,7 +87,6 @@ std::string Rtcm::add_CRC (const std::string& message_without_crc)
 bool Rtcm::check_CRC(const std::string & message)
 {
     crc_24_q_type CRC_RTCM_CHECK;
-
     // Convert message to binary
     std::string message_bin = Rtcm::hex_to_bin(message);
     // Check CRC
@@ -116,16 +116,29 @@ 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)
+    int remainder = static_cast<int>(std::fmod(s.length(), 4));
+
+    if (remainder != 0)
         {
-            s_aux.assign(s, i, 32);
+            s_aux.assign(s, 0 , remainder);
-            std::bitset<32> bs(s_aux);
+            boost::dynamic_bitset<> rembits(s_aux);
+            unsigned n = rembits.to_ulong();
+            ss << std::hex << n;
+        }
+
+    int start = std::max(remainder, 0);
+    for(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();
             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;
@@ -143,6 +156,7 @@ std::string Rtcm::hex_to_bin(const std::string& s)
     return s_aux;
 }
 
+
 unsigned long int Rtcm::bin_to_uint(const std::string& s)
 {
     if(s.length() > 32)
@@ -154,6 +168,7 @@ unsigned long int Rtcm::bin_to_uint(const std::string& s)
     return reading;
 }
 
+
 long int Rtcm::bin_to_int(const std::string& s)
 {
     if(s.length() > 32)
@@ -161,7 +176,20 @@ long int Rtcm::bin_to_int(const std::string& s)
             LOG(WARNING) << "Cannot convert to a long int";
             return 0;
         }
-    long int reading = strtol(s.c_str(), NULL, 2);
+    long int reading;
+
+    // Handle negative numbers
+    if(s.substr(0,1).compare("0"))
+        {
+            // Computing two's complement
+            boost::dynamic_bitset<> original_bitset(s);
+            original_bitset.flip();
+            reading = - (original_bitset.to_ulong() + 1);
+        }
+    else
+        {
+            reading = strtol(s.c_str(), NULL, 2);
+        }
     return reading;
 }
 
@@ -175,7 +203,7 @@ double Rtcm::bin_to_double(const std::string& s)
             return 0;
         }
 
-    long long int reading_int = strtoll(s.c_str(), NULL, 2);
+    long long int reading_int;
 
     // Handle negative numbers
     if(s.substr(0,1).compare("0"))
@@ -185,6 +213,10 @@ double Rtcm::bin_to_double(const std::string& s)
             original_bitset.flip();
             reading_int = - (original_bitset.to_ulong() + 1);
         }
+    else
+        {
+            reading_int = strtoll(s.c_str(), NULL, 2);
+        }
 
     reading = static_cast<double>(reading_int);
     return reading;
@@ -239,134 +271,12 @@ std::string Rtcm::build_message(std::string data)
 // *****************************************************************************************************
 
 
-/* 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<unsigned int>(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<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
-    index += 1;
-
-    glonass = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
-    index += 1;
-
-    galileo = static_cast<bool>(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());
-}
 
+// **********************************************
+//
+//   MESSAGE TYPE 1001 (GPS L1 OBSERVATIONS)
+//
+// **********************************************
 
 std::bitset<64> Rtcm::get_M1001_header(const Gps_Ephemeris & gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & pseudoranges,
         unsigned int ref_id, unsigned int smooth_int, bool sync_flag, bool divergence_free)
@@ -451,6 +361,186 @@ std::string Rtcm::print_M1001(const Gps_Ephemeris & gps_eph, double obs_time, co
 
 
 
+
+// **********************************************
+//
+//   MESSAGE TYPE 1005 (STATION DESCRIPTION)
+//
+// **********************************************
+
+
+/* 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;
+}
+
+std::string Rtcm::print_M1005( unsigned int ref_id, double ecef_x, double ecef_y, double ecef_z, bool gps, bool glonass, bool galileo, bool non_physical, bool single_oscillator, unsigned int quarter_cycle_indicator)
+{
+    unsigned int msg_number = 1005;
+    std::bitset<1> DF001_;
+
+    Rtcm::set_DF002(msg_number);
+    Rtcm::set_DF003(ref_id);
+    Rtcm::set_DF021();
+    Rtcm::set_DF022(gps);
+    Rtcm::set_DF023(glonass);
+    Rtcm::set_DF024(galileo);
+    DF141 = std::bitset<1>(non_physical);
+    DF001_ = std::bitset<1>("0");
+    Rtcm::set_DF025(ecef_x);
+    DF142 = std::bitset<1>(single_oscillator);
+    Rtcm::set_DF026(ecef_y);
+    DF364 = std::bitset<2>(quarter_cycle_indicator);
+    Rtcm::set_DF027(ecef_z);
+
+    std::string data = 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::string message = build_message(data);
+    return message;
+}
+
+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<unsigned int>(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<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+    index += 1;
+
+    glonass = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+    index += 1;
+
+    galileo = static_cast<bool>(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());
+}
+
+
+
+// **********************************************
+//
+//   MESSAGE TYPE 1019 (GPS EPHEMERIS)
+//
+// **********************************************
+
 std::string Rtcm::print_M1019(const Gps_Ephemeris & gps_eph)
 {
     unsigned int msg_number = 1019;
@@ -540,13 +630,12 @@ int Rtcm::read_M1019(const std::string & message, Gps_Ephemeris & gps_eph)
     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 index = preamble_length + reserved_field_length;
 
     unsigned int read_message_length = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
     index += 10;
@@ -558,59 +647,117 @@ int Rtcm::read_M1019(const std::string & message, Gps_Ephemeris & gps_eph)
         }
 
     // Check than the message number is correct
-    unsigned int msg_number = 1019;
+    unsigned int read_msg_number = Rtcm::bin_to_uint(message_bin.substr(index, 12));
-    Rtcm::set_DF002(msg_number);
-    std::bitset<12> read_msg_number(message_bin.substr(index, 12));
     index += 12;
 
-    if (DF002 != read_msg_number)
+    if (1019 != read_msg_number)
         {
             LOG(WARNING) << " This is not a M1019 message";
             return 1;
         }
 
+    // Fill Gps Ephemeris with message data content
     gps_eph.i_satellite_PRN = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 6)));
     index += 6;
 
-    // idea: define get_DFXXX?
+    gps_eph.i_GPS_week = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+    index += 10;
 
-//    Rtcm::set_DF002(msg_number);
+    gps_eph.i_SV_accuracy = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 4)));
-//       Rtcm::set_DF009(gps_eph);
+    index += 4;
-//       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);
 
+    gps_eph.i_code_on_L2 = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 2)));
+    index += 2;
+
+    gps_eph.d_IDOT = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 14))) * I_DOT_LSB;
+    index += 14;
+
+    gps_eph.d_IODE_SF2 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 8)));
+    gps_eph.d_IODE_SF3 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 8)));
+    index += 8;
+
+    gps_eph.d_Toc = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 16))) * T_OC_LSB;
+    index += 16;
+
+    gps_eph.d_A_f2 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 8))) * A_F2_LSB;
+    index += 8;
+
+    gps_eph.d_A_f1 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * A_F1_LSB;
+    index += 16;
+
+    gps_eph.d_A_f0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 22))) * A_F0_LSB;
+    index += 22;
+
+    gps_eph.d_IODC = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+    index += 10;
+
+    gps_eph.d_Crs = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_RS_LSB;
+    index += 16;
+
+    gps_eph.d_Delta_n = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * DELTA_N_LSB;
+    index += 16;
+
+    gps_eph.d_M_0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * M_0_LSB;
+    index += 32;
+
+    gps_eph.d_Cuc = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_UC_LSB;
+    index += 16;
+
+    gps_eph.d_e_eccentricity = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 32))) * E_LSB;
+    index += 32;
+
+    gps_eph.d_Cus = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_US_LSB;
+    index += 16;
+
+    gps_eph.d_sqrt_A = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 32))) * SQRT_A_LSB;
+    index += 32;
+
+    gps_eph.d_Toe = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 16))) * T_OE_LSB;
+    index += 16;
+
+    gps_eph.d_Cic = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_IC_LSB;
+    index += 16;
+
+    gps_eph.d_OMEGA0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * OMEGA_0_LSB;
+    index += 32;
+
+    gps_eph.d_Cis = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_IS_LSB;
+    index += 16;
+
+    gps_eph.d_i_0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * I_0_LSB;
+    index += 32;
+
+    gps_eph.d_Crc = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_RC_LSB;
+    index += 16;
+
+    gps_eph.d_OMEGA = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * OMEGA_LSB;
+    index += 32;
+
+    gps_eph.d_OMEGA_DOT = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 24))) * OMEGA_DOT_LSB;
+    index += 24;
+
+    gps_eph.d_TGD = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 8))) * T_GD_LSB;
+    index += 8;
+
+    gps_eph.i_SV_health = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 6)));
+    index += 6;
+
+    gps_eph.b_L2_P_data_flag = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+    index += 1;
+
+    gps_eph.b_fit_interval_flag = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
 
     return 0;
 }
 
 
+
+// **********************************************
+//
+//   MESSAGE TYPE 1045 (GALILEO EPHEMERIS)
+//
+// **********************************************
+
 std::string Rtcm::print_M1045(const Galileo_Ephemeris & gal_eph)
 {
     unsigned int msg_number = 1045;
@@ -688,17 +835,6 @@ std::string Rtcm::print_M1045(const Galileo_Ephemeris & gal_eph)
 
 
 
-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

src/core/system_parameters/rtcm.h

@@ -43,42 +43,57 @@
 #include "gps_navigation_message.h"
 
 /*!
- * \brief This class implements the RTCM 3.2 Stardard
+ * \brief This class implements the RTCM 3.2 Standard
  *
  */
 class Rtcm
 {
 public:
-    Rtcm();
+    Rtcm(); //<! Default constructor
 
     std::string print_M1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & pseudoranges);
 
-    std::string print_M1045(const Galileo_Ephemeris & gal_eph); //<! Galileo Ephemeris, should be broadcast every 2 minutes
+    /*!
+     * \brief Prints message type 1005 (Stationary Antenna Reference Point)
+     */
+    std::string print_M1005(unsigned int ref_id, double ecef_x, double ecef_y, double ecef_z, bool gps, bool glonass, bool galileo, bool non_physical, bool single_oscillator, unsigned int quarter_cycle_indicator);
 
     /*!
-     * \brief GPS Ephemeris, should be broadcast in the event that the IODC does not match the IODE, and every 2 minutes.
+     * \brief Verifies and reads messages of type 1005 (Stationary Antenna Reference Point)
      */
-    std::string print_M1019(const Gps_Ephemeris & gps_eph);
-    int read_M1019(const std::string & message, Gps_Ephemeris & gps_eph);
-
-
-
-    std::string bin_to_hex(const std::string& s);
-
-    std::string hex_to_bin(const std::string& s);
-
-    unsigned long int bin_to_uint(const std::string& s);
-    long int bin_to_int(const std::string& s);
-
-    unsigned long int hex_to_uint(const std::string& s);
-    long int hex_to_int(const std::string& s);
-
-    double bin_to_double(const std::string& s);
-
-    std::string print_M1005_test(); //<! For testing purposes
     int 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);
 
-    bool check_CRC(const std::string & message);
+    /*!
+     * \brief Prints message type 1019 (GPS Ephemeris), should be broadcast in the event that
+     * the IODC does not match the IODE, and every 2 minutes.
+     */
+    std::string print_M1019(const Gps_Ephemeris & gps_eph);
+
+    /*!
+     * \brief Verifies and reads messages of type 1019 (GPS Ephemeris)
+     */
+    int read_M1019(const std::string & message, Gps_Ephemeris & gps_eph);
+
+    /*!
+     * \brief Prints message type 1045 (Galileo Ephemeris)
+     */
+    std::string print_M1045(const Galileo_Ephemeris & gal_eph); //<! Galileo Ephemeris, should be broadcast every 2 minutes
+
+
+    std::string bin_to_hex(const std::string& s); //<! Returns a string of hexadecimal symbols from a string of binary symbols
+    std::string hex_to_bin(const std::string& s); //<! Returns a string of binary symbols from a string of hexadecimal symbols
+
+    unsigned long int bin_to_uint(const std::string& s); //<! Returns an unsigned long int from a string of binary symbols
+    long int bin_to_int(const std::string& s);           //<! Returns a long int from a string of binary symbols
+
+    unsigned long int hex_to_uint(const std::string& s); //<! Returns an unsigned long int from a string of hexadecimal symbols
+    long int hex_to_int(const std::string& s);           //<! Returns a long int from a string of hexadecimal symbols
+
+    double bin_to_double(const std::string& s);   //<! Returns double from a string of binary symbols
+
+    std::string print_M1005_test();   //<! For testing purposes
+
+    bool check_CRC(const std::string & message); //<! Checks that the CRC of a RTCM package is correct
 private:
     //
     // Messages
@@ -109,7 +124,7 @@ private:
 
     std::bitset<58> get_M1001_sat_content(const Gnss_Synchro & gnss_synchro);
 
-    std::bitset<138> get_M1002();  //  GPS observables
+    //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();
@@ -124,9 +139,7 @@ private:
     std::bitset<10> message_length;
     std::bitset<24> crc_frame;
     typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type;
-
     std::string add_CRC(const std::string& m);
-
     std::string build_message(std::string data); // adds 0s to complete a byte and adds the CRC
 
 

src/tests/formats/rtcm_test.cc

@@ -108,6 +108,14 @@ TEST(Rtcm_Test, Hex_to_int)
 }
 
 
+TEST(Rtcm_Test, Hex_to_uint)
+{
+    auto rtcm = std::make_shared<Rtcm>();
+    long unsigned int expected1 = 42;
+    EXPECT_EQ(expected1,  rtcm->hex_to_uint(rtcm->bin_to_hex("00101010")));
+}
+
+
 TEST(Rtcm_Test, Bin_to_double)
 {
     auto rtcm = std::make_shared<Rtcm>();
@@ -124,11 +132,47 @@ TEST(Rtcm_Test, Bin_to_double)
     EXPECT_DOUBLE_EQ(0, rtcm->bin_to_double(test3.to_string()));
 }
 
-TEST(Rtcm_Test, Test_Read_M1005)
+
+TEST(Rtcm_Test, Bin_to_uint)
+{
+    auto rtcm = std::make_shared<Rtcm>();
+    long unsigned int expected1 = 42;
+    EXPECT_EQ(expected1, rtcm->bin_to_uint("00101010"));
+    long unsigned int expected2 = 214;
+    EXPECT_EQ(expected2, rtcm->bin_to_uint("11010110"));
+}
+
+
+TEST(Rtcm_Test, Bin_to_int)
+{
+    auto rtcm = std::make_shared<Rtcm>();
+    long unsigned int expected1 = 42;
+    EXPECT_EQ(expected1, rtcm->bin_to_int("00101010"));
+    long unsigned int expected2 = -42;
+    EXPECT_EQ(expected2, rtcm->bin_to_int("11010110"));
+}
+
+
+TEST(Rtcm_Test, Check_CRC)
+{
+    auto rtcm = std::make_shared<Rtcm>();
+    EXPECT_EQ(true, rtcm->check_CRC("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B98"));
+    EXPECT_EQ(false, rtcm->check_CRC("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B99"));
+
+    EXPECT_EQ(true, rtcm->check_CRC(rtcm->print_M1005_test()));
+    EXPECT_EQ(true, rtcm->check_CRC(rtcm->print_M1005_test()));  // Run twice to check that CRC has no memory
+}
+
+
+TEST(Rtcm_Test, Test_MT1005)
 {
     auto rtcm = std::make_shared<Rtcm>();
     std::string reference_msg = rtcm->print_M1005_test();
 
+    std::string reference_msg2 = rtcm->print_M1005(2003, 1114104.5999, -4850729.7108, 3975521.4643, true, false, false, false, false, 0);
+
+    EXPECT_EQ(0, reference_msg.compare(reference_msg2));
+
     unsigned int ref_id;
     double ecef_x;
     double ecef_y;
@@ -160,13 +204,26 @@ TEST(Rtcm_Test, Test_Read_M1005)
     EXPECT_DOUBLE_EQ(3975521.4643, ecef_z);
 }
 
-TEST(Rtcm_Test, Check_CRC)
+
+
+TEST(Rtcm_Test, Test_MT1019)
 {
     auto rtcm = std::make_shared<Rtcm>();
-    EXPECT_EQ(true, rtcm->check_CRC("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B98"));
-    EXPECT_EQ(false, rtcm->check_CRC("D300133ED7D30202980EDEEF34B4BD62AC0941986F33360B99"));
 
-    EXPECT_EQ(true, rtcm->check_CRC(rtcm->print_M1005_test()));
+    Gps_Ephemeris gps_eph = Gps_Ephemeris();
-    EXPECT_EQ(true, rtcm->check_CRC(rtcm->print_M1005_test()));
+    Gps_Ephemeris gps_eph_read = Gps_Ephemeris();
+
+    gps_eph.i_satellite_PRN = 3;
+    gps_eph.d_IODC = 4;
+    gps_eph.d_e_eccentricity = 2.0 * E_LSB;
+    gps_eph.b_fit_interval_flag = true;
+    std::string tx_msg = rtcm->print_M1019(gps_eph);
+
+    EXPECT_EQ(0, rtcm->read_M1019(tx_msg, gps_eph_read));
+    EXPECT_EQ(3, gps_eph_read.i_satellite_PRN);
+    EXPECT_DOUBLE_EQ(4, gps_eph_read.d_IODC);
+    EXPECT_DOUBLE_EQ( 2.0 * E_LSB, gps_eph_read.d_e_eccentricity);
+    EXPECT_EQ(true, gps_eph_read.b_fit_interval_flag);
+    EXPECT_EQ(1, rtcm->read_M1019("FFFFFFFFFFF", gps_eph_read));
 }