diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 75438755c..e4cb7f608 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include #include +#include using google::LogMessage; @@ -545,34 +547,46 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); } - - if (d_ls_pvt->gps_ephemeris_map.size() > 0) + try { - if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + if (d_ls_pvt->gps_ephemeris_map.size() > 0) { - d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->galileo_ephemeris_map.size() > 0) + { + if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) + { + if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } } } - if (d_ls_pvt->galileo_ephemeris_map.size() > 0) + catch (const boost::exception& ex) { - if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } + std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << std::endl; + LOG(ERROR) << "RTCM boost exception: " << boost::diagnostic_information(ex); } - if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + catch (const std::exception& ex) { - if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } - } - if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) - { - if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } + std::cout << "RTCM std exception: " << ex.what() << std::endl; + LOG(ERROR) << "RTCM std exception: " << ex.what(); } } } @@ -591,10 +605,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // compute on the fly PVT solution if (flag_compute_pvt_output == true) { - bool pvt_result; - pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false); - - if (pvt_result == true) + if (d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false)) { if (std::fabs(current_RX_time - last_pvt_display_T_rx_s) * 1000.0 >= static_cast(d_display_rate_ms)) { @@ -1178,88 +1189,350 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } // ####################### RTCM MESSAGES ################# - if (b_rtcm_writing_started) + try { - if (type_of_rx == 1) // GPS L1 C/A + if (b_rtcm_writing_started) { - if (flag_write_RTCM_1019_output == true) + if (type_of_rx == 1) // GPS L1 C/A + { + if (flag_write_RTCM_1019_output == true) + { + for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gps_ephemeris_iter; + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + { + if (flag_write_RTCM_1045_output == true) + { + 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++) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gal_ephemeris_iter; + gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if (type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if (flag_write_RTCM_1019_output == true) + { + for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gps_ephemeris_iter; + 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())) + { + 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); + } + } + } + if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if (flag_write_RTCM_1019_output == true) + { + for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_1045_output == true) + { + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) + { + if (system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + gps_channel = i; + } + } + } + if (gal_channel == 0) + { + if (system.compare("E") == 0) + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + gal_channel = i; + } + } + } + i++; + } + if (flag_write_RTCM_MSM_output == true) + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if (flag_write_RTCM_MSM_output == true) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + { + if (flag_write_RTCM_1020_output == true) + { + 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++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + + std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A + { + if (flag_write_RTCM_1019_output == true) + { + for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_1020_output == true) + { + 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++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if (flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) + { + if (system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + gps_channel = i; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + glo_channel = i; + } + } + } + i++; + } + if (flag_write_RTCM_MSM_output == true) + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if (flag_write_RTCM_MSM_output == true) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B + { + if (flag_write_RTCM_1020_output == true) + { + for (std::map::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++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if (flag_write_RTCM_1045_output == true) + { + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gal_channel == 0) + { + if (system.compare("E") == 0) + { + // This is a channel with valid GPS signal + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + gal_channel = i; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = i; + } + } + } + i++; + } + if (flag_write_RTCM_MSM_output == true) + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if (flag_write_RTCM_MSM_output == true) + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + } + + if (!b_rtcm_writing_started) // the first time + { + if (type_of_rx == 1) // GPS L1 C/A { for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) { d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); } - } - if (flag_write_RTCM_MSM_output == true) - { - std::map::const_iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + + std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - } - 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) + if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (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++) { d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); } - } - if (flag_write_RTCM_MSM_output == true) - { - std::map::const_iterator gal_ephemeris_iter; - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + + std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - } - if (type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - if (flag_write_RTCM_1019_output == true) + if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) { d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); } - } - if (flag_write_RTCM_MSM_output == true) - { - std::map::const_iterator gps_ephemeris_iter; - 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()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + + 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())) { 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); } + b_rtcm_writing_started = true; } - } - if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if (flag_write_RTCM_1019_output == true) + if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++) + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } } - } - if (flag_write_RTCM_1045_output == true) - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + if (d_rtcm_MT1045_rate_ms != 0) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } } - } - if (flag_write_RTCM_MSM_output == true) - { - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { @@ -1270,7 +1543,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { // This is a channel with valid GPS signal gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { gps_channel = i; } @@ -1281,7 +1554,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("E") == 0) { galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { gal_channel = i; } @@ -1289,62 +1562,54 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } i++; } - if (flag_write_RTCM_MSM_output == true) + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0)) { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 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) or (type_of_rx == 24) or (type_of_rx == 25)) // GLONASS - { - if (flag_write_RTCM_1020_output == true) + if ((type_of_rx == 23) || (type_of_rx == 24) || (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++) { d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); } - } - std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); - if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A - { - if (flag_write_RTCM_1019_output == true) - { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - if (flag_write_RTCM_1020_output == true) + if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { - 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++) + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } } - } - if (flag_write_RTCM_MSM_output == true) - { + if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + 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++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); if (gps_channel == 0) @@ -1372,42 +1637,35 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } i++; } - if (flag_write_RTCM_MSM_output == true) + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + + b_rtcm_writing_started = true; + } + if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B + { + if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + 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++) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } - if (flag_write_RTCM_MSM_output == true) + if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); } } - } - } - if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B - { - if (flag_write_RTCM_1020_output == true) - { - 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++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - } - if (flag_write_RTCM_1045_output == true) - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - if (flag_write_RTCM_MSM_output == true) - { - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { @@ -1437,266 +1695,32 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } i++; } - if (flag_write_RTCM_MSM_output == true) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } } - - if (!b_rtcm_writing_started) // the first time + catch (const boost::exception& ex) { - if (type_of_rx == 1) // GPS L1 C/A - { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - - std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - 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++) - { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); - } - - std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); - - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - - 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()) 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); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if (d_rtcm_MT1045_rate_ms != 0) - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if (gps_channel == 0) - { - if (system.compare("G") == 0) - { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - gps_channel = i; - } - } - } - if (gal_channel == 0) - { - if (system.compare("E") == 0) - { - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - gal_channel = i; - } - } - } - i++; - } - - 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() 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) 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++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - - std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); - - if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A - { - if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - 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++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - } - - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if (gps_channel == 0) - { - if (system.compare("G") == 0) - { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - gps_channel = i; - } - } - } - if (glo_channel == 0) - { - if (system.compare("R") == 0) - { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - glo_channel = i; - } - } - } - i++; - } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - b_rtcm_writing_started = true; - } - if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B - { - if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - 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++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - } - if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if (gal_channel == 0) - { - if (system.compare("E") == 0) - { - // This is a channel with valid GPS signal - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) - { - gal_channel = i; - } - } - } - if (glo_channel == 0) - { - if (system.compare("R") == 0) - { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - glo_channel = i; - } - } - } - i++; - } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } + std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << std::endl; + LOG(ERROR) << "RTCM boost exception: " << boost::diagnostic_information(ex); + } + catch (const std::exception& ex) + { + std::cout << "RTCM std exception: " << ex.what() << std::endl; + LOG(ERROR) << "RTCM std exception: " << ex.what(); } } } // DEBUG MESSAGE: Display position in console output - if ((d_ls_pvt->is_valid_position() == true) and (flag_display_pvt == true)) + if (d_ls_pvt->is_valid_position() and flag_display_pvt) { 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/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index ebeefcccf..beea0297c 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -53,7 +53,7 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], signed int _prn) for (size_t i = 0; i < Galileo_E1_B_PRIMARY_CODE[prn].length(); i++) { hex_to_binary_converter(&_dest[index], Galileo_E1_B_PRIMARY_CODE[prn].at(i)); - index = index + 4; + index += 4; } } else if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2) @@ -61,13 +61,9 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], signed int _prn) for (size_t i = 0; i < Galileo_E1_C_PRIMARY_CODE[prn].length(); i++) { hex_to_binary_converter(&_dest[index], Galileo_E1_C_PRIMARY_CODE[prn].at(i)); - index = index + 4; + index += 4; } } - else - { - return; - } } @@ -107,6 +103,18 @@ void galileo_e1_sinboc_61_gen_int(int* _dest, int* _prn, unsigned int _length_ou } } +void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], unsigned int _prn) +{ + std::string _galileo_signal = _Signal; + unsigned int _codeLength = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + int primary_code_E1_chips[4092]; // _codeLength not accepted by Clang + galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip + for (unsigned int i = 0; i < _codeLength; i++) + { + _dest[2 * i] = static_cast(primary_code_E1_chips[i]); + _dest[2 * i + 1] = -_dest[2 * i]; + } +} void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3]) { @@ -137,8 +145,6 @@ void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3]) beta * static_cast(sinboc_61[i]); } } - else - return; } diff --git a/src/algorithms/libs/galileo_e1_signal_processing.h b/src/algorithms/libs/galileo_e1_signal_processing.h index 96f0fd175..5ce893638 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.h +++ b/src/algorithms/libs/galileo_e1_signal_processing.h @@ -34,6 +34,11 @@ #include +/*! + * \brief This function generates Galileo E1 code (can select E1B or E1C sinboc). + * + */ +void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], unsigned int _prn); /*! * \brief This function generates Galileo E1 code (can select E1B or E1C, cboc or sinboc diff --git a/src/algorithms/libs/gps_l2c_signal.cc b/src/algorithms/libs/gps_l2c_signal.cc index 68fb7c024..a0d266ab7 100644 --- a/src/algorithms/libs/gps_l2c_signal.cc +++ b/src/algorithms/libs/gps_l2c_signal.cc @@ -71,6 +71,23 @@ void gps_l2c_m_code_gen_complex(std::complex* _dest, unsigned int _prn) delete[] _code; } +void gps_l2c_m_code_gen_float(float* _dest, unsigned int _prn) +{ + int32_t* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; + + if (_prn > 0 and _prn < 51) + { + gps_l2c_m_code(_code, _prn); + } + + for (signed int i = 0; i < GPS_L2_M_CODE_LENGTH_CHIPS; i++) + { + _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + } + + delete[] _code; +} + /* * Generates complex GPS L2C M code for the desired SV ID and sampled to specific sampling frequency diff --git a/src/algorithms/libs/gps_l2c_signal.h b/src/algorithms/libs/gps_l2c_signal.h index a5f48a230..627588355 100644 --- a/src/algorithms/libs/gps_l2c_signal.h +++ b/src/algorithms/libs/gps_l2c_signal.h @@ -38,7 +38,7 @@ //!Generates complex GPS L2C M code for the desired SV ID void gps_l2c_m_code_gen_complex(std::complex* _dest, unsigned int _prn); - +void gps_l2c_m_code_gen_float(float* _dest, unsigned int _prn); //! Generates complex GPS L2C M code for the desired SV ID, and sampled to specific sampling frequency void gps_l2c_m_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, signed int _fs); diff --git a/src/algorithms/libs/gps_l5_signal.cc b/src/algorithms/libs/gps_l5_signal.cc index 7a443d20f..0bacbea7b 100644 --- a/src/algorithms/libs/gps_l5_signal.cc +++ b/src/algorithms/libs/gps_l5_signal.cc @@ -197,6 +197,22 @@ void gps_l5i_code_gen_complex(std::complex* _dest, unsigned int _prn) delete[] _code; } +void gps_l5i_code_gen_float(float* _dest, unsigned int _prn) +{ + int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS]; + + if (_prn > 0 and _prn < 51) + { + make_l5i(_code, _prn - 1); + } + + for (signed int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++) + { + _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + } + + delete[] _code; +} /* * Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency @@ -264,7 +280,22 @@ void gps_l5q_code_gen_complex(std::complex* _dest, unsigned int _prn) delete[] _code; } +void gps_l5q_code_gen_float(float* _dest, unsigned int _prn) +{ + int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS]; + if (_prn > 0 and _prn < 51) + { + make_l5q(_code, _prn - 1); + } + + for (signed int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++) + { + _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + } + + delete[] _code; +} /* * Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency */ diff --git a/src/algorithms/libs/gps_l5_signal.h b/src/algorithms/libs/gps_l5_signal.h index e9fee4e4b..928180e9a 100644 --- a/src/algorithms/libs/gps_l5_signal.h +++ b/src/algorithms/libs/gps_l5_signal.h @@ -38,9 +38,11 @@ //!Generates complex GPS L5i M code for the desired SV ID void gps_l5i_code_gen_complex(std::complex* _dest, unsigned int _prn); +void gps_l5i_code_gen_float(float* _dest, unsigned int _prn); //!Generates complex GPS L5q M code for the desired SV ID void gps_l5q_code_gen_complex(std::complex* _dest, unsigned int _prn); +void gps_l5q_code_gen_float(float* _dest, unsigned int _prn); //! Generates complex GPS L5i M code for the desired SV ID, and sampled to specific sampling frequency void gps_l5i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, signed int _fs); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index 79b0fdb07..af9666c03 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -70,9 +70,9 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( d_TOW_at_Preamble = 0.0; //initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); - for (int aux = 0; aux < GPS_L5_NH_CODE_LENGTH; aux++) + for (int aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++) { - if (GPS_L5_NH_CODE[aux] == 0) + if (GPS_L5i_NH_CODE[aux] == 0) { bits_NH[aux] = -1.0; } @@ -119,9 +119,9 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u int symbol_value = 0; //Search correlation with Neuman-Hofman Code (see IS-GPS-705D) - if (sym_hist.size() == GPS_L5_NH_CODE_LENGTH) + if (sym_hist.size() == GPS_L5i_NH_CODE_LENGTH) { - for (int i = 0; i < GPS_L5_NH_CODE_LENGTH; i++) + for (int i = 0; i < GPS_L5i_NH_CODE_LENGTH; i++) { if ((bits_NH[i] * sym_hist.at(i)) > 0.0) { @@ -132,7 +132,7 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u corr_NH -= 1; } } - if (abs(corr_NH) == GPS_L5_NH_CODE_LENGTH) + if (abs(corr_NH) == GPS_L5i_NH_CODE_LENGTH) { sync_NH = true; if (corr_NH > 0) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h index c13c2832b..59f0bf108 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h @@ -42,8 +42,7 @@ #include #include -extern "C" -{ +extern "C" { #include "cnav_msg.h" #include "edc.h" #include "bits.h" @@ -91,7 +90,7 @@ private: bool d_flag_valid_word; Gps_CNAV_Navigation_Message d_CNAV_Message; - double bits_NH[GPS_L5_NH_CODE_LENGTH]; + double bits_NH[GPS_L5i_NH_CODE_LENGTH]; std::deque sym_hist; bool sync_NH; bool new_sym; 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 acae5564a..daf6e898a 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 @@ -51,7 +51,6 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( //################# CONFIGURATION PARAMETERS ######################## int fs_in; int vector_length; - int f_if; bool dump; std::string dump_filename; std::string item_type; @@ -64,11 +63,10 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( float very_early_late_space_chips; float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; - + unified_ = configuration->property(role + ".unified", false); 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", 5.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); @@ -93,23 +91,45 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { - item_size_ = sizeof(gr_complex); - tracking_ = galileo_e1_dll_pll_veml_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, - early_late_space_chips, - very_early_late_space_chips, - early_late_space_narrow_chips, - very_early_late_space_narrow_chips, - extend_correlation_symbols, - track_pilot); + if (unified_) + { + char sig_[3] = "1B"; + 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, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, 'E', sig_); + } + else + { + tracking_ = galileo_e1_dll_pll_veml_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, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot); + } } else { @@ -130,7 +150,10 @@ GalileoE1DllPllVemlTracking::~GalileoE1DllPllVemlTracking() void GalileoE1DllPllVemlTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } @@ -140,13 +163,19 @@ void GalileoE1DllPllVemlTracking::start_tracking() void GalileoE1DllPllVemlTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GalileoE1DllPllVemlTracking::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); } @@ -170,11 +199,17 @@ void GalileoE1DllPllVemlTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h index e702ff14d..2d3dfd221 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h @@ -39,6 +39,7 @@ #include "tracking_interface.h" #include "galileo_e1_dll_pll_veml_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include @@ -95,11 +96,13 @@ public: private: galileo_e1_dll_pll_veml_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_E1_DLL_PLL_VEML_TRACKING_H_ 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 d58329575..e642b29a1 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 @@ -52,41 +52,64 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( //################# 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); 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); + 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 pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_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 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)); - + int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); + if (symbols_extended_correlator < 1) symbols_extended_correlator = 1; //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { - item_size_ = sizeof(gr_complex); - tracking_ = gps_l1_ca_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] = "1C"; + 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, + symbols_extended_correlator, + false, + 'G', sig_); + } + else + { + tracking_ = gps_l1_ca_dll_pll_make_tracking_cc( + 0, + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + early_late_space_chips); + } } else { @@ -105,7 +128,10 @@ GpsL1CaDllPllTracking::~GpsL1CaDllPllTracking() void GpsL1CaDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } @@ -115,13 +141,19 @@ void GpsL1CaDllPllTracking::start_tracking() void GpsL1CaDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GpsL1CaDllPllTracking::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); } @@ -145,11 +177,17 @@ void GpsL1CaDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GpsL1CaDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GpsL1CaDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h index ff5a91899..5bf34abfc 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h @@ -40,6 +40,7 @@ #include "tracking_interface.h" #include "gps_l1_ca_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -93,11 +94,13 @@ public: private: gps_l1_ca_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_L1_CA_DLL_PLL_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 4c4a2a550..8da922bb0 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -39,6 +39,7 @@ set(TRACKING_GR_BLOCKS_SOURCES glonass_l1_ca_dll_pll_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc + dll_pll_veml_tracking.cc ${OPT_TRACKING_BLOCKS} ) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc new file mode 100755 index 000000000..9a0c97c18 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -0,0 +1,1501 @@ +/*! + * \file dll_pll_veml_tracking.cc + * \brief Implementation of a code DLL + carrier PLL VEML (Very Early + * Minus Late) tracking block for Galileo E1 signals + * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com + * + * Code DLL + carrier PLL according to the algorithms described in: + * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency + * Approach, Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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 . + * + * ------------------------------------------------------------------------- + */ + +#include "dll_pll_veml_tracking.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "control_message_factory.h" +#include "MATH_CONSTANTS.h" + +#include "Galileo_E1.h" +#include "galileo_e1_signal_processing.h" +#include "Galileo_E5a.h" +#include "galileo_e5_signal_processing.h" +#include "GPS_L1_CA.h" +#include "gps_sdr_signal_processing.h" +#include "GPS_L2C.h" +#include "gps_l2c_signal.h" +#include "GPS_L5.h" +#include "gps_l5_signal.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using google::LogMessage; + +dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking( + double fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_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, + int extend_correlation_symbols, + bool track_pilot, + char system, char signal[3]) +{ + return dll_pll_veml_tracking_sptr(new dll_pll_veml_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, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, system, signal)); +} + + +void dll_pll_veml_tracking::forecast(int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; + } +} + + +dll_pll_veml_tracking::dll_pll_veml_tracking( + double fs_in, unsigned int vector_length, bool dump, + std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_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, + int extend_correlation_symbols, bool track_pilot, char system, char signal[3]) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry bit synchronization message port input + this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_out(pmt::mp("events")); + this->set_relative_rate(1.0 / static_cast(vector_length)); + + // initialize internal vars + d_dump = dump; + d_veml = false; + d_cloop = true; + d_synchonizing = false; + d_track_pilot = track_pilot; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + d_code_chip_rate = 0.0; + d_secondary_code_length = 0; + d_secondary_code_string = nullptr; + signal_type = std::string(signal); + if (system == 'G') + { + systemName = "GPS"; + if (signal_type.compare("1C") == 0) + { + d_signal_carrier_freq = GPS_L1_FREQ_HZ; + d_code_period = GPS_L1_CA_CODE_PERIOD; + d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); + //GPS L1 C/A does not have pilot component nor secondary code + d_secondary = false; + d_track_pilot = false; + interchange_iq = false; + } + else if (signal_type.compare("2S") == 0) + { + d_signal_carrier_freq = GPS_L2_FREQ_HZ; + d_code_period = GPS_L2_M_PERIOD; + d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; + d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; + d_correlation_length_ms = 20; + d_code_samples_per_chip = 1; + //GPS L2 does not have pilot component nor secondary code + d_secondary = false; + d_track_pilot = false; + interchange_iq = false; + } + else if (signal_type.compare("L5") == 0) + { + d_signal_carrier_freq = GPS_L5_FREQ_HZ; + d_code_period = GPS_L5i_PERIOD; + d_code_chip_rate = GPS_L5i_CODE_RATE_HZ; + d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(GPS_L5i_CODE_LENGTH_CHIPS); + //GPS L5 does not have pilot secondary code + d_secondary = true; + if (d_track_pilot) + { + d_secondary_code_length = static_cast(GPS_L5q_NH_CODE_LENGTH); + d_secondary_code_string = const_cast(&GPS_L5q_NH_CODE_STR); + interchange_iq = true; + } + else + { + d_secondary_code_length = static_cast(GPS_L5i_NH_CODE_LENGTH); + d_secondary_code_string = const_cast(&GPS_L5i_NH_CODE_STR); + interchange_iq = false; + } + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + } + } + else if (system == 'E') + { + systemName = "Galileo"; + if (signal_type.compare("1B") == 0) + { + d_signal_carrier_freq = Galileo_E1_FREQ_HZ; + d_code_period = Galileo_E1_CODE_PERIOD; + d_code_chip_rate = Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_length_chips = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + d_symbols_per_bit = 1; + d_correlation_length_ms = 4; + d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip + d_veml = true; + if (d_track_pilot) + { + d_secondary = true; + d_secondary_code_length = static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E1_C_SECONDARY_CODE); + } + else + { + d_secondary = false; + } + interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + } + else if (signal_type.compare("5X") == 0) + { + d_signal_carrier_freq = Galileo_E5a_FREQ_HZ; + d_code_period = GALILEO_E5a_CODE_PERIOD; + d_code_chip_rate = Galileo_E5a_CODE_CHIP_RATE_HZ; + d_symbols_per_bit = 20; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(Galileo_E5a_CODE_LENGTH_CHIPS); + d_secondary = true; + if (d_track_pilot) + { + interchange_iq = true; + d_secondary_code_length = static_cast(Galileo_E5a_Q_SECONDARY_CODE_LENGTH); + } + else + { + interchange_iq = false; + d_secondary_code_length = static_cast(Galileo_E5a_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E5a_I_SECONDARY_CODE); + } + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + } + } + else + { + LOG(WARNING) << "Invalid System argument when instantiating tracking blocks"; + std::cout << "Invalid System argument when instantiating tracking blocks" << std::endl; + } + T_chip_seconds = 0.0; + T_prn_seconds = 0.0; + T_prn_samples = 0.0; + K_blk_samples = 0.0; + + // Initialize tracking ========================================== + + // Set bandwidth of code and carrier loop filters + d_dll_bw_hz = dll_bw_hz; + d_pll_bw_hz = pll_bw_hz; + d_dll_bw_narrow_hz = dll_bw_narrow_hz; + d_pll_bw_narrow_hz = pll_bw_narrow_hz; + + d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); + d_code_loop_filter = Tracking_2nd_DLL_filter(static_cast(d_code_period)); + d_carrier_loop_filter = Tracking_2nd_PLL_filter(static_cast(d_code_period)); + + // Correlator spacing + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + d_very_early_late_spc_chips = very_early_late_space_chips; // Define very-early-late offset (in chips) + d_early_late_spc_narrow_chips = early_late_space_narrow_chips; // Define narrow early-late offset (in chips) + d_very_early_late_spc_narrow_chips = very_early_late_space_narrow_chips; // Define narrow very-early-late offset (in chips) + + // Initialization of local code replica + // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip + d_tracking_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + // correlator outputs (scalar) + if (d_veml) + { + // Very-Early, Early, Prompt, Late, Very-Late + d_n_correlator_taps = 5; + } + else + { + // Early, Prompt, Late + d_n_correlator_taps = 3; + } + + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + + // map memory pointers of correlator outputs + if (d_veml) + { + d_Very_Early = &d_correlator_outs[0]; + d_Early = &d_correlator_outs[1]; + d_Prompt = &d_correlator_outs[2]; + d_Late = &d_correlator_outs[3]; + d_Very_Late = &d_correlator_outs[4]; + d_local_code_shift_chips[0] = -d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[2]; + } + else + { + d_Very_Early = nullptr; + d_Early = &d_correlator_outs[0]; + d_Prompt = &d_correlator_outs[1]; + d_Late = &d_correlator_outs[2]; + d_Very_Late = nullptr; + d_local_code_shift_chips[0] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[1]; + } + + multicorrelator_cpu.init(2 * d_vector_length, d_n_correlator_taps); + + if (extend_correlation_symbols > 1) + { + d_enable_extended_integration = true; + d_extend_correlation_symbols = extend_correlation_symbols; + } + else + { + d_enable_extended_integration = false; + d_extend_correlation_symbols = 1; + } + + // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal + if (d_track_pilot) + { + // Extra correlator for the data component + correlator_data_cpu.init(2 * d_vector_length, 1); + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + } + + //--- Initializations ---// + // Initial code frequency basis of NCO + d_code_freq_chips = d_code_chip_rate; + // Residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // Residual carrier phase + d_rem_carr_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; + d_acq_sample_stamp = 0; + + d_current_prn_length_samples = static_cast(d_vector_length); + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[DLL_PLL_CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = DLL_PLL_CARRIER_LOCK_THRESHOLD; + + clear_tracking_vars(); + + d_acquisition_gnss_synchro = nullptr; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_acc_carrier_phase_rad = 0.0; + + d_extend_correlation_symbols_count = 0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_K_blk_samples = 0.0; + d_code_phase_samples = 0.0; + d_last_prompt = gr_complex(0.0, 0.0); + d_state = 0; // initial state: standby +} + + +void dll_pll_veml_tracking::start_tracking() +{ + gr::thread::scoped_lock l(d_setlock); + /* + * correct the code phase according to the delay between acq and trk + */ + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + + long int acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_fs_in; + DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; + DLOG(INFO) << "Number of seconds between Acquisition and Tracking = " << acq_trk_diff_seconds; + // Doppler effect + // Fd=(C/(C+Vr))*F + double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; + // new chip and prn sequence periods based on acq Doppler + d_code_freq_chips = radial_velocity * d_code_chip_rate; + d_code_phase_step_chips = d_code_freq_chips / d_fs_in; + double T_chip_mod_seconds = 1.0 / d_code_freq_chips; + double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); + double T_prn_mod_samples = T_prn_mod_seconds * d_fs_in; + + d_current_prn_length_samples = round(T_prn_mod_samples); + + double T_prn_true_seconds = static_cast(d_code_length_chips) / d_code_chip_rate; + double T_prn_true_samples = T_prn_true_seconds * d_fs_in; + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples = fmod(d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in, T_prn_true_samples); + if (corrected_acq_phase_samples < 0.0) + { + corrected_acq_phase_samples += T_prn_mod_samples; + } + double delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; + + d_acq_code_phase_samples = corrected_acq_phase_samples; + + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_fs_in; + + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter + + if (systemName.compare("GPS") == 0 and signal_type.compare("1C") == 0) + { + gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + } + else if (systemName.compare("GPS") == 0 and signal_type.compare("2S") == 0) + { + gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + } + else if (systemName.compare("GPS") == 0 and signal_type.compare("L5") == 0) + { + if (d_track_pilot) + { + gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + } + } + else if (systemName.compare("Galileo") == 0 and signal_type.compare("1B") == 0) + { + if (d_track_pilot) + { + char pilot_signal[3] = "1C"; + galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + } + } + else if (systemName.compare("Galileo") == 0 and signal_type.compare("5X") == 0) + { + gr_complex aux_code[d_code_length_chips]; + galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); + if (d_track_pilot) + { + d_secondary_code_string = const_cast(&Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + for (unsigned int i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].imag(); + d_data_code[i] = aux_code[i].real(); + } + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + for (unsigned int i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].real(); + } + } + } + + multicorrelator_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_tracking_code, d_local_code_shift_chips); + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_rad = 0.0; + d_cn0_estimation_counter = 0; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + } + + d_code_phase_samples = d_acq_code_phase_samples; + d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); + d_carrier_loop_filter.set_pdi(static_cast(d_code_period)); + d_code_loop_filter.set_pdi(static_cast(d_code_period)); + + // DEBUG OUTPUT + std::cout << "Tracking of " << systemName << " " << signal_type << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking pull-in + d_state = 1; + d_synchonizing = false; + d_cloop = true; + d_Prompt_buffer_deque.clear(); + d_last_prompt = gr_complex(0.0, 0.0); + LOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + << ". Code Phase correction [samples] = " << delay_correction_samples + << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; +} + + +dll_pll_veml_tracking::~dll_pll_veml_tracking() +{ + if (d_dump_file.is_open()) + { + try + { + d_dump_file.close(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } + } + if (d_dump) + { + if (d_channel == 0) + { + std::cout << "Writing .mat files ..."; + } + save_matfile(); + if (d_channel == 0) + { + std::cout << " done." << std::endl; + } + } + try + { + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_tracking_code); + if (d_track_pilot) + { + volk_gnsssdr_free(d_Prompt_Data); + volk_gnsssdr_free(d_data_code); + correlator_data_cpu.free(); + } + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } +} + + +bool dll_pll_veml_tracking::acquire_secondary() +{ + //******* preamble correlation ******** + int corr_value = 0; + for (unsigned int i = 0; i < d_secondary_code_length; i++) + { + if (d_Prompt_buffer_deque.at(i).real() < 0.0) // symbols clipping + { + if (d_secondary_code_string->at(i) == '0') + { + corr_value++; + } + else + { + corr_value--; + } + } + else + { + if (d_secondary_code_string->at(i) == '0') + { + corr_value--; + } + else + { + corr_value++; + } + } + } + + if (abs(corr_value) == d_secondary_code_length) + { + return true; + } + else + { + return false; + } +} + + +bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() +{ + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < DLL_PLL_CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; + d_cn0_estimation_counter++; + return true; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, static_cast(d_fs_in), static_cast(d_code_length_chips)); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < DLL_PLL_MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + return false; + } + else + { + return true; + } + } +} + + +// correlation requires: +// - updated remnant carrier phase in radians (rem_carr_phase_rad) +// - updated remnant code phase in samples (d_rem_code_phase_samples) +// - d_code_freq_chips +// - d_carrier_doppler_hz +void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) +{ + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), + d_vector_length); + + // DATA CORRELATOR (if tracking tracks the pilot signal) + if (d_track_pilot) + { + correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples); + correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), + d_vector_length); + } +} + + +void dll_pll_veml_tracking::run_dll_pll() +{ + // ################## PLL ########################################################## + // PLL discriminator + if (d_cloop) + { + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; + } + else + { + // Secondary code acquired. No symbols transition should be present in the signal + d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; + } + + // Carrier discriminator filter + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz); + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; + // New code Doppler frequency estimation + d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate; + + // ################## DLL ########################################################## + // DLL discriminator + if (d_veml) + { + d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] + } + else + { + d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu); // [chips/Ti] + } + // Code discriminator filter + d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] +} + + +void dll_pll_veml_tracking::clear_tracking_vars() +{ + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + if (d_track_pilot) *d_Prompt_Data = gr_complex(0.0, 0.0); + d_carr_error_hz = 0.0; + d_carr_error_filt_hz = 0.0; + d_code_error_chips = 0.0; + d_code_error_filt_chips = 0.0; + d_current_symbol = 0; + d_Prompt_buffer_deque.clear(); + d_last_prompt = gr_complex(0.0, 0.0); +} + +void dll_pll_veml_tracking::update_tracking_vars() +{ + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + double code_error_filt_secs = T_prn_seconds * d_code_error_filt_chips * T_chip_seconds; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + T_prn_samples = T_prn_seconds * d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * d_fs_in; + d_current_prn_length_samples = static_cast(round(K_blk_samples)); // round to a discrete number of samples + + //################### PLL COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_fs_in; + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad += d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / d_fs_in; + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / d_fs_in; +} + +void dll_pll_veml_tracking::save_correlation_results() +{ + if (d_secondary) + { + if (d_secondary_code_string->at(d_current_symbol) == '0') + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } + else + { + if (d_veml) + { + d_VE_accu -= *d_Very_Early; + d_VL_accu -= *d_Very_Late; + } + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol %= d_secondary_code_length; + } + else + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + d_current_symbol++; + d_current_symbol %= d_symbols_per_bit; + } + // If tracking pilot, disable Costas loop + if (d_track_pilot) + d_cloop = false; + else + d_cloop = true; +} + + +void dll_pll_veml_tracking::log_data(bool integrating) +{ + if (d_dump) + { + // Dump results to file + float prompt_I; + float prompt_Q; + float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; + float tmp_float; + double tmp_double; + if (d_track_pilot) + { + if (interchange_iq) + { + prompt_I = d_Prompt_Data->imag(); + prompt_Q = d_Prompt_Data->real(); + } + else + { + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); + } + } + else + { + if (interchange_iq) + { + prompt_I = d_Prompt->imag(); + prompt_Q = d_Prompt->real(); + } + else + { + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); + } + } + if (d_veml) + { + tmp_VE = std::abs(d_VE_accu); + tmp_VL = std::abs(d_VL_accu); + } + else + { + tmp_VE = 0.0; + tmp_VL = 0.0; + } + tmp_E = std::abs(d_E_accu); + tmp_P = std::abs(d_P_accu); + tmp_L = std::abs(d_L_accu); + if (integrating) + { + // It compensates the amplitude difference while integrating + float scale_factor = static_cast(d_extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); + tmp_VE *= scale_factor; + tmp_E *= scale_factor; + tmp_P *= scale_factor; + tmp_L *= scale_factor; + tmp_VL *= scale_factor; + } + + try + { + // Dump correlators output + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = d_carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } +} + + +int dll_pll_veml_tracking::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::thread::scoped_lock l(d_setlock); + const gr_complex *in = reinterpret_cast(input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + switch (d_state) + { + case 0: // Standby - Pass Through. Full throttle + { + d_sample_counter += ninput_items[0]; + consume_each(ninput_items[0]); + return 0; + break; + } + case 1: // Pull-in + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + // Fill the acquisition data + int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + double acq_trk_shif_correction_samples = static_cast(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + int samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + if (samples_offset < 0) + { + samples_offset = 0; + } + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_state = 2; + d_sample_counter += samples_offset; // count for the processed samples + consume_each(samples_offset); // shift input to perform alignment with local replica + return 0; + } + case 2: // Wide tracking and symbol synchronization + { + do_correlation_step(in); + // Save single correlation step variables + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + + // Check lock status + if (!cn0_and_tracking_lock_status()) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + bool next_state = false; + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_dll_pll(); + update_tracking_vars(); + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(false); + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_buffer_deque.push_back(*d_Prompt); + if (d_Prompt_buffer_deque.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + std::cout << "Secondary code locked for CH " << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + + d_Prompt_buffer_deque.pop_front(); + } + } + else //Signal does not have secondary code. Search a bit transition by sign change + { + if (d_synchonizing) + { + if (d_Prompt->real() * d_last_prompt.real() > 0.0) + { + d_current_symbol++; + } + else if (d_current_symbol > d_symbols_per_bit) + { + d_synchonizing = false; + d_current_symbol = 1; + } + else + { + d_current_symbol = 1; + d_last_prompt = *d_Prompt; + } + } + else if (d_last_prompt.real() != 0.0) + { + d_current_symbol++; + if (d_current_symbol == d_symbols_per_bit) next_state = true; + } + else + { + d_last_prompt = *d_Prompt; + d_synchonizing = true; + d_current_symbol = 1; + } + } + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_last_prompt = gr_complex(0.0, 0.0); + 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) + { + 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); + d_state = 3; // next state is the extended correlator integrator + LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + 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; + } + else + { + d_state = 4; + } + } + } + break; + } + case 3: // coherent integration (correlation time extension) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // perform a correlation step + do_correlation_step(in); + update_tracking_vars(); + save_correlation_results(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) + { + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } + } + else + { + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + } + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (d_extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + log_data(true); + break; + } + case 4: // narrow tracking + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + + // perform a correlation step + do_correlation_step(in); + save_correlation_results(); + + // check lock status + if (!cn0_and_tracking_lock_status()) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + run_dll_pll(); + update_tracking_vars(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) + { + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } + } + else + { + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + } + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(false); + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + if (d_enable_extended_integration) + { + d_state = 3; //new coherent integration (correlation time extension) cycle + } + } + } + } + consume_each(d_current_prn_length_samples); + d_sample_counter += d_current_prn_length_samples; + if (current_synchro_data.Flag_valid_symbol_output) + { + current_synchro_data.fs = static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + *out[0] = current_synchro_data; + return 1; + } + return 0; +} + + +int dll_pll_veml_tracking::save_matfile() +{ + // READ DUMP FILE + std::ifstream::pos_type size; + int number_of_double_vars = 1; + int number_of_float_vars = 17; + int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + + sizeof(float) * number_of_float_vars + sizeof(unsigned int); + 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; + } + // count number of epochs and rewind + long int num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + float *abs_VE = new float[num_epoch]; + float *abs_E = new float[num_epoch]; + float *abs_P = new float[num_epoch]; + float *abs_L = new float[num_epoch]; + float *abs_VL = new float[num_epoch]; + float *Prompt_I = new float[num_epoch]; + float *Prompt_Q = new float[num_epoch]; + unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; + float *acc_carrier_phase_rad = new float[num_epoch]; + float *carrier_doppler_hz = new float[num_epoch]; + float *code_freq_chips = new float[num_epoch]; + float *carr_error_hz = new float[num_epoch]; + float *carr_error_filt_hz = new float[num_epoch]; + float *code_error_chips = new float[num_epoch]; + float *code_error_filt_chips = new float[num_epoch]; + float *CN0_SNV_dB_Hz = new float[num_epoch]; + float *carrier_lock_test = new float[num_epoch]; + float *aux1 = new float[num_epoch]; + double *aux2 = new double[num_epoch]; + unsigned int *PRN = new unsigned int[num_epoch]; + + try + { + if (dump_file.is_open()) + { + for (long int i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); + dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = d_dump_filename; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != NULL) + { + size_t dims[2] = {1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 0; +} + + +void dll_pll_veml_tracking::set_channel(unsigned int channel) +{ + gr::thread::scoped_lock l(d_setlock); + d_channel = channel; + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump) + { + if (!d_dump_file.is_open()) + { + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + 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) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} + + +void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) +{ + gr::thread::scoped_lock l(d_setlock); + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h new file mode 100755 index 000000000..6d3ddef62 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -0,0 +1,243 @@ +/*! + * \file dll_pll_veml_tracking.h + * \brief Implementation of a code DLL + carrier PLL VEML (Very Early + * Minus Late) tracking block for Galileo E1 signals + * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com + * + * ------------------------------------------------------------------------- + * + * 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_DLL_PLL_VEML_TRACKING_H +#define GNSS_SDR_DLL_PLL_VEML_TRACKING_H + +#define DLL_PLL_CN0_ESTIMATION_SAMPLES 20 +#define DLL_PLL_MINIMUM_VALID_CN0 25 +#define DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER 50 +#define DLL_PLL_CARRIER_LOCK_THRESHOLD 0.85 + +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_2nd_PLL_filter.h" +#include "cpu_multicorrelator_real_codes.h" +#include +#include +#include + + +class dll_pll_veml_tracking; + +typedef boost::shared_ptr dll_pll_veml_tracking_sptr; + +dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int vector_length, + bool dump, std::string dump_filename, + float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_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, + int extend_correlation_symbols, bool track_pilot, + char system, char signal[3]); + +/*! + * \brief This class implements a code DLL + carrier PLL VEML (Very Early + * Minus Late) tracking block for Galileo E1 signals + */ +class dll_pll_veml_tracking : public gr::block +{ +public: + ~dll_pll_veml_tracking(); + + void set_channel(unsigned int channel); + void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); + void start_tracking(); + + /*! + * \brief Code DLL + carrier PLL according to the algorithms described in: + * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, + * Birkhauser, 2007 + */ + 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 dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int vector_length, + bool dump, std::string dump_filename, + float pll_bw_hz, float dll_bw_hz, float pll_bw_narrow_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, + int extend_correlation_symbols, bool track_pilot, + char system, char signal[3]); + + dll_pll_veml_tracking(double fs_in, unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_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, + int extend_correlation_symbols, + bool track_pilot, + char system, char signal[3]); + + bool cn0_and_tracking_lock_status(); + bool acquire_secondary(); + void do_correlation_step(const gr_complex *input_samples); + void run_dll_pll(); + void update_tracking_vars(); + void clear_tracking_vars(); + void save_correlation_results(); + void log_data(bool integrating); + int save_matfile(); + + // tracking configuration vars + bool d_dump; + bool d_veml; + bool d_cloop; + unsigned int d_vector_length; + unsigned int d_channel; + double d_fs_in; + Gnss_Synchro *d_acquisition_gnss_synchro; + + //Signal parameters + bool d_secondary; + bool interchange_iq; + double d_signal_carrier_freq; + double d_code_period; + double d_code_chip_rate; + unsigned int d_secondary_code_length; + unsigned int d_code_length_chips; + unsigned int d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) + int d_symbols_per_bit; + std::string systemName; + std::string signal_type; + std::string *d_secondary_code_string; + + //tracking state machine + int d_state; + bool d_synchonizing; + //Integration period in samples + int d_correlation_length_ms; + int d_n_correlator_taps; + float d_early_late_spc_chips; + float d_very_early_late_spc_chips; + float d_early_late_spc_narrow_chips; + float d_very_early_late_spc_narrow_chips; + + float *d_tracking_code; + float *d_data_code; + float *d_local_code_shift_chips; + float *d_prompt_data_shift; + cpu_multicorrelator_real_codes multicorrelator_cpu; + cpu_multicorrelator_real_codes correlator_data_cpu; //for data channel + /* TODO: currently the multicorrelator does not support adding extra correlator + with different local code, thus we need extra multicorrelator instance. + Implement this functionality inside multicorrelator class + as an enhancement to increase the performance + */ + gr_complex *d_correlator_outs; + gr_complex *d_Very_Early; + gr_complex *d_Early; + gr_complex *d_Prompt; + gr_complex *d_Late; + gr_complex *d_Very_Late; + + bool d_enable_extended_integration; + int d_extend_correlation_symbols; + int d_extend_correlation_symbols_count; + int d_current_symbol; + + gr_complex d_VE_accu; + gr_complex d_E_accu; + gr_complex d_P_accu; + gr_complex d_L_accu; + gr_complex d_VL_accu; + gr_complex d_last_prompt; + + bool d_track_pilot; + gr_complex *d_Prompt_Data; + + double d_code_phase_step_chips; + double d_carrier_phase_step_rad; + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + double d_rem_carr_phase_rad; + + // PLL and DLL filter library + Tracking_2nd_DLL_filter d_code_loop_filter; + Tracking_2nd_PLL_filter d_carrier_loop_filter; + + // acquisition + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + + // tracking parameters + float d_dll_bw_hz; + float d_pll_bw_hz; + float d_dll_bw_narrow_hz; + float d_pll_bw_narrow_hz; + // tracking vars + double d_carr_error_hz; + double d_carr_error_filt_hz; + double d_code_error_chips; + double d_code_error_filt_chips; + double d_K_blk_samples; + double d_code_freq_chips; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_rem_code_phase_chips; + double d_code_phase_samples; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; + //PRN period in samples + int d_current_prn_length_samples; + //processing samples counters + unsigned long int d_sample_counter; + unsigned long int d_acq_sample_stamp; + + // CN0 estimation and lock detector + int d_cn0_estimation_counter; + int d_carrier_lock_fail_counter; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + std::deque d_Prompt_buffer_deque; + gr_complex *d_Prompt_buffer; + + // file dump + std::string d_dump_filename; + std::ofstream d_dump_file; +}; + +#endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_H diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index 5de891064..6fa82351c 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -122,7 +122,7 @@ bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( lv_32fc_t phase_offset_as_complex[1]; phase_offset_as_complex[0] = lv_cmake(std::cos(rem_carrier_phase_in_rad), -std::sin(rem_carrier_phase_in_rad)); // call VOLK_GNSSSDR kernel - volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0, -phase_step_rad)), phase_offset_as_complex, (const float**)d_local_codes_resampled, d_n_correlators, signal_length_samples); + volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, const_cast(d_local_codes_resampled), d_n_correlators, signal_length_samples); return true; } diff --git a/src/algorithms/tracking/libs/lock_detectors.cc b/src/algorithms/tracking/libs/lock_detectors.cc index 331391978..4457e361f 100644 --- a/src/algorithms/tracking/libs/lock_detectors.cc +++ b/src/algorithms/tracking/libs/lock_detectors.cc @@ -67,20 +67,20 @@ */ float cn0_svn_estimator(gr_complex* Prompt_buffer, int length, long fs_in, double code_length) { - double SNR = 0; - double SNR_dB_Hz = 0; - double Psig = 0; - double Ptot = 0; + double SNR = 0.0; + double SNR_dB_Hz = 0.0; + double Psig = 0.0; + double Ptot = 0.0; for (int i = 0; i < length; i++) { Psig += std::abs(static_cast(Prompt_buffer[i].real())); Ptot += static_cast(Prompt_buffer[i].imag()) * static_cast(Prompt_buffer[i].imag()) + static_cast(Prompt_buffer[i].real()) * static_cast(Prompt_buffer[i].real()); } - Psig = Psig / static_cast(length); + Psig /= static_cast(length); Psig = Psig * Psig; - Ptot = Ptot / static_cast(length); + Ptot /= static_cast(length); SNR = Psig / (Ptot - Psig); - SNR_dB_Hz = 10 * log10(SNR) + 10 * log10(static_cast(fs_in) / 2) - 10 * log10(code_length); + SNR_dB_Hz = 10.0 * log10(SNR) + 10.0 * log10(static_cast(fs_in) / 2.0) - 10.0 * log10(code_length); return static_cast(SNR_dB_Hz); } @@ -96,10 +96,10 @@ float cn0_svn_estimator(gr_complex* Prompt_buffer, int length, long fs_in, doubl */ float carrier_lock_detector(gr_complex* Prompt_buffer, int length) { - float tmp_sum_I = 0; - float tmp_sum_Q = 0; - float NBD = 0; - float NBP = 0; + float tmp_sum_I = 0.0; + float tmp_sum_Q = 0.0; + float NBD = 0.0; + float NBP = 0.0; for (int i = 0; i < length; i++) { tmp_sum_I += Prompt_buffer[i].real(); diff --git a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc index fe234322e..3c9fdb117 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc +++ b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc @@ -41,11 +41,10 @@ void Tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k) { // Solve natural frequency - float Wn; - Wn = lbw * 8 * zeta / (4 * zeta * zeta + 1); + float Wn = lbw * 8.0 * zeta / (4.0 * zeta * zeta + 1.0); // solve for t1 & t2 *tau1 = k / (Wn * Wn); - *tau2 = (2.0 * zeta) / Wn; + *tau2 = 2.0 * zeta / Wn; } @@ -67,9 +66,7 @@ void Tracking_2nd_DLL_filter::initialize() float Tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator) { - float code_nco; - code_nco = d_old_code_nco + (d_tau2_code / d_tau1_code) * (DLL_discriminator - d_old_code_error) + (DLL_discriminator + d_old_code_error) * (d_pdi_code / (2 * d_tau1_code)); - //code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(DLL_discriminator - d_old_code_error) + DLL_discriminator * (d_pdi_code/d_tau1_code); + float code_nco = d_old_code_nco + (d_tau2_code / d_tau1_code) * (DLL_discriminator - d_old_code_error) + (DLL_discriminator + d_old_code_error) * (d_pdi_code / (2.0 * d_tau1_code)); d_old_code_nco = code_nco; d_old_code_error = DLL_discriminator; //[chips] return code_nco; diff --git a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h index 57660c682..62c570a21 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h +++ b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h @@ -49,13 +49,13 @@ class Tracking_2nd_DLL_filter { private: // PLL filter parameters - float d_tau1_code = 0; - float d_tau2_code = 0; - float d_pdi_code = 0; - float d_dllnoisebandwidth = 0; - float d_dlldampingratio = 0; - float d_old_code_error = 0; - float d_old_code_nco = 0; + float d_tau1_code = 0.0; + float d_tau2_code = 0.0; + float d_pdi_code = 0.0; + float d_dllnoisebandwidth = 0.0; + float d_dlldampingratio = 0.0; + float d_old_code_error = 0.0; + float d_old_code_nco = 0.0; void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k); public: diff --git a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc index 90ec3331f..b455ce546 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc +++ b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc @@ -40,11 +40,10 @@ void Tracking_2nd_PLL_filter::calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k) { // Solve natural frequency - float Wn; - Wn = lbw * 8 * zeta / (4 * zeta * zeta + 1); + float Wn = lbw * 8.0 * zeta / (4.0 * zeta * zeta + 1.0); // solve for t1 & t2 *tau1 = k / (Wn * Wn); - *tau2 = (2.0 * zeta) / Wn; + *tau2 = 2.0 * zeta / Wn; } @@ -71,8 +70,7 @@ void Tracking_2nd_PLL_filter::initialize() */ float Tracking_2nd_PLL_filter::get_carrier_nco(float PLL_discriminator) { - float carr_nco; - carr_nco = d_old_carr_nco + (d_tau2_carr / d_tau1_carr) * (PLL_discriminator - d_old_carr_error) + (PLL_discriminator + d_old_carr_error) * (d_pdi_carr / (2 * d_tau1_carr)); + float carr_nco = d_old_carr_nco + (d_tau2_carr / d_tau1_carr) * (PLL_discriminator - d_old_carr_error) + (PLL_discriminator + d_old_carr_error) * (d_pdi_carr / (2.0 * d_tau1_carr)); //carr_nco = d_old_carr_nco + (d_tau2_carr/d_tau1_carr)*(PLL_discriminator - d_old_carr_error) + PLL_discriminator * (d_pdi_carr/d_tau1_carr); d_old_carr_nco = carr_nco; d_old_carr_error = PLL_discriminator; diff --git a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h index d02cc17df..833e8a2c6 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h +++ b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h @@ -48,15 +48,15 @@ class Tracking_2nd_PLL_filter { private: // PLL filter parameters - float d_tau1_carr = 0; - float d_tau2_carr = 0; - float d_pdi_carr = 0; + float d_tau1_carr = 0.0; + float d_tau2_carr = 0.0; + float d_pdi_carr = 0.0; - float d_pllnoisebandwidth = 0; - float d_plldampingratio = 0; + float d_pllnoisebandwidth = 0.0; + float d_plldampingratio = 0.0; - float d_old_carr_error = 0; - float d_old_carr_nco = 0; + float d_old_carr_error = 0.0; + float d_old_carr_nco = 0.0; void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k); diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc index d4c9b2a48..0ab876591 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.cc +++ b/src/algorithms/tracking/libs/tracking_discriminators.cc @@ -83,7 +83,7 @@ double pll_cloop_two_quadrant_atan(gr_complex prompt_s1) } else { - return 0; + return 0.0; } } @@ -107,7 +107,7 @@ double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) } else { - return 0.5 * (P_early - P_late) / ((P_early + P_late)); + return 0.5 * (P_early - P_late) / (P_early + P_late); } } @@ -131,6 +131,6 @@ double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr } else { - return (P_early - P_late) / ((P_early + P_late)); + return (P_early - P_late) / (P_early + P_late); } } diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index 2571202c3..156a60d92 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -37,9 +37,6 @@ #include -#define MAX_LOOP_ORDER 3 -#define MAX_HISTORY_LENGTH 4 - Tracking_loop_filter::Tracking_loop_filter(float update_interval, float noise_bandwidth, int loop_order, @@ -50,8 +47,8 @@ Tracking_loop_filter::Tracking_loop_filter(float update_interval, d_noise_bandwidth(noise_bandwidth), d_update_interval(update_interval) { - d_inputs.resize(MAX_HISTORY_LENGTH, 0.0); - d_outputs.resize(MAX_HISTORY_LENGTH, 0.0); + d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); + d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); update_coefficients(); } @@ -62,8 +59,8 @@ Tracking_loop_filter::Tracking_loop_filter() d_noise_bandwidth(15.0), d_update_interval(0.001) { - d_inputs.resize(MAX_HISTORY_LENGTH, 0.0); - d_outputs.resize(MAX_HISTORY_LENGTH, 0.0); + d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); + d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); update_coefficients(); } @@ -75,12 +72,12 @@ Tracking_loop_filter::~Tracking_loop_filter() float Tracking_loop_filter::apply(float current_input) { // Now apply the filter coefficients: - float result = 0; + float result = 0.0; // Hanlde the old outputs first: for (unsigned int ii = 0; ii < d_output_coefficients.size(); ++ii) { - result += d_output_coefficients[ii] * d_outputs[(d_current_index + ii) % MAX_HISTORY_LENGTH]; + result += d_output_coefficients[ii] * d_outputs[(d_current_index + ii) % MAX_LOOP_HISTORY_LENGTH]; } // Now update the index to handle the inputs. @@ -93,7 +90,7 @@ float Tracking_loop_filter::apply(float current_input) d_current_index--; if (d_current_index < 0) { - d_current_index += MAX_HISTORY_LENGTH; + d_current_index += MAX_LOOP_HISTORY_LENGTH; } d_inputs[d_current_index] = current_input; @@ -101,7 +98,7 @@ float Tracking_loop_filter::apply(float current_input) for (unsigned int ii = 0; ii < d_input_coefficients.size(); ++ii) { - result += d_input_coefficients[ii] * d_inputs[(d_current_index + ii) % MAX_HISTORY_LENGTH]; + result += d_input_coefficients[ii] * d_inputs[(d_current_index + ii) % MAX_LOOP_HISTORY_LENGTH]; } @@ -122,7 +119,7 @@ void Tracking_loop_filter::update_coefficients(void) float wn; float T = d_update_interval; - float zeta = 1 / std::sqrt(2); + float zeta = 1.0 / std::sqrt(2.0); // The following is based on the bilinear transform approximation of // the analog integrator. The loop format is from Kaplan & Hegarty @@ -146,7 +143,7 @@ void Tracking_loop_filter::update_coefficients(void) d_input_coefficients[1] = g1 * T / 2.0; d_output_coefficients.resize(1); - d_output_coefficients[0] = 1; + d_output_coefficients[0] = 1.0; } else { @@ -157,28 +154,28 @@ void Tracking_loop_filter::update_coefficients(void) } break; case 2: - wn = d_noise_bandwidth * (8 * zeta) / (4 * zeta * zeta + 1); + wn = d_noise_bandwidth * (8.0 * zeta) / (4.0 * zeta * zeta + 1.0); g1 = wn * wn; - g2 = wn * 2 * zeta; + g2 = wn * 2.0 * zeta; if (d_include_last_integrator) { d_input_coefficients.resize(3); - d_input_coefficients[0] = T / 2 * (g1 * T / 2 + g2); - d_input_coefficients[1] = T * T / 2 * g1; - d_input_coefficients[2] = T / 2 * (g1 * T / 2 - g2); + d_input_coefficients[0] = T / 2.0 * (g1 * T / 2.0 + g2); + d_input_coefficients[1] = T * T / 2.0 * g1; + d_input_coefficients[2] = T / 2.0 * (g1 * T / 2.0 - g2); d_output_coefficients.resize(2); - d_output_coefficients[0] = 2; - d_output_coefficients[1] = -1; + d_output_coefficients[0] = 2.0; + d_output_coefficients[1] = -1.0; } else { d_input_coefficients.resize(2); d_input_coefficients[0] = (g1 * T / 2.0 + g2); - d_input_coefficients[1] = g1 * T / 2 - g2; + d_input_coefficients[1] = g1 * T / 2.0 - g2; d_output_coefficients.resize(1); - d_output_coefficients[0] = 1; + d_output_coefficients[0] = 1.0; } break; @@ -193,27 +190,27 @@ void Tracking_loop_filter::update_coefficients(void) if (d_include_last_integrator) { d_input_coefficients.resize(4); - d_input_coefficients[0] = T / 2 * (g3 + T / 2 * (g2 + T / 2 * g1)); - d_input_coefficients[1] = T / 2 * (-g3 + T / 2 * (g2 + 3 * T / 2 * g1)); - d_input_coefficients[2] = T / 2 * (-g3 - T / 2 * (g2 - 3 * T / 2 * g1)); - d_input_coefficients[3] = T / 2 * (g3 - T / 2 * (g2 - T / 2 * g1)); + d_input_coefficients[0] = T / 2.0 * (g3 + T / 2.0 * (g2 + T / 2.0 * g1)); + d_input_coefficients[1] = T / 2.0 * (-g3 + T / 2.0 * (g2 + 3.0 * T / 2.0 * g1)); + d_input_coefficients[2] = T / 2.0 * (-g3 - T / 2.0 * (g2 - 3.0 * T / 2.0 * g1)); + d_input_coefficients[3] = T / 2.0 * (g3 - T / 2.0 * (g2 - T / 2.0 * g1)); d_output_coefficients.resize(3); - d_output_coefficients[0] = 3; - d_output_coefficients[1] = -3; - d_output_coefficients[2] = 1; + d_output_coefficients[0] = 3.0; + d_output_coefficients[1] = -3.0; + d_output_coefficients[2] = 1.0; } else { d_input_coefficients.resize(3); - d_input_coefficients[0] = g3 + T / 2 * (g2 + T / 2 * g1); - d_input_coefficients[1] = g1 * T * T / 2 - 2 * g3; - d_input_coefficients[2] = g3 + T / 2 * (-g2 + T / 2 * g1); + d_input_coefficients[0] = g3 + T / 2.0 * (g2 + T / 2.0 * g1); + d_input_coefficients[1] = g1 * T * T / 2.0 - 2.0 * g3; + d_input_coefficients[2] = g3 + T / 2.0 * (-g2 + T / 2.0 * g1); d_output_coefficients.resize(2); - d_output_coefficients[0] = 2; - d_output_coefficients[1] = -1; + d_output_coefficients[0] = 2.0; + d_output_coefficients[1] = -1.0; } break; }; @@ -254,7 +251,7 @@ bool Tracking_loop_filter::get_include_last_integrator(void) const void Tracking_loop_filter::set_order(int loop_order) { - if (loop_order < 1 || loop_order > MAX_LOOP_ORDER) + if (loop_order < 1 or loop_order > MAX_LOOP_ORDER) { LOG(ERROR) << "Ignoring attempt to set loop order to " << loop_order << ". Maximum allowed order is: " << MAX_LOOP_ORDER @@ -274,7 +271,7 @@ int Tracking_loop_filter::get_order(void) const void Tracking_loop_filter::initialize(float initial_output) { - d_inputs.assign(MAX_HISTORY_LENGTH, 0.0); - d_outputs.assign(MAX_HISTORY_LENGTH, initial_output); - d_current_index = MAX_HISTORY_LENGTH - 1; + d_inputs.assign(MAX_LOOP_HISTORY_LENGTH, 0.0); + d_outputs.assign(MAX_LOOP_HISTORY_LENGTH, initial_output); + d_current_index = MAX_LOOP_HISTORY_LENGTH - 1; } diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.h b/src/algorithms/tracking/libs/tracking_loop_filter.h index 7fa486960..9a852ba68 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.h +++ b/src/algorithms/tracking/libs/tracking_loop_filter.h @@ -33,6 +33,8 @@ #ifndef GNSS_SDR_TRACKING_LOOP_FILTER_H_ #define GNSS_SDR_TRACKING_LOOP_FILTER_H_ +#define MAX_LOOP_ORDER 3 +#define MAX_LOOP_HISTORY_LENGTH 4 #include diff --git a/src/core/system_parameters/GPS_L5.h b/src/core/system_parameters/GPS_L5.h index af317cfc3..ff7ded0d4 100644 --- a/src/core/system_parameters/GPS_L5.h +++ b/src/core/system_parameters/GPS_L5.h @@ -36,6 +36,7 @@ #include "GPS_CNAV.h" #include "MATH_CONSTANTS.h" #include +#include // Physical constants @@ -181,7 +182,11 @@ const int GPS_L5_SYMBOLS_PER_BIT = 2; const int GPS_L5_SAMPLES_PER_SYMBOL = 10; const int GPS_L5_CNAV_DATA_PAGE_SYMBOLS = 600; const int GPS_L5_CNAV_DATA_PAGE_DURATION_S = 6; -const int GPS_L5_NH_CODE_LENGTH = 10; -const int GPS_L5_NH_CODE[10] = {0, 0, 0, 0, 1, 1, 0, 1, 0, 1}; +const int GPS_L5i_NH_CODE_LENGTH = 10; +const int GPS_L5i_NH_CODE[10] = {0, 0, 0, 0, 1, 1, 0, 1, 0, 1}; +const std::string GPS_L5i_NH_CODE_STR = "0000110101"; +const int GPS_L5q_NH_CODE_LENGTH = 20; +const int GPS_L5q_NH_CODE[20] = {0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 1, 0, 1, 0, 0, 1, 1, 1, 0}; +const std::string GPS_L5q_NH_CODE_STR = "00000100110101001110"; #endif /* GNSS_SDR_GPS_L5_H_ */ diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 267df4557..8f4c4d880 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -50,7 +50,7 @@ public: double Acq_delay_samples; //!< Set by Acquisition processing block double Acq_doppler_hz; //!< Set by Acquisition processing block unsigned long int Acq_samplestamp_samples; //!< Set by Acquisition processing block - bool Flag_valid_acquisition; //!< Set by Acquisition processing block + bool Flag_valid_acquisition = false; //!< Set by Acquisition processing block //Tracking long int fs; //!< Set by Tracking processing block double Prompt_I; //!< Set by Tracking processing block @@ -61,17 +61,17 @@ public: double Code_phase_samples; //!< Set by Tracking processing block unsigned long int Tracking_sample_counter; //!< Set by Tracking processing block - bool Flag_valid_symbol_output; //!< Set by Tracking processing block - int correlation_length_ms; //!< Set by Tracking processing block + bool Flag_valid_symbol_output = false; //!< Set by Tracking processing block + int correlation_length_ms; //!< Set by Tracking processing block //Telemetry Decoder - bool Flag_valid_word; //!< Set by Telemetry Decoder processing block + bool Flag_valid_word = false; //!< Set by Telemetry Decoder processing block double TOW_at_current_symbol_s; //!< Set by Telemetry Decoder processing block // Observables - double Pseudorange_m; //!< Set by Observables processing block - double RX_time; //!< Set by Observables processing block - bool Flag_valid_pseudorange; //!< Set by Observables processing block + double Pseudorange_m; //!< Set by Observables processing block + double RX_time; //!< Set by Observables processing block + bool Flag_valid_pseudorange = false; //!< Set by Observables processing block }; #endif diff --git a/src/utils/matlab/dll_pll_veml_plot_sample.m b/src/utils/matlab/dll_pll_veml_plot_sample.m new file mode 100644 index 000000000..15b86f098 --- /dev/null +++ b/src/utils/matlab/dll_pll_veml_plot_sample.m @@ -0,0 +1,84 @@ +% /*! +% * \file dll_pll_vml_plot_sample.m +% * \brief Read GNSS-SDR Tracking dump binary file using the provided +% function and plot some internal variables +% * \author Javier Arribas, 2011. jarribas(at)cttc.es +% * \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 . +% * +% * ------------------------------------------------------------------------- +% */ +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m','file') + addpath('./libs') +end + +samplingFreq = 5000000; %[Hz] +coherent_integration_time_ms = 20; %[ms] +channels = 5; % Number of channels +first_channel = 0; % Number of the first channel + +path = '/dump_dir/'; %% CHANGE THIS PATH + +for N=1:1:channels + tracking_log_path = [path 'track_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch BY YOUR dump_filename + GNSS_tracking(N)= dll_pll_veml_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).P.'; + trackResults(N).Q_P = zeros(1,length(GNSS_tracking(N).P)); + + trackResults(N).I_VE = GNSS_tracking(N).VE.'; + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).I_VL = GNSS_tracking(N).VL.'; + trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE)); + trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L)); + trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); + trackResults(N).data_I = GNSS_tracking(N).prompt_I.'; + trackResults(N).data_Q = GNSS_tracking(N).prompt_Q.'; + trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E)*coherent_integration_time_ms; + plotVEMLTracking(N,trackResults,settings) +end + + + diff --git a/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m new file mode 100644 index 000000000..daa2324dd --- /dev/null +++ b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m @@ -0,0 +1,153 @@ +% /*! +% * \file dll_pll_veml_read_tracking_dump.m +% * \brief Read GNSS-SDR Tracking dump binary file into MATLAB. +% * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2012 (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 . +% * +% * ------------------------------------------------------------------------- +% */ + +function [GNSS_tracking] = dll_pll_veml_read_tracking_dump (filename, count) + %% usage: dll_pll_veml_read_tracking_dump (filename, [count]) + %% + %% open GNSS-SDR tracking binary log file .dat and return the contents + %% + + m = nargchk (1,2,nargin); + + num_float_vars = 17; + num_unsigned_long_int_vars = 1; + num_double_vars = 1; + num_unsigned_int_vars = 1; + + if(~isempty(strfind(computer('arch'), '64'))) + % 64-bit computer + double_size_bytes = 8; + unsigned_long_int_size_bytes = 8; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; + else + double_size_bytes = 8; + unsigned_long_int_size_bytes = 4; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; + end + + skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ... + double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes; + + bytes_shift = 0; + + if (m) + usage (m); + end + + if (nargin < 2) + count = Inf; + end + %loops_counter = fread (f, count, 'uint32',4*12); + f = fopen (filename, 'rb'); + if (f < 0) + else + v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes); + bytes_shift = bytes_shift + unsigned_long_int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v18 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next double + v19 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); + bytes_shift = bytes_shift + double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next unsigned int + v20 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); + fclose (f); + + GNSS_tracking.VE = v1; + GNSS_tracking.E = v2; + GNSS_tracking.P = v3; + GNSS_tracking.L = v4; + GNSS_tracking.VL = v5; + GNSS_tracking.prompt_I = v6; + GNSS_tracking.prompt_Q = v7; + GNSS_tracking.PRN_start_sample = v8; + GNSS_tracking.acc_carrier_phase_rad = v9; + GNSS_tracking.carrier_doppler_hz = v10; + GNSS_tracking.code_freq_hz = v11; + GNSS_tracking.carr_error = v12; + GNSS_tracking.carr_nco = v13; + GNSS_tracking.code_error = v14; + GNSS_tracking.code_nco = v15; + GNSS_tracking.CN0_SNV_dB_Hz = v16; + GNSS_tracking.carrier_lock_test = v17; + GNSS_tracking.var1 = v18; + GNSS_tracking.var2 = v19; + GNSS_tracking.PRN = v20; + end + diff --git a/src/utils/matlab/libs/plotVEMLTracking.m b/src/utils/matlab/libs/plotVEMLTracking.m index 8af2225f9..f3846e164 100644 --- a/src/utils/matlab/libs/plotVEMLTracking.m +++ b/src/utils/matlab/libs/plotVEMLTracking.m @@ -69,8 +69,8 @@ for channelNr = channelList timeAxisInSeconds = (1:4:settings.msToProcess)/1000; %----- Discrete-Time Scatter Plot --------------------------------- - plot(handles(1, 1), trackResults(channelNr).I_P,... - trackResults(channelNr).Q_P, ... + plot(handles(1, 1), trackResults(channelNr).data_I,... + trackResults(channelNr).data_Q, ... '.'); grid (handles(1, 1)); @@ -80,8 +80,9 @@ for channelNr = channelList ylabel(handles(1, 1), 'Q prompt'); %----- Nav bits --------------------------------------------------- - plot (handles(1, 2), timeAxisInSeconds, ... - trackResults(channelNr).I_P); + t = (1:length(trackResults(channelNr).data_I)); + plot (handles(1, 2), t, ... + trackResults(channelNr).data_I); grid (handles(1, 2)); title (handles(1, 2), 'Bits of the navigation message'); @@ -89,7 +90,8 @@ for channelNr = channelList axis (handles(1, 2), 'tight'); %----- PLL discriminator unfiltered-------------------------------- - plot (handles(2, 1), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).pllDiscr)); + plot (handles(2, 1), t, ... trackResults(channelNr).pllDiscr, 'r'); grid (handles(2, 1)); @@ -99,7 +101,8 @@ for channelNr = channelList title (handles(2, 1), 'Raw PLL discriminator'); %----- Correlation ------------------------------------------------ - plot(handles(2, 2), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).I_VE)); + plot(handles(2, 2), t, ... [sqrt(trackResults(channelNr).I_VE.^2 + ... trackResults(channelNr).Q_VE.^2)', ... sqrt(trackResults(channelNr).I_E.^2 + ... @@ -127,7 +130,8 @@ for channelNr = channelList set(hLegend, 'Interpreter', 'Latex'); %----- PLL discriminator filtered---------------------------------- - plot (handles(3, 1), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).pllDiscrFilt)); + plot (handles(3, 1), t, ... trackResults(channelNr).pllDiscrFilt, 'b'); grid (handles(3, 1)); @@ -137,7 +141,8 @@ for channelNr = channelList title (handles(3, 1), 'Filtered PLL discriminator'); %----- DLL discriminator unfiltered-------------------------------- - plot (handles(3, 2), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).dllDiscr)); + plot (handles(3, 2), t, ... trackResults(channelNr).dllDiscr, 'r'); grid (handles(3, 2)); @@ -147,7 +152,8 @@ for channelNr = channelList title (handles(3, 2), 'Raw DLL discriminator'); %----- DLL discriminator filtered---------------------------------- - plot (handles(3, 3), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).dllDiscrFilt)); + plot (handles(3, 3), t, ... trackResults(channelNr).dllDiscrFilt, 'b'); grid (handles(3, 3));