mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 04:00:34 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2020-02-06 17:24:44 +01:00
commit 45a839b74f
2 changed files with 75 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -142,7 +142,13 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
mapStringValues_["B2"] = evBDS_B2; mapStringValues_["B2"] = evBDS_B2;
mapStringValues_["B3"] = evBDS_B3; mapStringValues_["B3"] = evBDS_B3;
initial_carrier_phase_offset_estimation_rads = std::vector<double>(nchannels, 0.0);
channel_initialized = std::vector<bool>(nchannels, false);
max_obs_block_rx_clock_offset_ms = conf_.max_obs_block_rx_clock_offset_ms; max_obs_block_rx_clock_offset_ms = conf_.max_obs_block_rx_clock_offset_ms;
d_output_rate_ms = conf_.output_rate_ms; d_output_rate_ms = conf_.output_rate_ms;
d_display_rate_ms = conf_.display_rate_ms; d_display_rate_ms = conf_.display_rate_ms;
d_report_rate_ms = 1000; // report every second PVT to gnss_synchro d_report_rate_ms = 1000; // report every second PVT to gnss_synchro
@ -1900,6 +1906,63 @@ std::map<int, Gnss_Synchro> rtklib_pvt_gs::interpolate_observables(std::map<int,
return interp_observables_map; return interp_observables_map;
} }
void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset()
{
//we have a valid PVT. First check if we need to reset the initial carrier phase offsets to match their pseudoranges
std::map<int, Gnss_Synchro>::iterator observables_iter;
for (observables_iter = gnss_observables_map.begin(); observables_iter != gnss_observables_map.end(); observables_iter++)
{
//check if an initialization is required (new satellite or loss of lock)
//it is set to false by the work function if the gnss_synchro is not valid
if (channel_initialized.at(observables_iter->second.Channel_ID) == false)
{
double wavelength_m = 0;
switch (mapStringValues_[observables_iter->second.Signal])
{
case evGPS_1C:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
break;
case evGPS_L5:
wavelength_m = SPEED_OF_LIGHT / FREQ5;
break;
case evSBAS_1C:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
break;
case evGAL_1B:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
break;
case evGAL_5X:
wavelength_m = SPEED_OF_LIGHT / FREQ5;
break;
case evGPS_2S:
wavelength_m = SPEED_OF_LIGHT / FREQ2;
break;
case evBDS_B3:
wavelength_m = SPEED_OF_LIGHT / FREQ3_BDS;
break;
case evGLO_1G:
wavelength_m = SPEED_OF_LIGHT / FREQ1_GLO;
break;
case evGLO_2G:
wavelength_m = SPEED_OF_LIGHT / FREQ2_GLO;
break;
case evBDS_B1:
wavelength_m = SPEED_OF_LIGHT / FREQ1_BDS;
break;
case evBDS_B2:
wavelength_m = SPEED_OF_LIGHT / FREQ2_BDS;
break;
default:
break;
}
initial_carrier_phase_offset_estimation_rads.at(observables_iter->second.Channel_ID) = (PI_2 * observables_iter->second.Pseudorange_m / wavelength_m) - observables_iter->second.Carrier_phase_rads;
channel_initialized.at(observables_iter->second.Channel_ID) = true;
DLOG(INFO) << "initialized carrier phase at channel " << observables_iter->second.Channel_ID;
}
//apply the carrier phase offset to this satellite
observables_iter->second.Carrier_phase_rads = observables_iter->second.Carrier_phase_rads + initial_carrier_phase_offset_estimation_rads.at(observables_iter->second.Channel_ID);
}
}
int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items, int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star& output_items __attribute__((unused))) gr_vector_void_star& output_items __attribute__((unused)))
@ -2021,6 +2084,10 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
} }
} }
} }
else
{
channel_initialized.at(i) = false; //the current channel is not reporting valid observable
}
} }
// ############ 2 COMPUTE THE PVT ################################ // ############ 2 COMPUTE THE PVT ################################
@ -2103,6 +2170,10 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
if (flag_pvt_valid == true) if (flag_pvt_valid == true)
{ {
//initialize (if needed) the accumulated phase offset and apply it to the active channels
//required to report accumulated phase cycles comparable to pseudoranges
initialize_and_apply_carrier_phase_offset();
double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s(); double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s();
if (d_enable_rx_clock_correction == true and fabs(Rx_clock_offset_s) > 0.000001) // 1us !! if (d_enable_rx_clock_correction == true and fabs(Rx_clock_offset_s) > 0.000001) // 1us !!
{ {

5
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@ -218,6 +218,9 @@ private:
std::map<int, Gnss_Synchro> gnss_observables_map_t0; std::map<int, Gnss_Synchro> gnss_observables_map_t0;
std::map<int, Gnss_Synchro> gnss_observables_map_t1; std::map<int, Gnss_Synchro> gnss_observables_map_t1;
std::vector<double> initial_carrier_phase_offset_estimation_rads;
std::vector<bool> channel_initialized;
uint32_t type_of_rx; uint32_t type_of_rx;
bool first_fix; bool first_fix;
@ -231,8 +234,8 @@ private:
bool send_sys_v_ttff_msg(ttff_msgbuf ttff); bool send_sys_v_ttff_msg(ttff_msgbuf ttff);
std::chrono::time_point<std::chrono::system_clock> start, end; std::chrono::time_point<std::chrono::system_clock> start, end;
void initialize_and_apply_carrier_phase_offset();
bool save_gnss_synchro_map_xml(const std::string& file_name); // debug helper function bool save_gnss_synchro_map_xml(const std::string& file_name); // debug helper function
bool load_gnss_synchro_map_xml(const std::string& file_name); // debug helper function bool load_gnss_synchro_map_xml(const std::string& file_name); // debug helper function
bool d_xml_storage; bool d_xml_storage;