diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index b33c68929..0ecfcc8fc 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -285,9 +285,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump d_ls_pvt = std::make_shared((int)nchannels, dump_ls_pvt_filename, d_dump, rtklib_options); d_ls_pvt->set_averaging_depth(d_averaging_depth); - d_sample_counter = 0; d_last_sample_nav_output = 0; - d_rx_time = 0.0; b_rinex_header_written = false; b_rinex_header_updated = false; @@ -409,20 +407,6 @@ bool rtklib_pvt_cc::observables_pairCompare_min(const std::pairPRN) - // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; - } -} - - bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) { /* Fill Sys V message structures */ @@ -439,49 +423,52 @@ bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) } -int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), +int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items , gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) { - d_sample_counter++; + Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer + + double d_rx_time[ninput_items[0]]; + for(unsigned int item = 0; item < ninput_items[0]; item++) + { + //d_sample_counter++; unsigned int gps_channel = 0; unsigned int gal_channel = 0; gnss_observables_map.clear(); - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer - - print_receiver_status(in); + //print_receiver_status(in); // ############ 1. READ PSEUDORANGES #### for (unsigned int i = 0; i < d_nchannels; i++) { - if (in[i][0].Flag_valid_pseudorange == true) + if (in[i][item].Flag_valid_pseudorange == true) { // store valid observables in a map. - gnss_observables_map.insert(std::pair(i, in[i][0])); - d_rx_time = in[i][0].RX_time; + gnss_observables_map.insert(std::pair(i, in[i][item])); + d_rx_time[item] = in[i][item].RX_time; if(d_ls_pvt->gps_ephemeris_map.size() > 0) { - std::map::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN); + std::map::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][item].PRN); if(tmp_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) { - d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time + d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][item].PRN)->second, d_rx_time[item], in[i][item]); // keep track of locking time } } if(d_ls_pvt->galileo_ephemeris_map.size() > 0) { - std::map::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN); + std::map::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][item].PRN); if(tmp_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time + d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][item].PRN)->second, d_rx_time[item], in[i][item]); // keep track of locking time } } if(d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) { - std::map::iterator tmp_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN); + std::map::iterator tmp_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][item].PRN); if(tmp_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) { - d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time + d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][item].PRN)->second, d_rx_time[item], in[i][item]); // keep track of locking time } } } @@ -492,6 +479,9 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v std::map::iterator gps_cnav_ephemeris_iter; std::map::iterator gnss_observables_iter; + long int rx_time_ms = static_cast((d_rx_time[item] * 1000.0)); + + /* * TYPE | RECEIVER * 0 | Unknown @@ -523,10 +513,10 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if (gnss_observables_map.size() > 0) { // compute on the fly PVT solution - if ((d_sample_counter % d_output_rate_ms) == 0) + if ((rx_time_ms % d_output_rate_ms) == 0) { bool pvt_result; - pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); + pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time[item], d_flag_averaging); if (pvt_result == true) { @@ -543,7 +533,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; ttff_msgbuf ttff; ttff.mtype = 1; - ttff.ttff = d_sample_counter; + ttff.ttff = d_rx_time[item]; //d_sample_counter; send_sys_v_ttff_msg(ttff); first_fix = false; } @@ -562,7 +552,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time[item]); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); b_rinex_header_written = true; // do not write header anymore } @@ -571,7 +561,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) { - rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time[item]); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); b_rinex_header_written = true; // do not write header anymore } @@ -580,7 +570,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item]); rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -590,7 +580,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v std::string signal("5X"); if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], signal); rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -600,7 +590,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v std::string signal("7X"); if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], signal); rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -609,7 +599,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) { - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time[item]); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); b_rinex_header_written = true; // do not write header anymore } @@ -620,7 +610,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) { std::string gal_signal("1B"); - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time[item], gal_signal); rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -630,7 +620,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) { std::string gal_signal("5X"); - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time[item], gal_signal); rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -640,7 +630,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) { std::string gal_signal("7X"); - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time[item], gal_signal); rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -650,7 +640,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) { std::string gal_signal("1B 5X"); - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gal_signal); rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -660,7 +650,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) { std::string gal_signal("1B 7X"); - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gal_signal); rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_written = true; // do not write header anymore } @@ -670,7 +660,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { // Limit the RINEX navigation output rate // Notice that d_sample_counter period is 4ms (for Galileo correlators) - if ((d_sample_counter - d_last_sample_nav_output) >= 6000) + if ((d_rx_time[item] - d_last_sample_nav_output) >= 6000) { if(type_of_rx == 1) // GPS L1 C/A only { @@ -697,7 +687,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); } - d_last_sample_nav_output = d_sample_counter; + d_last_sample_nav_output = d_rx_time[item]; //d_sample_counter; } galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); @@ -708,7 +698,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time[item], gnss_observables_map); } if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) { @@ -721,7 +711,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) { - rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time[item], gnss_observables_map); } if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { @@ -734,7 +724,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B"); + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "1B"); } if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { @@ -747,7 +737,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X"); + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "5X"); } if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { @@ -760,7 +750,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X"); + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "7X"); } if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { @@ -773,7 +763,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if( (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time[item], gnss_observables_map); } if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) { @@ -786,7 +776,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map); } if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) { @@ -799,7 +789,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X"); + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "1B 5X"); } if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { @@ -812,7 +802,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X"); + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "1B 7X"); } if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { @@ -828,52 +818,52 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if(type_of_rx == 1) // GPS L1 C/A { - if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0) + if((rx_time_ms % d_rtcm_MT1019_rate_ms) == 0) { for(std::map::iterator 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((d_sample_counter % d_rtcm_MSM_rate_ms) == 0) + if((rx_time_ms % d_rtcm_MSM_rate_ms) == 0) { std::map::iterator gps_ephemeris_iter; gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - 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, gps_ephemeris_iter->second, {}, {}, d_rx_time[item], 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((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4) ) == 0) + if((rx_time_ms % (d_rtcm_MT1045_rate_ms / 4) ) == 0) { for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) { d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); } } - if((d_sample_counter % (d_rtcm_MSM_rate_ms / 4) ) == 0) + if((rx_time_ms % (d_rtcm_MSM_rate_ms / 4) ) == 0) { std::map::iterator gal_ephemeris_iter; gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0); } } } if(type_of_rx == 7) // GPS L1 C/A + GPS L2C { - if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0) + if((rx_time_ms % d_rtcm_MT1019_rate_ms) == 0) { for(std::map::iterator 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((d_sample_counter % d_rtcm_MSM_rate_ms) == 0) + if((rx_time_ms % d_rtcm_MSM_rate_ms) == 0) { std::map::iterator gps_ephemeris_iter; gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); @@ -881,27 +871,27 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) { - 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); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0); } } } if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - if(((d_sample_counter % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0)) + if(((rx_time_ms % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0)) { 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(((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (d_rtcm_MT1045_rate_ms != 0)) + if(((rx_time_ms % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (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); } } - if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0)) + if(((rx_time_ms % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((rx_time_ms % (d_rtcm_MT1077_rate_ms / 4) ) == 0)) { //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); @@ -934,19 +924,19 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v } i++; } - if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) ) + if(((rx_time_ms % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) ) { 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[item], gnss_observables_map, 0, 0, 0, 0, 0); } } - if(((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) ) + if(((rx_time_ms % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) ) { if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - 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, gps_ephemeris_iter->second, {}, {}, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -966,7 +956,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - 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, gps_ephemeris_iter->second, {}, {}, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } @@ -982,7 +972,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } @@ -998,7 +988,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) { - 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); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } @@ -1051,12 +1041,12 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v 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); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time[item], 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); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } @@ -1065,14 +1055,14 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v } // DEBUG MESSAGE: Display position in console output - if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true) + if (((rx_time_ms % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true) { std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m] " << rx_time_ms<d_position_UTC_time) - << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " UTC2 using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) @@ -1089,11 +1079,11 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v double tmp_double; for (unsigned int i = 0; i < d_nchannels; i++) { - tmp_double = in[i][0].Pseudorange_m; + tmp_double = in[i][item].Pseudorange_m; d_dump_file.write((char*)&tmp_double, sizeof(double)); tmp_double = 0; d_dump_file.write((char*)&tmp_double, sizeof(double)); - d_dump_file.write((char*)&d_rx_time, sizeof(double)); + d_dump_file.write((char*)&d_rx_time[item], sizeof(double)); } } catch (const std::ifstream::failure& e) @@ -1102,7 +1092,7 @@ int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v } } } - - consume_each(1); //one by one - return 1; + } + consume_each(ninput_items[0]); + return ninput_items[0]; } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index ec09a2a83..ec364492f 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -123,7 +123,6 @@ private: int d_rtcm_MT1097_rate_ms; int d_rtcm_MSM_rate_ms; - void print_receiver_status(Gnss_Synchro** channels_synchronization_data); int d_last_status_print_seg; //for status printer unsigned int d_nchannels; @@ -133,7 +132,6 @@ private: bool d_flag_averaging; int d_output_rate_ms; int d_display_rate_ms; - long unsigned int d_sample_counter; long unsigned int d_last_sample_nav_output; std::shared_ptr rp; @@ -141,7 +139,6 @@ private: std::shared_ptr d_nmea_printer; std::shared_ptr d_geojson_printer; std::shared_ptr d_rtcm_printer; - double d_rx_time; std::shared_ptr d_ls_pvt; prcopt_t rtklib_options; diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 87e40b2d6..20add847b 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -31,8 +31,9 @@ #include "hybrid_ls_pvt.h" #include -#include "GPS_L1_CA.h" #include "Galileo_E1.h" +#include "GPS_L1_CA.h" +#include "GPS_L2C.h" using google::LogMessage; @@ -71,7 +72,8 @@ hybrid_ls_pvt::~hybrid_ls_pvt() d_dump_file.close(); } -bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging) + +bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, double hybrid_current_time, bool flag_averaging) { std::map::iterator gnss_observables_iter; std::map::iterator galileo_ephemeris_iter; @@ -88,6 +90,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou double GST = 0.0; double secondsperweek = 604800.0; + //double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s) double TX_time_corrected_s = 0.0; double SV_clock_bias_s = 0.0; @@ -98,7 +101,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // ******************************************************************************** int valid_obs = 0; //valid observations counter - for(gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) @@ -118,6 +120,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou W(valid_obs) = 1; // COMMON RX TIME PVT ALGORITHM + double Rx_time = hybrid_current_time; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s; // 2- compute the clock drift using the clock model (broadcast) for this SV @@ -140,7 +143,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST - GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, Rx_time); + GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time); // SV ECEF DEBUG OUTPUT DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN @@ -174,15 +177,15 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // first estimate of transmit time + double Rx_time = hybrid_current_time; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s; // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect - SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); + SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD; // 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect TX_time_corrected_s = Tx_time - SV_clock_bias_s; - //compute satellite position, clock bias + relativistic correction - double dts = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); + double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); //store satellite positions in a matrix satpos.resize(3, valid_obs + 1); @@ -198,31 +201,17 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou double P1_P2=(1.0-Gamma)*(gps_ephemeris_iter->second.d_TGD* GPS_C_m_s); double Code_bias_m= P1_P2/(1.0-Gamma); obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dts * GPS_C_m_s-Code_bias_m-d_rx_dt_s * GPS_C_m_s; + obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s-Code_bias_m-d_rx_dt_s * GPS_C_m_s; d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; // SV ECEF DEBUG OUTPUT LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN - << " TX Time corrected="<second.i_GPS_week),Rx_time); - gtime_t tx_time=gpst2time(adjgpsweek(gps_ephemeris_iter->second.i_GPS_week),Tx_time); - printf("RINEX RX TIME: %s,%f, TX TIME: %s,%f\n\r",time_str(rx_time,3),rx_time.sec,time_str(tx_time,3),tx_time.sec); - } - std::flush(std::cout); - gtime_t tx_time_corr=gpst2time(adjgpsweek(gps_ephemeris_iter->second.i_GPS_week),TX_time_corrected_s); - printf("SAT TX TIME [%i]: %s,%f PR:%f dt:%f\n\r",valid_obs,time_str(tx_time_corr,3),tx_time_corr.sec, obs(valid_obs),dts); - std::flush(std::cout); - //*** end debug - valid_obs++; // compute the UTC time for this SV (just to print the associated UTC timestamp) GPS_week = gps_ephemeris_iter->second.i_GPS_week; @@ -245,15 +234,17 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // first estimate of transmit time + double Rx_time = hybrid_current_time; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s; // 2- compute the clock drift using the clock model (broadcast) for this SV SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time); + // 3- compute the current ECEF position for this SV using corrected TX time TX_time_corrected_s = Tx_time - SV_clock_bias_s; //std::cout<<"TX time["<second.i_satellite_PRN<<"]="<second.satellitePosition(TX_time_corrected_s); - //std::cout<<"L2 Tx_time: "<second.d_satpos_X; @@ -262,7 +253,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // 4- fill the observations vector with the corrected observables obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s; + obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr*GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s; d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; @@ -296,6 +287,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // ****** SOLVE LEAST SQUARES****************************************************** // ******************************************************************************** d_valid_observations = valid_obs; + LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs; if(valid_obs >= 4) @@ -304,7 +296,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou DLOG(INFO) << "satpos=" << satpos; DLOG(INFO) << "obs=" << obs; DLOG(INFO) << "W=" << W; - try { // check if this is the initial position computation @@ -323,7 +314,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] - DLOG(INFO) << "Hybrid Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; + DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]"; // Compute GST and Gregorian time @@ -349,7 +340,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; // ###### Compute DOPs ######## - compute_DOP(); + hybrid_ls_pvt::compute_DOP(); // ######## LOG FILE ######### if(d_flag_dump_enabled == true) @@ -359,7 +350,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou { double tmp_double; // PVT GPS time - tmp_double = Rx_time; + tmp_double = hybrid_current_time; d_dump_file.write((char*)&tmp_double, sizeof(double)); // ECEF User Position East [m] tmp_double = rx_position_and_time(0); @@ -396,9 +387,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou { d_rx_dt_s = 0; //reset rx time estimation LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what(); - LOG(WARNING) << "satpos=" << satpos; - LOG(WARNING) << "obs=" << obs; - LOG(WARNING) << "W=" << W; b_valid_position = false; } } diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 6dcdbc4c0..65682f4d2 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -266,7 +266,7 @@ int Position_Gps_L1_System_Test::configure_receiver() const float dll_bw_narrow_hz = 2.0; const int extend_correlation_ms = 1; - const int display_rate_ms = 500; + const int display_rate_ms = 1000; const int output_rate_ms = 1000; const int averaging_depth = 1;