From e3daff4dcffb1dea8c003aabe2a0807cc3688ea9 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Tue, 22 Nov 2022 15:56:52 +0100 Subject: [PATCH] mod: output from vtl loop --- src/algorithms/PVT/libs/rtklib_solver.cc | 6 ++++-- src/algorithms/PVT/libs/vtl_engine.cc | 9 +++++++-- src/algorithms/PVT/libs/vtl_engine.h | 2 +- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 5c2427761..f19eafe9e 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1152,9 +1152,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] //new_vtl_data.debug_print(); - pvt_sol.rr[0]=vtl_engine.vtl_loop(new_vtl_data); + double PVT[6]={0}; + vtl_engine.vtl_loop(new_vtl_data, PVT); - // pvt_sol.rr[0]=new_vtl_data.rx_p(0); + pvt_sol.rr[0]=PVT[0]; + pvt_sol.rr[1]=PVT[1]; // pvt_sol.rr[1]=new_vtl_data.rx_p(1); // pvt_sol.rr[2]=new_vtl_data.rx_p(2); // pvt_sol.rr[3]=new_vtl_data.rx_v(0); diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index d50c0a105..7deb893d9 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -27,7 +27,7 @@ Vtl_Engine::~Vtl_Engine() { } -bool Vtl_Engine::vtl_loop(Vtl_Data new_data) +bool Vtl_Engine::vtl_loop(Vtl_Data new_data, double PVT_out[6]) { //TODO: Implement main VTL loop here using arma::as_scalar; @@ -174,7 +174,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data) new_data.kf_state=kf_x; //updated state estimation new_data.kf_P=kf_P_x; //update state estimation error covariance // States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S) - + PVT_out[0]=kf_x(0); + PVT_out[1]=kf_x(1); + PVT_out[2]=kf_x(2); + PVT_out[3]=kf_x(3); + PVT_out[4]=kf_x(4); + PVT_out[5]=kf_x(5); // kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S; // kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S; diff --git a/src/algorithms/PVT/libs/vtl_engine.h b/src/algorithms/PVT/libs/vtl_engine.h index 88e564508..b9ae6a703 100755 --- a/src/algorithms/PVT/libs/vtl_engine.h +++ b/src/algorithms/PVT/libs/vtl_engine.h @@ -42,7 +42,7 @@ public: void configure(Vtl_Conf config_); //set config parameters //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...) - bool vtl_loop(Vtl_Data new_data); + bool vtl_loop(Vtl_Data new_data, double PVT_out[6]); void reset(); // reset all internal states void debug_print(); // print debug information