From eb8539a725964e38b95cfe6e03ab9abe1dcea870 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 May 2016 11:01:10 +0200 Subject: [PATCH] fix TOW formatting in RTCM messages --- .../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc | 6 +- src/core/system_parameters/rtcm.cc | 81 +++++-------------- src/core/system_parameters/rtcm.h | 10 +-- 3 files changed, 26 insertions(+), 71 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 3aa628979..42e7dea1d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -339,10 +339,10 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g if((d_sample_counter % 1000) == 0) { std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();std::cout << d_rx_time << std::endl; if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - d_rtcm_printer->Print_Rtcm_MSM(4, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_pseudoranges_map, 1234, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_pseudoranges_map, 1234, 0, 0, 0, 0, 0); } } } @@ -358,7 +358,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - d_rtcm_printer->Print_Rtcm_MSM(4, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_pseudoranges_map, 1234, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_pseudoranges_map, 1234, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index e716070ca..f339c1dc6 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -397,7 +397,7 @@ std::string Rtcm::build_message(const std::string & data) const // // ******************************************************** -std::bitset<64> Rtcm::get_MT1001_4_header(unsigned int msg_number, const Gps_Ephemeris & gps_eph, double obs_time, const std::map & pseudoranges, +std::bitset<64> Rtcm::get_MT1001_4_header(unsigned int msg_number, double obs_time, const std::map & pseudoranges, unsigned int ref_id, unsigned int smooth_int, bool sync_flag, bool divergence_free) { unsigned int reference_station_id = ref_id; // Max: 4095 @@ -407,7 +407,7 @@ std::bitset<64> Rtcm::get_MT1001_4_header(unsigned int msg_number, const Gps_Eph unsigned int smoothing_interval = smooth_int; Rtcm::set_DF002(msg_number); Rtcm::set_DF003(reference_station_id); - Rtcm::set_DF004(gps_eph, obs_time); + Rtcm::set_DF004(obs_time); Rtcm::set_DF005(synchronous_GNSS_flag); Rtcm::set_DF006(pseudoranges_); Rtcm::set_DF007(divergence_free_smoothing_indicator); @@ -469,7 +469,7 @@ std::string Rtcm::print_MT1001(const Gps_Ephemeris & gps_eph, double obs_time, c } } - std::bitset<64> header = Rtcm::get_MT1001_4_header(1001, gps_eph, obs_time, pseudorangesL1, ref_id, smooth_int, sync_flag, divergence_free); + std::bitset<64> header = Rtcm::get_MT1001_4_header(1001, obs_time, pseudorangesL1, ref_id, smooth_int, sync_flag, divergence_free); std::string data = header.to_string(); for(pseudoranges_iter = pseudorangesL1.begin(); @@ -519,7 +519,7 @@ std::string Rtcm::print_MT1002(const Gps_Ephemeris & gps_eph, double obs_time, c } } - std::bitset<64> header = Rtcm::get_MT1001_4_header(1002, gps_eph, obs_time, pseudorangesL1, ref_id, smooth_int, sync_flag, divergence_free); + std::bitset<64> header = Rtcm::get_MT1001_4_header(1002, obs_time, pseudorangesL1, ref_id, smooth_int, sync_flag, divergence_free); std::string data = header.to_string(); for(pseudoranges_iter = pseudorangesL1.begin(); @@ -623,7 +623,7 @@ std::string Rtcm::print_MT1003(const Gps_Ephemeris & ephL1, const Gps_CNAV_Ephem } } - std::bitset<64> header = Rtcm::get_MT1001_4_header(1003, ephL1, obs_time, pseudorangesL1_with_L2, ref_id, smooth_int, sync_flag, divergence_free); + std::bitset<64> header = Rtcm::get_MT1001_4_header(1003, obs_time, pseudorangesL1_with_L2, ref_id, smooth_int, sync_flag, divergence_free); std::string data = header.to_string(); for(common_pseudoranges_iter = common_pseudoranges.begin(); @@ -733,7 +733,7 @@ std::string Rtcm::print_MT1004(const Gps_Ephemeris & ephL1, const Gps_CNAV_Ephem } } - std::bitset<64> header = Rtcm::get_MT1001_4_header(1004, ephL1, obs_time, pseudorangesL1_with_L2, ref_id, smooth_int, sync_flag, divergence_free); + std::bitset<64> header = Rtcm::get_MT1001_4_header(1004, obs_time, pseudorangesL1_with_L2, ref_id, smooth_int, sync_flag, divergence_free); std::string data = header.to_string(); for(common_pseudoranges_iter = common_pseudoranges.begin(); @@ -1600,8 +1600,7 @@ std::string Rtcm::print_MSM_1( const Gps_Ephemeris & gps_eph, msg_number = 1071; } - std::string header = Rtcm::get_MSM_header(msg_number, gps_eph, gps_cnav_eph, - gal_eph, + std::string header = Rtcm::get_MSM_header(msg_number, obs_time, pseudoranges, ref_id, @@ -1626,9 +1625,7 @@ std::string Rtcm::print_MSM_1( const Gps_Ephemeris & gps_eph, } -std::string Rtcm::get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & gps_eph, - const Gps_CNAV_Ephemeris & gps_cnav_eph, - const Galileo_Ephemeris & gal_eph, +std::string Rtcm::get_MSM_header(unsigned int msg_number, double obs_time, const std::map & pseudoranges, unsigned int ref_id, @@ -1640,18 +1637,7 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & { Rtcm::set_DF002(msg_number); Rtcm::set_DF003(ref_id); - if(gps_eph.i_satellite_PRN != 0) - { - Rtcm::set_DF004(gps_eph, obs_time); - } - else if(gps_cnav_eph.i_satellite_PRN != 0) - { - Rtcm::set_DF004(gps_cnav_eph, obs_time); - } - else - { - Rtcm::set_DF248(gal_eph, obs_time); - } + Rtcm::set_DF004(obs_time); Rtcm::set_DF393(more_messages); Rtcm::set_DF409(0); // Issue of Data Station. 0: not utilized std::bitset<7> DF001_ = std::bitset<7>("0000000"); @@ -1664,14 +1650,7 @@ std::string Rtcm::get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & Rtcm::set_DF395(pseudoranges); std::string header = DF002.to_string() + DF003.to_string(); - if(gps_eph.i_satellite_PRN != 0) - { - header += DF004.to_string(); - } - else - { - header += DF248.to_string(); - } + header += DF004.to_string(); header = header + DF393.to_string() + DF409.to_string() + DF001_.to_string() + @@ -1786,8 +1765,7 @@ std::string Rtcm::print_MSM_2( const Gps_Ephemeris & gps_eph, msg_number = 1072; } - std::string header = Rtcm::get_MSM_header(msg_number, gps_eph, gps_cnav_eph, - gal_eph, + std::string header = Rtcm::get_MSM_header(msg_number, obs_time, pseudoranges, ref_id, @@ -1882,8 +1860,7 @@ std::string Rtcm::print_MSM_3( const Gps_Ephemeris & gps_eph, msg_number = 1073; } - std::string header = Rtcm::get_MSM_header(msg_number, gps_eph, gps_cnav_eph, - gal_eph, + std::string header = Rtcm::get_MSM_header(msg_number, obs_time, pseudoranges, ref_id, @@ -1980,8 +1957,7 @@ std::string Rtcm::print_MSM_4( const Gps_Ephemeris & gps_eph, msg_number = 1074; } - std::string header = Rtcm::get_MSM_header(msg_number, gps_eph, gps_cnav_eph, - gal_eph, + std::string header = Rtcm::get_MSM_header(msg_number, obs_time, pseudoranges, ref_id, @@ -2121,8 +2097,7 @@ std::string Rtcm::print_MSM_5( const Gps_Ephemeris & gps_eph, msg_number = 1075; } - std::string header = Rtcm::get_MSM_header(msg_number, gps_eph, gps_cnav_eph, - gal_eph, + std::string header = Rtcm::get_MSM_header(msg_number, obs_time, pseudoranges, ref_id, @@ -2272,8 +2247,7 @@ std::string Rtcm::print_MSM_6( const Gps_Ephemeris & gps_eph, msg_number = 1076; } - std::string header = Rtcm::get_MSM_header(msg_number, gps_eph, gps_cnav_eph, - gal_eph, + std::string header = Rtcm::get_MSM_header(msg_number, obs_time, pseudoranges, ref_id, @@ -2374,8 +2348,7 @@ std::string Rtcm::print_MSM_7( const Gps_Ephemeris & gps_eph, msg_number = 1076; } - std::string header = Rtcm::get_MSM_header(msg_number, gps_eph, gps_cnav_eph, - gal_eph, + std::string header = Rtcm::get_MSM_header(msg_number, obs_time, pseudoranges, ref_id, @@ -2776,24 +2749,10 @@ int Rtcm::set_DF003(unsigned int ref_station_ID) } -int Rtcm::set_DF004(const Gps_Ephemeris & gps_eph, double obs_time) +int Rtcm::set_DF004(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_DF004(const Gps_CNAV_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)); + unsigned long int tow = static_cast(std::round(obs_time * 1000)); if(tow > 604799999) { LOG(WARNING) << "To large TOW! Set to the last millisecond of the week"; @@ -3331,10 +3290,10 @@ int Rtcm::set_DF137(const Gps_Ephemeris & gps_eph) } -int Rtcm::set_DF248(const Galileo_Ephemeris & gal_eph, double obs_time) +int Rtcm::set_DF248(double obs_time) { // TOW in milliseconds from the beginning of the Galileo week, measured in Galileo time - unsigned long int tow = static_cast(std::round((obs_time + 604800 * static_cast(gal_eph.WN_5)) * 1000)); + unsigned long int tow = static_cast(std::round(obs_time * 1000)); if(tow > 604799999) { LOG(WARNING) << "To large TOW! Set to the last millisecond of the week"; diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index 2460e6cf8..3931583e7 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -289,7 +289,6 @@ private: // Generation of messages content // std::bitset<64> get_MT1001_4_header(unsigned int msg_number, - const Gps_Ephemeris & gps_eph, double obs_time, const std::map & pseudoranges, unsigned int ref_id, @@ -304,9 +303,7 @@ private: std::bitset<152> get_MT1005_test(); - std::string get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & gps_eph, - const Gps_CNAV_Ephemeris & gps_cnav_eph, - const Galileo_Ephemeris & gal_eph, + std::string get_MSM_header(unsigned int msg_number, double obs_time, const std::map & pseudoranges, unsigned int ref_id, @@ -794,8 +791,7 @@ private: int set_DF003(unsigned int ref_station_ID); std::bitset<30> DF004; - int set_DF004(const Gps_Ephemeris & gps_eph, double obs_time); - int set_DF004(const Gps_CNAV_Ephemeris & gps_eph, double obs_time); + int set_DF004(double obs_time); std::bitset<1> DF005; int set_DF005(bool sync_flag); @@ -979,7 +975,7 @@ private: int set_DF142(const Gps_Ephemeris & gps_eph); std::bitset<30> DF248; - int set_DF248(const Galileo_Ephemeris & gal_eph, double obs_time); + int set_DF248(double obs_time); // Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045 std::bitset<6> DF252;