diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 9a4c0b899..757a2f9d0 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -84,17 +84,24 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); // RTCM message rates: least common multiple with output_rate_ms int rtcm_MT1019_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), output_rate_ms); + int rtcm_MT1020_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1020_rate_ms", 5000), output_rate_ms); int rtcm_MT1045_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), output_rate_ms); int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms); int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); + int rtcm_MT1087_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1087_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); std::map rtcm_msg_rate_ms; rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; + rtcm_msg_rate_ms[1020] = rtcm_MT1020_rate_ms; rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; for (int k = 1071; k < 1078; k++) // All GPS MSM { rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; } + for (int k = 1081; k < 1088; k++) // All GLONASS MSM + { + rtcm_msg_rate_ms[k] = rtcm_MT1087_rate_ms; + } for (int k = 1091; k < 1098; k++) // All Galileo MSM { rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index cbc13813a..1ebf824f7 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -180,6 +180,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) d_ls_pvt->galileo_almanac = *galileo_almanac; DLOG(INFO) << "New Galileo Almanac has arrived "; } + //**************** GLONASS GNAV Telemetry ************************** else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) { @@ -207,7 +208,6 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) // ### GLONASS GNAV Almanac ### std::shared_ptr glonass_gnav_almanac; glonass_gnav_almanac = boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map d_ls_pvt->glonass_gnav_almanac = *glonass_gnav_almanac; DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "; } @@ -279,6 +279,14 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump { d_rtcm_MT1019_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set } + if(rtcm_msg_rate_ms.find(1020) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1020_rate_ms = rtcm_msg_rate_ms[1020]; + } + else + { + d_rtcm_MT1020_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set + } if(rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) { d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; @@ -295,6 +303,14 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump { d_rtcm_MT1077_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set } + if(rtcm_msg_rate_ms.find(1087) != rtcm_msg_rate_ms.end()) // whatever between 1081 and 1087 + { + d_rtcm_MT1087_rate_ms = rtcm_msg_rate_ms[1087]; + } + else + { + d_rtcm_MT1087_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + } if(rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 { d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097]; @@ -316,8 +332,10 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump d_rx_time = 0.0; last_pvt_display_T_rx_s = 0.0; last_RTCM_1019_output_time = 0.0; + last_RTCM_1020_output_time = 0.0; last_RTCM_1045_output_time = 0.0; last_RTCM_1077_output_time = 0.0; + last_RTCM_1087_output_time = 0.0; last_RTCM_1097_output_time = 0.0; last_RTCM_MSM_output_time = 0.0; last_RINEX_obs_output_time = 0.0; @@ -499,8 +517,10 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite bool flag_display_pvt = false; bool flag_compute_pvt_output = false; bool flag_write_RTCM_1019_output = false; + bool flag_write_RTCM_1020_output = false; bool flag_write_RTCM_1045_output = false; bool flag_write_RTCM_1077_output = false; + bool flag_write_RTCM_1087_output = false; bool flag_write_RTCM_1097_output = false; bool flag_write_RTCM_MSM_output = false; bool flag_write_RINEX_obs_output = false; @@ -593,7 +613,11 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite flag_write_RTCM_1019_output = true; last_RTCM_1019_output_time = current_RX_time; } - + if ((std::fabs(current_RX_time - last_RTCM_1020_output_time) * 1000.0 >= static_cast(d_rtcm_MT1020_rate_ms)) && (d_rtcm_MT1020_rate_ms != 0) ) // allows deactivating messages by setting rate = 0 + { + flag_write_RTCM_1020_output = true; + last_RTCM_1020_output_time = current_RX_time; + } if ((std::fabs(current_RX_time - last_RTCM_1045_output_time) * 1000.0 >= static_cast(d_rtcm_MT1045_rate_ms)) && (d_rtcm_MT1045_rate_ms != 0) ) { flag_write_RTCM_1045_output = true; @@ -605,7 +629,11 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite flag_write_RTCM_1077_output = true; last_RTCM_1077_output_time = current_RX_time; } - + if ((std::fabs(current_RX_time - last_RTCM_1087_output_time) * 1000.0 >= static_cast(d_rtcm_MT1087_rate_ms)) && (d_rtcm_MT1087_rate_ms != 0) ) + { + flag_write_RTCM_1087_output = true; + last_RTCM_1087_output_time = current_RX_time; + } if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast(d_rtcm_MT1097_rate_ms)) && (d_rtcm_MT1097_rate_ms != 0) ) { flag_write_RTCM_1097_output = true; @@ -1097,7 +1125,6 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite } // ####################### RTCM MESSAGES ################# - //TODO Add RTCM logic for GLONASS satellites if(b_rtcm_writing_started) { if(type_of_rx == 1) // GPS L1 C/A @@ -1226,6 +1253,157 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite } } } + if((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + { + if(flag_write_RTCM_1020_output == true) + { + for(std::map::iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end(); glonass_gnav_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + + std::map::iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); + + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if(type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A + { + if(flag_write_RTCM_1019_output == true) + { + for(gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(flag_write_RTCM_1020_output == true) + { + for(std::map::iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end(); glonass_gnav_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if(flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(glo_channel == 0) + { + if(system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = i; + } + } + } + i++; + } + if(flag_write_RTCM_MSM_output == true) + { + + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if(flag_write_RTCM_MSM_output == true) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + if(type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B + { + if(flag_write_RTCM_1020_output == true) + { + for(std::map::iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.begin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end(); glonass_gnav_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if(flag_write_RTCM_1045_output == true) + { + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if(flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(glo_channel == 0) + { + if(system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = i; + } + } + } + i++; + } + if(flag_write_RTCM_MSM_output == true) + { + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if(flag_write_RTCM_MSM_output == true) + { + + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } } if(!b_rtcm_writing_started) // the first time diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 274289138..5e74aa357 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -99,8 +99,8 @@ public: Gps_CNAV_Iono gps_cnav_iono; Gps_CNAV_Utc_Model gps_cnav_utc_model; - Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< GLONASS GNAV UTC Model - Glonass_Gnav_Almanac glonass_gnav_almanac; //!< GLONASS GNAV Almanac Model + Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< Map storing GLONASS GNAV UTC Model + Glonass_Gnav_Almanac glonass_gnav_almanac; //!< Map storing GLONASS GNAV Almanac Model int count_valid_position; diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index f4ffafae8..45f13db82 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -74,7 +74,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) rtklib_sat.sat = glonass_gnav_eph.i_satellite_slot_number; /* satellite number */ rtklib_sat.iode = glonass_gnav_eph.d_t_b; /* IODE (0-6 bit of tb field) */ rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel; /* satellite frequency number */ - rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/ + rtklib_sat.svh = glonass_gnav_eph.d_l3rd_n; /* satellite health*/ rtklib_sat.sva = glonass_gnav_eph.d_F_T; /* satellite accuracy*/ rtklib_sat.age = glonass_gnav_eph.d_E_n; /* satellite age*/ rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn*1000; /* satellite position (ecef) (m) */ diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc index 002324e94..cbef6df64 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.cc +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -1,8 +1,9 @@ /*! * \file glonass_gnav_almanac.cc * \brief Interface of a GLONASS GNAV ALMANAC storage as described in GLONASS ICD (Edition 5.1) - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf - * \author Damian Miralles , 2017. dmiralles2009(at)gmail.com + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * @@ -36,10 +37,9 @@ Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() { i_satellite_freq_channel = 0; - d_tau_c = 0.0; - d_tau_gps = 0.0; - d_N_4 = 0.0; - d_N_A = 0.0; + i_satellite_PRN = 0; + i_satellite_slot_number = 0; + d_n_A = 0.0; d_H_n_A = 0.0; d_lambda_n_A = 0.0; @@ -56,7 +56,7 @@ Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() d_l_n = 0.0; } -void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i) +void Glonass_Gnav_Almanac::satellite_position(double N_A, double N_i, double t_i) { double T_nom = 43200; // [seconds] double i_nom = D2R*63.0; // [rad] @@ -85,7 +85,7 @@ void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i) double e2_y = 0.0; double e2_z = 0.0; // Compute time difference to reference time - Delta_t = (N_i - d_N_A) * 86400 + (t_i + d_t_lambda_n_A); + Delta_t = (N_i - N_A) * 86400 + (t_i + d_t_lambda_n_A); // Compute the actual inclination i = i_nom + d_Delta_i_n_A; diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index 1e1682e66..aafacb888 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -39,19 +39,14 @@ /*! * \brief This class is a storage for the GLONASS SV ALMANAC data as described GLONASS ICD (Edition 5.1) - * - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Almanac { public: - int i_satellite_freq_channel; //!< SV Frequency Channel NUMBER - double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] - double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] - double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] - double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year [days] double d_n_A; //!< Conventional number of satellite within GLONASS space segment [dimensionless] - double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless] + double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite as table 4.10 (0-31) [dimensionless] double d_lambda_n_A; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles] double d_t_lambda_n_A; //!< Time of first ascending node passage [s] double d_Delta_i_n_A; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] @@ -65,6 +60,11 @@ public: double d_C_n; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] double d_l_n; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] + // Satellite Identification Information + int i_satellite_freq_channel; //!< SV Frequency Channel Number + unsigned int i_satellite_PRN; //!< SV PRN Number, equivalent to slot number for compatibility with GPS + unsigned int i_satellite_slot_number; //!< SV Slot Number + // satellite positions double d_satpos_Xo; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. double d_satpos_Yo; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] @@ -93,10 +93,8 @@ public: if(version){}; archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel); - archive & make_nvp("d_tau_c", d_tau_c); - archive & make_nvp("d_tau_gps", d_tau_gps); - archive & make_nvp("d_N_4", d_N_4); - archive & make_nvp("d_N_A", d_N_A); + archive & make_nvp("i_satellite_PRN", i_satellite_PRN); + archive & make_nvp("i_satellite_slot_number", i_satellite_slot_number); archive & make_nvp("d_n_A", d_n_A); archive & make_nvp("d_H_n_A", d_H_n_A); archive & make_nvp("d_lambda_n_A", d_lambda_n_A); @@ -113,7 +111,7 @@ public: archive & make_nvp("d_l_n", d_l_n); } - void satellite_position(double N_i, double t_i); + void satellite_position(double N_A, double N_i, double t_i); /*! * Default constructor */ diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index ba376b620..f4d47ae53 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -1,11 +1,9 @@ /*! * \file glonass_gnav_ephemeris.cc * \brief Interface of a GLONASS GNAV EPHEMERIS storage and orbital model functions - * - * See Russian Institute of Space Device Engineering, - “Global Navigation Satellite System GLONASS Interface Control Document, - Navigational radiosignal in bands L1, L2,” Moscow, Edition 5.1, 2008. + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 3f8fa699c..447ced86d 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -43,8 +43,8 @@ /*! * \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD (Edition 5.1) - * - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Ephemeris { @@ -100,9 +100,10 @@ public: // Inmediate deliverables of ephemeris information //TODO check how freq channel is managed in gnav message. I think it is a number greater thn 0 - unsigned int i_satellite_freq_channel; //!< SV Frequency Channel Number - unsigned int i_satellite_PRN; //!< SV PRN NUMBER - unsigned int i_satellite_slot_number; //!< SV PRN NUMBER + // Satellite Identification Information + int i_satellite_freq_channel; //!< SV Frequency Channel Number + unsigned int i_satellite_PRN; //!< SV PRN Number, equivalent to slot number for compatibility with GPS + unsigned int i_satellite_slot_number; //!< SV Slot Number double d_TOD; //!< Time of Day of the ephemeris set based in start of frame [s] double d_D4Y; //!< Day of Year after latest leap year (4 year interval) double d_yr; //!< Current year @@ -139,6 +140,8 @@ public: if(version){}; archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel); //!< SV PRN frequency channel number + archive & make_nvp("i_satellite_PRN", i_satellite_PRN); + archive & make_nvp("i_satellite_slot_number", i_satellite_slot_number); archive & make_nvp("d_m", d_m); //!< String number within frame [dimensionless] archive & make_nvp("d_t_k", d_t_k); //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds] archive & make_nvp("d_t_b", d_t_b); //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes] diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 7cdbec768..b234bad71 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -1,8 +1,9 @@ /*! -m * \file glonass_gnav_navigation_message.cc + * \file glonass_gnav_navigation_message.cc * \brief Implementation of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1) - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * @@ -416,6 +417,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) // Fill in ephemeris deliverables in the code gnav_ephemeris.i_satellite_slot_number = gnav_ephemeris.d_n; + gnav_ephemeris.i_satellite_PRN = gnav_ephemeris.d_n; gnav_ephemeris.d_D4Y = gnav_ephemeris.d_N_T; flag_ephemeris_str_4 = true; @@ -480,12 +482,20 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9; gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14; - gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + if(i_satellite_slot_number == gnav_ephemeris.i_satellite_slot_number) { - gnav_ephemeris.i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A; + gnav_ephemeris.i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel; } flag_almanac_str_7 = true; } @@ -521,6 +531,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) -32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_9 = true; } case 10: @@ -551,6 +569,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_11 = true; } break; @@ -582,6 +608,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_13 = true; } case 14: @@ -621,6 +655,14 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)) - 32.0; gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + // Set satellite information for redundancy purposes + if(gnav_almanac[i_satellite_slot_number - 1].d_H_n_A > 24) + { + gnav_almanac[i_satellite_slot_number - 1].i_satellite_freq_channel = gnav_almanac[i_satellite_slot_number - 1].d_H_n_A - 32.0; + } + gnav_almanac[i_satellite_slot_number - 1].i_satellite_slot_number = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + gnav_almanac[i_satellite_slot_number - 1].i_satellite_PRN = gnav_almanac[i_satellite_slot_number - 1].d_n_A; + flag_almanac_str_15 = true; } default: diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h index 8fbb30e22..97e2a8d49 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.h +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -48,9 +48,8 @@ /*! * \brief This class decodes a GLONASS GNAV Data message as described in GLONASS ICD (Edition 5.1) - * - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdfdescribed in IS-GPS-200E - * + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Navigation_Message { diff --git a/src/core/system_parameters/glonass_gnav_utc_model.cc b/src/core/system_parameters/glonass_gnav_utc_model.cc index 55ca5ccee..ec9d5e20e 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.cc +++ b/src/core/system_parameters/glonass_gnav_utc_model.cc @@ -1,7 +1,9 @@ /* * \file glonass_gnav_utc_model.h * \brief Interface of a GLONASS GNAV UTC MODEL storage - * \author Damian Miralles, 2017. dmiralles2009(at).gmail.com + * \note Code added as part of GSoC 2017 program + * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- * diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index 6aca883f4..fa0886418 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -38,13 +38,14 @@ /*! * \brief This class is a storage for the GLONASS GNAV UTC MODEL data as described in GLONASS ICD (Edition 5.1) - * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf + * \note Code added as part of GSoC 2017 program + * \see GLONASS ICD */ class Glonass_Gnav_Utc_Model { public: bool valid; - // UTC + // Clock Parameters double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] @@ -60,7 +61,6 @@ public: { using boost::serialization::make_nvp; if(version){}; - archive & make_nvp("valid",valid); archive & make_nvp("d_tau_c", d_tau_c); archive & make_nvp("d_tau_gps", d_tau_gps); diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 3a82d5647..662006d92 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -1848,7 +1848,7 @@ int Rtcm::read_MT1020(const std::string & message, Glonass_Gnav_Ephemeris & glon glonass_gnav_eph.i_satellite_slot_number = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 6))); index += 6; - glonass_gnav_eph.i_satellite_freq_channel = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5))); + glonass_gnav_eph.i_satellite_freq_channel = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 5)) - 7.0); index += 5; glonass_gnav_alm_health = static_cast(Rtcm::bin_to_uint(message_bin.substr(index, 1))); @@ -3882,12 +3882,14 @@ int Rtcm::set_DF039(bool code_indicator) } -int Rtcm::set_DF040(unsigned int frequency_channel_number) +int Rtcm::set_DF040(int frequency_channel_number) { - unsigned int freq_ = frequency_channel_number; + unsigned int freq_ = frequency_channel_number + 7; if(freq_ > 20) { - LOG(WARNING) << "GLONASS Satellite Frequency Number must be between 0 and 20, but channel frequency number " << freq_ << " was found"; + LOG(WARNING) << "GLONASS Satellite Frequency Number Conversion Error." + << "Value must be between 0 and 20, but converted channel" + << "frequency number " << freq_ << " was found"; } DF040 = std::bitset<5>(freq_); @@ -3897,10 +3899,12 @@ int Rtcm::set_DF040(unsigned int frequency_channel_number) int Rtcm::set_DF040(const Glonass_Gnav_Ephemeris & glonass_gnav_eph) { - unsigned int freq_ = glonass_gnav_eph.i_satellite_freq_channel; + unsigned int freq_ = glonass_gnav_eph.i_satellite_freq_channel + 7; if(freq_ > 20) { - LOG(WARNING) << "GLONASS Satellite Frequency Number must be between 0 and 20, but channel frequency number " << freq_ << " was found"; + LOG(WARNING) << "GLONASS Satellite Frequency Number Conversion Error." + << "Value must be between 0 and 20, but converted channel" + << "frequency number " << freq_ << " was found"; } DF040 = std::bitset<5>(freq_); diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index ed4369923..3ba745217 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -133,9 +133,9 @@ public: * \brief Prints L1-Only GLONASS RTK Observables * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id); @@ -143,9 +143,9 @@ public: * \brief Prints Extended L1-Only GLONASS RTK Observables * \details This GLONASS message type is used when only L1 data is present and bandwidth is very tight, often 1012 is used in such cases. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map & observables, unsigned short station_id); @@ -153,9 +153,9 @@ public: * \brief Prints L1&L2 GLONASS RTK Observables * \details This GLONASS message type is not generally used or supported; type 1012 is to be preferred * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); @@ -163,9 +163,9 @@ public: * \brief Prints Extended L1&L2 GLONASS RTK Observables * \details This GLONASS message type is the most common observational message type, with L1/L2/SNR content. This is one of the most common messages found. * \note Code added as part of GSoC 2017 program - * \params glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param glonass_gnav_eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return string with message contents */ std::string print_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map & observables, unsigned short station_id); @@ -333,9 +333,9 @@ public: /*! * \brief Locks time period in which GLONASS signals have been continually tracked. * \note Code added as part of GSoC 2017 program - * \params eph GLONASS GNAV Broadcast Ephemeris - * \params obs_time Time of observation at the moment of printing - * \params observables Set of observables as defined by the platform + * \param eph GLONASS GNAV Broadcast Ephemeris + * \param obs_time Time of observation at the moment of printing + * \param observables Set of observables as defined by the platform * \return Returns the time period in which GLONASS signals have been continually tracked. */ unsigned int lock_time(const Glonass_Gnav_Ephemeris & eph, double obs_time, const Gnss_Synchro & gnss_synchro); @@ -1015,8 +1015,14 @@ private: std::bitset<8> DF032; - std::bitset<27> DF034; //!< GLONASS Epoch Time (tk) + /*! + * \brief Sets the Data Field value + * \note Code added as part of GSoC 2017 program + * \param obs_time Time of observation at the moment of printing + * \return returns 0 upon success + */ int set_DF034(double obs_time); + std::bitset<27> DF034; //!< GLONASS Epoch Time (tk) std::bitset<5> DF035; //!< No. of GLONASS Satellite Signals Processed int set_DF035(const std::map & observables); @@ -1035,7 +1041,7 @@ private: int set_DF039(bool code_indicator); std::bitset<5> DF040; //!< GLONASS Satellite Frequency Number - int set_DF040(unsigned int frequency_channel_number); + int set_DF040(int frequency_channel_number); int set_DF040(const Glonass_Gnav_Ephemeris & glonass_gnav_eph); std::bitset<25> DF041; //!< GLONASS L1 Pseudorange diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc index 6c02311cf..89cf2ae1d 100644 --- a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc @@ -53,10 +53,11 @@ TEST(GlonassGnavAlmanacTest, SatellitePosition) double Vxoi = -3.375497; // [m/s] double Vyoi = -0.161453; // [Кm/s] double Vzoi = 2.060844; // [Кm/s] + double N_A = 615; // [days] Glonass_Gnav_Almanac gnav_almanac; - gnav_almanac.d_N_A = 615; // [days] + gnav_almanac.d_lambda_n_A = -0.189986229; // [half cycles] gnav_almanac.d_t_lambda_n_A = 27122.09375; // [second] gnav_almanac.d_Delta_i_n_A = 0.011929512; // [half cycle] @@ -65,7 +66,7 @@ TEST(GlonassGnavAlmanacTest, SatellitePosition) gnav_almanac.d_epsilon_n_A = 0.001482010; // [unitless] gnav_almanac.d_omega_n_A = 0.440277100; // [Half cycle] - gnav_almanac.satellite_position(N_i, t_i); + gnav_almanac.satellite_position(N_A, N_i, t_i); ASSERT_TRUE(gnav_almanac.d_satpos_Xo - Xoi < DBL_EPSILON ); ASSERT_TRUE(gnav_almanac.d_satpos_Yo - Yoi < DBL_EPSILON );