Added velocity and clock drift to PVT

Interestingly RTKLIB does not store the clock drift in its sol_t
datatype, so I added this to the unused element dtr[5]. This may not be
compatible with future versions of RTKLIB.

Here we store velocity in ENU coordinates and the clock drift in parts
per million (PPM)

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
 {

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;
 };

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -1071,6 +1071,23 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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)