mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-25 22:43:14 +00:00
Code Issues Releases Wiki Activity

Adding configuration option to disable PVT observables clock correction

Browse Source
This commit is contained in:
Javier Arribas 2019-09-06 18:02:40 +02:00
parent 8b04de9462
commit 889e7b9695
5 changed files with 1927 additions and 1816 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -769,6 +769,8 @@ Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration,
// Show time in local zone
pvt_output_parameters.show_local_time_zone = configuration->property(role + ".show_local_time_zone", false);
//enable or disable rx clock corection in observables
pvt_output_parameters.enable_rx_clock_correction = configuration->property(role + ".enable_rx_clock_correction", true);
// make PVT object
pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";

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

@ -457,6 +457,14 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
d_local_time_str = std::string(" ") + time_zone_abrv + " (UTC " + utc_diff_str.substr(0, 3) + ":" + utc_diff_str.substr(3, 2) + ")";
}
d_waiting_obs_block_rx_clock_offset_correction_msg = false;
d_enable_rx_clock_correction = conf_.enable_rx_clock_correction;
if (d_enable_rx_clock_correction == true)
{
//setup two PVT solvers: internal solver for rx clock and user solver
// user PVT solver
d_user_pvt_solver = std::make_shared<Rtklib_Solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, d_dump_mat, rtk);
d_user_pvt_solver->set_averaging_depth(1);
@ -466,8 +474,14 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
internal_rtk.opt.mode = PMODE_SINGLE; // use single positioning mode in internal PVT solver
d_internal_pvt_solver = std::make_shared<Rtklib_Solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, false, false, internal_rtk);
d_internal_pvt_solver->set_averaging_depth(1);
d_waiting_obs_block_rx_clock_offset_correction_msg = false;
}
else
{
//only one solver, customized by the user options
d_internal_pvt_solver = std::make_shared<Rtklib_Solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, d_dump_mat, rtk);
d_internal_pvt_solver->set_averaging_depth(1);
d_user_pvt_solver = d_internal_pvt_solver;
}
start = std::chrono::system_clock::now();
}
@ -1100,15 +1114,21 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
}
}
d_internal_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph;
}
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Iono>))
{
// ### GPS IONO ###
std::shared_ptr<Gps_Iono> gps_iono;
gps_iono = boost::any_cast<std::shared_ptr<Gps_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_iono = *gps_iono;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->gps_iono = *gps_iono;
}
DLOG(INFO) << "New IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Utc_Model>))
@ -1117,7 +1137,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Gps_Utc_Model> gps_utc_model;
gps_utc_model = boost::any_cast<std::shared_ptr<Gps_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_utc_model = *gps_utc_model;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->gps_utc_model = *gps_utc_model;
}
DLOG(INFO) << "New UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Ephemeris>))
@ -1173,7 +1196,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
}
}
d_internal_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
}
DLOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>))
@ -1182,7 +1208,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Gps_CNAV_Iono> gps_cnav_iono;
gps_cnav_iono = boost::any_cast<std::shared_ptr<Gps_CNAV_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_cnav_iono = *gps_cnav_iono;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->gps_cnav_iono = *gps_cnav_iono;
}
DLOG(INFO) << "New CNAV IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Utc_Model>))
@ -1191,7 +1220,9 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Gps_CNAV_Utc_Model> gps_cnav_utc_model;
gps_cnav_utc_model = boost::any_cast<std::shared_ptr<Gps_CNAV_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
{
d_user_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
}
DLOG(INFO) << "New CNAV UTC record has arrived ";
}
@ -1201,7 +1232,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Gps_Almanac> gps_almanac;
gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac;
}
DLOG(INFO) << "New GPS almanac record has arrived ";
}
@ -1270,15 +1304,21 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
}
}
d_internal_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph;
}
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Iono>))
{
// ### Galileo IONO ###
std::shared_ptr<Galileo_Iono> galileo_iono;
galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->galileo_iono = *galileo_iono;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->galileo_iono = *galileo_iono;
}
DLOG(INFO) << "New IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Utc_Model>))
@ -1287,7 +1327,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Galileo_Utc_Model> galileo_utc_model;
galileo_utc_model = boost::any_cast<std::shared_ptr<Galileo_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->galileo_utc_model = *galileo_utc_model;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->galileo_utc_model = *galileo_utc_model;
}
DLOG(INFO) << "New UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac_Helper>))
@ -1303,18 +1346,27 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
if (sv1.i_satellite_PRN != 0)
{
d_internal_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1;
}
}
if (sv2.i_satellite_PRN != 0)
{
d_internal_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2;
}
}
if (sv3.i_satellite_PRN != 0)
{
d_internal_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3;
}
}
DLOG(INFO) << "New Galileo Almanac data have arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>))
@ -1324,8 +1376,11 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
galileo_alm = boost::any_cast<std::shared_ptr<Galileo_Almanac>>(pmt::any_ref(msg));
// update/insert new almanac record to the global almanac map
d_internal_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
}
}
// **************** GLONASS GNAV Telemetry **************************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>))
@ -1405,15 +1460,21 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
}
}
d_internal_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph;
}
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Utc_Model>))
{
// ### GLONASS GNAV UTC MODEL ###
std::shared_ptr<Glonass_Gnav_Utc_Model> glonass_gnav_utc_model;
glonass_gnav_utc_model = boost::any_cast<std::shared_ptr<Glonass_Gnav_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
}
DLOG(INFO) << "New GLONASS GNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Almanac>))
@ -1422,7 +1483,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Glonass_Gnav_Almanac> glonass_gnav_almanac;
glonass_gnav_almanac = boost::any_cast<std::shared_ptr<Glonass_Gnav_Almanac>>(pmt::any_ref(msg));
d_internal_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac;
}
DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "
<< ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A;
}
@ -1469,15 +1533,21 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
}
}
d_internal_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph;
}
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Iono>))
{
// ### BeiDou IONO ###
std::shared_ptr<Beidou_Dnav_Iono> bds_dnav_iono;
bds_dnav_iono = boost::any_cast<std::shared_ptr<Beidou_Dnav_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->beidou_dnav_iono = *bds_dnav_iono;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->beidou_dnav_iono = *bds_dnav_iono;
}
DLOG(INFO) << "New BeiDou DNAV IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>))
@ -1486,7 +1556,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Beidou_Dnav_Utc_Model> bds_dnav_utc_model;
bds_dnav_utc_model = boost::any_cast<std::shared_ptr<Beidou_Dnav_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
}
DLOG(INFO) << "New BeiDou DNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Almanac>))
@ -1495,7 +1568,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
std::shared_ptr<Beidou_Dnav_Almanac> bds_dnav_almanac;
bds_dnav_almanac = boost::any_cast<std::shared_ptr<Beidou_Dnav_Almanac>>(pmt::any_ref(msg));
d_internal_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac;
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac;
}
DLOG(INFO) << "New BeiDou DNAV almanac record has arrived ";
}
else
@ -1554,7 +1630,8 @@ void rtklib_pvt_gs::clear_ephemeris()
d_internal_pvt_solver->galileo_almanac_map.clear();
d_internal_pvt_solver->beidou_dnav_ephemeris_map.clear();
d_internal_pvt_solver->beidou_dnav_almanac_map.clear();
if (d_enable_rx_clock_correction == true)
{
d_user_pvt_solver->gps_ephemeris_map.clear();
d_user_pvt_solver->gps_almanac_map.clear();
d_user_pvt_solver->galileo_ephemeris_map.clear();
@ -1562,6 +1639,7 @@ void rtklib_pvt_gs::clear_ephemeris()
d_user_pvt_solver->beidou_dnav_ephemeris_map.clear();
d_user_pvt_solver->beidou_dnav_almanac_map.clear();
}
}
bool rtklib_pvt_gs::send_sys_v_ttff_msg(ttff_msgbuf ttff)
@ -1647,6 +1725,8 @@ bool rtklib_pvt_gs::get_latest_PVT(double* longitude_deg,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time) const
{
if (d_enable_rx_clock_correction == true)
{
if (d_user_pvt_solver->is_valid_position())
{
@ -1659,6 +1739,21 @@ bool rtklib_pvt_gs::get_latest_PVT(double* longitude_deg,
return true;
}
}
else
{
if (d_internal_pvt_solver->is_valid_position())
{
*latitude_deg = d_internal_pvt_solver->get_latitude();
*longitude_deg = d_internal_pvt_solver->get_longitude();
*height_m = d_internal_pvt_solver->get_height();
*ground_speed_kmh = d_internal_pvt_solver->get_speed_over_ground() * 3600.0 / 1000.0;
*course_over_ground_deg = d_internal_pvt_solver->get_course_over_ground();
*UTC_time = convert_to_time_t(d_internal_pvt_solver->get_position_UTC_time());
return true;
}
}
return false;
}
@ -1890,6 +1985,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
}
// ############ 2 COMPUTE THE PVT ################################
bool flag_pvt_valid = false;
if (gnss_observables_map.empty() == false)
{
// LOG(INFO) << "diff raw obs time: " << gnss_observables_map.cbegin()->second.RX_time * 1000.0 - old_time_debug;
@ -1909,6 +2005,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
}
}
else
{
if (d_enable_rx_clock_correction == true)
{
d_waiting_obs_block_rx_clock_offset_correction_msg = false;
gnss_observables_map_t0 = gnss_observables_map_t1;
@ -1938,6 +2036,12 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
}
}
}
else
{
flag_compute_pvt_output = false;
flag_pvt_valid = true;
}
}
}
// debug code
// else
@ -1948,10 +2052,13 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
// compute on the fly PVT solution
if (flag_compute_pvt_output == true)
{
if (d_user_pvt_solver->get_PVT(gnss_observables_map, false))
flag_pvt_valid = d_user_pvt_solver->get_PVT(gnss_observables_map, false);
}
if (flag_pvt_valid == true)
{
double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s();
if (fabs(Rx_clock_offset_s) > 0.000001) // 1us !!
if (d_enable_rx_clock_correction == true and fabs(Rx_clock_offset_s) > 0.000001) // 1us !!
{
LOG(INFO) << "Warning: Rx clock offset at interpolated RX time: " << Rx_clock_offset_s * 1000.0 << "[ms]"
<< " at RX time: " << static_cast<uint32_t>(d_rx_time * 1000.0) << " [ms]";
@ -3910,7 +4017,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
}
}
}
}
// DEBUG MESSAGE: Display position in console output
if (d_user_pvt_solver->is_valid_position() and flag_display_pvt)

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

@ -213,6 +213,7 @@ private:
int32_t max_obs_block_rx_clock_offset_ms;
bool d_waiting_obs_block_rx_clock_offset_correction_msg;
bool d_enable_rx_clock_correction;
std::map<int, Gnss_Synchro> gnss_observables_map;
std::map<int, Gnss_Synchro> gnss_observables_map_t0;
std::map<int, Gnss_Synchro> gnss_observables_map_t1;

1
src/algorithms/PVT/libs/pvt_conf.cc
View File

@ -72,6 +72,7 @@ Pvt_Conf::Pvt_Conf()
xml_output_path = std::string(".");
rtcm_output_file_path = std::string(".");
enable_rx_clock_correction=true;
monitor_enabled = false;
protobuf_enabled = true;
udp_port = 0;

1
src/algorithms/PVT/libs/pvt_conf.h
View File

@ -89,6 +89,7 @@ public:
std::string udp_addresses;
int udp_port;
bool enable_rx_clock_correction;
bool show_local_time_zone;
Pvt_Conf();