[WIP] Adding velocity and clock drift

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc

@@ -28,7 +28,6 @@
  * -------------------------------------------------------------------------
  */
 
-#include "rtklib_pvt_gs.h"
 #include "MATH_CONSTANTS.h"
 #include "beidou_dnav_almanac.h"
 #include "beidou_dnav_ephemeris.h"
@@ -61,6 +60,7 @@
 #include "pvt_conf.h"
 #include "rinex_printer.h"
 #include "rtcm_printer.h"
+#include "rtklib_pvt_gs.h"
 #include "rtklib_solver.h"
 #include <boost/any.hpp>                   // for any_cast, any
 #include <boost/archive/xml_iarchive.hpp>  // for xml_iarchive
@@ -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/pvt_solution.cc

@@ -419,7 +419,7 @@ arma::vec Pvt_Solution::get_rx_pos() const
     return d_rx_pos;
 }
 
-void Pvt_Solution::set_rx_vel(arma::vec vel)
+void Pvt_Solution::set_rx_vel(const arma::vec &vel)
 {
     d_rx_vel = vel;
 }

src/algorithms/PVT/libs/pvt_solution.h

@@ -72,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);
 

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -1071,21 +1071,15 @@ 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(0) = enuv[0];
-                    rx_vel_enu(1) = vel_enu[1];
+                    rx_vel_enu(1) = enuv[1];
-                    rx_vel_enu(2) = vel_enu[2];
+                    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;
+                    double clock_drift_ppm = pvt_sol.dtr[5] / GPS_C_M_S * 1e6;
 
                     this->set_clock_drift_ppm(clock_drift_ppm);
 

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