mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-12-03 15:18:08 +00:00
Code Issues Releases Wiki Activity

refactor: lifted common code out of if ... else ...

Browse Source
This commit is contained in:
Victor Castillo
2025-06-12 13:36:36 +02:00
committed by Carles Fernandez
parent e3620664b3
commit ad0ae1e5c0
1 changed files with 10 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1552,19 +1552,18 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
if (d_conf.enable_pvt_kf == true)
{
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};
}
if (d_pvt_kf.is_initialized() == false)
{
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,
kf_update_interval_s,
@@ -1575,16 +1574,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
}
else
{
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]