mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

code style

This commit is contained in:
Antonio Ramos 2018-03-08 18:32:55 +01:00
parent c41efa9cfe
commit ab7cabc8e8
2 changed files with 311 additions and 258 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

@ -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<int, Gps_Ephemeris>::const_iterator tmp_eph_iter_gps = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN);
std::map<int, Galileo_Ephemeris>::const_iterator tmp_eph_iter_gal = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN);
std::map<int, Gps_CNAV_Ephemeris>::const_iterator tmp_eph_iter_cnav = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN);
std::map<int, Glonass_Gnav_Ephemeris>::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<int, Gnss_Synchro>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<int, Gps_CNAV_Ephemeris>::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<int, Galileo_Ephemeris>::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<int, Gps_Ephemeris>::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin();
std::map<int, Gps_CNAV_Ephemeris>::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<int, Glonass_Gnav_Ephemeris>::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()

469
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -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<Gnss_Synchro>());
}
for (unsigned int i = 0; i < d_nchannels; i++)
{
d_gnss_synchro_history.push_back(std::deque<Gnss_Synchro>());
}
// ############# 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<long int>(size) / static_cast<long int>(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<long *>(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<Gnss_Synchro>& data, const double& ti)
bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, std::deque<Gnss_Synchro> &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<Gnss_Synchro>::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<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs));
}
else { return 0.0; }
if (a.Flag_valid_word)
{
return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(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<Gnss_Synchro>& data)
void hybrid_observables_cc::clean_history(std::deque<Gnss_Synchro> &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<Gnss_Synchro>& data)
void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Synchro> &data)
{
std::vector<Gnss_Synchro>::iterator it;
@ -356,180 +382,207 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
////////////////////////////////////////////////////////
#ifndef NDEBUG
std::vector<Gnss_Synchro>::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<double>::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<const Gnss_Synchro**>(&input_items[0]);
Gnss_Synchro** out = reinterpret_cast<Gnss_Synchro**>(&output_items[0]);
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]);
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&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<std::deque<Gnss_Synchro>>::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<Gnss_Synchro> 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<Gnss_Synchro>::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<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][0].TOW_at_current_symbol_s;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Carrier_Doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Pseudorange_m;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][0].PRN);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][0].Flag_valid_pseudorange);
d_dump_file.write(reinterpret_cast<char *>(&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<char*>(&tmp_double), sizeof(double));
tmp_double = out[i][0].TOW_at_current_symbol_s;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Carrier_Doppler_hz;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Pseudorange_m;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][0].PRN);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][0].Flag_valid_pseudorange);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
d_dump = false;
}
}
return 1;
}