Merge branch 'odrisci-add_clock_drift' into next

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

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

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

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
 {

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

src/algorithms/PVT/libs/rtklib_solver.cc

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

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

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