diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index e0dd13900..cf5eb2d39 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1985,6 +1985,40 @@ void rtklib_pvt_gs::update_HAS_corrections() int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items __attribute__((unused))) { + std::vector tags{}; + get_tags_in_range(tags, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items, pmt::mp("extra_data")); + + Vtl_Data::imu_data_t imu_data{}; + imu_data.init_storage(); + if (not tags.empty()) + { + // There should only be one tag + std::vector extra_data_bytes = pmt::u8vector_elements(tags.front().value); + + // Conversion from NED to ENU is done here + // | 0 1 0 | + // P_NED_ENU = | 1 0 0 | + // | 0 0 -1 | + + // Plus temporary inversion of the North values due to IMU placement + // Plus temporary inversion of the Up values due to IMU placement + // I think, the IMU is mounted rotated 180 deg around the East axis + + imu_data.vel(0) = static_cast(*reinterpret_cast(&extra_data_bytes[1 * sizeof(float)])); + imu_data.vel(1) =- static_cast(*reinterpret_cast(&extra_data_bytes[0 * sizeof(float)])); + imu_data.vel(2) = static_cast(*reinterpret_cast(&extra_data_bytes[2 * sizeof(float)])); + imu_data.acc(0) = static_cast(*reinterpret_cast(&extra_data_bytes[4 * sizeof(float)])); + imu_data.acc(1) = -static_cast(*reinterpret_cast(&extra_data_bytes[3 * sizeof(float)])); + imu_data.acc(2) = static_cast(*reinterpret_cast(&extra_data_bytes[5 * sizeof(float)])); + + imu_data.ang_vel(0) = static_cast(*reinterpret_cast(&extra_data_bytes[7 * sizeof(float)])); + imu_data.ang_vel(1) = -static_cast(*reinterpret_cast(&extra_data_bytes[6 * sizeof(float)])); + imu_data.ang_vel(2) = static_cast(*reinterpret_cast(&extra_data_bytes[8 * sizeof(float)])); + imu_data.ang_acc(0) = static_cast(*reinterpret_cast(&extra_data_bytes[10 * sizeof(float)])); + imu_data.ang_acc(1) = -static_cast(*reinterpret_cast(&extra_data_bytes[9 * sizeof(float)])); + imu_data.ang_acc(2) = static_cast(*reinterpret_cast(&extra_data_bytes[11 * sizeof(float)])); + } + // *************** time tags **************** if (d_enable_rx_clock_correction == false) // todo: currently only works if clock correction is disabled { @@ -2181,7 +2215,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, false, d_enable_vtl, d_close_vtl_loop)) + if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, false, d_enable_vtl, d_close_vtl_loop, imu_data)) { // ****** experimental VTL tests if (d_close_vtl_loop == true and d_enable_vtl == true) @@ -2373,7 +2407,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // VTP To.Do: Check why get_PVT is triggered twice. Leave only one get_PVT. // flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false, false, false); - flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, false, d_enable_vtl, d_close_vtl_loop); + flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, d_observable_interval_ms / 1000.0, false, d_enable_vtl, d_close_vtl_loop, imu_data); } if (flag_pvt_valid == true) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 7dde00c89..683d2c9c7 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1203,7 +1203,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 flag_averaging, bool enable_vtl, bool close_vtl_loop) +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s, bool flag_averaging, bool enable_vtl, bool close_vtl_loop, const Vtl_Data::imu_data_t& imu_data) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -1981,6 +1981,8 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ vtl_data.rx_dts(0) = rx_position_and_time[3]; vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] + vtl_data.imu_data = imu_data; + vtl_engine.vtl_loop(vtl_data); } else diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 554357a3d..ac5937159 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -92,7 +92,7 @@ public: ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, double kf_update_interval_s, bool flag_averaging, bool enable_vtl, bool close_vtl_loop); + bool get_PVT(const std::map& gnss_observables_map, double kf_update_interval_s, bool flag_averaging, bool enable_vtl, bool close_vtl_loop, const Vtl_Data::imu_data_t& imu_data); Vtl_Data vtl_data; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index 47e3228aa..b26fb9e21 100644 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -49,6 +49,16 @@ void Vtl_Data::init_storage(int n_sats) epoch_tow_s = 0; sample_counter = 0; + + imu_data.init_storage(); +} + +void Vtl_Data::imu_data_t::init_storage() +{ + vel = arma::vec(3); + acc = arma::vec(3); + ang_vel = arma::vec(3); + ang_acc = arma::vec(3); } void Vtl_Data::debug_print() diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 502b99351..7187a523a 100644 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -61,6 +61,18 @@ public: double PV[6]; // position and Velocity double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] + + // IMU data + struct imu_data_t + { + void init_storage(); + + arma::colvec vel; // Receiver ENU velocity from IMU [m/s] + arma::colvec acc; // Receiver ENU acceleration from IMU [m/(s^2)] + arma::colvec ang_vel; // Receiver ENU angular velocity from IMU [rad/s] + arma::colvec ang_acc; // Receiver ENU angular velocity from IMU [rad/(s^2)] + } imu_data; + void debug_print(); };