Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

src/algorithms/PVT/adapters/rtklib_pvt.cc

@@ -49,6 +49,21 @@ namespace bc = boost::integer;
 using google::LogMessage;
 
 
+bool RtklibPvt::get_latest_PVT(double* longitude_deg,
+    double* latitude_deg,
+    double* height_m,
+    double* ground_speed_kmh,
+    double* course_over_ground_deg,
+    time_t* UTC_time)
+{
+    return pvt_->get_latest_PVT(longitude_deg,
+        latitude_deg,
+        height_m,
+        ground_speed_kmh,
+        course_over_ground_deg,
+        UTC_time);
+}
+
 void RtklibPvt::clear_ephemeris()
 {
     pvt_->clear_ephemeris();

src/algorithms/PVT/adapters/rtklib_pvt.h

@@ -86,6 +86,13 @@ public:
         return sizeof(gr_complex);
     }
 
+    bool get_latest_PVT(double* longitude_deg,
+        double* latitude_deg,
+        double* height_m,
+        double* ground_speed_kmh,
+        double* course_over_ground_deg,
+        time_t* UTC_time);
+
 private:
     rtklib_pvt_cc_sptr pvt_;
     rtk_t rtk;

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc

@@ -47,6 +47,7 @@
 #include <iostream>
 #include <map>
 #include <exception>
+#include <boost/date_time/posix_time/conversion.hpp>
 #if OLD_BOOST
 #include <boost/math/common_factor_rt.hpp>
 namespace bc = boost::math;
@@ -893,9 +894,36 @@ bool rtklib_pvt_cc::load_gnss_synchro_map_xml(const std::string file_name)
 }
 
 
+bool rtklib_pvt_cc::get_latest_PVT(double* longitude_deg,
+    double* latitude_deg,
+    double* height_m,
+    double* ground_speed_kmh,
+    double* course_over_ground_deg,
+    time_t* UTC_time)
+{
+    gr::thread::scoped_lock lock(d_setlock);
+    if (d_ls_pvt->is_valid_position())
+        {
+            *latitude_deg = d_ls_pvt->get_latitude();
+            *longitude_deg = d_ls_pvt->get_longitude();
+            *height_m = d_ls_pvt->get_height();
+            *ground_speed_kmh = d_ls_pvt->get_speed_over_ground() * 3600.0 / 1000.0;
+            *course_over_ground_deg = d_ls_pvt->get_course_over_ground();
+            *UTC_time = boost::posix_time::to_time_t(d_ls_pvt->get_position_UTC_time());
+
+            return true;
+        }
+    else
+        {
+            return false;
+        }
+}
+
 int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_items,
     gr_vector_void_star& output_items __attribute__((unused)))
 {
+    gr::thread::scoped_lock l(d_setlock);
+
     for (int32_t epoch = 0; epoch < noutput_items; epoch++)
         {
             bool flag_display_pvt = false;

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h

@@ -60,7 +60,7 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t n_channels,
     rtk_t& rtk);
 
 /*!
- * \brief This class implements a block that computes the PVT solution with Galileo E1 signals
+ * \brief This class implements a block that computes the PVT solution using the RTKLIB integrated library
  */
 class rtklib_pvt_cc : public gr::sync_block
 {
@@ -159,6 +159,17 @@ public:
      */
     void clear_ephemeris();
 
+
+    /*!
+     * \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available
+     */
+    bool get_latest_PVT(double* longitude_deg,
+        double* latitude_deg,
+        double* height_m,
+        double* ground_speed_kmh,
+        double* course_over_ground_deg,
+        time_t* UTC_time);
+
     ~rtklib_pvt_cc();  //!< Default destructor
 
     int work(int noutput_items, gr_vector_const_void_star& input_items,

src/algorithms/PVT/libs/pvt_solution.cc

@@ -43,6 +43,8 @@ Pvt_Solution::Pvt_Solution()
     d_latitude_d = 0.0;
     d_longitude_d = 0.0;
     d_height_m = 0.0;
+    d_speed_over_ground_m_s = 0.0;
+    d_course_over_ground_d = 0.0;
     d_avg_latitude_d = 0.0;
     d_avg_longitude_d = 0.0;
     d_avg_height_m = 0.0;
@@ -126,6 +128,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
     d_latitude_d = phi * 180.0 / GPS_PI;
     d_longitude_d = lambda * 180.0 / GPS_PI;
     d_height_m = h;
+    //todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
     return 0;
 }
 
@@ -516,6 +519,25 @@ double Pvt_Solution::get_height() const
     return d_height_m;
 }
 
+double Pvt_Solution::get_speed_over_ground() const
+{
+    return d_speed_over_ground_m_s;
+}
+
+void Pvt_Solution::set_speed_over_ground(double speed_m_s)
+{
+    d_speed_over_ground_m_s = speed_m_s;
+}
+
+void Pvt_Solution::set_course_over_ground(double cog_deg)
+{
+    d_course_over_ground_d = cog_deg;
+}
+
+double Pvt_Solution::get_course_over_ground() const
+{
+    return d_course_over_ground_d;
+}
 
 double Pvt_Solution::get_avg_latitude() const
 {

src/algorithms/PVT/libs/pvt_solution.h

@@ -49,9 +49,11 @@ class Pvt_Solution
 private:
     double d_rx_dt_s;  // RX time offset [s]
 
-    double d_latitude_d;   // RX position Latitude WGS84 [deg]
-    double d_longitude_d;  // RX position Longitude WGS84 [deg]
-    double d_height_m;     // RX position height WGS84 [m]
+    double d_latitude_d;             // RX position Latitude WGS84 [deg]
+    double d_longitude_d;            // RX position Longitude WGS84 [deg]
+    double d_height_m;               // RX position height WGS84 [m]
+    double d_speed_over_ground_m_s;  // RX speed over ground [m/s]
+    double d_course_over_ground_d;   // RX course over ground [deg]
 
     double d_avg_latitude_d;   // Averaged latitude in degrees
     double d_avg_longitude_d;  // Averaged longitude in degrees
@@ -86,6 +88,12 @@ public:
     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]
+
+    double get_course_over_ground() const;        //!< Get RX course over ground [deg]
+    void set_course_over_ground(double cog_deg);  //!< Set RX course over ground [deg]
+
     double get_avg_latitude() const;   //!< Get RX position averaged Latitude WGS84 [deg]
     double get_avg_longitude() const;  //!< Get RX position averaged Longitude WGS84 [deg]
     double get_avg_height() const;     //!< Get RX position averaged height WGS84 [m]

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -827,6 +827,22 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                             rx_position_and_time(3) = pvt_sol.dtr[0] / GPS_C_m_s;  // the receiver clock offset is expressed in [meters], so we convert it into [s]
                         }
                     this->set_rx_pos(rx_position_and_time.rows(0, 2));  // save ECEF position for the next iteration
+
+                    //compute Ground speed and COG
+                    double ground_speed_ms = 0.0;
+                    double pos[3];
+                    double enuv[3];
+                    ecef2pos(pvt_sol.rr, pos);
+                    ecef2enu(pos, &pvt_sol.rr[3], enuv);
+                    this->set_speed_over_ground(norm_rtk(enuv, 2));
+                    double new_cog;
+                    if (ground_speed_ms >= 1.0)
+                        {
+                            new_cog = atan2(enuv[0], enuv[1]) * R2D;
+                            if (new_cog < 0.0) new_cog += 360.0;
+                            this->set_course_over_ground(new_cog);
+                        }
+
                     //observable fix:
                     //double offset_s = this->get_time_offset_s();
                     //this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s));  // accumulate the rx time error for the next iteration [meters]->[seconds]