From ab7cabc8e87603bbe2b052fb58248fd51b905817 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 8 Mar 2018 18:32:55 +0100 Subject: [PATCH] code style --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 100 ++-- .../gnuradio_blocks/hybrid_observables_cc.cc | 469 ++++++++++-------- 2 files changed, 311 insertions(+), 258 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 43da2f97f..75438755c 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -534,13 +534,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])); @@ -601,36 +601,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; @@ -776,7 +776,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); @@ -786,7 +786,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); @@ -796,7 +796,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); @@ -806,7 +806,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); @@ -867,7 +867,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); @@ -883,7 +883,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"); @@ -894,7 +894,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); @@ -919,7 +919,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); } @@ -927,15 +927,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); } @@ -972,7 +972,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); @@ -985,7 +985,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); @@ -998,7 +998,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); @@ -1011,7 +1011,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); @@ -1024,7 +1024,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); @@ -1037,7 +1037,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); @@ -1046,11 +1046,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); @@ -1059,11 +1059,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); @@ -1076,7 +1076,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); @@ -1089,7 +1089,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); @@ -1102,7 +1102,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); @@ -1115,7 +1115,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); @@ -1128,7 +1128,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); @@ -1137,11 +1137,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); @@ -1150,11 +1150,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); @@ -1163,11 +1163,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); @@ -1200,7 +1200,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } } } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + if ((type_of_rx == 4) or (type_of_rx == 5) or (type_of_rx == 6) or (type_of_rx == 14) or (type_of_rx == 15)) // Galileo { if (flag_write_RTCM_1045_output == true) { @@ -1234,7 +1234,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_ephemeris_iter; gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - 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())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -1305,7 +1305,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } } } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + if ((type_of_rx == 23) or (type_of_rx == 24) or (type_of_rx == 25)) // GLONASS { if (flag_write_RTCM_1020_output == true) { @@ -1472,7 +1472,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } b_rtcm_writing_started = true; } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + if ((type_of_rx == 4) or (type_of_rx == 5) or (type_of_rx == 6) or (type_of_rx == 14) or (type_of_rx == 15)) // Galileo { for (std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_ephemeris_iter++) { @@ -1497,7 +1497,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - 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())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -1550,18 +1550,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item i++; } - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() and (d_rtcm_MT1077_rate_ms != 0)) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0)) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() and (d_rtcm_MT1097_rate_ms != 0)) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + if ((type_of_rx == 23) or (type_of_rx == 24) or (type_of_rx == 25)) // GLONASS { for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) { @@ -1696,7 +1696,7 @@ 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() == true) && (flag_display_pvt == true)) + if ((d_ls_pvt->is_valid_position() == true) and (flag_display_pvt == true)) { 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() diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index c5e2c983e..20e5c6948 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -51,62 +51,64 @@ hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, } -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))) +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))) { d_dump = dump; d_nchannels = nchannels_out; d_dump_filename = dump_filename; T_rx_s = 0.0; - T_rx_step_s = 0.001; // 1 ms - max_delta = 0.15; // 150 ms + 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.push_back(std::deque()); - } + for (unsigned int i = 0; i < d_nchannels; i++) + { + d_gnss_synchro_history.push_back(std::deque()); + } // ############# ENABLE DATA FILE LOG ################# if (d_dump) - { - if (!d_dump_file.is_open()) { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - d_dump = false; - } + if (!d_dump_file.is_open()) + { + try + { + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception opening observables dump file " << e.what(); + d_dump = false; + } + } } - } } hybrid_observables_cc::~hybrid_observables_cc() { if (d_dump_file.is_open()) - { - try { d_dump_file.close(); } - catch(const std::exception & ex) { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + try + { + d_dump_file.close(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + } + } + if (d_dump) + { + std::cout << "Writing observables .mat files ..."; + save_matfile(); + std::cout << " done." << std::endl; } - } - if(d_dump) - { - std::cout << "Writing observables .mat files ..."; - save_matfile(); - std::cout << " done." << std::endl; - } } @@ -118,12 +120,15 @@ int hybrid_observables_cc::save_matfile() int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels; std::ifstream dump_file; dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - try { dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); } - catch(const std::ifstream::failure &e) - { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; - } + try + { + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; + } // count number of epochs and rewind long int num_epoch = 0; if (dump_file.is_open()) @@ -132,14 +137,17 @@ int hybrid_observables_cc::save_matfile() num_epoch = static_cast(size) / static_cast(epoch_size_bytes); dump_file.seekg(0, std::ios::beg); } - else { return 1; } - double ** RX_time = new double * [d_nchannels]; - double ** TOW_at_current_symbol_s = new double * [d_nchannels]; - double ** Carrier_Doppler_hz = new double * [d_nchannels]; - double ** Carrier_phase_cycles = new double * [d_nchannels]; - double ** Pseudorange_m = new double * [d_nchannels]; - double ** PRN = new double * [d_nchannels]; - double ** Flag_valid_pseudorange = new double * [d_nchannels]; + else + { + return 1; + } + double **RX_time = new double *[d_nchannels]; + double **TOW_at_current_symbol_s = new double *[d_nchannels]; + double **Carrier_Doppler_hz = new double *[d_nchannels]; + double **Carrier_phase_cycles = new double *[d_nchannels]; + double **Pseudorange_m = new double *[d_nchannels]; + double **PRN = new double *[d_nchannels]; + double **Flag_valid_pseudorange = new double *[d_nchannels]; for (unsigned int i = 0; i < d_nchannels; i++) { @@ -223,7 +231,10 @@ int hybrid_observables_cc::save_matfile() mat_t *matfp; matvar_t *matvar; std::string filename = d_dump_filename; - if(filename.size() > 4) { filename.erase(filename.end() - 4, filename.end()); } + 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) @@ -287,66 +298,81 @@ int hybrid_observables_cc::save_matfile() return 0; } -bool hybrid_observables_cc::interpolate_data(Gnss_Synchro& out, std::deque& data, const double& ti) +bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, std::deque &data, const double &ti) { - if((ti < data.front().RX_time) or (ti > data.back().RX_time)) { return false; } + 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 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; + 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++; - } + aux++; + } arma::interp1(t, dop, tiv, result); - out.Carrier_Doppler_hz = result(0); + out.Carrier_Doppler_hz = result(0); arma::interp1(t, cph, tiv, result); - out.Carrier_phase_rads = result(0); + out.Carrier_phase_rads = result(0); arma::interp1(t, tow, tiv, result); out.TOW_at_current_symbol_s = result(0); return result.is_finite(); } -double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro& a) +double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro &a) { - if(a.Flag_valid_word) - { - return((static_cast(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast(a.fs)); - } - else { return 0.0; } + if (a.Flag_valid_word) + { + return ((static_cast(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast(a.fs)); + } + else + { + return 0.0; + } } void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), - gr_vector_int &ninput_items_required) + gr_vector_int &ninput_items_required) { - for(unsigned int i = 0; i < d_nchannels; i++) { ninput_items_required[i] = 0; } + for (unsigned int i = 0; i < d_nchannels; i++) + { + ninput_items_required[i] = 0; + } ninput_items_required[d_nchannels] = 1; } -void hybrid_observables_cc::clean_history(std::deque& data) +void hybrid_observables_cc::clean_history(std::deque &data) { - while(data.size() > 0) - { - if((T_rx_s - data.front().RX_time) > max_delta) { data.pop_front(); } - else { return; } - } + while (data.size() > 0) + { + if ((T_rx_s - data.front().RX_time) > max_delta) + { + data.pop_front(); + } + else + { + return; + } + } } -void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector& data) +void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector &data) { std::vector::iterator it; @@ -356,180 +382,207 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(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++) + double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters + for (it = data.begin(); it != (data.end() - 1); it++) { - 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_) + for (it2 = it + 1; it2 != data.end(); it2++) { - 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; + 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; - } + for (it = data.begin(); it != data.end(); it++) + { + if (it->TOW_at_current_symbol_s > TOW_ref and it->Signal[0] != '2') + { + 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) + 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]); - Gnss_Synchro** out = reinterpret_cast(&output_items[0]); + const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); unsigned int i; int total_input_items = 0; - for(i = 0; i < d_nchannels; i++) { total_input_items += ninput_items[i]; } + for (i = 0; i < d_nchannels; i++) + { + total_input_items += ninput_items[i]; + } consume(d_nchannels, 1); T_rx_s += T_rx_step_s; ////////////////////////////////////////////////////////////////////////// - if((total_input_items == 0) and (d_num_valid_channels == 0)) - { - return 0; - } + if ((total_input_items == 0) and (d_num_valid_channels == 0)) + { + return 0; + } ////////////////////////////////////////////////////////////////////////// std::vector>::iterator it; if (total_input_items > 0) - { - i = 0; - for(it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - if(ninput_items[i] > 0) - { - // Add the new Gnss_Synchros to their corresponding deque - for(int aux = 0; aux < ninput_items[i]; aux++) + i = 0; + for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - if(in[i][aux].Flag_valid_word) - { - 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) + if (ninput_items[i] > 0) { - if(it->front().PRN != it->back().PRN) { it->clear(); } + // Add the new Gnss_Synchros to their corresponding deque + for (int aux = 0; aux < ninput_items[i]; aux++) + { + if (in[i][aux].Flag_valid_word) + { + 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) + { + if (it->front().PRN != it->back().PRN) + { + 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; } - 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; } - - for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold - { - if(valid_channels[i]) + if (d_num_valid_channels == 0) { - clean_history(d_gnss_synchro_history.at(i)); - if(d_gnss_synchro_history.at(i).size() < 2) { valid_channels[i] = false; } + return 0; + } + + 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) + { + valid_channels[i] = false; + } + } } - } // 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)) { return 0; } + if ((d_num_valid_channels == 0) or (T_rx_s_out < 0.0)) + { + return 0; + } std::vector epoch_data; i = 0; - for(it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) - { - if(valid_channels[i]) + for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - 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; - } + 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++; } - i++; - } d_num_valid_channels = valid_channels.count(); - if(d_num_valid_channels == 0) { return 0; } + 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]) + for (i = 0; i < d_nchannels; i++) { - out[i][0] = (*it2); - out[i][0].Flag_valid_pseudorange = true; - it2++; + 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; + } } - else + if (d_dump) { - out[i][0] = Gnss_Synchro(); - out[i][0].Flag_valid_pseudorange = false; + // 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; + } } - } - 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; }