mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-23 23:46:59 +00:00
Code Issues Releases Wiki Activity

Unpack IMU measurements in PVT block

Browse Source
This commit is contained in:
Victor Castillo 2024-10-06 13:22:27 +02:00
parent 82de4ab6de
commit ecd45d9861
No known key found for this signature in database
GPG Key ID: 8EF1FC8B7182F608
5 changed files with 62 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<gr::tag_t> 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<uint8_t> 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<double>(*reinterpret_cast<float*>(&extra_data_bytes[1 * sizeof(float)]));
imu_data.vel(1) =- static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[0 * sizeof(float)]));
imu_data.vel(2) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[2 * sizeof(float)]));
imu_data.acc(0) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[4 * sizeof(float)]));
imu_data.acc(1) = -static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[3 * sizeof(float)]));
imu_data.acc(2) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[5 * sizeof(float)]));
imu_data.ang_vel(0) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[7 * sizeof(float)]));
imu_data.ang_vel(1) = -static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[6 * sizeof(float)]));
imu_data.ang_vel(2) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[8 * sizeof(float)]));
imu_data.ang_acc(0) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[10 * sizeof(float)]));
imu_data.ang_acc(1) = -static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[9 * sizeof(float)]));
imu_data.ang_acc(2) = static_cast<double>(*reinterpret_cast<float*>(&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)

4
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -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<int, Gnss_Synchro> &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<int, Gnss_Synchro> &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<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@ -1981,6 +1981,8 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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

2
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -92,7 +92,7 @@ public:
~Rtklib_Solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double kf_update_interval_s, bool flag_averaging, bool enable_vtl, bool close_vtl_loop);
bool get_PVT(const std::map<int, Gnss_Synchro>& 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;

10
src/algorithms/PVT/libs/vtl_data.cc
View File

@ -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()

12
src/algorithms/PVT/libs/vtl_data.h
View File

@ -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();
};