diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 1d1eb6fa3..d8fc7236d 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -86,6 +86,10 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, pvt_output_parameters.system_ecef_pos_sd_m = configuration->property(role + ".kf_system_ecef_pos_sd_m", 2.0); pvt_output_parameters.system_ecef_vel_sd_ms = configuration->property(role + ".kf_system_ecef_vel_sd_ms", 0.5); + // PVT Sensor data settings + SensorDataSourceConfiguration sensor_data_configuration{configuration}; + pvt_output_parameters.kf_use_imu_vel = configuration->property(role + ".kf_use_imu_vel", false); + // NMEA Printer settings pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); @@ -628,7 +632,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, } // make PVT object - pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); + pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk, sensor_data_configuration); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; if (out_streams_ > 0) { diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index aa2fe4dbe..2fc310f62 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -114,17 +114,20 @@ namespace wht = std; rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels, const Pvt_Conf& conf_, - const rtk_t& rtk) + const rtk_t& rtk, + const SensorDataSourceConfiguration& sensor_data_configuration) { return rtklib_pvt_gs_sptr(new rtklib_pvt_gs(nchannels, conf_, - rtk)); + rtk, + sensor_data_configuration)); } rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, const Pvt_Conf& conf_, - const rtk_t& rtk) + const rtk_t& rtk, + const SensorDataSourceConfiguration& sensor_data_configuration) : gr::sync_block("rtklib_pvt_gs", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(0, 0, 0)), @@ -605,6 +608,16 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, } } + // Sensor Data + std::vector required_sensors{}; + if (conf_.kf_use_imu_vel) + { + required_sensors.emplace_back(SensorIdentifier::IMU_VEL_X); + required_sensors.emplace_back(SensorIdentifier::IMU_VEL_Y); + required_sensors.emplace_back(SensorIdentifier::IMU_VEL_Z); + } + d_sensor_data_aggregator = std::make_unique(sensor_data_configuration, required_sensors); + d_start = std::chrono::system_clock::now(); } @@ -1989,8 +2002,8 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item gr_vector_void_star& output_items __attribute__((unused))) { std::vector sensor_tags; - this->get_tags_in_range(sensor_tags, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items, pmt::mp("sensor_data")); - SensorDataAggregator sensor_data{sensor_tags}; + this->get_tags_in_range(sensor_tags, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items, d_sensor_data_aggregator->SENSOR_DATA_TAG); + d_sensor_data_aggregator->update(sensor_tags); for (int32_t epoch = 0; epoch < noutput_items; epoch++) { @@ -2159,7 +2172,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // old_time_debug = d_gnss_observables_map.cbegin()->second.RX_time * 1000.0; uint32_t current_RX_time_ms = 0; // #### solve PVT and store the corrected observable set - if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0)) + if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, *d_sensor_data_aggregator)) { d_pvt_errors_counter = 0; // Reset consecutive PVT error counter const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); @@ -2273,7 +2286,7 @@ 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) { - flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_output_rate_ms / 1000.0); + flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_output_rate_ms / 1000.0, *d_sensor_data_aggregator); } if (flag_pvt_valid == true) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index b54b3d670..4f2692f6e 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -23,6 +23,8 @@ #include "gnss_time.h" #include "osnma_data.h" #include "rtklib.h" +#include "sensor_data/sensor_data_aggregator.h" +#include "sensor_data/sensor_data_source_configuration.h" #include #include #include @@ -74,7 +76,8 @@ using rtklib_pvt_gs_sptr = gnss_shared_ptr; rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels, const Pvt_Conf& conf_, - const rtk_t& rtk); + const rtk_t& rtk, + const SensorDataSourceConfiguration& sensor_data_configuration); /*! * \brief This class implements a block that computes the PVT solution using the RTKLIB integrated library @@ -135,11 +138,13 @@ public: private: friend rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels, const Pvt_Conf& conf_, - const rtk_t& rtk); + const rtk_t& rtk, + const SensorDataSourceConfiguration& sensor_data_configuration); rtklib_pvt_gs(uint32_t nchannels, const Pvt_Conf& conf_, - const rtk_t& rtk); + const rtk_t& rtk, + const SensorDataSourceConfiguration& sensor_data_configuration); void log_source_timetag_info(double RX_time_ns, double TAG_time_ns); @@ -173,6 +178,8 @@ private: std::fstream d_log_timetag_file; + std::unique_ptr d_sensor_data_aggregator; + std::shared_ptr d_internal_pvt_solver; std::shared_ptr d_user_pvt_solver; diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index f014325ba..f7dbfea0a 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -103,6 +103,9 @@ public: double measures_ecef_vel_sd_ms = 0.1; double system_ecef_pos_sd_m = 0.01; double system_ecef_vel_sd_ms = 0.001; + + // Sensor Data parameters + bool kf_use_imu_vel = false; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 08dcaa2ec..a06afbdb1 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -906,7 +906,7 @@ void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, ui } -bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s) +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s, const SensorDataAggregator &sensor_data_aggregator) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -1556,6 +1556,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; + if (d_conf.kf_use_imu_vel) + { + v = { + sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_X).value, + sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Y).value, + sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Z).value}; + } + d_pvt_kf.init_Kf(p, v, @@ -1569,6 +1577,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; + if (d_conf.kf_use_imu_vel) + { + v = { + sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_X).value, + sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Y).value, + sensor_data_aggregator.get_last_f32(SensorIdentifier::IMU_VEL_Z).value}; + } + d_pvt_kf.run_Kf(p, v); d_pvt_kf.get_pv_Kf(p, v); pvt_sol.rr[0] = p[0]; // [m] diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 3fcb50172..4ce472581 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -61,6 +61,7 @@ #include "pvt_solution.h" #include "rtklib.h" #include "rtklib_conversions.h" +#include "sensor_data/sensor_data_aggregator.h" #include #include #include @@ -90,7 +91,7 @@ public: ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, double kf_update_interval_s); + bool get_PVT(const std::map& gnss_observables_map, double kf_update_interval_s, const SensorDataAggregator& sensor_data_aggregator); double get_hdop() const override; double get_vdop() const override;