diff --git a/CMakeLists.txt b/CMakeLists.txt index d5f1f79a9..563adfc03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -317,7 +317,7 @@ set(GNSSSDR_GNURADIO_MIN_VERSION "3.7.3") set(GNSSSDR_BOOST_MIN_VERSION "1.45") set(GNSSSDR_PYTHON_MIN_VERSION "2.7") set(GNSSSDR_MAKO_MIN_VERSION "0.4.2") -set(GNSSSDR_ARMADILLO_MIN_VERSION "4.200.0") +set(GNSSSDR_ARMADILLO_MIN_VERSION "5.300.0") set(GNSSSDR_MATIO_MIN_VERSION "1.5.3") diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index a839fcc63..91267e33d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -38,6 +38,7 @@ #include #include #include +#include "display.h" #include #include #include @@ -386,7 +387,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc() msgctl(sysv_msqid, IPC_RMID, NULL); //save GPS L2CM ephemeris to XML file - std::string file_name = "eph_GPS_L2CM_L5.xml"; + std::string file_name = "eph_GPS_CNAV.xml"; if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) { @@ -535,13 +536,13 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // ############ 1. READ PSEUDORANGES #### for (unsigned int i = 0; i < d_nchannels; i++) { - if (in[i][epoch].Flag_valid_pseudorange == true) + if (in[i][epoch].Flag_valid_pseudorange) { std::map::const_iterator tmp_eph_iter_gps = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_gal = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_cnav = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_glo_gnav = d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN); - if (((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0)) || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1G") == 0)) || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2G") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("L5") == 0))) + if (((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("1C") == 0)) or ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("2S") == 0)) or ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("1B") == 0)) or ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("5X") == 0)) or ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("1G") == 0)) or ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("2G") == 0)) or ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("L5") == 0))) { // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); @@ -611,36 +612,36 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item flag_display_pvt = true; last_pvt_display_T_rx_s = current_RX_time; } - if ((std::fabs(current_RX_time - last_RTCM_1019_output_time) * 1000.0 >= static_cast(d_rtcm_MT1019_rate_ms)) && (d_rtcm_MT1019_rate_ms != 0)) // allows deactivating messages by setting rate = 0 + if ((std::fabs(current_RX_time - last_RTCM_1019_output_time) * 1000.0 >= static_cast(d_rtcm_MT1019_rate_ms)) and (d_rtcm_MT1019_rate_ms != 0)) // allows deactivating messages by setting rate = 0 { 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 + if ((std::fabs(current_RX_time - last_RTCM_1020_output_time) * 1000.0 >= static_cast(d_rtcm_MT1020_rate_ms)) and (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)) + if ((std::fabs(current_RX_time - last_RTCM_1045_output_time) * 1000.0 >= static_cast(d_rtcm_MT1045_rate_ms)) and (d_rtcm_MT1045_rate_ms != 0)) { flag_write_RTCM_1045_output = true; last_RTCM_1045_output_time = current_RX_time; } - if ((std::fabs(current_RX_time - last_RTCM_1077_output_time) * 1000.0 >= static_cast(d_rtcm_MT1077_rate_ms)) && (d_rtcm_MT1077_rate_ms != 0)) + if ((std::fabs(current_RX_time - last_RTCM_1077_output_time) * 1000.0 >= static_cast(d_rtcm_MT1077_rate_ms)) and (d_rtcm_MT1077_rate_ms != 0)) { 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)) + if ((std::fabs(current_RX_time - last_RTCM_1087_output_time) * 1000.0 >= static_cast(d_rtcm_MT1087_rate_ms)) and (d_rtcm_MT1087_rate_ms != 0)) { 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)) + if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast(d_rtcm_MT1097_rate_ms)) and (d_rtcm_MT1097_rate_ms != 0)) { last_RTCM_1097_output_time = current_RX_time; } - if ((std::fabs(current_RX_time - last_RTCM_MSM_output_time) * 1000.0 >= static_cast(d_rtcm_MSM_rate_ms)) && (d_rtcm_MSM_rate_ms != 0)) + if ((std::fabs(current_RX_time - last_RTCM_MSM_output_time) * 1000.0 >= static_cast(d_rtcm_MSM_rate_ms)) and (d_rtcm_MSM_rate_ms != 0)) { flag_write_RTCM_MSM_output = true; last_RTCM_MSM_output_time = current_RX_time; @@ -789,7 +790,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); @@ -799,7 +800,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string gal_signal("1B"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); @@ -809,7 +810,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 10) // GPS L1 C/A + Galileo E5a { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string gal_signal("5X"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); @@ -819,7 +820,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 11) // GPS L1 C/A + Galileo E5b { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string gal_signal("7X"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); @@ -880,7 +881,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string glo_signal("1G"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); @@ -896,7 +897,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { std::string glo_signal("1G"); std::string gal_signal("1B"); @@ -907,7 +908,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 28) // GPS L2C + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { std::string glo_signal("1G"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); @@ -969,7 +970,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6)) // Galileo + if ((type_of_rx == 4) or (type_of_rx == 5) or (type_of_rx == 6)) // Galileo { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); } @@ -977,15 +978,15 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); } - if ((type_of_rx == 9) || (type_of_rx == 10) || (type_of_rx == 11)) // GPS L1 C/A + Galileo + if ((type_of_rx == 9) or (type_of_rx == 10) or (type_of_rx == 11)) // GPS L1 C/A + Galileo { rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); } - if ((type_of_rx == 14) || (type_of_rx == 15)) // Galileo E1B + Galileo E5a + if ((type_of_rx == 14) or (type_of_rx == 15)) // Galileo E1B + Galileo E5a { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS L1 C/A, GLONASS L2 C/A + if ((type_of_rx == 23) or (type_of_rx == 24) or (type_of_rx == 25)) // GLONASS L1 C/A, GLONASS L2 C/A { rp->log_rinex_nav(rp->navGloFile, d_ls_pvt->glonass_gnav_ephemeris_map); } @@ -1040,7 +1041,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); @@ -1053,7 +1054,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); @@ -1066,7 +1067,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); @@ -1079,7 +1080,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1092,7 +1093,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1105,7 +1106,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1114,11 +1115,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); @@ -1127,11 +1128,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); @@ -1144,7 +1145,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1157,7 +1158,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1170,7 +1171,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C"); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) { rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); @@ -1183,7 +1184,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "2C"); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) { rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); @@ -1196,7 +1197,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C 2C"); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) { rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); @@ -1205,11 +1206,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -1218,11 +1219,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -1231,11 +1232,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 28) // GPS L2C + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -2064,9 +2065,9 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // DEBUG MESSAGE: Display position in console output if (d_ls_pvt->is_valid_position() and flag_display_pvt) { - std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) + std::cout << TEXT_BOLD_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() - << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << std::endl; + << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl; LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index a501b66ad..4dcee81a0 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -132,7 +132,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); - gnss_observables_iter++) + gnss_observables_iter++) //CHECK INCONSISTENCY when combining GLONASS + other system { switch (gnss_observables_iter->second.System) { @@ -241,6 +241,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); if (gps_ephemeris_iter != gps_ephemeris_map.cend()) { + /* By the moment, GPS L2 observables are not used in pseudorange computations if GPS L1 is available // 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris // (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure for (int i = 0; i < valid_obs; i++) @@ -250,11 +251,12 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs], gnss_observables_iter->second, - gps_cnav_ephemeris_iter->second.i_GPS_week, + eph_data[i].week, 1); //Band 2 (L2) break; } } + */ } else { @@ -404,7 +406,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ // ********************************************************************** this->set_valid_position(false); - if (valid_obs > 0 || glo_valid_obs > 0) + if ((valid_obs + glo_valid_obs) > 3) { int result = 0; nav_t nav_data; @@ -495,5 +497,5 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ } } } - return this->is_valid_position(); + return is_valid_position(); } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index fd1c8fe91..bfe53f6af 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -1,12 +1,12 @@ /*! * \file gnss_sdr_sample_counter.cc * \brief Simple block to report the current receiver time based on the output of the tracking or telemetry blocks - * \author Javier Arribas 2017. jarribas(at)cttc.es + * \author Javier Arribas 2018. jarribas(at)cttc.es * * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -32,40 +32,101 @@ #include "gnss_sdr_sample_counter.h" #include "gnss_synchro.h" #include +#include +#include +#include -gnss_sdr_sample_counter::gnss_sdr_sample_counter() : gr::sync_block("sample_counter", - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), - gr::io_signature::make(0, 0, 0)) +gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr::sync_decimator("sample_counter", + gr::io_signature::make(1, 1, _size), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + static_cast(std::floor(_fs * 0.001))) { - this->message_port_register_out(pmt::mp("sample_counter")); - last_T_rx_s = 0; - report_interval_s = 1; //default reporting 1 second - flag_enable_send_msg = false; //enable it for reporting time with asynchronous message + message_port_register_out(pmt::mp("sample_counter")); + set_max_noutput_items(1); + current_T_rx_ms = 0; + current_s = 0; + current_m = 0; + current_h = 0; + current_days = 0; + report_interval_ms = 1000; // default reporting 1 second + flag_enable_send_msg = false; // enable it for reporting time with asynchronous message + flag_m = false; + flag_h = false; + flag_days = false; } -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter() +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size) { - gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter()); + gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _size)); return sample_counter_; } -int gnss_sdr_sample_counter::work(int noutput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items __attribute__((unused))) +int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), + gr_vector_const_void_star &input_items __attribute__((unused)), + gr_vector_void_star &output_items) { - const Gnss_Synchro *in = reinterpret_cast(input_items[0]); // input - - double current_T_rx_s = in[noutput_items - 1].Tracking_sample_counter / static_cast(in[noutput_items - 1].fs); - if ((current_T_rx_s - last_T_rx_s) > report_interval_s) + Gnss_Synchro *out = reinterpret_cast(output_items[0]); + out[0] = Gnss_Synchro(); + if ((current_T_rx_ms % report_interval_ms) == 0) { - std::cout << "Current receiver time: " << floor(current_T_rx_s) << " [s]" << std::endl; - if (flag_enable_send_msg == true) + current_s++; + if ((current_s % 60) == 0) { - this->message_port_pub(pmt::mp("receiver_time"), pmt::from_double(current_T_rx_s)); + current_s = 0; + current_m++; + flag_m = true; + if ((current_m % 60) == 0) + { + current_m = 0; + current_h++; + flag_h = true; + if ((current_h % 24) == 0) + { + current_h = 0; + current_days++; + flag_days = true; + } + } + } + + if (flag_days) + { + std::string day; + if (current_days == 1) + { + day = " day "; + } + else + { + day = " days "; + } + std::cout << "Current receiver time: " << current_days << day << current_h << " h " << current_m << " min " << current_s << " s" << std::endl; + } + else + { + if (flag_h) + { + std::cout << "Current receiver time: " << current_h << " h " << current_m << " min " << current_s << " s" << std::endl; + } + else + { + if (flag_m) + { + std::cout << "Current receiver time: " << current_m << " min " << current_s << " s" << std::endl; + } + else + { + std::cout << "Current receiver time: " << current_s << " s" << std::endl; + } + } + } + if (flag_enable_send_msg) + { + message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); } - last_T_rx_s = current_T_rx_s; } - return noutput_items; + current_T_rx_ms++; + return 1; } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index afb56ce49..761894855 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -1,12 +1,12 @@ /*! * \file gnss_sdr_sample_counter.h * \brief Simple block to report the current receiver time based on the output of the tracking or telemetry blocks - * \author Javier Arribas 2017. jarribas(at)cttc.es + * \author Javier Arribas 2018. jarribas(at)cttc.es * * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -28,10 +28,10 @@ * * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_sample_counter_H_ -#define GNSS_SDR_sample_counter_H_ +#ifndef GNSS_SDR_SAMPLE_COUNTER_H_ +#define GNSS_SDR_SAMPLE_COUNTER_H_ -#include +#include #include @@ -39,20 +39,28 @@ class gnss_sdr_sample_counter; typedef boost::shared_ptr gnss_sdr_sample_counter_sptr; -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(); +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size); -class gnss_sdr_sample_counter : public gr::sync_block +class gnss_sdr_sample_counter : public gr::sync_decimator { - friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(); - gnss_sdr_sample_counter(); - double last_T_rx_s; - double report_interval_s; +private: + gnss_sdr_sample_counter(double _fs, size_t _size); + long long int current_T_rx_ms; // Receiver time in ms since the beginning of the run + unsigned int current_s; // Receiver time in seconds, modulo 60 + bool flag_m; // True if the receiver has been running for at least 1 minute + unsigned int current_m; // Receiver time in minutes, modulo 60 + bool flag_h; // True if the receiver has been running for at least 1 hour + unsigned int current_h; // Receiver time in hours, modulo 24 + bool flag_days; // True if the receiver has been running for at least 1 day + unsigned int current_days; // Receiver time in days since the beginning of the run + int report_interval_ms; bool flag_enable_send_msg; public: + friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); }; -#endif /*GNSS_SDR_sample_counter_H_*/ +#endif /*GNSS_SDR_SAMPLE_COUNTER_H_*/ diff --git a/src/algorithms/libs/pass_through.h b/src/algorithms/libs/pass_through.h index 1a9538ae4..f1d8c5f72 100644 --- a/src/algorithms/libs/pass_through.h +++ b/src/algorithms/libs/pass_through.h @@ -88,7 +88,6 @@ private: std::string role_; unsigned int in_streams_; unsigned int out_streams_; - //gr_kludge_copy_sptr kludge_copy_; gr::blocks::copy::sptr kludge_copy_; size_t item_size_; conjugate_cc_sptr conjugate_cc_; diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 93a5be2a9..88e8969e1 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -452,27 +452,27 @@ typedef struct } alm_t; -typedef struct -{ /* GPS/QZS/GAL broadcast ephemeris type */ - int sat; /* satellite number */ - int iode, iodc; /* IODE,IODC */ - int sva; /* SV accuracy (URA index) */ - int svh; /* SV health (0:ok) */ - int week; /* GPS/QZS: gps week, GAL: galileo week */ - int code; /* GPS/QZS: code on L2, GAL/BDS: data sources */ - int flag; /* GPS/QZS: L2 P data flag, BDS: nav type */ - gtime_t toe, toc, ttr; /* Toe,Toc,T_trans */ - /* SV orbit parameters */ - double A, e, i0, OMG0, omg, M0, deln, OMGd, idot; - double crc, crs, cuc, cus, cic, cis; - double toes; /* Toe (s) in week */ - double fit; /* fit interval (h) */ - double f0, f1, f2; /* SV clock parameters (af0,af1,af2) */ - double tgd[4]; /* group delay parameters */ - /* GPS/QZS:tgd[0]=TGD */ - /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ - /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ - double Adot, ndot; /* Adot,ndot for CNAV */ +typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ + int sat; /* satellite number */ + int iode,iodc; /* IODE,IODC */ + int sva; /* SV accuracy (URA index) */ + int svh; /* SV health (0:ok) */ + int week; /* GPS/QZS: gps week, GAL: galileo week */ + int code; /* GPS/QZS: code on L2, GAL/BDS: data sources */ + int flag; /* GPS/QZS: L2 P data flag, BDS: nav type */ + gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */ + /* SV orbit parameters */ + double A,e,i0,OMG0,omg,M0,deln,OMGd,idot; + double crc,crs,cuc,cus,cic,cis; + double toes; /* Toe (s) in week */ + double fit; /* fit interval (h) */ + double f0,f1,f2; /* SV clock parameters (af0,af1,af2) */ + double tgd[4]; /* group delay parameters */ + /* GPS/QZS:tgd[0]=TGD */ + /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ + /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ + double isc[4]; /* GPS :isc[0]=ISCL1, isc[1]=ISCL2, isc[2]=ISCL5I, isc[3]=ISCL5Q */ + double Adot,ndot; /* Adot,ndot for CNAV */ } eph_t; diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 730fe2814..2ff6a575c 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -116,7 +116,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glona eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO; rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; @@ -174,7 +174,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; rtklib_sat.sat = gps_eph.i_satellite_PRN; rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; rtklib_sat.M0 = gps_eph.d_M_0; @@ -199,9 +199,9 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) rtklib_sat.f1 = gps_eph.d_A_f1; rtklib_sat.f2 = gps_eph.d_A_f2; rtklib_sat.tgd[0] = gps_eph.d_TGD; - rtklib_sat.tgd[1] = 0; - rtklib_sat.tgd[2] = 0; - rtklib_sat.tgd[3] = 0; + rtklib_sat.tgd[1] = 0.0; + rtklib_sat.tgd[2] = 0.0; + rtklib_sat.tgd[3] = 0.0; rtklib_sat.toes = gps_eph.d_Toe; rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_eph.d_Toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, gps_eph.d_TOW); @@ -231,7 +231,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; @@ -260,9 +260,13 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) rtklib_sat.f1 = gps_cnav_eph.d_A_f1; rtklib_sat.f2 = gps_cnav_eph.d_A_f2; rtklib_sat.tgd[0] = gps_cnav_eph.d_TGD; - rtklib_sat.tgd[1] = 0; - rtklib_sat.tgd[2] = 0; - rtklib_sat.tgd[3] = 0; + rtklib_sat.tgd[1] = 0.0; + rtklib_sat.tgd[2] = 0.0; + rtklib_sat.tgd[3] = 0.0; + rtklib_sat.isc[0] = gps_cnav_eph.d_ISCL1; + rtklib_sat.isc[1] = gps_cnav_eph.d_ISCL2; + rtklib_sat.isc[2] = gps_cnav_eph.d_ISCL5I; + rtklib_sat.isc[3] = gps_cnav_eph.d_ISCL5Q; rtklib_sat.toes = gps_cnav_eph.d_Toe1; rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_cnav_eph.d_Toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, gps_cnav_eph.d_TOW); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 3ef5c7185..31b60c8e6 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -78,18 +78,70 @@ double gettgd(int sat, const nav_t *nav) return 0.0; } +/* get isc parameter (m) -----------------------------------------------------*/ +double getiscl1(int sat, const nav_t *nav) +{ + for (int i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[0]; + } + return 0.0; +} + +double getiscl2(int sat, const nav_t *nav) +{ + for (int i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[1]; + } + return 0.0; +} + +double getiscl5i(int sat, const nav_t *nav) +{ + for (int i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[2]; + } + return 0.0; +} + +double getiscl5q(int sat, const nav_t *nav) +{ + for (int i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[3]; + } + return 0.0; +} /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var) { const double *lam = nav->lam[obs->sat - 1]; - double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma_; - int i = 0, j = 1, sys; - + double PC = 0.0; + double P1 = 0.0; + double P2 = 0.0; + double P1_P2 = 0.0; + double P1_C1 = 0.0; + double P2_C2 = 0.0; + //Intersignal corrections (m). See GPS IS-200 CNAV message + double ISCl1 = 0.0; + double ISCl2 = 0.0; + double ISCl5i = 0.0; + double ISCl5q = 0.0; + double gamma_ = 0.0; + int i = 0; + int j = 1; + int sys = satsys(obs->sat, NULL); *var = 0.0; - if (!(sys = satsys(obs->sat, NULL))) + if (sys == SYS_NONE) { trace(4, "prange: satsys NULL\n"); return 0.0; @@ -97,12 +149,11 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if (sys & (SYS_GAL | SYS_SBS)) + if (sys == SYS_GAL or sys == SYS_SBS) { j = 2; } - - if (sys == SYS_GPS) + else if (sys == SYS_GPS or sys == SYS_GLO) { if (obs->code[1] != CODE_NONE) { @@ -114,7 +165,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } } - if (NFREQ < 2 || lam[i] == 0.0 || lam[j] == 0.0) + if (lam[i] == 0.0 or lam[j] == 0.0) { trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); @@ -139,7 +190,11 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } } } - gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ + /* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */ + if (sys == SYS_GPS or sys == SYS_GAL or sys == SYS_GLO) + { + gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); + } P1 = obs->P[i]; P2 = obs->P[j]; P1_P2 = nav->cbias[obs->sat - 1][0]; @@ -147,10 +202,20 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P2_C2 = nav->cbias[obs->sat - 1][2]; /* if no P1-P2 DCB, use TGD instead */ - if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) //CHECK! + if (P1_P2 == 0.0) { - P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav); + P1_P2 = gettgd(obs->sat, nav); } + + if (sys == SYS_GPS) + { + ISCl1 = getiscl1(obs->sat, nav); + ISCl2 = getiscl2(obs->sat, nav); + ISCl5i = getiscl5i(obs->sat, nav); + ISCl5q = getiscl5q(obs->sat, nav); + } + + //CHECK IF IT IS STILL NEEDED if (opt->ionoopt == IONOOPT_IFLC) { /* dual-frequency */ @@ -170,25 +235,59 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* iono-free combination */ PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); } + //////////////////////////////////////////// else { /* single-frequency */ - if ((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) + if (obs->code[i] == CODE_NONE and obs->code[j] == CODE_NONE) { return 0.0; } - else if ((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE)) + else if (obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) { P1 += P1_C1; /* C1->P1 */ - PC = P1 - P1_P2 / (1.0 - gamma_); + PC = P1 + P1_P2; } - else if ((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) + else if (obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) { - P2 += P2_C2; /* C2->P2 */ - PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + if (sys == SYS_GPS) + { + P2 += P2_C2; /* C2->P2 */ + //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + if (obs->code[j] == CODE_L2S) //L2 single freq. + { + PC = P2 + P1_P2 - ISCl2; + } + else if (obs->code[j] == CODE_L5X) //L5 single freq. + { + PC = P2 + P1_P2 - ISCl5i; + } + } + else if (sys == SYS_GAL or sys == SYS_GLO) // Gal. E5a single freq. + { + P2 += P2_C2; /* C2->P2 */ + PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + } } /* dual-frequency */ - else + else if (sys == SYS_GPS) + { + if (obs->code[j] == CODE_L2S) /* L1 + L2 */ + { + //By the moment, GPS L2 pseudoranges are not used + //PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; + P1 += P1_C1; /* C1->P1 */ + PC = P1 + P1_P2; + } + else if (obs->code[j] == CODE_L5X) /* L1 + L5 */ + { + //By the moment, GPS L5 pseudoranges are not used + //PC = (P2 + ISCl5i - gamma_ * (P1 + ISCl5i)) / (1.0 - gamma_) - P1_P2; + P1 += P1_C1; /* C1->P1 */ + PC = P1 + P1_P2; + } + } + else if (sys == SYS_GAL or sys == SYS_GLO) /* E1 + E5a */ { P1 += P1_C1; P2 += P2_C2; @@ -199,9 +298,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { PC -= P1_C1; } /* sbas clock based C1 */ - *var = std::pow(ERR_CBIAS, 2.0); - return PC; } diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 596d6b443..13becebef 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -69,6 +69,12 @@ double varerr(const prcopt_t *opt, double el, int sys); /* get tgd parameter (m) -----------------------------------------------------*/ double gettgd(int sat, const nav_t *nav); +/* get isc parameter (m) -----------------------------------------------------*/ +double getiscl1(int sat, const nav_t *nav); +double getiscl2(int sat, const nav_t *nav); +double getiscl5i(int sat, const nav_t *nav); +double getiscl5q(int sat, const nav_t *nav); + /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var); diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm.cc b/src/algorithms/libs/rtklib/rtklib_rtcm.cc index 40ec2f543..e122ebead 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm.cc @@ -69,7 +69,7 @@ int init_rtcm(rtcm_t *rtcm) obsd_t data0 = {{0, 0.0}, 0, 0, {0}, {0}, {0}, {0.0}, {0.0}, {0.0}}; eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0}; ssr_t ssr0 = {{{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0'}; diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm2.cc b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc index a754db2e0..c7af88ccd 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm2.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc @@ -219,7 +219,7 @@ int decode_type17(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; int i = 48, week, prn, sat; diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm3.cc b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc index d8fb3c334..babc88f5f 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm3.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc @@ -846,7 +846,7 @@ int decode_type1019(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, sys = SYS_GPS; @@ -1293,7 +1293,7 @@ int decode_type1044(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, sys = SYS_QZS; @@ -1398,7 +1398,7 @@ int decode_type1045(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, e5a_hs, e5a_dvs, sys = SYS_GAL; @@ -1502,7 +1502,7 @@ int decode_type1046(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, e5a_hs, e5a_dvs, sys = SYS_GAL; @@ -1606,7 +1606,7 @@ int decode_type1047(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; ; double toc, sqrtA; char *msg; @@ -1716,7 +1716,7 @@ int decode_type63(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, sys = SYS_BDS; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index bf4c7bf8a..20f939dd4 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -253,12 +253,11 @@ const unsigned int tbl_CRC24Q[] = { 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538}; -extern "C" -{ - void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); - extern void dgetrf_(int *, int *, double *, int *, int *, int *); - extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); - extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); +extern "C" { +void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); +extern void dgetrf_(int *, int *, double *, int *, int *, int *); +extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); +extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); } @@ -1388,10 +1387,10 @@ double time2gpst(gtime_t t, int *week) { gtime_t t0 = epoch2time(gpst0); time_t sec = t.time - t0.time; - int w = (int)(sec / (86400 * 7)); + int w = static_cast(sec / 604800); if (week) *week = w; - return (double)(sec - (double)w * 86400 * 7) + t.sec; + return (static_cast(sec - static_cast(w * 604800)) + t.sec); } @@ -2993,7 +2992,7 @@ int readnav(const char *file, nav_t *nav) { FILE *fp; eph_t eph0 = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; geph_t geph0 = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {}, {}, {}, 0.0, 0.0, 0.0}; char buff[4096], *p; long toe_time, tof_time, toc_time, ttr_time; diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index c835ac688..6f27b9ca6 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -592,7 +592,7 @@ int rtksvrinit(rtksvr_t *svr) '0', '0', '0', 0, 0, 0}; eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0}; seph_t seph0 = {0, {0, 0.0}, {0, 0.0}, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, 0.0}; diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index 0bed0b89b..9a3b65367 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -32,40 +32,27 @@ #include "hybrid_observables.h" #include "configuration_interface.h" -#include "Galileo_E1.h" -#include "GPS_L1_CA.h" #include using google::LogMessage; HybridObservables::HybridObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : role_(role), - in_streams_(in_streams), - out_streams_(out_streams) + std::string role, unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), out_streams_(out_streams) { std::string default_dump_filename = "./observables.dat"; DLOG(INFO) << "role " << role; dump_ = configuration->property(role + ".dump", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - unsigned int default_depth = 0; - if (GPS_L1_CA_HISTORY_DEEP == GALILEO_E1_HISTORY_DEEP) - { - default_depth = GPS_L1_CA_HISTORY_DEEP; - } - else - { - default_depth = 500; - } - unsigned int history_deep = configuration->property(role + ".history_depth", default_depth); - observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); - DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; + + observables_ = hybrid_make_observables_cc(in_streams_, out_streams_, dump_, dump_filename_); + DLOG(INFO) << "Observables block ID (" << observables_->unique_id() << ")"; } -HybridObservables::~HybridObservables() {} +HybridObservables::~HybridObservables() +{} void HybridObservables::connect(gr::top_block_sptr top_block) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 60273fcbd..6e95da824 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -1,11 +1,12 @@ /*! * \file hybrid_observables_cc.cc - * \brief Implementation of the pseudorange computation block for Galileo E1 + * \brief Implementation of the observables computation block * \author Javier Arribas 2017. jarribas(at)cttc.es + * \author Antonio Ramos 2018. antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -29,50 +30,52 @@ */ #include "hybrid_observables_cc.h" -#include "Galileo_E1.h" +#include "display.h" #include "GPS_L1_CA.h" #include #include #include -#include -#include #include #include #include #include -#include -#include -#include +#include using google::LogMessage; -hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) +hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename) { - return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels, dump, dump_filename, deep_history)); + return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels_in, nchannels_out, dump, dump_filename)); } -hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) : gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) +hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, + unsigned int nchannels_out, + bool dump, + std::string dump_filename) : gr::block("hybrid_observables_cc", + gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) { - // initialize internal vars d_dump = dump; - d_nchannels = nchannels; + d_nchannels = nchannels_out; d_dump_filename = dump_filename; - history_deep = deep_history; T_rx_s = 0.0; - T_rx_step_s = 1e-3; // todo: move to gnss-sdr config + T_rx_step_s = 0.001; // 1 ms + max_delta = 0.15; // 150 ms + valid_channels.resize(d_nchannels, false); + d_num_valid_channels = 0; + for (unsigned int i = 0; i < d_nchannels; i++) { - d_gnss_synchro_history_queue.push_back(std::deque()); + d_gnss_synchro_history.push_back(std::deque()); } // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) + if (d_dump) { - if (d_dump_file.is_open() == false) + if (!d_dump_file.is_open()) { try { @@ -83,6 +86,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, catch (const std::ifstream::failure &e) { LOG(WARNING) << "Exception opening observables dump file " << e.what(); + d_dump = false; } } } @@ -91,7 +95,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, hybrid_observables_cc::~hybrid_observables_cc() { - if (d_dump_file.is_open() == true) + if (d_dump_file.is_open()) { try { @@ -102,10 +106,10 @@ hybrid_observables_cc::~hybrid_observables_cc() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } - if (d_dump == true) + if (d_dump) { std::cout << "Writing observables .mat files ..."; - hybrid_observables_cc::save_matfile(); + save_matfile(); std::cout << " done." << std::endl; } } @@ -230,7 +234,10 @@ int hybrid_observables_cc::save_matfile() mat_t *matfp; matvar_t *matvar; std::string filename = d_dump_filename; - filename.erase(filename.length() - 4, 4); + if (filename.size() > 4) + { + filename.erase(filename.end() - 4, filename.end()); + } filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if (reinterpret_cast(matfp) != NULL) @@ -295,303 +302,294 @@ int hybrid_observables_cc::save_matfile() } -bool Hybrid_pairCompare_gnss_synchro_sample_counter(const std::pair &a, const std::pair &b) +bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, std::deque &data, const double &ti) { - return (a.second.Tracking_sample_counter) < (b.second.Tracking_sample_counter); + if ((ti < data.front().RX_time) or (ti > data.back().RX_time)) + { + return false; + } + std::deque::iterator it; + + arma::vec t = arma::vec(data.size()); + arma::vec dop = t; + arma::vec cph = t; + arma::vec tow = t; + arma::vec tiv = arma::vec(1); + arma::vec result; + tiv(0) = ti; + + unsigned int aux = 0; + for (it = data.begin(); it != data.end(); it++) + { + t(aux) = it->RX_time; + dop(aux) = it->Carrier_Doppler_hz; + cph(aux) = it->Carrier_phase_rads; + tow(aux) = it->TOW_at_current_symbol_s; + + aux++; + } + arma::interp1(t, dop, tiv, result); + out.Carrier_Doppler_hz = result(0); + arma::interp1(t, cph, tiv, result); + out.Carrier_phase_rads = result(0); + arma::interp1(t, tow, tiv, result); + out.TOW_at_current_symbol_s = result(0); + + return result.is_finite(); } -bool Hybrid_valueCompare_gnss_synchro_sample_counter(const Gnss_Synchro &a, unsigned long int b) +double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro &a) { - return (a.Tracking_sample_counter) < (b); + if (a.Flag_valid_word) + { + return ((static_cast(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast(a.fs)); + } + else + { + return 0.0; + } } -bool Hybrid_valueCompare_gnss_synchro_receiver_time(const Gnss_Synchro &a, double b) +void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), + gr_vector_int &ninput_items_required) { - return ((static_cast(a.Tracking_sample_counter) + static_cast(a.Code_phase_samples)) / static_cast(a.fs)) < (b); -} - - -bool Hybrid_pairCompare_gnss_synchro_d_TOW(const std::pair &a, const std::pair &b) -{ - return (a.second.TOW_at_current_symbol_s) < (b.second.TOW_at_current_symbol_s); -} - - -bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro &a, double b) -{ - return (a.TOW_at_current_symbol_s) < (b); -} - - -void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) -{ - bool zero_samples = true; for (unsigned int i = 0; i < d_nchannels; i++) { - int items = detail()->input(i)->items_available(); - if (items > 0) zero_samples = false; - ninput_items_required[i] = items; // set the required available samples in each call + ninput_items_required[i] = 0; } + ninput_items_required[d_nchannels] = 1; +} - if (zero_samples == true) + +void hybrid_observables_cc::clean_history(std::deque &data) +{ + while (data.size() > 0) { - for (unsigned int i = 0; i < d_nchannels; i++) + if ((T_rx_s - data.front().RX_time) > max_delta) { - ninput_items_required[i] = 1; // set the required available samples in each call + data.pop_front(); + } + else + { + return; } } } -int hybrid_observables_cc::general_work(int noutput_items, - gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, +void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector &data) +{ + std::vector::iterator it; + +/////////////////////// DEBUG ////////////////////////// +// Logs if there is a pseudorange difference between +// signals of the same satellite higher than a threshold +//////////////////////////////////////////////////////// +#ifndef NDEBUG + std::vector::iterator it2; + double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters + for (it = data.begin(); it != (data.end() - 1); it++) + { + for (it2 = it + 1; it2 != data.end(); it2++) + { + if (it->PRN == it2->PRN and it->System == it2->System) + { + double tow_dif_ = std::fabs(it->TOW_at_current_symbol_s - it2->TOW_at_current_symbol_s); + if (tow_dif_ > thr_) + { + DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal + << ". TOW difference in PRN " << it->PRN + << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT + << " meters in pseudorange"; + std::cout << TEXT_RED << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal + << ". TOW difference in PRN " << it->PRN + << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT + << " meters in pseudorange" << TEXT_RESET << std::endl; + } + } + } + } +#endif + /////////////////////////////////////////////////////////// + + double TOW_ref = std::numeric_limits::lowest(); + for (it = data.begin(); it != data.end(); it++) + { + if (it->TOW_at_current_symbol_s > TOW_ref) + { + TOW_ref = it->TOW_at_current_symbol_s; + } + } + for (it = data.begin(); it != data.end(); it++) + { + double traveltime_s = TOW_ref - it->TOW_at_current_symbol_s + GPS_STARTOFFSET_ms / 1000.0; + it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0; + it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; + } +} + + +int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)), + gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // Get the output buffer pointer - int n_outputs = 0; - int n_consume[d_nchannels]; - double past_history_s = 100e-3; + const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - Gnss_Synchro current_gnss_synchro[d_nchannels]; - Gnss_Synchro aux = Gnss_Synchro(); - for (unsigned int i = 0; i < d_nchannels; i++) + unsigned int i; + int total_input_items = 0; + for (i = 0; i < d_nchannels; i++) { - current_gnss_synchro[i] = aux; + total_input_items += ninput_items[i]; } - /* - * 1. Read the GNSS SYNCHRO objects from available channels. - * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel - * Record all synchronization data into queues - */ - for (unsigned int i = 0; i < d_nchannels; i++) + consume(d_nchannels, 1); + T_rx_s += T_rx_step_s; + + ////////////////////////////////////////////////////////////////////////// + if ((total_input_items == 0) and (d_num_valid_channels == 0)) { - n_consume[i] = ninput_items[i]; // full throttle - for (int j = 0; j < n_consume[i]; j++) - { - d_gnss_synchro_history_queue[i].push_back(in[i][j]); - } + return 0; } + ////////////////////////////////////////////////////////////////////////// - bool channel_history_ok; - do + std::vector>::iterator it; + if (total_input_items > 0) { - channel_history_ok = true; - for (unsigned int i = 0; i < d_nchannels; i++) + i = 0; + for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - if (d_gnss_synchro_history_queue[i].size() < history_deep) + if (ninput_items[i] > 0) { - channel_history_ok = false; - } - } - if (channel_history_ok == true) - { - std::map::const_iterator gnss_synchro_map_iter; - std::deque::const_iterator gnss_synchro_deque_iter; - - // 1. If the RX time is not set, set the Rx time - if (T_rx_s == 0) - { - // 0. Read a gnss_synchro snapshot from the queue and store it in a map - std::map gnss_synchro_map; - for (unsigned int i = 0; i < d_nchannels; i++) + // Add the new Gnss_Synchros to their corresponding deque + for (int aux = 0; aux < ninput_items[i]; aux++) { - gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].front().Channel_ID, - d_gnss_synchro_history_queue[i].front())); - } - gnss_synchro_map_iter = min_element(gnss_synchro_map.cbegin(), - gnss_synchro_map.cend(), - Hybrid_pairCompare_gnss_synchro_sample_counter); - T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / static_cast(gnss_synchro_map_iter->second.fs); - T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms - T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate - } - - // 2. Realign RX time in all valid channels - std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx - std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate - // shift channels history to match the reference TOW - for (unsigned int i = 0; i < d_nchannels; i++) - { - gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), - d_gnss_synchro_history_queue[i].cend(), - T_rx_s, - Hybrid_valueCompare_gnss_synchro_receiver_time); - if (gnss_synchro_deque_iter != d_gnss_synchro_history_queue[i].cend()) - { - if (gnss_synchro_deque_iter->Flag_valid_word == true) + if (in[i][aux].Flag_valid_word) { - double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s = T_rx_channel - T_rx_s; - - // check that T_rx difference is less than a threshold (the correlation interval) - if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) + it->push_back(in[i][aux]); + it->back().RX_time = compute_T_rx_s(in[i][aux]); + // Check if the last Gnss_Synchro comes from the same satellite as the previous ones + if (it->size() > 1) { - // record the word structure in a map for pseudorange computation - // save the previous observable - int distance = std::distance(d_gnss_synchro_history_queue[i].cbegin(), gnss_synchro_deque_iter); - if (distance > 0) + if (it->front().PRN != it->back().PRN) { - if (d_gnss_synchro_history_queue[i].at(distance - 1).Flag_valid_word) - { - double T_rx_channel_prev = static_cast(d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; - if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) - { - realigned_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); - adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - adjacent_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); - } - } - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + it->clear(); } } } } + consume(i, ninput_items[i]); } + i++; + } + } + for (i = 0; i < d_nchannels; i++) + { + if (d_gnss_synchro_history.at(i).size() > 2) + { + valid_channels[i] = true; + } + else + { + valid_channels[i] = false; + } + } + d_num_valid_channels = valid_channels.count(); + // Check if there is any valid channel after reading the new incoming Gnss_Synchro data + if (d_num_valid_channels == 0) + { + return 0; + } - if (!realigned_gnss_synchro_map.empty()) + for (i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold + { + if (valid_channels[i]) + { + clean_history(d_gnss_synchro_history.at(i)); + if (d_gnss_synchro_history.at(i).size() < 2) { - /* - * 2.1 Use CURRENT set of measurements and find the nearest satellite - * common RX time algorithm - */ - // what is the most recent symbol TOW in the current set? -> this will be the reference symbol - gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.cbegin(), - realigned_gnss_synchro_map.cend(), - Hybrid_pairCompare_gnss_synchro_d_TOW); - double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - - // compute interpolated TOW value at T_rx_s - int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; - Gnss_Synchro adj_obs; - adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); - double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; - - double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; - - double selected_T_rx_s = T_rx_s; - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + - (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); - - // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double channel_T_rx_s; - double channel_fs_hz; - double channel_TOW_s; - for (gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) - { - channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; - // compute interpolated observation values - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - // TOW at the selected receiver time T_rx_s - int element_key = gnss_synchro_map_iter->second.Channel_ID; - try - { - adj_obs = adjacent_gnss_synchro_map.at(element_key); - } - catch (const std::exception &ex) - { - continue; - } - - double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; - - double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); - - // Doppler and Accumulated carrier phase - double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); - double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); - - // compute the pseudorange (no rx time offset correction) - traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; - // convert to meters - pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] - // update the pseudorange object - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; - // Save the estimated RX time (no RX clock offset correction yet!) - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; - - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; - } - - if (d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) - { - tmp_double = current_gnss_synchro[i].RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Pseudorange_m; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].PRN; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } - - for (unsigned int i = 0; i < d_nchannels; i++) - { - out[i][n_outputs] = current_gnss_synchro[i]; - } - - n_outputs++; - } - - // Move RX time - T_rx_s = T_rx_s + T_rx_step_s; - // pop old elements from queue - for (unsigned int i = 0; i < d_nchannels; i++) - { - while (static_cast(d_gnss_synchro_history_queue[i].front().Tracking_sample_counter) / static_cast(d_gnss_synchro_history_queue[i].front().fs) < (T_rx_s - past_history_s)) - { - d_gnss_synchro_history_queue[i].pop_front(); - } + valid_channels[i] = false; } } } - while (channel_history_ok == true && noutput_items > n_outputs); - // Multi-rate consume! - for (unsigned int i = 0; i < d_nchannels; i++) + // Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time + d_num_valid_channels = valid_channels.count(); + double T_rx_s_out = T_rx_s - (max_delta / 2.0); + if ((d_num_valid_channels == 0) or (T_rx_s_out < 0.0)) { - consume(i, n_consume[i]); // which input, how many items + return 0; } - return n_outputs; + std::vector epoch_data; + i = 0; + for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) + { + if (valid_channels[i]) + { + Gnss_Synchro interpolated_gnss_synchro = it->back(); + if (interpolate_data(interpolated_gnss_synchro, *it, T_rx_s_out)) + { + epoch_data.push_back(interpolated_gnss_synchro); + } + else + { + valid_channels[i] = false; + } + } + i++; + } + d_num_valid_channels = valid_channels.count(); + if (d_num_valid_channels == 0) + { + return 0; + } + correct_TOW_and_compute_prange(epoch_data); + std::vector::iterator it2 = epoch_data.begin(); + for (i = 0; i < d_nchannels; i++) + { + if (valid_channels[i]) + { + out[i][0] = (*it2); + out[i][0].Flag_valid_pseudorange = true; + it2++; + } + else + { + out[i][0] = Gnss_Synchro(); + out[i][0].Flag_valid_pseudorange = false; + } + } + if (d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + for (i = 0; i < d_nchannels; i++) + { + tmp_double = out[i][0].RX_time; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].TOW_at_current_symbol_s; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Carrier_Doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Pseudorange_m; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(out[i][0].PRN); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(out[i][0].Flag_valid_pseudorange); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + d_dump = false; + } + } + return 1; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index 33372b689..aedba43ab 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -1,12 +1,13 @@ /*! * \file hybrid_observables_cc.h - * \brief Interface of the observables computation block for Galileo E1 + * \brief Interface of the observables computation block * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com * \author Javier Arribas 2013. jarribas(at)cttc.es + * \author Antonio Ramos 2018. antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -35,8 +36,11 @@ #include "gnss_synchro.h" #include +#include #include #include +#include +#include class hybrid_observables_cc; @@ -44,36 +48,40 @@ class hybrid_observables_cc; typedef boost::shared_ptr hybrid_observables_cc_sptr; hybrid_observables_cc_sptr -hybrid_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename, unsigned int deep_history); +hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); /*! - * \brief This class implements a block that computes Galileo observables + * \brief This class implements a block that computes observables */ class hybrid_observables_cc : public gr::block { public: ~hybrid_observables_cc(); - int general_work(int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - void forecast(int noutput_items, gr_vector_int &ninput_items_required); + int general_work(int noutput_items, gr_vector_int& ninput_items, + gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + void forecast(int noutput_items, gr_vector_int& ninput_items_required); private: friend hybrid_observables_cc_sptr - hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); + hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); + hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); + void clean_history(std::deque& data); + double compute_T_rx_s(const Gnss_Synchro& a); + bool interpolate_data(Gnss_Synchro& out, std::deque& data, const double& ti); + void correct_TOW_and_compute_prange(std::vector& data); + int save_matfile(); //Tracking observable history - std::vector> d_gnss_synchro_history_queue; - + std::vector> d_gnss_synchro_history; + boost::dynamic_bitset<> valid_channels; double T_rx_s; double T_rx_step_s; + double max_delta; bool d_dump; unsigned int d_nchannels; - unsigned int history_deep; + unsigned int d_num_valid_channels; std::string d_dump_filename; std::ofstream d_dump_file; - - int save_matfile(); }; #endif diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc index 764c35aac..a774577a5 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc @@ -399,32 +399,32 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute if (d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) { //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_5 + GALILEO_INAV_PAGE_PART_SECONDS + (static_cast(required_symbols)) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; + d_TOW_at_current_symbol = d_nav.TOW_5 + static_cast(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast(required_symbols - 1) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; d_nav.flag_TOW_5 = false; } else if (d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) { //TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_6 + GALILEO_INAV_PAGE_PART_SECONDS + (static_cast(required_symbols)) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; + d_TOW_at_current_symbol = d_nav.TOW_6 + static_cast(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast(required_symbols - 1) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; d_nav.flag_TOW_6 = false; } else { //this page has no timing information - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; } } else //if there is not a new preamble, we define the TOW of the current symbol { - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; } //if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived { - delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64))); + delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0))); } if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 0e6b2e347..580bc8bdc 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -93,8 +93,8 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_GPS_frame_4bytes = 0; d_prev_GPS_frame_4bytes = 0; d_flag_parity = false; - d_TOW_at_Preamble = 0; - d_TOW_at_current_symbol = 0; + d_TOW_at_Preamble = 0.0; + d_TOW_at_current_symbol = 0.0; flag_TOW_set = false; d_average_count = 0; d_flag_preamble = false; @@ -104,6 +104,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_channel = 0; flag_PLL_180_deg_phase_locked = false; d_preamble_time_samples = 0; + d_TOW_at_current_symbol_ms = 0; } @@ -350,18 +351,22 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ //double decoder_latency_ms=(double)(current_symbol.Tracking_sample_counter-d_symbol_history.at(0).Tracking_sample_counter) // /(double)current_symbol.fs; // update TOW at the preamble instant (account with decoder latency) - d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; - d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0; + d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2.0 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; + d_TOW_at_current_symbol_ms = static_cast(d_GPS_FSM.d_nav.d_TOW) * 1000 + 161; + //d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0; + d_TOW_at_current_symbol = d_TOW_at_Preamble; flag_TOW_set = true; d_flag_new_tow_available = false; } else { - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD; + d_TOW_at_current_symbol += GPS_L1_CA_CODE_PERIOD; + d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; } - current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; + current_symbol.TOW_at_current_symbol_s = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + //current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_symbol.Flag_valid_word = flag_TOW_set; if (flag_PLL_180_deg_phase_locked == true) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index 9229b918c..8904f7e85 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -107,8 +107,9 @@ private: unsigned long int d_preamble_time_samples; - long double d_TOW_at_Preamble; - long double d_TOW_at_current_symbol; + double d_TOW_at_Preamble; + double d_TOW_at_current_symbol; + unsigned int d_TOW_at_current_symbol_ms; bool flag_TOW_set; bool flag_PLL_180_deg_phase_locked; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc index 16198b50d..0325041d3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc @@ -32,6 +32,7 @@ #include "gps_l2c_telemetry_decoder_cc.h" #include "gnss_synchro.h" +#include "display.h" #include #include #include @@ -132,32 +133,31 @@ int gps_l2c_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( { // get ephemeris object for this SV std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_ephemeris()); - std::cout << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << std::endl; + std::cout << TEXT_BLUE << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_CNAV_Message.have_new_iono() == true) { std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_iono()); - std::cout << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << std::endl; + std::cout << TEXT_BLUE << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_CNAV_Message.have_new_utc_model() == true) { std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_utc_model()); - std::cout << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl; + std::cout << TEXT_BLUE << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } //update TOW at the preamble instant - d_TOW_at_Preamble = static_cast(msg.tow); - //std::cout<<"["<<(int)msg.prn<<"] deco delay: "<(msg.tow); //* The time of the last input symbol can be computed from the message ToW and //* delay by the formulae: //* \code //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; - d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; + //d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; d_flag_valid_word = true; } else diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index f6ca8df87..012cc62ad 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -38,6 +38,7 @@ #include "configuration_interface.h" #include "Galileo_E1.h" #include "gnss_sdr_flags.h" +#include "display.h" #include @@ -49,43 +50,40 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float pll_bw_narrow_hz; - float dll_bw_hz; - float dll_bw_narrow_hz; - float early_late_space_chips; - float very_early_late_space_chips; - float early_late_space_narrow_chips; - float very_early_late_space_narrow_chips; - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); - dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); - int extend_correlation_symbols; - extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); - very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); - early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); - very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.6); - + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); + float very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + float very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.6); bool track_pilot = configuration->property(role + ".track_pilot", false); - + if (extend_correlation_symbols < 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Galileo E1. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (4 ms)" << TEXT_RESET << std::endl; + } + else if (!track_pilot and extend_correlation_symbols > 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Galileo E1. Extended coherent integration is not allowed when tracking the data component. Coherent integration has been set to 4 ms (1 symbol)" << TEXT_RESET << std::endl; + } + if ((extend_correlation_symbols > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) + { + std::cout << TEXT_RED << "WARNING: Galileo E1. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)); + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + int vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) @@ -116,7 +114,6 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( } channel_ = 0; - DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; } diff --git a/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc index 6f4535fe2..f2253197a 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc @@ -75,7 +75,7 @@ GalileoE1TcpConnectorTracking::GalileoE1TcpConnectorTracking( very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); port_ch0 = configuration->property(role + ".port_ch0", 2060); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index 87c11f48a..5a61f2007 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -40,6 +40,7 @@ #include "configuration_interface.h" #include "Galileo_E5a.h" #include "gnss_sdr_flags.h" +#include "display.h" #include using google::LogMessage; @@ -50,55 +51,67 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - int f_if; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; - float pll_bw_narrow_hz; - float dll_bw_narrow_hz; - int ti_ms; - float early_late_space_chips; - item_type = configuration->property(role + ".item_type", default_item_type); - //vector_length = configuration->property(role + ".vector_length", 2048); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 12000000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + unified_ = configuration->property(role + ".unified", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 20.0); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 20.0); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); - dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); - ti_ms = configuration->property(role + ".ti_ms", 3); - - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); + int ti_ms = configuration->property(role + ".ti_ms", 3); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(fs_in / (Galileo_E5a_CODE_CHIP_RATE_HZ / Galileo_E5a_CODE_LENGTH_CHIPS)); - + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + int vector_length = std::round(fs_in / (Galileo_E5a_CODE_CHIP_RATE_HZ / Galileo_E5a_CODE_LENGTH_CHIPS)); + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (extend_correlation_symbols < 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Galileo E5a. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + } + else if (!track_pilot and extend_correlation_symbols > Galileo_E5a_I_SECONDARY_CODE_LENGTH) + { + extend_correlation_symbols = Galileo_E5a_I_SECONDARY_CODE_LENGTH; + std::cout << TEXT_RED << "WARNING: Galileo E5a. extend_correlation_symbols must be lower than 21 when tracking the data component. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; + } + if ((extend_correlation_symbols > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) + { + std::cout << TEXT_RED << "WARNING: Galileo E5a. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - tracking_ = galileo_e5a_dll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - ti_ms, - early_late_space_chips); + if (unified_) + { + char sig_[3] = "5X"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, dll_bw_narrow_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_narrow_chips, + early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, 'E', sig_); + } + else + { + tracking_ = galileo_e5a_dll_pll_make_tracking_cc( + 0, fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, pll_bw_narrow_hz, + dll_bw_narrow_hz, ti_ms, + early_late_space_chips); + } } else { @@ -117,7 +130,10 @@ GalileoE5aDllPllTracking::~GalileoE5aDllPllTracking() void GalileoE5aDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } /* @@ -126,13 +142,19 @@ void GalileoE5aDllPllTracking::start_tracking() void GalileoE5aDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GalileoE5aDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } void GalileoE5aDllPllTracking::connect(gr::top_block_sptr top_block) @@ -153,10 +175,16 @@ void GalileoE5aDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GalileoE5aDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GalileoE5aDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h index b56cf2945..69d5600e4 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h @@ -41,6 +41,7 @@ #include "tracking_interface.h" #include "galileo_e5a_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -94,11 +95,13 @@ public: private: galileo_e5a_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; #endif /* GNSS_SDR_GALILEO_E5A_DLL_PLL_TRACKING_H_ */ diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc index 481be8080..960fd928c 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc @@ -80,8 +80,7 @@ GlonassL1CaDllPllCAidTracking::GlonassL1CaDllPllCAidTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc index cf082d8b6..493097f27 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc @@ -72,7 +72,7 @@ GlonassL1CaDllPllTracking::GlonassL1CaDllPllTracking( if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc index 59f0dedb0..2e284bab8 100644 --- a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc @@ -78,8 +78,7 @@ GlonassL2CaDllPllCAidTracking::GlonassL2CaDllPllCAidTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L2_CA_CODE_RATE_HZ / GLONASS_L2_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc index 12cd75953..3b029aae3 100644 --- a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc @@ -70,7 +70,7 @@ GlonassL2CaDllPllTracking::GlonassL2CaDllPllTracking( if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L2_CA_CODE_RATE_HZ / GLONASS_L2_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc index 87c4a26c7..1b0b9b93b 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc @@ -79,8 +79,7 @@ GpsL1CaDllPllCAidTracking::GpsL1CaDllPllCAidTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index b95b003f2..66e82aca9 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -40,6 +40,7 @@ #include "configuration_interface.h" #include "GPS_L1_CA.h" #include "gnss_sdr_flags.h" +#include "display.h" #include using google::LogMessage; @@ -50,16 +51,11 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - dump = configuration->property(role + ".dump", false); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); @@ -69,24 +65,37 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! - vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + int vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); - if (symbols_extended_correlator < 1) symbols_extended_correlator = 1; + if (symbols_extended_correlator < 1) + { + symbols_extended_correlator = 1; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be bigger than 1. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + } + else if (symbols_extended_correlator > 20) + { + symbols_extended_correlator = 20; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; + } + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (track_pilot) + { + std::cout << TEXT_RED << "WARNING: GPS L1 C/A does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; + } + if ((symbols_extended_correlator > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) + { + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { - item_size_ = sizeof(gr_complex); char sig_[3] = "1C"; + item_size_ = sizeof(gr_complex); tracking_ = dll_pll_veml_make_tracking( - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, + fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, dll_bw_narrow_hz, early_late_space_chips, early_late_space_chips, early_late_space_narrow_chips, diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc index 181e07317..7a881edca 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc @@ -72,8 +72,7 @@ GpsL1CaDllPllTrackingGPU::GpsL1CaDllPllTrackingGPU( if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc index 3502ec617..aab34cf76 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc @@ -67,7 +67,7 @@ GpsL1CaTcpConnectorTracking::GpsL1CaTcpConnectorTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); port_ch0 = configuration->property(role + ".port_ch0", 2060); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index 839ef334c..563316bf5 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -39,6 +39,7 @@ #include "configuration_interface.h" #include "GPS_L2C.h" #include "gnss_sdr_flags.h" +#include "display.h" #include @@ -50,44 +51,54 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - int f_if; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; - float early_late_space_chips; - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 2.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.75); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + unified_ = configuration->property(role + ".unified", false); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); - + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); + int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); + if (symbols_extended_correlator != 1) + { + std::cout << TEXT_RED << "WARNING: Extended coherent integration is not allowed in GPS L2. Coherent integration has been set to 20 ms (1 symbol)" << TEXT_RESET << std::endl; + } + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (track_pilot) + { + std::cout << TEXT_RED << "WARNING: GPS L2 does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - tracking_ = gps_l2_m_dll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); + if (unified_) + { + char sig_[3] = "2S"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, pll_bw_hz, dll_bw_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_chips, + early_late_space_chips, + 1, false, 'G', sig_); + } + else + { + tracking_ = gps_l2_m_dll_pll_make_tracking_cc( + 0, fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, + early_late_space_chips); + } } else { @@ -106,7 +117,10 @@ GpsL2MDllPllTracking::~GpsL2MDllPllTracking() void GpsL2MDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } /* @@ -115,13 +129,19 @@ void GpsL2MDllPllTracking::start_tracking() void GpsL2MDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GpsL2MDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } void GpsL2MDllPllTracking::connect(gr::top_block_sptr top_block) @@ -142,10 +162,16 @@ void GpsL2MDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GpsL2MDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GpsL2MDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h index ae7498a40..77741e9b1 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h @@ -40,6 +40,7 @@ #include "tracking_interface.h" #include "gps_l2_m_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -93,11 +94,13 @@ public: private: gps_l2_m_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; #endif // GNSS_SDR_gps_l2_m_dll_pll_tracking_H_ diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc index 10c180037..d28255416 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc @@ -39,6 +39,7 @@ #include "configuration_interface.h" #include "GPS_L5.h" #include "gnss_sdr_flags.h" +#include "display.h" #include @@ -50,44 +51,65 @@ GpsL5iDllPllTracking::GpsL5iDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - int f_if; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; - float early_late_space_chips; - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + unified_ = configuration->property(role + ".unified", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5i_CODE_RATE_HZ) / static_cast(GPS_L5i_CODE_LENGTH_CHIPS))); - + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5i_CODE_RATE_HZ) / static_cast(GPS_L5i_CODE_LENGTH_CHIPS))); + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (extend_correlation_symbols < 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: GPS L5. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + } + else if (!track_pilot and extend_correlation_symbols > GPS_L5i_NH_CODE_LENGTH) + { + extend_correlation_symbols = GPS_L5i_NH_CODE_LENGTH; + std::cout << TEXT_RED << "WARNING: GPS L5. extend_correlation_symbols must be lower than 11 when tracking the data component. Coherent integration has been set to 10 symbols (10 ms)" << TEXT_RESET << std::endl; + } + if ((extend_correlation_symbols > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) + { + std::cout << TEXT_RED << "WARNING: GPS L5. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - tracking_ = gps_l5i_dll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); + if (unified_) + { + char sig_[3] = "L5"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, dll_bw_narrow_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_narrow_chips, + early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, 'G', sig_); + } + else + { + tracking_ = gps_l5i_dll_pll_make_tracking_cc( + 0, fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, + early_late_space_chips); + } } else { @@ -106,7 +128,10 @@ GpsL5iDllPllTracking::~GpsL5iDllPllTracking() void GpsL5iDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } /* @@ -115,13 +140,19 @@ void GpsL5iDllPllTracking::start_tracking() void GpsL5iDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GpsL5iDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } void GpsL5iDllPllTracking::connect(gr::top_block_sptr top_block) @@ -142,10 +173,16 @@ void GpsL5iDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GpsL5iDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GpsL5iDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h index c54115251..472de2466 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h @@ -34,11 +34,12 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_gps_l5i_dll_pll_tracking_H_ -#define GNSS_SDR_gps_l5i_dll_pll_tracking_H_ +#ifndef GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_H_ +#define GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_H_ #include "tracking_interface.h" #include "gps_l5i_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -92,11 +93,13 @@ public: private: gps_l5i_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; -#endif // GNSS_SDR_gps_l5i_dll_pll_tracking_H_ +#endif // GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index e32905196..a89dfefc9 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -916,6 +916,7 @@ void dll_pll_veml_tracking::log_data(bool integrating) tmp_L = std::abs(d_L_accu); if (integrating) { + //TODO: Improve this solution! // It compensates the amplitude difference while integrating if (d_extend_correlation_symbols_count > 0) { @@ -1315,7 +1316,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_Prompt_buffer_deque.pop_front(); } } - else // Signal does not have secondary code. Search a bit transition by sign change + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change { if (d_synchonizing) { @@ -1346,6 +1347,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_current_symbol = 1; } } + else + { + next_state = true; + } if (next_state) { // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); @@ -1357,29 +1362,14 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_Prompt_buffer_deque.clear(); d_current_symbol = 0; d_synchonizing = false; - // Set narrow taps delay values [chips] - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - if (d_veml) - { - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - else - { - d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - // UPDATE INTEGRATION TIME if (d_enable_extended_integration) { + // UPDATE INTEGRATION TIME d_extend_correlation_symbols_count = 0; - float new_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); - d_carrier_loop_filter.set_pdi(new_correlation_time_s); - d_code_loop_filter.set_pdi(new_correlation_time_s); + float new_correlation_time = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); + d_carrier_loop_filter.set_pdi(new_correlation_time); + d_code_loop_filter.set_pdi(new_correlation_time); d_state = 3; // next state is the extended correlator integrator LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " << d_channel @@ -1387,6 +1377,21 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " << d_channel << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // Set narrow taps delay values [chips] + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } } else { diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index e028893ca..a7c0062a2 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -267,8 +267,8 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error_filt_hz; - float code_error_filt_chips; + float carr_error_filt_hz = 0.0; + float code_error_filt_chips = 0.0; tcp_packet_data tcp_data; // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index f4fe4c018..cd4c86652 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -297,10 +297,9 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attrib gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error; - float carr_nco; - float code_error; - float code_nco; + float carr_error = 0.0; + float code_error = 0.0; + float code_nco = 0.0; tcp_packet_data tcp_data; // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index d3b654a8d..3270f79d6 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -251,8 +251,15 @@ std::unique_ptr GNSSBlockFactory::GetObservables(std::shared GPS_channels += configuration->property("Channels_2S.count", 0); GPS_channels += configuration->property("Channels_L5.count", 0); unsigned int Glonass_channels = configuration->property("Channels_1G.count", 0); - Glonass_channels += configuration->property("Channels_2G.count", 0); - return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels + Glonass_channels, Galileo_channels + GPS_channels + Glonass_channels); + unsigned int extra_channels = 1; // For monitor channel sample counter + return GetBlock(configuration, "Observables", implementation, + Galileo_channels + + GPS_channels + + Glonass_channels + + extra_channels, + Galileo_channels + + GPS_channels + + Glonass_channels); } diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 0488da8e4..eec75d2f0 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -61,8 +61,6 @@ GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr configurati GNSSFlowgraph::~GNSSFlowgraph() {} - - void GNSSFlowgraph::start() { if (running_) @@ -246,6 +244,30 @@ void GNSSFlowgraph::connect() } DLOG(INFO) << "Signal source connected to signal conditioner"; + //connect the signal source to sample counter + //connect the sample counter to Observables + try + { + double fs = static_cast(configuration_->property("GNSS-SDR.internal_fs_sps", 0)); + if (fs == 0.0) + { + LOG(WARNING) << "Set GNSS-SDR.internal_fs_sps in configuration file"; + std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; + throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); + } + ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0)); + top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); + top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse + } + catch (const std::exception& e) + { + LOG(WARNING) << "Can't connect sample counter"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; + } + + // Signal conditioner (selected_signal_source) >> channels (i) (dependent of their associated SignalSource_ID) int selected_signal_conditioner_ID; for (unsigned int i = 0; i < channels_count_; i++) @@ -294,12 +316,6 @@ void GNSSFlowgraph::connect() { LOG(INFO) << "Channel " << i << " connected to observables in standby mode"; } - //connect the sample counter to the channel 0 - if (i == 0) - { - ch_out_sample_counter = gnss_sdr_make_sample_counter(); - top_block_->connect(channels_.at(i)->get_right_block(), 0, ch_out_sample_counter, 0); - } } /* diff --git a/src/core/system_parameters/GPS_CNAV.h b/src/core/system_parameters/GPS_CNAV.h index d155f77c0..04a62b748 100644 --- a/src/core/system_parameters/GPS_CNAV.h +++ b/src/core/system_parameters/GPS_CNAV.h @@ -122,13 +122,13 @@ const std::vector > CNAV_URA_NED1({{55, 3}}); const std::vector > CNAV_URA_NED2({{58, 3}}); const std::vector > CNAV_TOC({{61, 11}}); const double CNAV_TOC_LSB = 300.0; -const std::vector > CNAV_AF0({{72, 26}}); -const double CNAV_AF0_LSB = TWO_N60; -const std::vector > CNAV_AF1({{98, 20}}); +const std::vector > CNAV_AF0({{72,26}}); +const double CNAV_AF0_LSB = TWO_N35; +const std::vector > CNAV_AF1({{98,20}}); const double CNAV_AF1_LSB = TWO_N48; -const std::vector > CNAV_AF2({{118, 10}}); -const double CNAV_AF2_LSB = TWO_N35; -const std::vector > CNAV_TGD({{128, 13}}); +const std::vector > CNAV_AF2({{118,10}}); +const double CNAV_AF2_LSB = TWO_N60; +const std::vector > CNAV_TGD({{128,13}}); const double CNAV_TGD_LSB = TWO_N35; const std::vector > CNAV_ISCL1({{141, 13}}); const double CNAV_ISCL1_LSB = TWO_N35; diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 180ec521b..c5ae95618 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -53,6 +53,7 @@ const double GPS_L1_FREQ_HZ = FREQ1; //!< L1 [Hz] const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] +const unsigned int GPS_L1_CA_CODE_PERIOD_MS = 1; //!< GPS L1 C/A code period [ms] const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period [seconds] /*! @@ -70,7 +71,7 @@ const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (thi // OBSERVABLE HISTORY DEEP FOR INTERPOLATION -const int GPS_L1_CA_HISTORY_DEEP = 500; +const int GPS_L1_CA_HISTORY_DEEP = 100; // NAVIGATION MESSAGE DEMODULATION AND DECODING diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 70e7b5696..1201d7c5a 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -31,6 +31,8 @@ #ifndef GNSS_SDR_MATH_CONSTANTS_H_ #define GNSS_SDR_MATH_CONSTANTS_H_ +#include + /* Constants for scaling the ephemeris found in the data message the format is the following: TWO_N5 -> 2^-5, TWO_P4 -> 2^4, PI_TWO_N43 -> Pi*2^-43, etc etc Additionally some of the PI*2^N terms are used in the tracking stuff diff --git a/src/core/system_parameters/display.h b/src/core/system_parameters/display.h new file mode 100644 index 000000000..362e66f6f --- /dev/null +++ b/src/core/system_parameters/display.h @@ -0,0 +1,82 @@ +/*! + * \file display.h + * \brief Defines useful display constants + * \author Antonio Ramos, 2018. antonio.ramos(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_DISPLAY_H_ +#define GNSS_SDR_DISPLAY_H_ + +#include + +#ifndef NO_DISPLAY_COLORS +#define DISPLAY_COLORS 1 +#endif + + +#ifdef DISPLAY_COLORS + +const std::string TEXT_RESET = "\033[0m"; +const std::string TEXT_BLACK = "\033[30m"; +const std::string TEXT_RED = "\033[31m"; +const std::string TEXT_GREEN = "\033[32m"; +const std::string TEXT_YELLOW = "\033[33m"; +const std::string TEXT_BLUE = "\033[34m"; +const std::string TEXT_MAGENTA = "\033[35m"; +const std::string TEXT_CYAN = "\033[36m"; +const std::string TEXT_WHITE = "\033[37m"; +const std::string TEXT_BOLD_BLACK = "\033[1m\033[30m"; +const std::string TEXT_BOLD_RED = "\033[1m\033[31m"; +const std::string TEXT_BOLD_GREEN = "\033[1m\033[32m"; +const std::string TEXT_BOLD_YELLOW = "\033[1m\033[33m"; +const std::string TEXT_BOLD_BLUE = "\033[1m\033[34m"; +const std::string TEXT_BOLD_MAGENTA = "\033[1m\033[35m"; +const std::string TEXT_BOLD_CYAN = "\033[1m\033[36m"; +const std::string TEXT_BOLD_WHITE = "\033[1m\033[37m"; + +#else + +const std::string TEXT_RESET = ""; +const std::string TEXT_BLACK = ""; +const std::string TEXT_RED = ""; +const std::string TEXT_GREEN = ""; +const std::string TEXT_YELLOW = ""; +const std::string TEXT_BLUE = ""; +const std::string TEXT_MAGENTA = ""; +const std::string TEXT_CYAN = ""; +const std::string TEXT_WHITE = ""; +const std::string TEXT_BOLD_BLACK = ""; +const std::string TEXT_BOLD_RED = ""; +const std::string TEXT_BOLD_GREEN = ""; +const std::string TEXT_BOLD_YELLOW = ""; +const std::string TEXT_BOLD_BLUE = ""; +const std::string TEXT_BOLD_MAGENTA = ""; +const std::string TEXT_BOLD_CYAN = ""; +const std::string TEXT_BOLD_WHITE = ""; + +#endif /* DISPLAY_COLORS */ +#endif /* GNSS_SDR_DISPLAY_H_ */ diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index d10c2d8c6..7b63c0857 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -138,25 +138,30 @@ public: { }; - archive& make_nvp("i_satellite_PRN", i_satellite_PRN); // SV PRN NUMBER - archive& make_nvp("d_TOW", d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] - archive& make_nvp("d_Crs", d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] - archive& make_nvp("d_M_0", d_M_0); //!< Mean Anomaly at Reference Time [semi-circles] - archive& make_nvp("d_Cuc", d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] - archive& make_nvp("d_e_eccentricity", d_e_eccentricity); //!< Eccentricity [dimensionless] - archive& make_nvp("d_Cus", d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] - archive& make_nvp("d_Toe1", d_Toe1); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] - archive& make_nvp("d_Toe2", d_Toe2); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] - archive& make_nvp("d_Toc", d_Toc); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] - archive& make_nvp("d_Cic", d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] - archive& make_nvp("d_OMEGA0", d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] - archive& make_nvp("d_Cis", d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] - archive& make_nvp("d_i_0", d_i_0); //!< Inclination Angle at Reference Time [semi-circles] - archive& make_nvp("d_Crc", d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] - archive& make_nvp("d_OMEGA", d_OMEGA); //!< Argument of Perigee [semi-cicles] - archive& make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] - archive& make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] - archive& make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("i_satellite_PRN", i_satellite_PRN); // SV PRN NUMBER + archive& make_nvp("d_TOW", d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + archive& make_nvp("d_Crs", d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] + archive& make_nvp("d_M_0", d_M_0); //!< Mean Anomaly at Reference Time [semi-circles] + archive& make_nvp("d_Cuc", d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] + archive& make_nvp("d_e_eccentricity", d_e_eccentricity); //!< Eccentricity [dimensionless] + archive& make_nvp("d_Cus", d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] + archive& make_nvp("d_Toe1", d_Toe1); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + archive& make_nvp("d_Toe2", d_Toe2); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + archive& make_nvp("d_Toc", d_Toc); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] + archive& make_nvp("d_Cic", d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] + archive& make_nvp("d_OMEGA0", d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] + archive& make_nvp("d_Cis", d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] + archive& make_nvp("d_i_0", d_i_0); //!< Inclination Angle at Reference Time [semi-circles] + archive& make_nvp("d_Crc", d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] + archive& make_nvp("d_OMEGA", d_OMEGA); //!< Argument of Perigee [semi-cicles] + archive& make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] + archive& make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] + archive& make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL1", d_ISCL1); //!< Estimated Group Delay Differential: L1P(Y)-L1C/A correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL2", d_ISCL2); //!< Estimated Group Delay Differential: L1P(Y)-L2C correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL5I", d_ISCL5I); //!< Estimated Group Delay Differential: L1P(Y)-L5i correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL5Q", d_ISCL5Q); //!< Estimated Group Delay Differential: L1P(Y)-L5q correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_DELTA_A", d_DELTA_A); //!< Semi-major axis difference at reference time [m] archive& make_nvp("d_A_DOT", d_A_DOT); //!< Change rate in semi-major axis [m/s] archive& make_nvp("d_DELTA_OMEGA_DOT", d_DELTA_OMEGA_DOT); //!< Rate of Right Ascension difference [semi-circles/s] diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 20a0dd5e7..d188060bd 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -179,7 +179,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_TOW)); - d_TOW = d_TOW * CNAV_TOW_LSB; + d_TOW *= CNAV_TOW_LSB; ephemeris_record.d_TOW = d_TOW; alert_flag = static_cast(read_navigation_bool(data_bits, CNAV_ALERT_FLAG)); @@ -193,24 +193,24 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_WN)); ephemeris_record.i_signal_health = static_cast(read_navigation_unsigned(data_bits, CNAV_HEALTH)); ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); - ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB; + ephemeris_record.d_Top *= CNAV_TOP1_LSB; ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA)); ephemeris_record.d_Toe1 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE1)); - ephemeris_record.d_Toe1 = ephemeris_record.d_Toe1 * CNAV_TOE1_LSB; + ephemeris_record.d_Toe1 *= CNAV_TOE1_LSB; ephemeris_record.d_DELTA_A = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_A)); - ephemeris_record.d_DELTA_A = ephemeris_record.d_DELTA_A * CNAV_DELTA_A_LSB; + ephemeris_record.d_DELTA_A *= CNAV_DELTA_A_LSB; ephemeris_record.d_A_DOT = static_cast(read_navigation_signed(data_bits, CNAV_A_DOT)); - ephemeris_record.d_A_DOT = ephemeris_record.d_A_DOT * CNAV_A_DOT_LSB; + ephemeris_record.d_A_DOT *= CNAV_A_DOT_LSB; ephemeris_record.d_Delta_n = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0)); - ephemeris_record.d_Delta_n = ephemeris_record.d_Delta_n * CNAV_DELTA_N0_LSB; + ephemeris_record.d_Delta_n *= CNAV_DELTA_N0_LSB; ephemeris_record.d_DELTA_DOT_N = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0_DOT)); - ephemeris_record.d_DELTA_DOT_N = ephemeris_record.d_DELTA_DOT_N * CNAV_DELTA_N0_DOT_LSB; + ephemeris_record.d_DELTA_DOT_N *= CNAV_DELTA_N0_DOT_LSB; ephemeris_record.d_M_0 = static_cast(read_navigation_signed(data_bits, CNAV_M0)); - ephemeris_record.d_M_0 = ephemeris_record.d_M_0 * CNAV_M0_LSB; + ephemeris_record.d_M_0 *= CNAV_M0_LSB; ephemeris_record.d_e_eccentricity = static_cast(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY)); - ephemeris_record.d_e_eccentricity = ephemeris_record.d_e_eccentricity * CNAV_E_ECCENTRICITY_LSB; + ephemeris_record.d_e_eccentricity *= CNAV_E_ECCENTRICITY_LSB; ephemeris_record.d_OMEGA = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA)); - ephemeris_record.d_OMEGA = ephemeris_record.d_OMEGA * CNAV_OMEGA_LSB; + ephemeris_record.d_OMEGA *= CNAV_OMEGA_LSB; ephemeris_record.b_integrity_status_flag = static_cast(read_navigation_bool(data_bits, CNAV_INTEGRITY_FLAG)); ephemeris_record.b_l2c_phasing_flag = static_cast(read_navigation_bool(data_bits, CNAV_L2_PHASING_FLAG)); @@ -219,53 +219,79 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_TOE2)); - ephemeris_record.d_Toe2 = ephemeris_record.d_Toe2 * CNAV_TOE2_LSB; + ephemeris_record.d_Toe2 *= CNAV_TOE2_LSB; ephemeris_record.d_OMEGA0 = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA0)); - ephemeris_record.d_OMEGA0 = ephemeris_record.d_OMEGA0 * CNAV_OMEGA0_LSB; + ephemeris_record.d_OMEGA0 *= CNAV_OMEGA0_LSB; ephemeris_record.d_DELTA_OMEGA_DOT = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_OMEGA_DOT)); - ephemeris_record.d_DELTA_OMEGA_DOT = ephemeris_record.d_DELTA_OMEGA_DOT * CNAV_DELTA_OMEGA_DOT_LSB; + ephemeris_record.d_DELTA_OMEGA_DOT *= CNAV_DELTA_OMEGA_DOT_LSB; ephemeris_record.d_i_0 = static_cast(read_navigation_signed(data_bits, CNAV_I0)); - ephemeris_record.d_i_0 = ephemeris_record.d_i_0 * CNAV_I0_LSB; + ephemeris_record.d_i_0 *= CNAV_I0_LSB; ephemeris_record.d_IDOT = static_cast(read_navigation_signed(data_bits, CNAV_I0_DOT)); - ephemeris_record.d_IDOT = ephemeris_record.d_IDOT * CNAV_I0_DOT_LSB; + ephemeris_record.d_IDOT *= CNAV_I0_DOT_LSB; ephemeris_record.d_Cis = static_cast(read_navigation_signed(data_bits, CNAV_CIS)); - ephemeris_record.d_Cis = ephemeris_record.d_Cis * CNAV_CIS_LSB; + ephemeris_record.d_Cis *= CNAV_CIS_LSB; ephemeris_record.d_Cic = static_cast(read_navigation_signed(data_bits, CNAV_CIC)); - ephemeris_record.d_Cic = ephemeris_record.d_Cic * CNAV_CIC_LSB; + ephemeris_record.d_Cic *= CNAV_CIC_LSB; ephemeris_record.d_Crs = static_cast(read_navigation_signed(data_bits, CNAV_CRS)); - ephemeris_record.d_Crs = ephemeris_record.d_Crs * CNAV_CRS_LSB; + ephemeris_record.d_Crs *= CNAV_CRS_LSB; ephemeris_record.d_Crc = static_cast(read_navigation_signed(data_bits, CNAV_CRC)); - ephemeris_record.d_Cic = ephemeris_record.d_Cic * CNAV_CRC_LSB; + ephemeris_record.d_Crc *= CNAV_CRC_LSB; ephemeris_record.d_Cus = static_cast(read_navigation_signed(data_bits, CNAV_CUS)); - ephemeris_record.d_Cus = ephemeris_record.d_Cus * CNAV_CUS_LSB; + ephemeris_record.d_Cus *= CNAV_CUS_LSB; ephemeris_record.d_Cuc = static_cast(read_navigation_signed(data_bits, CNAV_CUC)); - ephemeris_record.d_Cuc = ephemeris_record.d_Cuc * CNAV_CUS_LSB; + ephemeris_record.d_Cuc *= CNAV_CUC_LSB; b_flag_ephemeris_2 = true; break; case 30: // (CLOCK, IONO, GRUP DELAY) //clock ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); - ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB; + ephemeris_record.d_Toc *= CNAV_TOC_LSB; ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA_NED0)); ephemeris_record.d_URA1 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED1)); ephemeris_record.d_URA2 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED2)); ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); - ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB; + ephemeris_record.d_A_f0 *= CNAV_AF0_LSB; ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); - ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB; + ephemeris_record.d_A_f1 *= CNAV_AF1_LSB; ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); - ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; + ephemeris_record.d_A_f2 *= CNAV_AF2_LSB; //group delays + //Check if the grup delay values are not available. See IS-GPS-200, Table 30-IV. + //Bit string "1000000000000" is -4096 in 2 complement ephemeris_record.d_TGD = static_cast(read_navigation_signed(data_bits, CNAV_TGD)); - ephemeris_record.d_TGD = ephemeris_record.d_TGD * CNAV_TGD_LSB; + if (ephemeris_record.d_TGD < -4095.9) + { + ephemeris_record.d_TGD = 0.0; + } + ephemeris_record.d_TGD *= CNAV_TGD_LSB; + ephemeris_record.d_ISCL1 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL1)); - ephemeris_record.d_ISCL1 = ephemeris_record.d_ISCL1 * CNAV_ISCL1_LSB; + if (ephemeris_record.d_ISCL1 < -4095.9) + { + ephemeris_record.d_ISCL1 = 0.0; + } + ephemeris_record.d_ISCL1 *= CNAV_ISCL1_LSB; + ephemeris_record.d_ISCL2 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL2)); - ephemeris_record.d_ISCL2 = ephemeris_record.d_ISCL2 * CNAV_ISCL2_LSB; + if (ephemeris_record.d_ISCL2 < -4095.9) + { + ephemeris_record.d_ISCL2 = 0.0; + } + ephemeris_record.d_ISCL2 *= CNAV_ISCL2_LSB; + ephemeris_record.d_ISCL5I = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5I)); - ephemeris_record.d_ISCL5I = ephemeris_record.d_ISCL5I * CNAV_ISCL5I_LSB; + if (ephemeris_record.d_ISCL5I < -4095.9) + { + ephemeris_record.d_ISCL5I = 0.0; + } + ephemeris_record.d_ISCL5I *= CNAV_ISCL5I_LSB; + ephemeris_record.d_ISCL5Q = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5Q)); - ephemeris_record.d_ISCL5Q = ephemeris_record.d_ISCL5Q * CNAV_ISCL5Q_LSB; + if (ephemeris_record.d_ISCL5Q < -4095.9) + { + ephemeris_record.d_ISCL5Q = 0.0; + } + ephemeris_record.d_ISCL5Q *= CNAV_ISCL5Q_LSB; //iono iono_record.d_alpha0 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA0)); iono_record.d_alpha0 = iono_record.d_alpha0 * CNAV_ALPHA0_LSB; @@ -297,7 +323,6 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_signed(data_bits, CNAV_AF2)); ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; - utc_model_record.d_A0 = static_cast(read_navigation_signed(data_bits, CNAV_A0)); utc_model_record.d_A0 = utc_model_record.d_A0 * CNAV_A0_LSB; utc_model_record.d_A1 = static_cast(read_navigation_signed(data_bits, CNAV_A1)); diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index 4e5263e0b..7d58a2baa 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -39,7 +39,7 @@ m * \file gps_navigation_message.cc void Gps_Navigation_Message::reset() { b_valid_ephemeris_set_flag = false; - d_TOW = 0; + d_TOW = 0.0; d_TOW_SF1 = 0; d_TOW_SF2 = 0; d_TOW_SF3 = 0; @@ -70,7 +70,7 @@ void Gps_Navigation_Message::reset() i_SV_accuracy = 0; i_SV_health = 0; d_TGD = 0; - d_IODC = -1; + d_IODC = -1.0; i_AODO = 0; b_fit_interval_flag = false; @@ -279,116 +279,114 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe) // Decode all 5 sub-frames switch (subframe_ID) - { - //--- Decode the sub-frame id ------------------------------------------ - // ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf - case 1: - //--- It is subframe 1 ------------------------------------- - // Compute the time of week (TOW) of the first sub-frames in the array ==== - // The transmitted TOW is actual TOW of the next subframe - // (the variable subframe at this point contains bits of the last subframe). - //TOW = bin2dec(subframe(31:47)) * 6; - d_TOW_SF1 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - //we are in the first subframe (the transmitted TOW is the start time of the next subframe) ! - d_TOW_SF1 = d_TOW_SF1 * 6; - d_TOW = d_TOW_SF1; // Set transmission time - b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); - b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); - b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); - i_GPS_week = static_cast(read_navigation_unsigned(subframe_bits, GPS_WEEK)); - i_SV_accuracy = static_cast(read_navigation_unsigned(subframe_bits, SV_ACCURACY)); // (20.3.3.3.1.3) - i_SV_health = static_cast(read_navigation_unsigned(subframe_bits, SV_HEALTH)); - b_L2_P_data_flag = read_navigation_bool(subframe_bits, L2_P_DATA_FLAG); // - i_code_on_L2 = static_cast(read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2)); - d_TGD = static_cast(read_navigation_signed(subframe_bits, T_GD)); - d_TGD = d_TGD * T_GD_LSB; - d_IODC = static_cast(read_navigation_unsigned(subframe_bits, IODC)); - d_Toc = static_cast(read_navigation_unsigned(subframe_bits, T_OC)); - d_Toc = d_Toc * T_OC_LSB; - d_A_f0 = static_cast(read_navigation_signed(subframe_bits, A_F0)); - d_A_f0 = d_A_f0 * A_F0_LSB; - d_A_f1 = static_cast(read_navigation_signed(subframe_bits, A_F1)); - d_A_f1 = d_A_f1 * A_F1_LSB; - d_A_f2 = static_cast(read_navigation_signed(subframe_bits, A_F2)); - d_A_f2 = d_A_f2 * A_F2_LSB; + { + //--- Decode the sub-frame id ------------------------------------------ + // ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf + case 1: + //--- It is subframe 1 ------------------------------------- + // Compute the time of week (TOW) of the first sub-frames in the array ==== + // The transmitted TOW is actual TOW of the next subframe + // (the variable subframe at this point contains bits of the last subframe). + //TOW = bin2dec(subframe(31:47)) * 6; + d_TOW_SF1 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + //we are in the first subframe (the transmitted TOW is the start time of the next subframe) ! + d_TOW_SF1 = d_TOW_SF1 * 6.0; + d_TOW = d_TOW_SF1; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + i_GPS_week = static_cast(read_navigation_unsigned(subframe_bits, GPS_WEEK)); + i_SV_accuracy = static_cast(read_navigation_unsigned(subframe_bits, SV_ACCURACY)); // (20.3.3.3.1.3) + i_SV_health = static_cast(read_navigation_unsigned(subframe_bits, SV_HEALTH)); + b_L2_P_data_flag = read_navigation_bool(subframe_bits, L2_P_DATA_FLAG); // + i_code_on_L2 = static_cast(read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2)); + d_TGD = static_cast(read_navigation_signed(subframe_bits, T_GD)); + d_TGD = d_TGD * T_GD_LSB; + d_IODC = static_cast(read_navigation_unsigned(subframe_bits, IODC)); + d_Toc = static_cast(read_navigation_unsigned(subframe_bits, T_OC)); + d_Toc = d_Toc * T_OC_LSB; + d_A_f0 = static_cast(read_navigation_signed(subframe_bits, A_F0)); + d_A_f0 = d_A_f0 * A_F0_LSB; + d_A_f1 = static_cast(read_navigation_signed(subframe_bits, A_F1)); + d_A_f1 = d_A_f1 * A_F1_LSB; + d_A_f2 = static_cast(read_navigation_signed(subframe_bits, A_F2)); + d_A_f2 = d_A_f2 * A_F2_LSB; break; - case 2: //--- It is subframe 2 ------------------- - d_TOW_SF2 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF2 = d_TOW_SF2 * 6; - d_TOW = d_TOW_SF2; // Set transmission time - b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); - b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); - b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); - d_IODE_SF2 = static_cast(read_navigation_unsigned(subframe_bits, IODE_SF2)); - d_Crs = static_cast(read_navigation_signed(subframe_bits, C_RS)); - d_Crs = d_Crs * C_RS_LSB; - d_Delta_n = static_cast(read_navigation_signed(subframe_bits, DELTA_N)); - d_Delta_n = d_Delta_n * DELTA_N_LSB; - d_M_0 = static_cast(read_navigation_signed(subframe_bits, M_0)); - d_M_0 = d_M_0 * M_0_LSB; - d_Cuc = static_cast(read_navigation_signed(subframe_bits, C_UC)); - d_Cuc = d_Cuc * C_UC_LSB; - d_e_eccentricity = static_cast(read_navigation_unsigned(subframe_bits, E)); - d_e_eccentricity = d_e_eccentricity * E_LSB; - d_Cus = static_cast(read_navigation_signed(subframe_bits, C_US)); - d_Cus = d_Cus * C_US_LSB; - d_sqrt_A = static_cast(read_navigation_unsigned(subframe_bits, SQRT_A)); - d_sqrt_A = d_sqrt_A * SQRT_A_LSB; - d_Toe = static_cast(read_navigation_unsigned(subframe_bits, T_OE)); - d_Toe = d_Toe * T_OE_LSB; - b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG); - i_AODO = static_cast(read_navigation_unsigned(subframe_bits, AODO)); - i_AODO = i_AODO * AODO_LSB; + case 2: //--- It is subframe 2 ------------------- + d_TOW_SF2 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF2 = d_TOW_SF2 * 6.0; + d_TOW = d_TOW_SF2; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + d_IODE_SF2 = static_cast(read_navigation_unsigned(subframe_bits, IODE_SF2)); + d_Crs = static_cast(read_navigation_signed(subframe_bits, C_RS)); + d_Crs = d_Crs * C_RS_LSB; + d_Delta_n = static_cast(read_navigation_signed(subframe_bits, DELTA_N)); + d_Delta_n = d_Delta_n * DELTA_N_LSB; + d_M_0 = static_cast(read_navigation_signed(subframe_bits, M_0)); + d_M_0 = d_M_0 * M_0_LSB; + d_Cuc = static_cast(read_navigation_signed(subframe_bits, C_UC)); + d_Cuc = d_Cuc * C_UC_LSB; + d_e_eccentricity = static_cast(read_navigation_unsigned(subframe_bits, E)); + d_e_eccentricity = d_e_eccentricity * E_LSB; + d_Cus = static_cast(read_navigation_signed(subframe_bits, C_US)); + d_Cus = d_Cus * C_US_LSB; + d_sqrt_A = static_cast(read_navigation_unsigned(subframe_bits, SQRT_A)); + d_sqrt_A = d_sqrt_A * SQRT_A_LSB; + d_Toe = static_cast(read_navigation_unsigned(subframe_bits, T_OE)); + d_Toe = d_Toe * T_OE_LSB; + b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG); + i_AODO = static_cast(read_navigation_unsigned(subframe_bits, AODO)); + i_AODO = i_AODO * AODO_LSB; break; - case 3: // --- It is subframe 3 ------------------------------------- - d_TOW_SF3 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF3 = d_TOW_SF3 * 6; - d_TOW = d_TOW_SF3; // Set transmission time - b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); - b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); - b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); - d_Cic = static_cast(read_navigation_signed(subframe_bits, C_IC)); - d_Cic = d_Cic * C_IC_LSB; - d_OMEGA0 = static_cast(read_navigation_signed(subframe_bits, OMEGA_0)); - d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB; - d_Cis = static_cast(read_navigation_signed(subframe_bits, C_IS)); - d_Cis = d_Cis * C_IS_LSB; - d_i_0 = static_cast(read_navigation_signed(subframe_bits, I_0)); - d_i_0 = d_i_0 * I_0_LSB; - d_Crc = static_cast(read_navigation_signed(subframe_bits, C_RC)); - d_Crc = d_Crc * C_RC_LSB; - d_OMEGA = static_cast(read_navigation_signed(subframe_bits, OMEGA)); - d_OMEGA = d_OMEGA * OMEGA_LSB; - d_OMEGA_DOT = static_cast(read_navigation_signed(subframe_bits, OMEGA_DOT)); - d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB; - d_IODE_SF3 = static_cast(read_navigation_unsigned(subframe_bits, IODE_SF3)); - d_IDOT = static_cast(read_navigation_signed(subframe_bits, I_DOT)); - d_IDOT = d_IDOT * I_DOT_LSB; + case 3: // --- It is subframe 3 ------------------------------------- + d_TOW_SF3 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF3 = d_TOW_SF3 * 6.0; + d_TOW = d_TOW_SF3; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + d_Cic = static_cast(read_navigation_signed(subframe_bits, C_IC)); + d_Cic = d_Cic * C_IC_LSB; + d_OMEGA0 = static_cast(read_navigation_signed(subframe_bits, OMEGA_0)); + d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB; + d_Cis = static_cast(read_navigation_signed(subframe_bits, C_IS)); + d_Cis = d_Cis * C_IS_LSB; + d_i_0 = static_cast(read_navigation_signed(subframe_bits, I_0)); + d_i_0 = d_i_0 * I_0_LSB; + d_Crc = static_cast(read_navigation_signed(subframe_bits, C_RC)); + d_Crc = d_Crc * C_RC_LSB; + d_OMEGA = static_cast(read_navigation_signed(subframe_bits, OMEGA)); + d_OMEGA = d_OMEGA * OMEGA_LSB; + d_OMEGA_DOT = static_cast(read_navigation_signed(subframe_bits, OMEGA_DOT)); + d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB; + d_IODE_SF3 = static_cast(read_navigation_unsigned(subframe_bits, IODE_SF3)); + d_IDOT = static_cast(read_navigation_signed(subframe_bits, I_DOT)); + d_IDOT = d_IDOT * I_DOT_LSB; break; - case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32) - int SV_data_ID; - int SV_page; - d_TOW_SF4 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF4 = d_TOW_SF4 * 6; - d_TOW = d_TOW_SF4; // Set transmission time - b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); - b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); - b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); - SV_data_ID = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); - SV_page = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); - if (SV_page > 24 && SV_page < 33) // Page 4 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) - { - //! \TODO read almanac - if (SV_data_ID) - { - } - } + case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32) + int SV_data_ID; + int SV_page; + d_TOW_SF4 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF4 = d_TOW_SF4 * 6.0; + d_TOW = d_TOW_SF4; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + SV_data_ID = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); + SV_page = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); + if (SV_page > 24 && SV_page < 33) // Page 4 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) + { + //! \TODO read almanac + if(SV_data_ID){} + } if (SV_page == 52) // Page 13 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) { @@ -449,55 +447,53 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe) break; - case 5: //--- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time. - int SV_data_ID_5; - int SV_page_5; - d_TOW_SF5 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF5 = d_TOW_SF5 * 6; - d_TOW = d_TOW_SF5; // Set transmission time - b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); - b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); - b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); - SV_data_ID_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); - SV_page_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); - if (SV_page_5 < 25) - { - //! \TODO read almanac - if (SV_data_ID_5) - { - } - } - if (SV_page_5 == 51) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) - { - d_Toa = static_cast(read_navigation_unsigned(subframe_bits, T_OA)); - d_Toa = d_Toa * T_OA_LSB; - i_WN_A = static_cast(read_navigation_unsigned(subframe_bits, WN_A)); - almanacHealth[1] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV1)); - almanacHealth[2] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV2)); - almanacHealth[3] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV3)); - almanacHealth[4] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV4)); - almanacHealth[5] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV5)); - almanacHealth[6] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV6)); - almanacHealth[7] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV7)); - almanacHealth[8] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV8)); - almanacHealth[9] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV9)); - almanacHealth[10] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV10)); - almanacHealth[11] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV11)); - almanacHealth[12] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV12)); - almanacHealth[13] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV13)); - almanacHealth[14] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV14)); - almanacHealth[15] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV15)); - almanacHealth[16] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV16)); - almanacHealth[17] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV17)); - almanacHealth[18] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV18)); - almanacHealth[19] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV19)); - almanacHealth[20] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV20)); - almanacHealth[21] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV21)); - almanacHealth[22] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV22)); - almanacHealth[23] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV23)); - almanacHealth[24] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV24)); - } - break; + case 5://--- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time. + int SV_data_ID_5; + int SV_page_5; + d_TOW_SF5 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF5 = d_TOW_SF5 * 6.0; + d_TOW = d_TOW_SF5; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + SV_data_ID_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); + SV_page_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); + if (SV_page_5 < 25) + { + //! \TODO read almanac + if(SV_data_ID_5){} + } + if (SV_page_5 == 51) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) + { + d_Toa = static_cast(read_navigation_unsigned(subframe_bits, T_OA)); + d_Toa = d_Toa * T_OA_LSB; + i_WN_A = static_cast(read_navigation_unsigned(subframe_bits, WN_A)); + almanacHealth[1] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV1)); + almanacHealth[2] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV2)); + almanacHealth[3] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV3)); + almanacHealth[4] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV4)); + almanacHealth[5] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV5)); + almanacHealth[6] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV6)); + almanacHealth[7] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV7)); + almanacHealth[8] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV8)); + almanacHealth[9] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV9)); + almanacHealth[10] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV10)); + almanacHealth[11] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV11)); + almanacHealth[12] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV12)); + almanacHealth[13] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV13)); + almanacHealth[14] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV14)); + almanacHealth[15] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV15)); + almanacHealth[16] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV16)); + almanacHealth[17] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV17)); + almanacHealth[18] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV18)); + almanacHealth[19] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV19)); + almanacHealth[20] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV20)); + almanacHealth[21] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV21)); + almanacHealth[22] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV22)); + almanacHealth[23] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV23)); + almanacHealth[24] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV24)); + } + break; default: break; @@ -672,9 +668,9 @@ bool Gps_Navigation_Message::satellite_validation() // First Step: // check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception // and check if the data have been filled (!=0) - if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0) + if (d_TOW_SF1 != 0.0 and d_TOW_SF2 != 0.0 and d_TOW_SF3 != 0.0) { - if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC != -1) + if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC != -1.0) { flag_data_valid = true; b_valid_ephemeris_set_flag = true; diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 82f4e97d9..c43b47a07 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -351,9 +351,6 @@ include_directories( # Unit testing ################################################################################ if(ENABLE_UNIT_TESTING) - if( ${ARMADILLO_VERSION_STRING} STRGREATER "5.300") # make sure interp1 is present - add_definitions(-DMODERN_ARMADILLO) - endif( ${ARMADILLO_VERSION_STRING} STRGREATER "5.300") add_executable(run_tests ${CMAKE_CURRENT_SOURCE_DIR}/test_main.cc) target_link_libraries(run_tests ${CLANG_FLAGS} diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 49a2865db..cf47da15d 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -146,13 +146,11 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc" #include "unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc" #include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc" -#if MODERN_ARMADILLO #include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc" #include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc" #include "unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc" #include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc" #endif -#endif #include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" #include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc" diff --git a/src/tests/unit-tests/control-plane/control_thread_test.cc b/src/tests/unit-tests/control-plane/control_thread_test.cc index ca22a8797..2019e40f3 100644 --- a/src/tests/unit-tests/control-plane/control_thread_test.cc +++ b/src/tests/unit-tests/control-plane/control_thread_test.cc @@ -117,6 +117,7 @@ TEST_F(ControlThreadTest, InstantiateRunControlMessages) config->set_property("Observables.item_type", "gr_complex"); config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.item_type", "gr_complex"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); std::shared_ptr control_thread = std::make_shared(config); @@ -177,6 +178,7 @@ TEST_F(ControlThreadTest, InstantiateRunControlMessages2) config->set_property("Observables.item_type", "gr_complex"); config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.item_type", "gr_complex"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); std::unique_ptr control_thread2(new ControlThread(config)); @@ -240,6 +242,7 @@ TEST_F(ControlThreadTest, StopReceiverProgrammatically) config->set_property("Observables.item_type", "gr_complex"); config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.item_type", "gr_complex"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); std::shared_ptr control_thread = std::make_shared(config); gr::msg_queue::sptr control_queue = gr::msg_queue::make(0); diff --git a/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc b/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc index 5a99cd62d..db6282590 100644 --- a/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc +++ b/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc @@ -49,6 +49,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopOldNotation) std::shared_ptr config = std::make_shared(); config->set_property("GNSS-SDR.SUPL_gps_enabled", "false"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); @@ -82,7 +83,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopOldNotation) TEST(GNSSFlowgraph, InstantiateConnectStartStop) { std::shared_ptr config = std::make_shared(); - + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); @@ -116,7 +117,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStop) TEST(GNSSFlowgraph, InstantiateConnectStartStopGalileoE1B) { std::shared_ptr config = std::make_shared(); - + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); @@ -151,7 +152,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopGalileoE1B) TEST(GNSSFlowgraph, InstantiateConnectStartStopHybrid) { std::shared_ptr config = std::make_shared(); - + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index af29edce9..14e8bb760 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -36,11 +36,8 @@ #include #include #include -#include -#include #include #include -#include #include #include "GPS_L1_CA.h" #include "gnss_satellite.h" @@ -57,9 +54,10 @@ #include "observables_dump_reader.h" #include "tlm_dump_reader.h" #include "gps_l1_ca_dll_pll_tracking.h" -#include "gps_l1_ca_dll_pll_c_aid_tracking.h" #include "hybrid_observables.h" #include "signal_generator_flags.h" +#include "gnss_sdr_sample_counter.h" +#include // ######## GNURADIO BLOCK MESSAGE RECEVER FOR TRACKING MESSAGES ######### @@ -186,18 +184,17 @@ public: int configure_generator(); int generate_signal(); void check_results_carrier_phase( - arma::vec& true_ch0_phase_cycles, - arma::vec& true_ch1_phase_cycles, - arma::vec& true_ch0_tow_s, - arma::vec& measuded_ch0_phase_cycles, - arma::vec& measuded_ch1_phase_cycles, - arma::vec& measuded_ch0_RX_time_s); + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1); void check_results_code_psudorange( - arma::vec& true_ch0_dist_m, arma::vec& true_ch1_dist_m, - arma::vec& true_ch0_tow_s, - arma::vec& measuded_ch0_Pseudorange_m, - arma::vec& measuded_ch1_Pseudorange_m, - arma::vec& measuded_ch0_RX_time_s); + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1); HybridObservablesTest() { @@ -284,39 +281,49 @@ void HybridObservablesTest::configure_receiver() // Set Tracking config->set_property("Tracking_1C.item_type", "gr_complex"); - config->set_property("Tracking_1C.if", "0"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); - config->set_property("Tracking_1C.pll_bw_hz", "15.0"); + config->set_property("Tracking_1C.pll_bw_hz", "35.0"); config->set_property("Tracking_1C.dll_bw_hz", "0.5"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); + config->set_property("Tracking_1C.unified", "true"); config->set_property("TelemetryDecoder_1C.dump", "true"); config->set_property("Observables.dump", "true"); } void HybridObservablesTest::check_results_carrier_phase( - arma::vec& true_ch0_phase_cycles, - arma::vec& true_ch1_phase_cycles, - arma::vec& true_ch0_tow_s, - arma::vec& measuded_ch0_phase_cycles, - arma::vec& measuded_ch1_phase_cycles, - arma::vec& measuded_ch0_RX_time_s) + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1) { //1. True value interpolation to match the measurement times + double t0 = std::max(measured_ch0(0, 0), measured_ch1(0, 0)); + int size1 = measured_ch0.col(0).n_rows; + int size2 = measured_ch1.col(0).n_rows; + double t1 = std::min(measured_ch0(size1 - 1, 0), measured_ch1(size2 - 1, 0)); + arma::vec t = arma::linspace(t0, t1, floor((t1 - t0) * 1e3)); + arma::vec true_ch0_phase_interp; arma::vec true_ch1_phase_interp; - arma::interp1(true_ch0_tow_s, true_ch0_phase_cycles, measuded_ch0_RX_time_s, true_ch0_phase_interp); - arma::interp1(true_ch0_tow_s, true_ch1_phase_cycles, measuded_ch0_RX_time_s, true_ch1_phase_interp); + arma::interp1(true_tow_s, true_ch0.col(3), t, true_ch0_phase_interp); + arma::interp1(true_tow_s, true_ch1.col(3), t, true_ch1_phase_interp); + + arma::vec meas_ch0_phase_interp; + arma::vec meas_ch1_phase_interp; + arma::interp1(measured_ch0.col(0), measured_ch0.col(3), t, meas_ch0_phase_interp); + arma::interp1(measured_ch1.col(0), measured_ch1.col(3), t, meas_ch1_phase_interp); //2. RMSE arma::vec err_ch0_cycles; arma::vec err_ch1_cycles; //compute error without the accumulated carrier phase offsets (which depends on the receiver starting time) - err_ch0_cycles = measuded_ch0_phase_cycles - true_ch0_phase_interp - measuded_ch0_phase_cycles(0) + true_ch0_phase_interp(0); - err_ch1_cycles = measuded_ch1_phase_cycles - true_ch1_phase_interp - measuded_ch1_phase_cycles(0) + true_ch1_phase_interp(0); + err_ch0_cycles = meas_ch0_phase_interp - true_ch0_phase_interp - meas_ch0_phase_interp(0) + true_ch0_phase_interp(0); + err_ch1_cycles = meas_ch1_phase_interp - true_ch1_phase_interp - meas_ch1_phase_interp(0) + true_ch1_phase_interp(0); arma::vec err2_ch0 = arma::square(err_ch0_cycles); double rmse_ch0 = sqrt(arma::mean(err2_ch0)); @@ -343,58 +350,68 @@ void HybridObservablesTest::check_results_carrier_phase( //5. report std::streamsize ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Channel 0 Carrier phase RMSE=" - << rmse_ch0 << ", mean=" << error_mean_ch0 - << ", stdev=" << sqrt(error_var_ch0) - << " (max,min)=" << max_error_ch0 + std::cout << std::setprecision(10) << "Channel 0 Carrier phase RMSE = " + << rmse_ch0 << ", mean = " << error_mean_ch0 + << ", stdev = " << sqrt(error_var_ch0) + << " (max,min) = " << max_error_ch0 << "," << min_error_ch0 << " [cycles]" << std::endl; std::cout.precision(ss); - ASSERT_LT(rmse_ch0, 1e-2); - ASSERT_LT(error_mean_ch0, 1e-2); - ASSERT_GT(error_mean_ch0, -1e-2); - ASSERT_LT(error_var_ch0, 1e-2); + ASSERT_LT(rmse_ch0, 5e-2); + ASSERT_LT(error_mean_ch0, 5e-2); + ASSERT_GT(error_mean_ch0, -5e-2); + ASSERT_LT(error_var_ch0, 5e-2); ASSERT_LT(max_error_ch0, 5e-2); ASSERT_GT(min_error_ch0, -5e-2); //5. report ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Channel 1 Carrier phase RMSE=" - << rmse_ch1 << ", mean=" << error_mean_ch1 - << ", stdev=" << sqrt(error_var_ch1) - << " (max,min)=" << max_error_ch1 + std::cout << std::setprecision(10) << "Channel 1 Carrier phase RMSE = " + << rmse_ch1 << ", mean = " << error_mean_ch1 + << ", stdev = " << sqrt(error_var_ch1) + << " (max,min) = " << max_error_ch1 << "," << min_error_ch1 << " [cycles]" << std::endl; std::cout.precision(ss); - ASSERT_LT(rmse_ch1, 1e-2); - ASSERT_LT(error_mean_ch1, 1e-2); - ASSERT_GT(error_mean_ch1, -1e-2); - ASSERT_LT(error_var_ch1, 1e-2); + ASSERT_LT(rmse_ch1, 5e-2); + ASSERT_LT(error_mean_ch1, 5e-2); + ASSERT_GT(error_mean_ch1, -5e-2); + ASSERT_LT(error_var_ch1, 5e-2); ASSERT_LT(max_error_ch1, 5e-2); ASSERT_GT(min_error_ch1, -5e-2); } void HybridObservablesTest::check_results_code_psudorange( - arma::vec& true_ch0_dist_m, - arma::vec& true_ch1_dist_m, - arma::vec& true_ch0_tow_s, - arma::vec& measuded_ch0_Pseudorange_m, - arma::vec& measuded_ch1_Pseudorange_m, - arma::vec& measuded_ch0_RX_time_s) + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1) { //1. True value interpolation to match the measurement times + double t0 = std::max(measured_ch0(0, 0), measured_ch1(0, 0)); + int size1 = measured_ch0.col(0).n_rows; + int size2 = measured_ch1.col(0).n_rows; + double t1 = std::min(measured_ch0(size1 - 1, 0), measured_ch1(size2 - 1, 0)); + arma::vec t = arma::linspace(t0, t1, floor((t1 - t0) * 1e3)); + arma::vec true_ch0_dist_interp; arma::vec true_ch1_dist_interp; - arma::interp1(true_ch0_tow_s, true_ch0_dist_m, measuded_ch0_RX_time_s, true_ch0_dist_interp); - arma::interp1(true_ch0_tow_s, true_ch1_dist_m, measuded_ch0_RX_time_s, true_ch1_dist_interp); + arma::interp1(true_tow_s, true_ch0.col(1), t, true_ch0_dist_interp); + arma::interp1(true_tow_s, true_ch1.col(1), t, true_ch1_dist_interp); + + arma::vec meas_ch0_dist_interp; + arma::vec meas_ch1_dist_interp; + arma::interp1(measured_ch0.col(0), measured_ch0.col(4), t, meas_ch0_dist_interp); + arma::interp1(measured_ch1.col(0), measured_ch1.col(4), t, meas_ch1_dist_interp); // generate delta pseudoranges arma::vec delta_true_dist_m = true_ch0_dist_interp - true_ch1_dist_interp; - arma::vec delta_measured_dist_m = measuded_ch0_Pseudorange_m - measuded_ch1_Pseudorange_m; + arma::vec delta_measured_dist_m = meas_ch0_dist_interp - meas_ch1_dist_interp; //2. RMSE arma::vec err; @@ -413,10 +430,10 @@ void HybridObservablesTest::check_results_code_psudorange( //5. report std::streamsize ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Delta Observables RMSE=" - << rmse << ", mean=" << error_mean - << ", stdev=" << sqrt(error_var) - << " (max,min)=" << max_error + std::cout << std::setprecision(10) << "Delta Observables RMSE = " + << rmse << ", mean = " << error_mean + << ", stdev = " << sqrt(error_var) + << " (max,min) = " << max_error << "," << min_error << " [meters]" << std::endl; std::cout.precision(ss); @@ -425,8 +442,8 @@ void HybridObservablesTest::check_results_code_psudorange( ASSERT_LT(error_mean, 0.5); ASSERT_GT(error_mean, -0.5); ASSERT_LT(error_var, 0.5); - ASSERT_LT(max_error, 2); - ASSERT_GT(min_error, -2); + ASSERT_LT(max_error, 2.0); + ASSERT_GT(min_error, -2.0); } @@ -474,9 +491,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) top_block = gr::make_top_block("Telemetry_Decoder test"); std::shared_ptr tracking_ch0 = std::make_shared(config.get(), "Tracking_1C", 1, 1); - //std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); - //std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); boost::shared_ptr msg_rx_ch0 = HybridObservablesTest_msg_rx_make(); boost::shared_ptr msg_rx_ch1 = HybridObservablesTest_msg_rx_make(); @@ -528,7 +543,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) boost::shared_ptr tlm_msg_rx_ch2 = HybridObservablesTest_tlm_msg_rx_make(); //Observables - std::shared_ptr observables(new HybridObservables(config.get(), "Observables", 2, 2)); + std::shared_ptr observables(new HybridObservables(config.get(), "Observables", 3, 2)); ASSERT_NO_THROW({ tracking_ch0->set_channel(gnss_synchro_ch0.Channel_ID); @@ -552,7 +567,10 @@ TEST_F(HybridObservablesTest, ValidationOfResults) gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); gr::blocks::null_sink::sptr sink_ch0 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); gr::blocks::null_sink::sptr sink_ch1 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq), sizeof(gr_complex)); top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); + top_block->connect(gr_interleaved_char_to_complex, 0, samp_counter, 0); + //ch0 top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch0->get_left_block(), 0); top_block->connect(tracking_ch0->get_right_block(), 0, tlm_ch0->get_left_block(), 0); @@ -566,6 +584,8 @@ TEST_F(HybridObservablesTest, ValidationOfResults) top_block->connect(observables->get_right_block(), 0, sink_ch0, 0); top_block->connect(observables->get_right_block(), 1, sink_ch1, 0); + top_block->connect(samp_counter, 0, observables->get_left_block(), 2); + }) << "Failure connecting the blocks."; tracking_ch0->start_tracking(); @@ -587,20 +607,15 @@ TEST_F(HybridObservablesTest, ValidationOfResults) if (true_observables.open_obs_file(std::string("./obs_out.bin")) == false) { throw std::exception(); - }; + } }) << "Failure opening true observables file"; - long int nepoch = true_observables.num_epochs(); + unsigned int nepoch = static_cast(true_observables.num_epochs()); - std::cout << "True observation epochs=" << nepoch << std::endl; - arma::vec true_ch0_dist_m = arma::zeros(nepoch, 1); - arma::vec true_ch0_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); - arma::vec true_ch0_Doppler_Hz = arma::zeros(nepoch, 1); - arma::vec true_ch0_tow_s = arma::zeros(nepoch, 1); - arma::vec true_ch1_dist_m = arma::zeros(nepoch, 1); - arma::vec true_ch1_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); - arma::vec true_ch1_Doppler_Hz = arma::zeros(nepoch, 1); - arma::vec true_ch1_tow_s = arma::zeros(nepoch, 1); + std::cout << "True observation epochs = " << nepoch << std::endl; + // Matrices for storing columnwise true GPS time, Range, Doppler and Carrier phase + arma::mat true_ch0 = arma::zeros(nepoch, 4); + arma::mat true_ch1 = arma::zeros(nepoch, 4); true_observables.restart(); long int epoch_counter = 0; @@ -609,23 +624,23 @@ TEST_F(HybridObservablesTest, ValidationOfResults) { if (round(true_observables.prn[0]) != gnss_synchro_ch0.PRN) { - std::cout << "True observables SV PRN do not match" << round(true_observables.prn[1]) << std::endl; + std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; throw std::exception(); } if (round(true_observables.prn[1]) != gnss_synchro_ch1.PRN) { - std::cout << "True observables SV PRN do not match " << round(true_observables.prn[1]) << std::endl; + std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; throw std::exception(); } - true_ch0_tow_s(epoch_counter) = true_observables.gps_time_sec[0]; - true_ch0_dist_m(epoch_counter) = true_observables.dist_m[0]; - true_ch0_Doppler_Hz(epoch_counter) = true_observables.doppler_l1_hz[0]; - true_ch0_acc_carrier_phase_cycles(epoch_counter) = true_observables.acc_carrier_phase_l1_cycles[0]; + true_ch0(epoch_counter, 0) = true_observables.gps_time_sec[0]; + true_ch0(epoch_counter, 1) = true_observables.dist_m[0]; + true_ch0(epoch_counter, 2) = true_observables.doppler_l1_hz[0]; + true_ch0(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[0]; - true_ch1_tow_s(epoch_counter) = true_observables.gps_time_sec[1]; - true_ch1_dist_m(epoch_counter) = true_observables.dist_m[1]; - true_ch1_Doppler_Hz(epoch_counter) = true_observables.doppler_l1_hz[1]; - true_ch1_acc_carrier_phase_cycles(epoch_counter) = true_observables.acc_carrier_phase_l1_cycles[1]; + true_ch1(epoch_counter, 0) = true_observables.gps_time_sec[1]; + true_ch1(epoch_counter, 1) = true_observables.dist_m[1]; + true_ch1(epoch_counter, 2) = true_observables.doppler_l1_hz[1]; + true_ch1(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[1]; epoch_counter++; } @@ -637,83 +652,85 @@ TEST_F(HybridObservablesTest, ValidationOfResults) if (estimated_observables.open_obs_file(std::string("./observables.dat")) == false) { throw std::exception(); - }; + } }) << "Failure opening dump observables file"; - nepoch = estimated_observables.num_epochs(); - std::cout << "Measured observation epochs=" << nepoch << std::endl; + nepoch = static_cast(estimated_observables.num_epochs()); + std::cout << "Measured observation epochs = " << nepoch << std::endl; - arma::vec measuded_ch0_RX_time_s = arma::zeros(nepoch, 1); - arma::vec measuded_ch0_TOW_at_current_symbol_s = arma::zeros(nepoch, 1); - arma::vec measuded_ch0_Carrier_Doppler_hz = arma::zeros(nepoch, 1); - arma::vec measuded_ch0_Acc_carrier_phase_hz = arma::zeros(nepoch, 1); - arma::vec measuded_ch0_Pseudorange_m = arma::zeros(nepoch, 1); - - arma::vec measuded_ch1_RX_time_s = arma::zeros(nepoch, 1); - arma::vec measuded_ch1_TOW_at_current_symbol_s = arma::zeros(nepoch, 1); - arma::vec measuded_ch1_Carrier_Doppler_hz = arma::zeros(nepoch, 1); - arma::vec measuded_ch1_Acc_carrier_phase_hz = arma::zeros(nepoch, 1); - arma::vec measuded_ch1_Pseudorange_m = arma::zeros(nepoch, 1); + // Matrices for storing columnwise measured RX_time, TOW, Doppler, Carrier phase and Pseudorange + arma::mat measured_ch0 = arma::zeros(nepoch, 5); + arma::mat measured_ch1 = arma::zeros(nepoch, 5); estimated_observables.restart(); - epoch_counter = 0; + long int epoch_counter2 = 0; while (estimated_observables.read_binary_obs()) { - measuded_ch0_RX_time_s(epoch_counter) = estimated_observables.RX_time[0]; - measuded_ch0_TOW_at_current_symbol_s(epoch_counter) = estimated_observables.TOW_at_current_symbol_s[0]; - measuded_ch0_Carrier_Doppler_hz(epoch_counter) = estimated_observables.Carrier_Doppler_hz[0]; - measuded_ch0_Acc_carrier_phase_hz(epoch_counter) = estimated_observables.Acc_carrier_phase_hz[0]; - measuded_ch0_Pseudorange_m(epoch_counter) = estimated_observables.Pseudorange_m[0]; + if (static_cast(estimated_observables.valid[0])) + { + measured_ch0(epoch_counter, 0) = estimated_observables.RX_time[0]; + measured_ch0(epoch_counter, 1) = estimated_observables.TOW_at_current_symbol_s[0]; + measured_ch0(epoch_counter, 2) = estimated_observables.Carrier_Doppler_hz[0]; + measured_ch0(epoch_counter, 3) = estimated_observables.Acc_carrier_phase_hz[0]; + measured_ch0(epoch_counter, 4) = estimated_observables.Pseudorange_m[0]; + epoch_counter++; + } + if (static_cast(estimated_observables.valid[1])) + { + measured_ch1(epoch_counter2, 0) = estimated_observables.RX_time[1]; + measured_ch1(epoch_counter2, 1) = estimated_observables.TOW_at_current_symbol_s[1]; + measured_ch1(epoch_counter2, 2) = estimated_observables.Carrier_Doppler_hz[1]; + measured_ch1(epoch_counter2, 3) = estimated_observables.Acc_carrier_phase_hz[1]; + measured_ch1(epoch_counter2, 4) = estimated_observables.Pseudorange_m[1]; + epoch_counter2++; + } + } - measuded_ch1_RX_time_s(epoch_counter) = estimated_observables.RX_time[1]; - measuded_ch1_TOW_at_current_symbol_s(epoch_counter) = estimated_observables.TOW_at_current_symbol_s[1]; - measuded_ch1_Carrier_Doppler_hz(epoch_counter) = estimated_observables.Carrier_Doppler_hz[1]; - measuded_ch1_Acc_carrier_phase_hz(epoch_counter) = estimated_observables.Acc_carrier_phase_hz[1]; - measuded_ch1_Pseudorange_m(epoch_counter) = estimated_observables.Pseudorange_m[1]; - - epoch_counter++; + //Cut measurement tail zeros + arma::uvec index = arma::find(measured_ch0.col(0) > 0.0, 1, "last"); + if ((index.size() > 0) and index(0) < (nepoch - 1)) + { + measured_ch0.shed_rows(index(0) + 1, nepoch - 1); + } + index = arma::find(measured_ch1.col(0) > 0.0, 1, "last"); + if ((index.size() > 0) and index(0) < (nepoch - 1)) + { + measured_ch1.shed_rows(index(0) + 1, nepoch - 1); } //Cut measurement initial transitory of the measurements - arma::uvec initial_meas_point = arma::find(measuded_ch0_RX_time_s >= true_ch0_tow_s(0), 1, "first"); + index = arma::find(measured_ch0.col(0) >= true_ch0(0, 0), 1, "first"); + if ((index.size() > 0) and (index(0) > 0)) + { + measured_ch0.shed_rows(0, index(0)); + } + index = arma::find(measured_ch1.col(0) >= true_ch1(0, 0), 1, "first"); + if ((index.size() > 0) and (index(0) > 0)) + { + measured_ch1.shed_rows(0, index(0)); + } - measuded_ch0_RX_time_s = measuded_ch0_RX_time_s.subvec(initial_meas_point(0), measuded_ch0_RX_time_s.size() - 1); - measuded_ch0_Pseudorange_m = measuded_ch0_Pseudorange_m.subvec(initial_meas_point(0), measuded_ch0_Pseudorange_m.size() - 1); - measuded_ch0_Acc_carrier_phase_hz = measuded_ch0_Acc_carrier_phase_hz.subvec(initial_meas_point(0), measuded_ch0_Acc_carrier_phase_hz.size() - 1); - - measuded_ch1_RX_time_s = measuded_ch1_RX_time_s.subvec(initial_meas_point(0), measuded_ch1_RX_time_s.size() - 1); - measuded_ch1_Pseudorange_m = measuded_ch1_Pseudorange_m.subvec(initial_meas_point(0), measuded_ch1_Pseudorange_m.size() - 1); - measuded_ch1_Acc_carrier_phase_hz = measuded_ch1_Acc_carrier_phase_hz.subvec(initial_meas_point(0), measuded_ch1_Acc_carrier_phase_hz.size() - 1); - - //correct the clock error using true values (it is not possible for a receiver to correct + //Correct the clock error using true values (it is not possible for a receiver to correct //the receiver clock offset error at the observables level because it is required the //decoding of the ephemeris data and solve the PVT equations) - //find the reference satellite and compute the receiver time offset at obsevable level + //Find the reference satellite (the nearest) and compute the receiver time offset at observable level arma::vec receiver_time_offset_s; - if (measuded_ch0_Pseudorange_m(0) < measuded_ch1_Pseudorange_m(0)) + if (measured_ch0(0, 4) < measured_ch1(0, 4)) { - receiver_time_offset_s = true_ch0_dist_m / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + receiver_time_offset_s = true_ch0.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; } else { - receiver_time_offset_s = true_ch1_dist_m / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + receiver_time_offset_s = true_ch1.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; } - arma::vec corrected_reference_TOW_s = true_ch0_tow_s - receiver_time_offset_s; + arma::vec corrected_reference_TOW_s = true_ch0.col(0) - receiver_time_offset_s; + std::cout << "Receiver time offset: " << receiver_time_offset_s(0) * 1e3 << " [ms]" << std::endl; - std::cout << " receiver_time_offset_s [0]: " << receiver_time_offset_s(0) << std::endl; + //Compare measured observables + check_results_code_psudorange(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1); + check_results_carrier_phase(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1); - //compare measured observables - check_results_code_psudorange(true_ch0_dist_m, true_ch1_dist_m, corrected_reference_TOW_s, - measuded_ch0_Pseudorange_m, measuded_ch1_Pseudorange_m, measuded_ch0_RX_time_s); - - check_results_carrier_phase(true_ch0_acc_carrier_phase_cycles, - true_ch1_acc_carrier_phase_cycles, - corrected_reference_TOW_s, - measuded_ch0_Acc_carrier_phase_hz, - measuded_ch1_Acc_carrier_phase_hz, - measuded_ch0_RX_time_s); - - std::cout << "Test completed in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl; + std::cout << "Test completed in " << elapsed_seconds.count() << " [s]" << std::endl; } diff --git a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc index 340e472e5..bececcd16 100644 --- a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc @@ -265,13 +265,12 @@ void GpsL1CATelemetryDecoderTest::configure_receiver() // Set Tracking config->set_property("Tracking_1C.item_type", "gr_complex"); - config->set_property("Tracking_1C.if", "0"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); config->set_property("Tracking_1C.pll_bw_hz", "20.0"); config->set_property("Tracking_1C.dll_bw_hz", "1.5"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); - + config->set_property("Tracking_1C.unified", "true"); config->set_property("TelemetryDecoder_1C.dump", "true"); } @@ -293,9 +292,9 @@ void GpsL1CATelemetryDecoderTest::check_results(arma::vec& true_time_s, arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp); //2. RMSE - arma::vec err; + //arma::vec err = meas_value - true_value_interp + 0.001; + arma::vec err = meas_value - true_value_interp - 0.001; - err = meas_value - true_value_interp + 0.001; arma::vec err2 = arma::square(err); double rmse = sqrt(arma::mean(err2)); @@ -317,10 +316,10 @@ void GpsL1CATelemetryDecoderTest::check_results(arma::vec& true_time_s, << " [Seconds]" << std::endl; std::cout.precision(ss); - ASSERT_LT(rmse, 0.2E-6); - ASSERT_LT(error_mean, 0.2E-6); - ASSERT_GT(error_mean, -0.2E-6); - ASSERT_LT(error_var, 0.2E-6); + ASSERT_LT(rmse, 0.3E-6); + ASSERT_LT(error_mean, 0.3E-6); + ASSERT_GT(error_mean, -0.3E-6); + ASSERT_LT(error_var, 0.3E-6); ASSERT_LT(max_error, 0.5E-6); ASSERT_GT(min_error, -0.5E-6); } diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc index c1eded696..8a8049803 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc @@ -39,6 +39,7 @@ #include #include #include +#include #include "gnss_block_factory.h" #include "gnss_block_interface.h" #include "in_memory_configuration.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc index 9d29b837d..97d1d18f0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc @@ -409,7 +409,7 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults) true_obs_data.restart(); std::cout << "Initial Doppler [Hz]=" << true_obs_data.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data.prn_delay_chips << std::endl; - gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; + gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * static_cast(baseband_sampling_freq) * GPS_L1_CA_CODE_PERIOD; gnss_synchro.Acq_doppler_hz = true_obs_data.doppler_l1_hz; gnss_synchro.Acq_samplestamp_samples = 0; diff --git a/src/utils/matlab/libs/geoFunctions/deg2dms.m b/src/utils/matlab/libs/geoFunctions/deg2dms.m index 1f925894d..948e885db 100644 --- a/src/utils/matlab/libs/geoFunctions/deg2dms.m +++ b/src/utils/matlab/libs/geoFunctions/deg2dms.m @@ -9,7 +9,7 @@ function dmsOutput = deg2dms(deg) %%% Save the sign for later processing neg_arg = false; if deg < 0 - % Only positive numbers should be used while spliting into deg/min/sec + % Only positive numbers should be used while splitting into deg/min/sec deg = -deg; neg_arg = true; end diff --git a/src/utils/matlab/libs/geoFunctions/dms2mat.m b/src/utils/matlab/libs/geoFunctions/dms2mat.m index a5c449673..6cafb2488 100644 --- a/src/utils/matlab/libs/geoFunctions/dms2mat.m +++ b/src/utils/matlab/libs/geoFunctions/dms2mat.m @@ -84,7 +84,7 @@ msign = signvec .* (d==0 & m~=0); ssign = signvec .* (d==0 & m==0 & s~=0); % In the application of signs below, the comparison with 0 is used so that -% the sign vector contains only +1 and -1. Any zero occurances causes +% the sign vector contains only +1 and -1. Any zero occurrences causes % data to be lost when the sign has been applied to a higher component % of d:m:s. Use fix function to eliminate potential round-off errors. diff --git a/src/utils/matlab/libs/plotNavigation.m b/src/utils/matlab/libs/plotNavigation.m index 9e27fc4d2..08460c702 100644 --- a/src/utils/matlab/libs/plotNavigation.m +++ b/src/utils/matlab/libs/plotNavigation.m @@ -47,7 +47,7 @@ function plotNavigation(navSolutions, settings,plot_skyplot) if (~isempty(navSolutions)) %% If reference position is not provided, then set reference position - %% to the average postion + %% to the average position if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ... || isnan(settings.truePosition.U)