diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 7dd0000c2..7c2417cd5 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -316,6 +316,16 @@ void Pvt_Solution::set_time_offset_s(double offset) d_rx_dt_s = offset; } +double Pvt_Solution::get_clock_drift_ppm() const +{ + return d_rx_clock_drift_ppm; +} + + +void Pvt_Solution::set_clock_drift_ppm(double clock_drift_ppm) +{ + d_rx_clock_drift_ppm = clock_drift_ppm; +} double Pvt_Solution::get_latitude() const { @@ -404,11 +414,22 @@ void Pvt_Solution::set_rx_pos(const arma::vec &pos) } -arma::vec Pvt_Solution::get_rx_pos() const +const arma::vec& Pvt_Solution::get_rx_pos() const { return d_rx_pos; } +void Pvt_Solution::set_rx_vel(arma::vec vel) +{ + d_rx_vel = vel; +} + + +const arma::vec& Pvt_Solution::get_rx_vel() const +{ + return d_rx_vel; +} + boost::posix_time::ptime Pvt_Solution::get_position_UTC_time() const { diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index f1f94f04d..ae8a6ab00 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -53,6 +53,8 @@ public: double get_time_offset_s() const; //!< Get RX time offset [s] void set_time_offset_s(double offset); //!< Set RX time offset [s] + double get_clock_drift_ppm() const; //!< Get the Rx clock drift [ppm] + void set_clock_drift_ppm(double clock_drift_ppm ); //!< Set the Rx clock drift [ppm] double get_latitude() const; //!< Get RX position Latitude WGS84 [deg] double get_longitude() const; //!< Get RX position Longitude WGS84 [deg] double get_height() const; //!< Get RX position height WGS84 [m] @@ -133,6 +135,7 @@ protected: bool d_pre_2009_file; // Flag to correct week rollover in post processing mode for signals older than 2009 private: double d_rx_dt_s; // RX time offset [s] + double d_rx_clock_drift_ppm; // RX clock drift [ppm] double d_latitude_d; // RX position Latitude WGS84 [deg] double d_longitude_d; // RX position Longitude WGS84 [deg] @@ -154,6 +157,7 @@ private: int d_averaging_depth; // Length of averaging window arma::vec d_rx_pos; + arma::vec d_rx_vel; boost::posix_time::ptime d_position_UTC_time; int d_valid_observations; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 75aa0f6ea..fdf249835 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1071,6 +1071,23 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ monitor_pvt.pdop = dop_[1]; monitor_pvt.hdop = dop_[2]; monitor_pvt.vdop = dop_[3]; + double vel_enu[3]; + double pos[3]; + pos[0] = rx_position_and_time(0); + pos[1] = rx_position_and_time(1); + pos[2] = rx_position_and_time(2); + ecef2enu( pos, &pvt_sol.rr[3], vel_enu ); + + arma::vec rx_vel_enu(3); + rx_vel_enu(0) = vel_enu[ 0 ]; + rx_vel_enu(1) = vel_enu[ 1 ]; + rx_vel_enu(2) = vel_enu[ 2 ]; + + this->set_rx_vel( rx_vel_enu ); + + double clock_drift_ppm = pvt_sol.dtr[5] / GPS_C_m_s *1e6; + + this->set_clock_drift_ppm( clock_drift_ppm ); // ######## LOG FILE ######### if (d_flag_dump_enabled == true)