diff --git a/docs/doxygen/Doxyfile.in b/docs/doxygen/Doxyfile.in index ab11a667f..9d0978e3f 100644 --- a/docs/doxygen/Doxyfile.in +++ b/docs/doxygen/Doxyfile.in @@ -129,7 +129,7 @@ INLINE_INHERITED_MEMB = NO # path before files name in the file list and in the header files. If set # to NO the shortest path that makes the file name unique will be used. -FULL_PATH_NAMES = YES +FULL_PATH_NAMES = NO # If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag # can be used to strip a user-defined part of the path. Stripping is @@ -703,7 +703,7 @@ EXAMPLE_RECURSIVE = NO # directories that contain image that are included in the documentation (see # the \image command). -IMAGE_PATH = @top_srcdir@/docs/doxygen/images/ +IMAGE_PATH = @top_srcdir@/docs/doxygen/images/ # The INPUT_FILTER tag can be used to specify a program that doxygen should # invoke to filter for each input file. Doxygen will invoke the filter program @@ -791,7 +791,7 @@ REFERENCES_LINK_SOURCE = YES # The default value is: YES. # This tag requires that the tag SOURCE_BROWSER is set to YES. -SOURCE_TOOLTIPS = YES +SOURCE_TOOLTIPS = YES # If the USE_HTAGS tag is set to YES then the references to source code # will point to the HTML generated by the htags(1) tool instead of doxygen @@ -901,7 +901,7 @@ HTML_COLORSTYLE_GAMMA = 80 # page will contain the date and time when the page was generated. Setting # this to NO can help when comparing the output of multiple runs. -HTML_TIMESTAMP = YES +HTML_TIMESTAMP = NO # If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, # files or namespaces will be aligned in HTML using tables. If set to @@ -1164,7 +1164,7 @@ MATHJAX_EXTENSIONS = # example see the documentation. # This tag requires that the tag USE_MATHJAX is set to YES. -MATHJAX_CODEFILE = +MATHJAX_CODEFILE = # When the SEARCHENGINE tag is enabled doxygen will generate a search box # for the HTML output. The underlying search engine uses javascript @@ -1677,7 +1677,7 @@ DOT_FONTSIZE = 10 # different font using DOT_FONTNAME you can set the path where dot # can find it using this tag. -DOT_FONTPATH = +DOT_FONTPATH = # If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen # will generate a graph for each documented class showing the direct and diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 5509f6bfc..a839fcc63 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -34,12 +34,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include using google::LogMessage; @@ -544,34 +546,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(); } } } @@ -590,10 +604,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)) { @@ -1274,88 +1285,480 @@ 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 (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 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 == 30) // GLONASS L2 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) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (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(); + + 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++) { @@ -1366,7 +1769,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; } @@ -1377,7 +1780,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; } @@ -1385,62 +1788,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) || (type_of_rx == 24) || (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) @@ -1468,42 +1863,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++) { @@ -1533,44 +1921,36 @@ 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); + } + 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 (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 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_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + 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 { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + 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 (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 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++) + 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) @@ -1598,42 +1978,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 == 30) // GLONASS L2 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 == 30) // GLONASS L2 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++) { @@ -1663,381 +2036,33 @@ 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) || (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); - } - - 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()) && (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() && (d_rtcm_MT1077_rate_ms != 0)) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0)) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS - { - 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); - } - } - if (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 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 == 30) // GLONASS L2 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) && (flag_display_pvt == true)) + if (d_ls_pvt->is_valid_position() and flag_display_pvt) { std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " 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/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index e22c6f509..c034b251c 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -25,7 +25,7 @@ if(ENABLE_FPGA) SET(OPT_TRACKING_ADAPTERS ${OPT_TRACKING_ADAPTERS} gps_l1_ca_dll_pll_c_aid_tracking_fpga.cc) endif(ENABLE_FPGA) -set(TRACKING_ADAPTER_SOURCES +set(TRACKING_ADAPTER_SOURCES galileo_e1_dll_pll_veml_tracking.cc galileo_e1_tcp_connector_tracking.cc gps_l1_ca_dll_pll_tracking.cc @@ -40,7 +40,7 @@ set(TRACKING_ADAPTER_SOURCES glonass_l2_ca_dll_pll_c_aid_tracking.cc ${OPT_TRACKING_ADAPTERS} ) - + include_directories( ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_SOURCE_DIR}/src/core/system_parameters 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..f6ca8df87 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,9 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( float very_early_late_space_chips; float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; - item_type = configuration->property(role + ".item_type", default_item_type); 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); @@ -94,8 +91,9 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - tracking_ = galileo_e1_dll_pll_veml_make_tracking_cc( - f_if, + + char sig_[3] = "1B"; + tracking_ = dll_pll_veml_make_tracking( fs_in, vector_length, dump, @@ -109,7 +107,7 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( early_late_space_narrow_chips, very_early_late_space_narrow_chips, extend_correlation_symbols, - track_pilot); + track_pilot, 'E', sig_); } else { 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..ab72a2efb 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 @@ -38,7 +38,7 @@ #define GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_H_ #include "tracking_interface.h" -#include "galileo_e1_dll_pll_veml_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include @@ -94,7 +94,7 @@ public: void start_tracking() override; private: - galileo_e1_dll_pll_veml_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_; size_t item_size_; unsigned int channel_; std::string role_; 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..b95b003f2 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,48 @@ 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); + 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, + char sig_[3] = "1C"; + tracking_ = dll_pll_veml_make_tracking( fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, - early_late_space_chips); + 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 { 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..2b5557917 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 @@ -39,7 +39,7 @@ #define GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_H_ #include "tracking_interface.h" -#include "gps_l1_ca_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -92,7 +92,7 @@ public: void start_tracking() override; private: - gps_l1_ca_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_; size_t item_size_; unsigned int channel_; std::string role_; diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index ae9846acc..3c3fb03d7 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -19,7 +19,7 @@ if(ENABLE_CUDA) set(OPT_TRACKING_BLOCKS ${OPT_TRACKING_BLOCKS} gps_l1_ca_dll_pll_tracking_gpu_cc.cc) set(OPT_TRACKING_INCLUDES ${OPT_TRACKING_INCLUDES} ${CUDA_INCLUDE_DIRS}) - set(OPT_TRACKING_LIBRARIES ${OPT_TRACKING_LIBRARIES} ${CUDA_LIBRARIES}) + set(OPT_TRACKING_LIBRARIES ${OPT_TRACKING_LIBRARIES} ${CUDA_LIBRARIES}) endif(ENABLE_CUDA) if(ENABLE_FPGA) @@ -27,9 +27,7 @@ if(ENABLE_FPGA) endif(ENABLE_FPGA) set(TRACKING_GR_BLOCKS_SOURCES - galileo_e1_dll_pll_veml_tracking_cc.cc galileo_e1_tcp_connector_tracking_cc.cc - gps_l1_ca_dll_pll_tracking_cc.cc gps_l1_ca_tcp_connector_tracking_cc.cc galileo_e5a_dll_pll_tracking_cc.cc gps_l2_m_dll_pll_tracking_cc.cc @@ -42,7 +40,8 @@ set(TRACKING_GR_BLOCKS_SOURCES glonass_l2_ca_dll_pll_tracking_cc.cc glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc - ${OPT_TRACKING_BLOCKS} + dll_pll_veml_tracking.cc + ${OPT_TRACKING_BLOCKS} ) include_directories( 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..e32905196 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -0,0 +1,1532 @@ +/*! + * \file dll_pll_veml_tracking.cc + * \brief Implementation of a code DLL + carrier PLL tracking block. + * \author Antonio Ramos, 2018 antonio.ramosdet(at)gmail.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-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 . + * + * ------------------------------------------------------------------------- + */ + +#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 "gnss_sdr_flags.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::cerr << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + d_correlation_length_ms = 1; + d_secondary = false; + interchange_iq = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0; + d_symbols_per_bit = 0; + } + } + 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; + d_correlation_length_ms = 1; + d_secondary = false; + interchange_iq = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0; + d_symbols_per_bit = 0; + } + } + else + { + LOG(WARNING) << "Invalid System argument when instantiating tracking blocks"; + std::cerr << "Invalid System argument when instantiating tracking blocks" << std::endl; + d_correlation_length_ms = 1; + d_secondary = false; + interchange_iq = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0; + d_symbols_per_bit = 0; + } + 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())); + } + else + { + d_Prompt_Data = nullptr; + d_data_code = nullptr; + } + + //--- 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[FLAGS_cn0_samples]; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = FLAGS_carrier_lock_th; + + 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); + 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 = std::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 = std::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 = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); + 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(); + } + } + volk_gnsssdr_free(aux_code); + } + + 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 < FLAGS_cn0_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, FLAGS_cn0_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, FLAGS_cn0_samples); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) + { + 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 ALIGNMENT ####################### + // 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 + if (d_extend_correlation_symbols_count > 0) + { + 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::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; +} + + +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 - Consume samples at full throttle, do nothing + { + 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) + unsigned long 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 = std::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 * d_acq_code_phase_samples; + 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; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h old mode 100644 new mode 100755 similarity index 60% rename from src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h rename to src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index c82d01681..1065f44ef --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -1,12 +1,11 @@ /*! - * \file galileo_e1_dll_pll_veml_tracking_cc.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 + * \file dll_pll_veml_tracking.h + * \brief Implementation of a code DLL + carrier PLL tracking block. + * \author Antonio Ramos, 2018 antonio.ramosdet(at)gmail.com * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -29,8 +28,8 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H -#define GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H +#ifndef GNSS_SDR_DLL_PLL_VEML_TRACKING_H +#define GNSS_SDR_DLL_PLL_VEML_TRACKING_H #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" @@ -39,72 +38,50 @@ #include #include #include -#include -class galileo_e1_dll_pll_veml_tracking_cc; +class dll_pll_veml_tracking; -typedef boost::shared_ptr galileo_e1_dll_pll_veml_tracking_cc_sptr; +typedef boost::shared_ptr dll_pll_veml_tracking_sptr; -galileo_e1_dll_pll_veml_tracking_cc_sptr -galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, - long 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, +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); + 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 + * \brief This class implements a code DLL + carrier PLL tracking block. */ -class galileo_e1_dll_pll_veml_tracking_cc : public gr::block +class dll_pll_veml_tracking : public gr::block { public: - ~galileo_e1_dll_pll_veml_tracking_cc(); + ~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 galileo_e1_dll_pll_veml_tracking_cc_sptr - galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, - long 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, + 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); + int extend_correlation_symbols, bool track_pilot, + char system, char signal[3]); - galileo_e1_dll_pll_veml_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, + dll_pll_veml_tracking(double fs_in, unsigned int vector_length, bool dump, std::string dump_filename, float pll_bw_hz, @@ -116,61 +93,74 @@ private: float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, int extend_correlation_symbols, - bool track_pilot); + bool track_pilot, + char system, char signal[3]); bool cn0_and_tracking_lock_status(); - void do_correlation_step(const gr_complex *input_samples); - void run_dll_pll(bool disable_costas_loop); - void update_local_code(); - void update_local_carrier(); 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 log_data(); + void save_correlation_results(); + void log_data(bool integrating); + int save_matfile(); // tracking configuration vars - unsigned int d_vector_length; bool d_dump; - - Gnss_Synchro *d_acquisition_gnss_synchro; + bool d_veml; + bool d_cloop; + unsigned int d_vector_length; unsigned int d_channel; - long d_if_freq; - long d_fs_in; + 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_samples; + int d_correlation_length_ms; int d_n_correlator_taps; - double d_early_late_spc_chips; - double d_very_early_late_spc_chips; - - double d_early_late_spc_narrow_chips; - double d_very_early_late_spc_narrow_chips; + 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; - gr_complex *d_correlator_outs; + float *d_prompt_data_shift; cpu_multicorrelator_real_codes multicorrelator_cpu; - //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 - float *d_local_code_data_shift_chips; 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; - bool d_enable_extended_integration; int d_current_symbol; gr_complex d_VE_accu; @@ -178,6 +168,7 @@ private: 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; @@ -206,39 +197,34 @@ private: 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; - - //PRN period in 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 + // 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; - std::deque d_Prompt_buffer_deque; - gr_complex *d_Prompt_buffer; + int d_carrier_lock_fail_counter; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; - int d_carrier_lock_fail_counter; + std::deque d_Prompt_buffer_deque; + gr_complex *d_Prompt_buffer; // file dump std::string d_dump_filename; std::ofstream d_dump_file; - - std::map systemName; - std::string sys; - - int save_matfile(); }; -#endif //GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H +#endif // GNSS_SDR_DLL_PLL_VEML_TRACKING_H diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc deleted file mode 100644 index 4cfbe9305..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ /dev/null @@ -1,1236 +0,0 @@ -/*! - * \file galileo_e1_dll_pll_veml_tracking_cc.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 "galileo_e1_dll_pll_veml_tracking_cc.h" -#include "galileo_e1_signal_processing.h" -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "Galileo_E1.h" -#include "control_message_factory.h" -#include "gnss_sdr_flags.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using google::LogMessage; - -galileo_e1_dll_pll_veml_tracking_cc_sptr -galileo_e1_dll_pll_veml_make_tracking_cc( - long if_freq, - long 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) -{ - return galileo_e1_dll_pll_veml_tracking_cc_sptr(new galileo_e1_dll_pll_veml_tracking_cc(if_freq, - 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)); -} - - -void galileo_e1_dll_pll_veml_tracking_cc::forecast(int noutput_items, - gr_vector_int &ninput_items_required) -{ - if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } -} - - -galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( - long if_freq, - long 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) : gr::block("galileo_e1_dll_pll_veml_tracking_cc", 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->set_relative_rate(1.0 / vector_length); - - this->message_port_register_out(pmt::mp("events")); - - // initialize internal vars - d_dump = dump; - d_if_freq = if_freq; - d_fs_in = fs_in; - d_vector_length = vector_length; - d_dump_filename = dump_filename; - d_code_loop_filter = Tracking_2nd_DLL_filter(Galileo_E1_CODE_PERIOD); - d_carrier_loop_filter = Tracking_2nd_PLL_filter(Galileo_E1_CODE_PERIOD); - - // 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); - - // 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 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); - - // correlator outputs (scalar) - d_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late - d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - // map memory pointers of correlator outputs - 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 = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = -d_very_early_late_spc_chips; - d_local_code_shift_chips[1] = -d_early_late_spc_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_chips; - - d_correlation_length_samples = d_vector_length; - multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); - - d_extend_correlation_symbols = extend_correlation_symbols; - // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal - d_track_pilot = track_pilot; - if (d_track_pilot) - { - // extended integration control - if (d_extend_correlation_symbols > 1) - { - d_enable_extended_integration = true; - } - else - { - d_enable_extended_integration = false; - } - // Extra correlator for the data component - d_local_code_data_shift_chips = static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); - d_local_code_data_shift_chips[0] = 0.0; - correlator_data_cpu.init(2 * d_correlation_length_samples, 1); - d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_Prompt_Data[0] = gr_complex(0, 0); - d_data_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); - } - else - { - // Disable extended integration if data component tracking is selected - d_enable_extended_integration = false; - d_local_code_data_shift_chips = nullptr; - d_data_code = nullptr; - d_Prompt_Data = nullptr; - } - - //--- Initializations ------------------------------ - // Initial code frequency basis of NCO - d_code_freq_chips = static_cast(Galileo_E1_CODE_CHIP_RATE_HZ); - // 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_sample_counter_seconds = 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[FLAGS_cn0_samples]; - d_carrier_lock_test = 1; - d_CN0_SNV_dB_Hz = 0; - d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = FLAGS_carrier_lock_th; - - systemName["E"] = std::string("Galileo"); - - clear_tracking_vars(); - - d_acquisition_gnss_synchro = 0; - 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_state = 0; // initial state: standby -} - - -void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() -{ - /* - * 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; - double acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); - // Doppler effect - // Fd=(C/(C+Vr))*F - double radial_velocity = (Galileo_E1_FREQ_HZ + d_acq_carrier_doppler_hz) / Galileo_E1_FREQ_HZ; - // new chip and prn sequence periods based on acq Doppler - double T_chip_mod_seconds; - double T_prn_mod_seconds; - double T_prn_mod_samples; - d_code_freq_chips = radial_velocity * Galileo_E1_CODE_CHIP_RATE_HZ; - d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); - T_chip_mod_seconds = 1 / d_code_freq_chips; - T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); - - d_current_prn_length_samples = round(T_prn_mod_samples); - - double T_prn_true_seconds = Galileo_E1_B_CODE_LENGTH_CHIPS / Galileo_E1_CODE_CHIP_RATE_HZ; - double T_prn_true_samples = T_prn_true_seconds * static_cast(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, delay_correction_samples; - corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); - if (corrected_acq_phase_samples < 0) - { - corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; - } - 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 = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(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 (d_track_pilot) - { - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_float_sampled(d_tracking_code, - pilot_signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - galileo_e1_code_gen_float_sampled(d_data_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - d_Prompt_Data[0] = gr_complex(0, 0); // clean data correlator output - correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), - d_data_code, - d_local_code_shift_chips); - } - else - { - galileo_e1_code_gen_float_sampled(d_tracking_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - } - - multicorrelator_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), d_tracking_code, d_local_code_shift_chips); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - - d_carrier_lock_fail_counter = 0; - d_rem_code_phase_samples = 0; - d_rem_carr_phase_rad = 0.0; - d_rem_code_phase_chips = 0.0; - d_acc_carrier_phase_rad = 0.0; - - d_code_phase_samples = d_acq_code_phase_samples; - - std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0, 1); - - // DEBUG OUTPUT - std::cout << "Tracking of Galileo E1 signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - // enable tracking pull-in - d_state = 1; - - 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; -} - - -galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() -{ - 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 ..."; - } - galileo_e1_dll_pll_veml_tracking_cc::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); - volk_gnsssdr_free(d_local_code_data_shift_chips); - correlator_data_cpu.free(); - } - delete[] d_Prompt_buffer; - multicorrelator_cpu.free(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } -} - - -bool galileo_e1_dll_pll_veml_tracking_cc::acquire_secondary() -{ - //******* preamble correlation ******** - int corr_value = 0; - for (unsigned int i = 0; i < Galileo_E1_C_SECONDARY_CODE_LENGTH; i++) - { - if (d_Prompt_buffer_deque.at(i).real() < 0) // symbols clipping - { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') - { - corr_value++; - } - else - { - corr_value--; - } - } - else - { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') - { - corr_value--; - } - else - { - corr_value++; - } - } - } - - if (abs(corr_value) == Galileo_E1_C_SECONDARY_CODE_LENGTH) - { - return true; - } - else - { - return false; - } -} - - -bool galileo_e1_dll_pll_veml_tracking_cc::cn0_and_tracking_lock_status() -{ - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < FLAGS_cn0_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, FLAGS_cn0_samples, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) - { - 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 galileo_e1_dll_pll_veml_tracking_cc::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, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); - - // 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, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); - } -} - - -void galileo_e1_dll_pll_veml_tracking_cc::run_dll_pll(bool disable_costas_loop) -{ - // ################## PLL ########################################################## - // PLL discriminator - if (disable_costas_loop == true) - { - // Secondary code acquired. No symbols transition should be present in the signal - d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; - } - else - { - // Costas loop discriminator, insensitive to 180 deg phase transitions - d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; - } - - // 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 = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); - - // ################## DLL ########################################################## - // DLL discriminator - d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_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 galileo_e1_dll_pll_veml_tracking_cc::clear_tracking_vars() -{ - *d_Very_Early = gr_complex(0, 0); - *d_Early = gr_complex(0, 0); - *d_Prompt = gr_complex(0, 0); - *d_Late = gr_complex(0, 0); - *d_Very_Late = gr_complex(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; -} - - -void galileo_e1_dll_pll_veml_tracking_cc::log_data() -{ - 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; - - prompt_I = static_cast(d_P_accu.real()); - prompt_Q = static_cast(d_P_accu.imag()); - - tmp_VE = std::abs(d_VE_accu); - tmp_E = std::abs(d_E_accu); - tmp_P = std::abs(d_P_accu); - tmp_L = std::abs(d_L_accu); - tmp_VL = std::abs(d_VL_accu); - - 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 galileo_e1_dll_pll_veml_tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - // Block input data and block output stream pointers - const gr_complex *in = reinterpret_cast(input_items[0]); - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - Gnss_Synchro current_synchro_data = Gnss_Synchro(); - - switch (d_state) - { - case 0: // standby - bypass - { - current_synchro_data.Tracking_sample_counter = d_sample_counter; - break; - } - case 1: // pull-in - { - /* - * Signal alignment (skip samples until the incoming signal is aligned with local replica) - */ - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.fs = d_fs_in; - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples - consume_each(samples_offset); // shift input to perform alignment with local replica - d_state = 2; // next state is the symbol synchronization - return 0; - } - case 2: // wide tracking and symbol synchronization - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; - // perform a correlation step - do_correlation_step(in); - // save single correlation step variables - d_VE_accu = *d_Very_Early; - d_E_accu = *d_Early; - d_P_accu = *d_Prompt; - d_L_accu = *d_Late; - d_VL_accu = *d_Very_Late; - // check lock status - if (cn0_and_tracking_lock_status() == false) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - // perform DLL/PLL tracking loop computations - run_dll_pll(false); - - // ################## PLL COMMANDS ################################################# - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL COMMANDS ################################################# - // Code error from DLL - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; // [seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### - // 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 - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - // ########### Output the tracking results to Telemetry block ########## - if (d_track_pilot) - { - 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.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - 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 = Galileo_E1_CODE_PERIOD_MS; - - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - - //std::cout<<(d_Prompt->real()>0); - if (d_enable_extended_integration) - { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_buffer_deque.push_back(*d_Prompt); - if (d_Prompt_buffer_deque.size() == Galileo_E1_C_SECONDARY_CODE_LENGTH) - { - if (acquire_secondary() == true) - { - d_extend_correlation_symbols_count = 0; - // reset extended correlator - d_VE_accu = gr_complex(0, 0); - d_E_accu = gr_complex(0, 0); - d_P_accu = gr_complex(0, 0); - d_L_accu = gr_complex(0, 0); - d_VL_accu = gr_complex(0, 0); - d_Prompt_buffer_deque.clear(); - d_current_symbol = 0; - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips; - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; - - LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - //std::cout << " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl; - //std::cout << " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; - - // UPDATE INTEGRATION TIME - double new_correlation_time_s = static_cast(d_extend_correlation_symbols) * Galileo_E1_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 - } - - d_Prompt_buffer_deque.pop_front(); - } - } - } - break; - } - case 3: // coherent integration (correlation time extension) - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; - // perform a correlation step - do_correlation_step(in); - // correct the integration sign using the current symbol of the secondary code - if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu += *d_Very_Early; - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - d_VL_accu += *d_Very_Late; - } - else - { - d_VE_accu -= *d_Very_Early; - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; - d_VL_accu -= *d_Very_Late; - } - d_current_symbol++; - // secondary code roll-up - d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; - - // PLL/DLL not enabled, we are in the middle of a coherent integration - // 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 - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### - // 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 - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples; - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - 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 = Galileo_E1_CODE_PERIOD_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; - } - break; - } - case 4: // narrow tracking - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // perform a correlation step - do_correlation_step(in); - - // correct the integration using the current symbol - if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu += *d_Very_Early; - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - d_VL_accu += *d_Very_Late; - } - else - { - d_VE_accu -= *d_Very_Early; - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; - d_VL_accu -= *d_Very_Late; - } - d_current_symbol++; - // secondary code roll-up - d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; - - // check lock status - if (cn0_and_tracking_lock_status() == false) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - run_dll_pll(true); // Costas loop disabled, use four quadrant atan - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL ########################################################## - // Code phase accumulator - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### - // 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 - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - 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 = Galileo_E1_CODE_PERIOD_MS; - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - // reset extended correlator - d_VE_accu = gr_complex(0, 0); - d_E_accu = gr_complex(0, 0); - d_P_accu = gr_complex(0, 0); - d_L_accu = gr_complex(0, 0); - d_VL_accu = gr_complex(0, 0); - d_state = 3; //new coherent integration (correlation time extension) cycle - } - } - } - - //assign the GNURadio block output data - // current_synchro_data.System = {'E'}; - // std::string str_aux = "1B"; - // const char * str = str_aux.c_str(); // get a C style null terminated string - // std::memcpy(static_cast(current_synchro_data.Signal), str, 3); - - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - - consume_each(d_current_prn_length_samples); // this is required for gr_block derivates - d_sample_counter += d_current_prn_length_samples; // count for the processed samples - - if (current_synchro_data.Flag_valid_symbol_output) - { - return 1; - } - else - { - return 0; - } -} - - -int galileo_e1_dll_pll_veml_tracking_cc::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 galileo_e1_dll_pll_veml_tracking_cc::set_channel(unsigned int channel) -{ - d_channel = channel; - LOG(INFO) << "Tracking Channel set to " << d_channel; - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - 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 galileo_e1_dll_pll_veml_tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) -{ - d_acquisition_gnss_synchro = p_gnss_synchro; -} diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index b8c70db8f..e028893ca 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -443,21 +443,18 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri // MULTIPLEXED FILE RECORDING - Record results to file float prompt_I; float prompt_Q; - float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; + float tmp_E, tmp_P, tmp_L; + float tmp_VE = 0.0; + float tmp_VL = 0.0; float tmp_float; - tmp_float = 0; - double tmp_double; - prompt_I = (*d_Prompt).real(); - prompt_Q = (*d_Prompt).imag(); - tmp_VE = std::abs(*d_Very_Early); - tmp_E = std::abs(*d_Early); - tmp_P = std::abs(*d_Prompt); - tmp_L = std::abs(*d_Late); - tmp_VL = std::abs(*d_Very_Late); - + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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)); @@ -469,30 +466,33 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri // PRN start sample stamp d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); - + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); - - //PLL commands + tmp_float = d_carrier_doppler_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); - - //DLL commands + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); - - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; + tmp_float = d_CN0_SNV_dB_Hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index cbc8c4cf1..df57be51e 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -831,7 +831,9 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs[1].real(); prompt_Q = d_correlator_outs[1].imag(); tmp_E = std::abs(d_correlator_outs[0]); @@ -839,42 +841,45 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - double if_freq_carrier = d_carrier_doppler_hz + d_if_freq + (DFRQ1_GLO * static_cast(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN))); - d_dump_file.write(reinterpret_cast(&if_freq_carrier), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc index 48fe205b6..313756921 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -822,49 +822,55 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs_16sc[1].real(); prompt_Q = d_correlator_outs_16sc[1].imag(); - tmp_E = std::abs(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); - tmp_P = std::abs(std::complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); - tmp_L = std::abs(std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); + tmp_E = std::abs(gr_complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); + tmp_P = std::abs(gr_complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); + tmp_L = std::abs(gr_complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc index e72fafeff..d35b6159c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc @@ -675,8 +675,9 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; - unsigned long int tmp_long; + float tmp_float; + float tmp_VE = 0.0; + float tmp_VL = 0.0; prompt_I = d_correlator_outs[1].real(); prompt_Q = d_correlator_outs[1].imag(); tmp_E = std::abs(d_correlator_outs[0]); @@ -684,41 +685,45 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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 - tmp_long = d_sample_counter + d_current_prn_length_samples; - d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_frequency_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - + 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 - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - + tmp_float = carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - + tmp_float = code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_rem_code_phase_samples; + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter); - 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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc index d49fcd5a2..53f8f704b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc @@ -828,7 +828,9 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs[1].real(); prompt_Q = d_correlator_outs[1].imag(); tmp_E = std::abs(d_correlator_outs[0]); @@ -836,42 +838,45 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - double if_freq_carrier = d_carrier_doppler_hz + d_if_freq + (DFRQ2_GLO * static_cast(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN))); - d_dump_file.write(reinterpret_cast(&if_freq_carrier), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc index 9e96f5e39..decc5d5ca 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc @@ -820,49 +820,55 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs_16sc[1].real(); prompt_Q = d_correlator_outs_16sc[1].imag(); - tmp_E = std::abs(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); - tmp_P = std::abs(std::complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); - tmp_L = std::abs(std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); + tmp_E = std::abs(gr_complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); + tmp_P = std::abs(gr_complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); + tmp_L = std::abs(gr_complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc index 166c60574..f6428bc5c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc @@ -675,8 +675,9 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; - unsigned long int tmp_long; + float tmp_float; + float tmp_VE = 0.0; + float tmp_VL = 0.0; prompt_I = d_correlator_outs[1].real(); prompt_Q = d_correlator_outs[1].imag(); tmp_E = std::abs(d_correlator_outs[0]); @@ -684,41 +685,45 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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 - tmp_long = d_sample_counter + d_current_prn_length_samples; - d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_frequency_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - + 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 - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - + tmp_float = carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - + tmp_float = code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_rem_code_phase_samples; + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter); - 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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc index 703ef118d..c242a10b8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -810,7 +810,9 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __attrib float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs[1].real(); prompt_Q = d_correlator_outs[1].imag(); tmp_E = std::abs(d_correlator_outs[0]); @@ -818,41 +820,45 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __attrib tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S);; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc index 42509b2de..1d1561a21 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc @@ -632,49 +632,55 @@ int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::general_work( float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs_16sc[1].real(); prompt_Q = d_correlator_outs_16sc[1].imag(); - tmp_E = std::abs(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); - tmp_P = std::abs(std::complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); - tmp_L = std::abs(std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); + tmp_E = std::abs(gr_complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); + tmp_P = std::abs(gr_complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); + tmp_L = std::abs(gr_complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc index aa26a24f4..9a4332f06 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -812,49 +812,55 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __attrib float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs_16sc[1].real(); prompt_Q = d_correlator_outs_16sc[1].imag(); - tmp_E = std::abs(std::complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); - tmp_P = std::abs(std::complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); - tmp_L = std::abs(std::complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); + tmp_E = std::abs(gr_complex(d_correlator_outs_16sc[0].real(), d_correlator_outs_16sc[0].imag())); + tmp_P = std::abs(gr_complex(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())); + tmp_L = std::abs(gr_complex(d_correlator_outs_16sc[2].real(), d_correlator_outs_16sc[2].imag())); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_error_filt_chips_Ti), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc deleted file mode 100644 index 494c4cfb4..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ /dev/null @@ -1,768 +0,0 @@ -/*! - * \file gps_l1_ca_dll_pll_tracking_cc.cc - * \brief Implementation of a code DLL + carrier PLL tracking block - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es - * - * 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-2015 (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 "gps_l1_ca_dll_pll_tracking_cc.h" -#include "gps_sdr_signal_processing.h" -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "gnss_sdr_flags.h" -#include "GPS_L1_CA.h" -#include "control_message_factory.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -using google::LogMessage; - -gps_l1_ca_dll_pll_tracking_cc_sptr -gps_l1_ca_dll_pll_make_tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) -{ - return gps_l1_ca_dll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Pll_Tracking_cc(if_freq, - fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast(int noutput_items, - gr_vector_int &ninput_items_required) -{ - if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } -} - - -Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) : gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", 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")); - - // initialize internal vars - d_dump = dump; - d_if_freq = if_freq; - d_fs_in = fs_in; - d_vector_length = vector_length; - d_dump_filename = dump_filename; - - d_current_prn_length_samples = static_cast(d_vector_length); - - // Initialize tracking ========================================== - d_code_loop_filter.set_DLL_BW(dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); - - //--- DLL variables -------------------------------------------------------- - d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) - - // Initialization of local code replica - // Get space for a vector with the C/A code replica sampled 1x/chip - d_ca_code = static_cast(volk_gnsssdr_malloc(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); - - // correlator outputs (scalar) - d_n_correlator_taps = 3; // Early, Prompt, and Late - d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = -d_early_late_spc_chips; - d_local_code_shift_chips[1] = 0.0; - d_local_code_shift_chips[2] = d_early_late_spc_chips; - - multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps); - - //--- Perform initializations ------------------------------ - // define initial code frequency basis of NCO - d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; - // define residual code phase (in chips) - d_rem_code_phase_samples = 0.0; - // define residual carrier phase - d_rem_carr_phase_rad = 0.0; - - // sample synchronization - d_sample_counter = 0; - //d_sample_counter_seconds = 0; - d_acq_sample_stamp = 0; - - d_enable_tracking = false; - d_pull_in = false; - - // CN0 estimation and lock detector buffers - d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; - d_carrier_lock_test = 1; - d_CN0_SNV_dB_Hz = 0; - d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = FLAGS_carrier_lock_th; - - systemName["G"] = std::string("GPS"); - systemName["S"] = std::string("SBAS"); - - d_acquisition_gnss_synchro = 0; - 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_code_phase_samples = 0.0; - d_rem_code_phase_chips = 0.0; - d_code_phase_step_chips = 0.0; - d_carrier_phase_step_rad = 0.0; - - set_relative_rate(1.0 / static_cast(d_vector_length)); -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() -{ - gr::thread::scoped_lock lk(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; - double acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); - // Doppler effect - // Fd=(C/(C+Vr))*F - double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; - // new chip and prn sequence periods based on acq Doppler - double T_chip_mod_seconds; - double T_prn_mod_seconds; - double T_prn_mod_samples; - d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; - d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); - T_chip_mod_seconds = 1 / d_code_freq_chips; - T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); - - d_current_prn_length_samples = round(T_prn_mod_samples); - - double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; - double T_prn_true_samples = T_prn_true_seconds * static_cast(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, delay_correction_samples; - corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); - if (corrected_acq_phase_samples < 0) - { - corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; - } - 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 = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(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 - - // generate local reference ALWAYS starting at chip 1 (1 sample per chip) - gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); - - multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - - d_carrier_lock_fail_counter = 0; - d_rem_code_phase_samples = 0; - d_rem_carr_phase_rad = 0.0; - d_rem_code_phase_chips = 0.0; - d_acc_carrier_phase_rad = 0.0; - - d_code_phase_samples = d_acq_code_phase_samples; - - std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0, 1); - - // DEBUG OUTPUT - std::cout << "Tracking of GPS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - - // enable tracking - d_pull_in = true; - d_enable_tracking = true; - - 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; -} - - -int Gps_L1_Ca_Dll_Pll_Tracking_cc::save_matfile() -{ - // READ DUMP FILE - std::ifstream::pos_type size; - int number_of_double_vars = 11; - int number_of_float_vars = 5; - 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_E = new float[num_epoch]; - float *abs_P = new float[num_epoch]; - float *abs_L = 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]; - double *acc_carrier_phase_rad = new double[num_epoch]; - double *carrier_doppler_hz = new double[num_epoch]; - double *code_freq_chips = new double[num_epoch]; - double *carr_error_hz = new double[num_epoch]; - double *carr_error_filt_hz = new double[num_epoch]; - double *code_error_chips = new double[num_epoch]; - double *code_error_filt_chips = new double[num_epoch]; - double *CN0_SNV_dB_Hz = new double[num_epoch]; - double *carrier_lock_test = new double[num_epoch]; - double *aux1 = new double[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_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(&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(double)); - dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&aux1[i]), sizeof(double)); - 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_E; - delete[] abs_P; - delete[] abs_L; - 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_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("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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_DOUBLE, MAT_T_DOUBLE, 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_E; - delete[] abs_P; - delete[] abs_L; - 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; -} - - -Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc() -{ - 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 ..."; - } - Gps_L1_Ca_Dll_Pll_Tracking_cc::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_ca_code); - delete[] d_Prompt_buffer; - multicorrelator_cpu.free(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } -} - - -int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - gr::thread::scoped_lock lk(d_setlock); - // process vars - double carr_error_hz = 0.0; - double carr_error_filt_hz = 0.0; - double code_error_chips = 0.0; - double code_error_filt_chips = 0.0; - - // Block input data and block output stream pointers - const gr_complex *in = reinterpret_cast(input_items[0]); - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - Gnss_Synchro current_synchro_data = Gnss_Synchro(); - - if (d_enable_tracking == true) - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Receiver signal alignment - if (d_pull_in == true) - { - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples - d_pull_in = false; - // take into account the carrier cycles accumulated in the pull in signal alignment - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.fs = d_fs_in; - current_synchro_data.correlation_length_ms = 1; - consume_each(samples_offset); // shift input to perform alignment with local replica - return 0; - } - - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_current_prn_length_samples); - - // ################## PLL ########################################################## - // PLL discriminator - // Update PLL discriminator [rads/Ti -> Secs/Ti] - carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output - // Carrier discriminator filter - carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; - // New code Doppler frequency estimation - d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); - - // ################## DLL ########################################################## - // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late - // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] - double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); - double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); //[seconds] - //double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; // [seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### - // 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 - //double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); - //double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = 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 = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); - // carrier phase accumulator - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * 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 / static_cast(d_fs_in); - // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); - - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < FLAGS_cn0_samples) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt - d_cn0_estimation_counter++; - } - else - { - d_cn0_estimation_counter = 0; - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) - { - 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; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); - current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; - 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 = 1; - } - else - { - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0, 0); - } - - current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; - current_synchro_data.System = {'G'}; - current_synchro_data.correlation_length_ms = 1; - } - - //assign the GNURadio block output data - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - if (d_dump) - { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - double tmp_double; - unsigned long int tmp_long; - prompt_I = d_correlator_outs[1].real(); - prompt_Q = d_correlator_outs[1].imag(); - tmp_E = std::abs(d_correlator_outs[0]); - tmp_P = std::abs(d_correlator_outs[1]); - tmp_L = std::abs(d_correlator_outs[2]); - try - { - // EPR - 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)); - // 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 - tmp_long = d_sample_counter + d_current_prn_length_samples; - d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - // PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - - // DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - - // AUX vars (for debug purposes) - tmp_double = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter); - 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(); - } - } - - consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates - d_sample_counter += d_current_prn_length_samples; // count for the processed samples - - if (current_synchro_data.Flag_valid_symbol_output) - { - return 1; - } - else - { - return 0; - } -} - - -void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_channel(unsigned int channel) -{ - d_channel = channel; - LOG(INFO) << "Tracking Channel set to " << d_channel; - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - 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 Gps_L1_Ca_Dll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) -{ - d_acquisition_gnss_synchro = p_gnss_synchro; -} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h deleted file mode 100644 index 575b4feb4..000000000 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ /dev/null @@ -1,167 +0,0 @@ -/*! - * \file gps_l1_ca_dll_pll_tracking_cc.h - * \brief Interface of a code DLL + carrier PLL tracking block - * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com - * Javier Arribas, 2011. jarribas(at)cttc.es - * Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com - * - * 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 - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (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_GPS_L1_CA_DLL_PLL_TRACKING_CC_H -#define GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H - -#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 -#include - -class Gps_L1_Ca_Dll_Pll_Tracking_cc; - -typedef boost::shared_ptr - gps_l1_ca_dll_pll_tracking_cc_sptr; - -gps_l1_ca_dll_pll_tracking_cc_sptr -gps_l1_ca_dll_pll_make_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - -/*! - * \brief This class implements a DLL + PLL tracking loop block - */ -class Gps_L1_Ca_Dll_Pll_Tracking_cc : public gr::block -{ -public: - ~Gps_L1_Ca_Dll_Pll_Tracking_cc(); - - void set_channel(unsigned int channel); - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); - void start_tracking(); - - 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 gps_l1_ca_dll_pll_tracking_cc_sptr - gps_l1_ca_dll_pll_make_tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - Gps_L1_Ca_Dll_Pll_Tracking_cc(long if_freq, - long fs_in, unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips); - - int save_matfile(); - // tracking configuration vars - unsigned int d_vector_length; - bool d_dump; - - Gnss_Synchro* d_acquisition_gnss_synchro; - unsigned int d_channel; - - long d_if_freq; - long d_fs_in; - - double d_early_late_spc_chips; - - // remaining code phase and carrier phase between tracking loops - double d_rem_code_phase_samples; - double d_rem_code_phase_chips; - 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; - // correlator - int d_n_correlator_taps; - float* d_ca_code; - float* d_local_code_shift_chips; - gr_complex* d_correlator_outs; - cpu_multicorrelator_real_codes multicorrelator_cpu; - - // tracking vars - double d_code_freq_chips; - double d_code_phase_step_chips; - double d_carrier_doppler_hz; - double d_carrier_phase_step_rad; - double d_acc_carrier_phase_rad; - double d_code_phase_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; - gr_complex* d_Prompt_buffer; - double d_carrier_lock_test; - double d_CN0_SNV_dB_Hz; - double d_carrier_lock_threshold; - int d_carrier_lock_fail_counter; - - // control vars - bool d_enable_tracking; - bool d_pull_in; - - // file dump - std::string d_dump_filename; - std::ofstream d_dump_file; - - std::map systemName; - std::string sys; -}; - -#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index bdbf2771b..759128d0b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -468,7 +468,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; - double tmp_double; + float tmp_VE = 0.0; + float tmp_VL = 0.0; + float tmp_float; prompt_I = d_correlator_outs[1].real(); prompt_Q = d_correlator_outs[1].imag(); tmp_E = std::abs(d_correlator_outs[0]); @@ -476,41 +478,45 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); - + tmp_float = d_acc_carrier_phase_cycles * GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_phase_error_secs_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips_Ti), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - + 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 = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips_Ti; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - + 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_double = d_rem_code_phase_samples; + tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_correlation_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)); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index f2d709c21..f4fe4c018 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -482,48 +482,55 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attrib float prompt_I; float prompt_Q; float tmp_E, tmp_P, tmp_L; + float tmp_VE = 0.0; + float tmp_VL = 0.0; float tmp_float; - prompt_I = (*d_Prompt).real(); - prompt_Q = (*d_Prompt).imag(); - tmp_E = std::abs(*d_Early); - tmp_P = std::abs(*d_Prompt); - tmp_L = std::abs(*d_Late); + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); try { - // EPR + // 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 - //tmp_float=(float)d_sample_counter; d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); - - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_hz), sizeof(float)); - - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_nco), sizeof(float)); - - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_nco), sizeof(float)); - - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); - - // AUX vars (for debug purposes) - tmp_float = 0; + tmp_float = d_acc_carrier_phase_rad; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_sample_counter_seconds), sizeof(double)); - + // 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_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = carr_error; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error; + 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 = 0.0; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + double tmp_double = static_cast(d_sample_counter + d_correlation_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)); 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/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 987f2c735..0488da8e4 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -544,7 +544,7 @@ void GNSSFlowgraph::set_signals_list() configuration_->property("Channels_1B.count", 0) + configuration_->property("Channels_5X.count", 0) + configuration_->property("Channels_1G.count", 0) + - configuration_->property("Channels_2G.count", 0) + + configuration_->property("Channels_2G.count", 0) + configuration_->property("Channels_5X.count", 0) + configuration_->property("Channels_L5.count", 0); @@ -723,8 +723,8 @@ void GNSSFlowgraph::set_signals_list() if (configuration_->property("Channels_1G.count", 0) > 0) { /* - * Loop to create the list of GLONASS L1 C/A signals - */ + * Loop to create the list of GLONASS L1 C/A signals + */ for (available_gnss_prn_iter = available_glonass_prn.begin(); available_gnss_prn_iter != available_glonass_prn.end(); available_gnss_prn_iter++) @@ -738,8 +738,8 @@ void GNSSFlowgraph::set_signals_list() if (configuration_->property("Channels_2G.count", 0) > 0) { /* - * Loop to create the list of GLONASS L2 C/A signals - */ + * Loop to create the list of GLONASS L2 C/A signals + */ for (available_gnss_prn_iter = available_glonass_prn.begin(); available_gnss_prn_iter != available_glonass_prn.end(); available_gnss_prn_iter++) 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/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc index b8f259501..0f8dd9dc3 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc @@ -35,24 +35,24 @@ bool tracking_dump_reader::read_binary_obs() { try { + d_dump_file.read(reinterpret_cast(&abs_VE), sizeof(float)); d_dump_file.read(reinterpret_cast(&abs_E), sizeof(float)); d_dump_file.read(reinterpret_cast(&abs_P), sizeof(float)); d_dump_file.read(reinterpret_cast(&abs_L), sizeof(float)); + d_dump_file.read(reinterpret_cast(&abs_VL), sizeof(float)); d_dump_file.read(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.read(reinterpret_cast(&prompt_Q), sizeof(float)); - d_dump_file.read(reinterpret_cast(&PRN_start_sample_count), sizeof(unsigned long int)); - - d_dump_file.read(reinterpret_cast(&acc_carrier_phase_rad), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carrier_doppler_hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&code_freq_chips), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.read(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - d_dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carrier_lock_test), sizeof(double)); - d_dump_file.read(reinterpret_cast(&aux1), sizeof(double)); + d_dump_file.read(reinterpret_cast(&acc_carrier_phase_rad), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carrier_doppler_hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&code_freq_chips), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carr_error_hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&code_error_chips), sizeof(float)); + d_dump_file.read(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + d_dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carrier_lock_test), sizeof(float)); + d_dump_file.read(reinterpret_cast(&aux1), sizeof(float)); d_dump_file.read(reinterpret_cast(&aux2), sizeof(double)); d_dump_file.read(reinterpret_cast(&PRN), sizeof(unsigned int)); } @@ -82,8 +82,8 @@ bool tracking_dump_reader::restart() long int tracking_dump_reader::num_epochs() { std::ifstream::pos_type size; - int number_of_double_vars = 11; - int number_of_float_vars = 5; + 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 tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h index 2a56fdd1c..e36dec1c8 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h @@ -45,10 +45,12 @@ public: bool open_obs_file(std::string out_file); //tracking dump variables - // EPR + // VEPLVL + float abs_VE; float abs_E; float abs_P; float abs_L; + float abs_VL; // PROMPT I and Q (to analyze navigation symbols) float prompt_I; float prompt_Q; @@ -56,26 +58,26 @@ public: unsigned long int PRN_start_sample_count; // accumulated carrier phase - double acc_carrier_phase_rad; + float acc_carrier_phase_rad; // carrier and code frequency - double carrier_doppler_hz; - double code_freq_chips; + float carrier_doppler_hz; + float code_freq_chips; // PLL commands - double carr_error_hz; - double carr_error_filt_hz; + float carr_error_hz; + float carr_error_filt_hz; // DLL commands - double code_error_chips; - double code_error_filt_chips; + float code_error_chips; + float code_error_filt_chips; // CN0 and carrier lock test - double CN0_SNV_dB_Hz; - double carrier_lock_test; + float CN0_SNV_dB_Hz; + float carrier_lock_test; // AUX vars (for debug purposes) - double aux1; + float aux1; double aux2; unsigned int PRN; diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc index d0ff41697..f21ae5811 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc @@ -225,6 +225,9 @@ void GpsL1CADllPllTrackingTest::configure_receiver() config->set_property("Tracking_1C.pll_bw_hz", "20.0"); config->set_property("Tracking_1C.dll_bw_hz", "2.0"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); + config->set_property("Tracking_1C.pll_bw_narrow_hz", "20.0"); + config->set_property("Tracking_1C.dll_bw_narrow_hz", "2.0"); + config->set_property("Tracking_1C.early_late_space_narrow_chips", "0.5"); config->set_property("Tracking_1C.extend_correlation_ms", "1"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); 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));