diff --git a/docs/protobuf/monitor_pvt.proto b/docs/protobuf/monitor_pvt.proto index 3a2de2397..d0ebc0673 100644 --- a/docs/protobuf/monitor_pvt.proto +++ b/docs/protobuf/monitor_pvt.proto @@ -38,4 +38,6 @@ double gdop = 25; // Geometric Dilution of Precision double pdop = 26; // Position (3D) Dilution of Precision double hdop = 27; // Horizontal Dilution of Precision double vdop = 28; // Vertical Dilution of Precision + +double user_clk_drift_ppm = 29; // User clock drift [ppm] } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 1e83da190..cfcf7cc08 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -4168,6 +4168,15 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::cout << std::setprecision(ss); DLOG(INFO) << "RX clock offset: " << d_user_pvt_solver->get_time_offset_s() << "[s]"; + std::cout + << TEXT_BOLD_GREEN + << "Velocity: " << std::fixed << std::setprecision(3) + << "East: " << d_user_pvt_solver->get_rx_vel()[0] << " [m/s], North: " << d_user_pvt_solver->get_rx_vel()[1] + << " [m/s], Up = " << d_user_pvt_solver->get_rx_vel()[2] << " [m/s]" << TEXT_RESET << std::endl; + + std::cout << std::setprecision(ss); + DLOG(INFO) << "RX clock drift: " << d_user_pvt_solver->get_clock_drift_ppm() << " [ppm]"; + // boost::posix_time::ptime p_time; // gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_user_pvt_solver->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_time); // p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); diff --git a/src/algorithms/PVT/libs/monitor_pvt.h b/src/algorithms/PVT/libs/monitor_pvt.h index 582877947..09b6b9370 100644 --- a/src/algorithms/PVT/libs/monitor_pvt.h +++ b/src/algorithms/PVT/libs/monitor_pvt.h @@ -89,6 +89,9 @@ public: double hdop; double vdop; + // User clock drift [ppm] + double user_clk_drift_ppm; + /*! * \brief This member function serializes and restores * Monitor_Pvt objects from a byte stream. @@ -134,6 +137,8 @@ public: ar& BOOST_SERIALIZATION_NVP(pdop); ar& BOOST_SERIALIZATION_NVP(hdop); ar& BOOST_SERIALIZATION_NVP(vdop); + + ar& BOOST_SERIALIZATION_NVP(user_clk_drift_ppm); } }; diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 7dd0000c2..8a8527845 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 { @@ -409,6 +419,17 @@ arma::vec Pvt_Solution::get_rx_pos() const return d_rx_pos; } +void Pvt_Solution::set_rx_vel(const arma::vec &vel) +{ + d_rx_vel = vel; +} + + +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..ede8a5881 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -53,9 +53,11 @@ 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_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] + 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] double get_speed_over_ground() const; //!< Get RX speed over ground [m/s] void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s] @@ -70,6 +72,9 @@ public: void set_rx_pos(const arma::vec &pos); //!< Set position: Latitude [deg], longitude [deg], height [m] arma::vec get_rx_pos() const; + void set_rx_vel(const arma::vec &vel); //!< Set velocity: East [m/s], North [m/s], Up [m/s] + arma::vec get_rx_vel() const; + bool is_valid_position() const; void set_valid_position(bool is_valid); @@ -132,7 +137,8 @@ public: 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_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 +160,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..9381aa0bb 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1072,6 +1072,19 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ monitor_pvt.hdop = dop_[2]; monitor_pvt.vdop = dop_[3]; + arma::vec rx_vel_enu(3); + rx_vel_enu(0) = enuv[0]; + rx_vel_enu(1) = enuv[1]; + rx_vel_enu(2) = enuv[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); + // User clock drift [ppm] + monitor_pvt.user_clk_drift_ppm = clock_drift_ppm; + // ######## LOG FILE ######### if (d_flag_dump_enabled == true) { diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 07d643b9e..55b9689d4 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -116,6 +116,7 @@ public: monitor_.set_pdop(monitor->pdop); monitor_.set_hdop(monitor->hdop); monitor_.set_vdop(monitor->vdop); + monitor_.set_user_clk_drift_ppm(monitor->user_clk_drift_ppm); monitor_.SerializeToString(&data); return data; @@ -153,6 +154,7 @@ public: monitor.pdop = mon.pdop(); monitor.hdop = mon.hdop(); monitor.vdop = mon.vdop(); + monitor.user_clk_drift_ppm = mon.user_clk_drift_ppm(); return monitor; } diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 9f5de53bb..4a02e6df4 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -972,6 +972,7 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, { sol->rr[i + 3] = x[i]; } + sol->dtr[5] = x[3]; break; } }