mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-31 11:19:18 +00:00
Code Issues Releases Wiki Activity

Fix the bug fix

Browse Source
This commit is contained in:
Javier Arribas 2017-04-28 15:38:31 +02:00
parent c0c1838deb
commit 44f748cb41
3 changed files with 930 additions and 864 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -208,6 +208,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
gr::io_signature::make(0, 0, sizeof(gr_complex)))
{
d_output_rate_ms = output_rate_ms;
d_display_rate_ms = display_rate_ms;
d_dump = dump;
@ -285,7 +286,8 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
d_ls_pvt = std::make_shared<rtklib_solver>((int)nchannels, dump_ls_pvt_filename, d_dump, rtklib_options);
d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_last_sample_nav_output = 0;
d_rx_time = 0.0;
last_pvt_display_T_rx_s=0.0;
b_rinex_header_written = false;
b_rinex_header_updated = false;
@ -407,6 +409,20 @@ bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair<int,Gnss_Synchro
}
void rtklib_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronization_data)
{
// Print the current receiver status using std::cout every second
int current_rx_seg = floor((double)channels_synchronization_data[0][0].Tracking_sample_counter/(double)channels_synchronization_data[0][0].fs);
if ( current_rx_seg != d_last_status_print_seg)
{
d_last_status_print_seg = current_rx_seg;
std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush;
//DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
// << ", 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 */
@ -423,52 +439,55 @@ bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff)
}
int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items ,
int rtklib_pvt_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 __attribute__((unused)))
{
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++)
for(int epoch=0;epoch<ninput_items[0];epoch++)
{
//d_sample_counter++;
bool flag_display_pvt=false;
bool flag_make_pvt_output=false;
bool flag_make_RTCM_output=false;
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);
// ############ 1. READ PSEUDORANGES ####
for (unsigned int i = 0; i < d_nchannels; i++)
{
if (in[i][item].Flag_valid_pseudorange == true)
if (in[i][epoch].Flag_valid_pseudorange == true)
{
// store valid observables in a map.
gnss_observables_map.insert(std::pair<int,Gnss_Synchro>(i, in[i][item]));
d_rx_time[item] = in[i][item].RX_time;
gnss_observables_map.insert(std::pair<int,Gnss_Synchro>(i, in[i][epoch]));
if(d_ls_pvt->gps_ephemeris_map.size() > 0)
{
std::map<int,Gps_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][item].PRN);
std::map<int,Gps_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].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][item].PRN)->second, d_rx_time[item], in[i][item]); // keep track of locking time
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)
{
std::map<int,Galileo_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][item].PRN);
std::map<int,Galileo_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].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][item].PRN)->second, d_rx_time[item], in[i][item]); // keep track of locking time
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)
{
std::map<int,Gps_CNAV_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][item].PRN);
std::map<int,Gps_CNAV_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].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][item].PRN)->second, d_rx_time[item], in[i][item]); // keep track of locking time
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
}
}
}
@ -479,9 +498,6 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
std::map<int, Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
std::map<int, Gnss_Synchro>::iterator gnss_observables_iter;
long int rx_time_ms = static_cast<long int>((d_rx_time[item] * 1000.0));
/*
* TYPE | RECEIVER
* 0 | Unknown
@ -512,11 +528,30 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
// ############ 2 COMPUTE THE PVT ################################
if (gnss_observables_map.size() > 0)
{
if (fabs(gnss_observables_map.begin()->second.RX_time -last_pvt_display_T_rx_s)*1000.0>=(double)d_display_rate_ms)
{
flag_display_pvt=true;
last_pvt_display_T_rx_s=gnss_observables_map.begin()->second.RX_time;
}
if (fabs(gnss_observables_map.begin()->second.RX_time -d_rx_time)*1000.0>=(double)d_output_rate_ms)
{
flag_make_pvt_output=true;
d_rx_time =gnss_observables_map.begin()->second.RX_time;
}
if (fabs(gnss_observables_map.begin()->second.RX_time -d_rx_time)*1000.0>=(double)d_rtcm_MT1019_rate_ms)
{
flag_make_RTCM_output=true;
}
// compute on the fly PVT solution
if ((rx_time_ms % d_output_rate_ms) == 0)
if (flag_make_pvt_output==true)
{
bool pvt_result;
pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time[item], d_flag_averaging);
pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging);
if (pvt_result == true)
{
@ -533,7 +568,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
ttff_msgbuf ttff;
ttff.mtype = 1;
ttff.ttff = d_rx_time[item]; //d_sample_counter;
ttff.ttff = 0;//d_sample_counter;
send_sys_v_ttff_msg(ttff);
first_fix = false;
}
@ -552,7 +587,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
{
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time[item]);
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time);
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
}
@ -561,7 +596,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
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[item]);
rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time);
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
}
@ -570,7 +605,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())
{
rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item]);
rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time);
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
}
@ -580,7 +615,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], signal);
rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, 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
}
@ -590,7 +625,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], signal);
rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, 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
}
@ -599,7 +634,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
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[item]);
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time);
rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model);
b_rinex_header_written = true; // do not write header anymore
}
@ -610,7 +645,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], gal_signal);
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, 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
}
@ -620,7 +655,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], gal_signal);
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, 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 +665,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], gal_signal);
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, 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 +675,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], gal_signal);
rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, 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
}
@ -650,7 +685,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], gal_signal);
rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, 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 +695,8 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
// Limit the RINEX navigation output rate
// Notice that d_sample_counter period is 4ms (for Galileo correlators)
if ((d_rx_time[item] - d_last_sample_nav_output) >= 6000)
// if ((d_sample_counter - d_last_sample_nav_output) >= 6000)
if(false)
{
if(type_of_rx == 1) // GPS L1 C/A only
{
@ -687,7 +723,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map);
}
d_last_sample_nav_output = d_rx_time[item]; //d_sample_counter;
//d_last_sample_nav_output = 0;//d_sample_counter;
}
galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin();
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
@ -698,7 +734,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
{
rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time[item], gnss_observables_map);
rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map);
}
if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0))
{
@ -711,7 +747,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
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[item], gnss_observables_map);
rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map);
}
if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0))
{
@ -724,7 +760,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())
{
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "1B");
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B");
}
if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0))
{
@ -737,7 +773,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())
{
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "5X");
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X");
}
if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0))
{
@ -750,7 +786,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())
{
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "7X");
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X");
}
if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0))
{
@ -763,7 +799,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
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[item], gnss_observables_map);
rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map);
}
if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0))
{
@ -776,7 +812,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
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[item], gnss_observables_map);
rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map);
}
if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0))
{
@ -789,7 +825,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())
{
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "1B 5X");
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X");
}
if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0))
{
@ -802,7 +838,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())
{
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, "1B 7X");
rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X");
}
if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0))
{
@ -818,52 +854,52 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
{
if(type_of_rx == 1) // GPS L1 C/A
{
if((rx_time_ms % d_rtcm_MT1019_rate_ms) == 0)
if(flag_make_RTCM_output)
{
for(std::map<int,Gps_Ephemeris>::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((rx_time_ms % d_rtcm_MSM_rate_ms) == 0)
if(flag_make_RTCM_output)
{
std::map<int,Gps_Ephemeris>::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[item], 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((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo
{
if((rx_time_ms % (d_rtcm_MT1045_rate_ms / 4) ) == 0)
if(flag_make_RTCM_output)
{
for(std::map<int,Galileo_Ephemeris>::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((rx_time_ms % (d_rtcm_MSM_rate_ms / 4) ) == 0)
if(flag_make_RTCM_output)
{
std::map<int,Galileo_Ephemeris>::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[item], gnss_observables_map, 0, 0, 0, 0, 0);
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((rx_time_ms % d_rtcm_MT1019_rate_ms) == 0)
if(flag_make_RTCM_output)
{
for(std::map<int,Gps_Ephemeris>::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((rx_time_ms % d_rtcm_MSM_rate_ms) == 0)
if(flag_make_RTCM_output)
{
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
@ -871,27 +907,27 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], 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, gnss_observables_map, 0, 0, 0, 0, 0);
}
}
}
if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B
{
if(((rx_time_ms % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0))
if(flag_make_RTCM_output)
{
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(((rx_time_ms % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (d_rtcm_MT1045_rate_ms != 0))
if(flag_make_RTCM_output)
{
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(((rx_time_ms % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((rx_time_ms % (d_rtcm_MT1077_rate_ms / 4) ) == 0))
if(flag_make_RTCM_output)
{
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
@ -924,19 +960,19 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
}
i++;
}
if(((rx_time_ms % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) )
if(flag_make_RTCM_output)
{
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[item], 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(((rx_time_ms % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) )
if(flag_make_RTCM_output)
{
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[item], 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);
}
}
}
@ -956,7 +992,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], 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);
}
b_rtcm_writing_started = true;
}
@ -972,7 +1008,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], gnss_observables_map, 0, 0, 0, 0, 0);
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;
}
@ -988,7 +1024,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], 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, gnss_observables_map, 0, 0, 0, 0, 0);
}
b_rtcm_writing_started = true;
}
@ -1041,12 +1077,12 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
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[item], 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 (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[item], 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;
}
@ -1055,14 +1091,14 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
}
// DEBUG MESSAGE: Display position in console output
if (((rx_time_ms % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true)
if(d_ls_pvt->b_valid_position==true && flag_display_pvt==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] " << rx_time_ms<<std::endl;
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " 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
<< " 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::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
@ -1079,11 +1115,11 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
double tmp_double;
for (unsigned int i = 0; i < d_nchannels; i++)
{
tmp_double = in[i][item].Pseudorange_m;
tmp_double = in[i][epoch].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[item], sizeof(double));
d_dump_file.write((char*)&d_rx_time, sizeof(double));
}
}
catch (const std::ifstream::failure& e)
@ -1093,6 +1129,7 @@ int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items
}
}
}
consume_each(ninput_items[0]);
return ninput_items[0];
consume_each(ninput_items[0]); //one by one
return 1;
}

7
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@ -123,6 +123,7 @@ 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;
@ -132,14 +133,16 @@ private:
bool d_flag_averaging;
int d_output_rate_ms;
int d_display_rate_ms;
long unsigned int d_last_sample_nav_output;
//long unsigned int d_sample_counter;
//long unsigned int d_last_sample_nav_output;
std::shared_ptr<Rinex_Printer> rp;
std::shared_ptr<Kml_Printer> d_kml_dump;
std::shared_ptr<Nmea_Printer> d_nmea_printer;
std::shared_ptr<GeoJSON_Printer> d_geojson_printer;
std::shared_ptr<Rtcm_Printer> d_rtcm_printer;
double d_rx_time;
double last_pvt_display_T_rx_s;
std::shared_ptr<rtklib_solver> d_ls_pvt;
prcopt_t rtklib_options;
std::map<int,Gnss_Synchro> gnss_observables_map;

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

@ -112,7 +112,7 @@ bool Hybrid_valueCompare_gnss_synchro_sample_counter(const Gnss_Synchro& a, unsi
bool Hybrid_valueCompare_gnss_synchro_receiver_time(const Gnss_Synchro& a, double b)
{
return ((double)a.Tracking_sample_counter/(double)a.fs) < (b);
return (((double)a.Tracking_sample_counter+a.Code_phase_samples)/(double)a.fs) < (b);
}
bool Hybrid_pairCompare_gnss_synchro_d_TOW(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b)
@ -217,25 +217,25 @@ int hybrid_observables_cc::general_work (int noutput_items,
int distance=std::distance(d_gnss_synchro_history_queue[i].begin(), gnss_synchro_deque_iter);
if (distance>0)
{
// double T_rx_channel_prev=(double)d_gnss_synchro_history_queue[i].at(distance-1).Tracking_sample_counter/(double)gnss_synchro_deque_iter->fs;
// double delta_T_rx_s_prev=T_rx_channel_prev-T_rx_s;
// if (fabs(delta_T_rx_s_prev)<fabs(delta_T_rx_s))
// {
// realigned_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(
// d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID,
// d_gnss_synchro_history_queue[i].at(distance-1)));
// adjacent_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(gnss_synchro_deque_iter->Channel_ID,
// *gnss_synchro_deque_iter));
// }else{
double T_rx_channel_prev=(double)d_gnss_synchro_history_queue[i].at(distance-1).Tracking_sample_counter/(double)gnss_synchro_deque_iter->fs;
double delta_T_rx_s_prev=T_rx_channel_prev-T_rx_s;
if (fabs(delta_T_rx_s_prev)<fabs(delta_T_rx_s))
{
realigned_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(
d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID,
d_gnss_synchro_history_queue[i].at(distance-1)));
adjacent_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(gnss_synchro_deque_iter->Channel_ID,
*gnss_synchro_deque_iter));
}else{
realigned_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(gnss_synchro_deque_iter->Channel_ID,
*gnss_synchro_deque_iter));
adjacent_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(
d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID,
d_gnss_synchro_history_queue[i].at(distance-1)));
// }
// }else{
// realigned_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(gnss_synchro_deque_iter->Channel_ID,
// *gnss_synchro_deque_iter));
}
}else{
realigned_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(gnss_synchro_deque_iter->Channel_ID,
*gnss_synchro_deque_iter));
}
}else{
@ -254,27 +254,65 @@ int hybrid_observables_cc::general_work (int noutput_items,
gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.begin(),
realigned_gnss_synchro_map.end(),
Hybrid_pairCompare_gnss_synchro_d_TOW);
double ref_fs_hz=(double)gnss_synchro_map_iter->second.fs;
// compute interpolated TOW value at T_rx_s
int ref_channel_key=gnss_synchro_map_iter->second.Channel_ID;
Gnss_Synchro adj_obs=adjacent_gnss_synchro_map.at(ref_channel_key);
double ref_adj_T_rx_s=(double)adj_obs.Tracking_sample_counter/ref_fs_hz
+adj_obs.Code_phase_samples/ref_fs_hz;
double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s;
double d_ref_T_rx_s=(double)gnss_synchro_map_iter->second.Tracking_sample_counter/ref_fs_hz
+gnss_synchro_map_iter->second.Code_phase_samples/ref_fs_hz;
double selected_T_rx_s=T_rx_s;
// two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1)
double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s+(selected_T_rx_s-ref_adj_T_rx_s)
*(d_TOW_reference-adj_obs.TOW_at_current_symbol_s)/(d_ref_T_rx_s-ref_adj_T_rx_s);
//std::cout<<"DELTA T REF:"<<T_rx_s-ref_adj_T_rx_s<<std::endl;
//std::cout<<"ref TOW:"<<d_TOW_reference<<" ref_TOW_at_T_rx_s:"<<ref_TOW_at_T_rx_s<<std::endl;
// std::cout << std::fixed;
// std::cout << std::setprecision(2);
// std::cout<<"d_TOW_reference:"<<d_TOW_reference*1000.0<<std::endl;
double d_ref_PRN_phase_samples = gnss_synchro_map_iter->second.Code_phase_samples;
//std::cout<<"OBS SV REF SAT: "<<gnss_synchro_map_iter->second.PRN<<std::endl;
// Now compute RX time differences due to the PRN alignment in the correlators
double traveltime_ms;
double pseudorange_m;
double channel_T_rx_s;
double channel_fs_hz;
double channel_TOW_s;
double delta_T_rx_s;
double delta_PRN_phase_s;
//std::cout<<"T_rx_s: "<<T_rx_s<<std::endl;
for(gnss_synchro_map_iter = realigned_gnss_synchro_map.begin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.end(); gnss_synchro_map_iter++)
{
channel_fs_hz=(double)gnss_synchro_map_iter->second.fs;
channel_TOW_s=gnss_synchro_map_iter->second.TOW_at_current_symbol_s;
channel_T_rx_s=(double)gnss_synchro_map_iter->second.Tracking_sample_counter/channel_fs_hz
+gnss_synchro_map_iter->second.Code_phase_samples/channel_fs_hz;
// compute interpolated observation values
// two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1)
// TOW at the selected receiver time T_rx_s
int element_key=gnss_synchro_map_iter->second.Channel_ID;
adj_obs=adjacent_gnss_synchro_map.at(element_key);
double adj_T_rx_s=(double)adj_obs.Tracking_sample_counter/channel_fs_hz
+adj_obs.Code_phase_samples/channel_fs_hz;
double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s+(selected_T_rx_s-adj_T_rx_s)
*(channel_TOW_s-adj_obs.TOW_at_current_symbol_s)/(channel_T_rx_s-adj_T_rx_s);
//Doppler and Accumulated carrier phase
double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads+(selected_T_rx_s-adj_T_rx_s)
*(gnss_synchro_map_iter->second.Carrier_phase_rads-adj_obs.Carrier_phase_rads)/(channel_T_rx_s-adj_T_rx_s);
double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz+(selected_T_rx_s-adj_T_rx_s)
*(gnss_synchro_map_iter->second.Carrier_Doppler_hz-adj_obs.Carrier_Doppler_hz)/(channel_T_rx_s-adj_T_rx_s);
delta_T_rx_s = ((double)gnss_synchro_map_iter->second.Tracking_sample_counter/(double)gnss_synchro_map_iter->second.fs - T_rx_s);
delta_PRN_phase_s = (gnss_synchro_map_iter->second.Code_phase_samples - d_ref_PRN_phase_samples)/(double)gnss_synchro_map_iter->second.fs;
//compute the pseudorange (no rx time offset correction)
traveltime_ms = (d_TOW_reference - gnss_synchro_map_iter->second.TOW_at_current_symbol_s) * 1000.0
+ delta_T_rx_s*1000.0 + delta_PRN_phase_s*1000.0
traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0
+ GPS_STARTOFFSET_ms;
//convert to meters
pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
@ -283,28 +321,16 @@ int hybrid_observables_cc::general_work (int noutput_items,
current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true;
// Save the estimated RX time (no RX clock offset correction yet!)
current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = d_TOW_reference + GPS_STARTOFFSET_ms / 1000.0;
// compute interpolated observation values for Doppler and Accumulate carrier phase
// two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1)
int element_key=gnss_synchro_map_iter->second.Channel_ID;
Gnss_Synchro adj_obs=adjacent_gnss_synchro_map.at(element_key);
double adj_delta_T_rx_s=((double)adj_obs.Tracking_sample_counter/(double)adj_obs.fs - T_rx_s);
double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads+adj_delta_T_rx_s
*(gnss_synchro_map_iter->second.Carrier_phase_rads-adj_obs.Carrier_phase_rads)/(delta_T_rx_s-adj_delta_T_rx_s);
double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz+adj_delta_T_rx_s
*(gnss_synchro_map_iter->second.Carrier_Doppler_hz-adj_obs.Carrier_Doppler_hz)/(delta_T_rx_s-adj_delta_T_rx_s);
current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0;
current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads;
current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz;
//debug
//double delta_T_rx_s_previous=((double)adjacent_gnss_synchro_map.at(gnss_synchro_map_iter->second.Channel_ID).Tracking_sample_counter/(double)gnss_synchro_map_iter->second.fs - T_rx_s);
// std::cout<<"["<<gnss_synchro_map_iter->second.PRN<<"] delta_rx_t: "<<delta_T_rx_s*1000.0
// <<" [ms] (prev: "<<delta_T_rx_s_previous*1000.0<<") delta_TOW_ms: "<<(d_TOW_reference - gnss_synchro_map_iter->second.TOW_at_current_symbol_s) * 1000.0
// std::cout<<"["<<gnss_synchro_map_iter->second.PRN<<"] delta_TOW at T_rx: "<<(ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s)*1000.0
// <<" [ms] delta_TOW_ms: "<<(d_TOW_reference - gnss_synchro_map_iter->second.TOW_at_current_symbol_s) * 1000.0
// <<" Pr: "<<pseudorange_m<<" [m]"
// <<" Doppler estim: "<<gnss_synchro_map_iter->second.Carrier_Doppler_hz
// <<" Doppler inter: "<<Carrier_Doppler_lin_hz
// <<std::endl;
}