Adding Tele Command status for reporting the Position, Speed and Course over ground and Time

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_latitude_d;             // RX position Latitude WGS84 [deg]
-    double d_longitude_d;  // RX position Longitude WGS84 [deg]
+    double d_longitude_d;            // RX position Longitude WGS84 [deg]
-    double d_height_m;     // RX position height WGS84 [m]
+    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]

src/core/interfaces/pvt_interface.h

@@ -61,6 +61,13 @@ public:
     virtual std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() = 0;
     virtual std::map<int, Gps_Almanac> get_gps_almanac() = 0;
     virtual std::map<int, Galileo_Almanac> get_galileo_almanac() = 0;
+
+    virtual 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) = 0;
 };
 
 #endif /* GNSS_SDR_PVT_INTERFACE_H_ */

src/core/receiver/control_thread.cc

@@ -161,6 +161,7 @@ int ControlThread::run()
     sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
 
     //start the telecommand listener thread
+    cmd_interface_.set_pvt(flowgraph_->get_pvt());
     cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);
 
 
@@ -718,25 +719,33 @@ void ControlThread::apply_action(unsigned int what)
     switch (what)
         {
         case 0:
-            DLOG(INFO) << "Received action STOP";
+            LOG(INFO) << "Received action STOP";
             stop_ = true;
             applied_actions_++;
             break;
         case 1:
-            DLOG(INFO) << "Received action RESTART";
+            LOG(INFO) << "Received action RESTART";
             stop_ = true;
             restart_ = true;
             applied_actions_++;
             break;
+        case 11:
+            LOG(INFO) << "Receiver action COLDSTART";
+            //delete all ephemeris and almanac information from maps (also the PVT map queue)
+            pvt_ptr = flowgraph_->get_pvt();
+            pvt_ptr->clear_ephemeris();
+            //todo: reorder the satellite queues to the receiver default startup order.
+            //This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
+            break;
         case 12:
-            DLOG(INFO) << "Receiver action HOTSTART";
+            LOG(INFO) << "Receiver action HOTSTART";
             visible_satellites = get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
             //reorder the satellite queue to acquire first those visible satellites
             flowgraph_->priorize_satellites(visible_satellites);
             //start again the satellite acquisitions (done in chained applyaction to flowgraph)
             break;
         case 13:
-            DLOG(INFO) << "Receiver action WARMSTART";
+            LOG(INFO) << "Receiver action WARMSTART";
             //delete all ephemeris and almanac information from maps (also the PVT map queue)
             pvt_ptr = flowgraph_->get_pvt();
             pvt_ptr->clear_ephemeris();
@@ -750,7 +759,7 @@ void ControlThread::apply_action(unsigned int what)
             //start again the satellite acquisitions (done in chained applyaction to flowgraph)
             break;
         default:
-            DLOG(INFO) << "Unrecognized action.";
+            LOG(INFO) << "Unrecognized action.";
             break;
         }
 }
@@ -775,6 +784,13 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
     std::vector<std::pair<int, Gnss_Satellite>> available_satellites;
 
     std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
+    struct tm tstruct;
+    char buf[80];
+    tstruct = *gmtime(&rx_utc_time);
+    strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct);
+    std::string str_time = std::string(buf);
+    std::cout << "Get visible satellites at " << str_time
+              << " UTC, assuming RX position " << LLH(0) << " [deg], " << LLH(1) << " [deg], " << LLH(2) << " [m]" << std::endl;
 
     std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris();
     for (std::map<int, Gps_Ephemeris>::iterator it = gps_eph_map.begin(); it != gps_eph_map.end(); ++it)
@@ -789,10 +805,10 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
             arma::vec dx = r_sat_eb_e - r_eb_e;
             topocent(&Az, &El, &dist_m, r_eb_e, dx);
-            std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
             //push sat
             if (El > 0)
                 {
+                    std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                     available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                         (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
                 }
@@ -811,10 +827,10 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
             arma::vec dx = r_sat_eb_e - r_eb_e;
             topocent(&Az, &El, &dist_m, r_eb_e, dx);
-            std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
             //push sat
             if (El > 0)
                 {
+                    std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                     available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                         (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
                 }
@@ -832,10 +848,10 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
             arma::vec dx = r_sat_eb_e - r_eb_e;
             topocent(&Az, &El, &dist_m, r_eb_e, dx);
-            std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
             //push sat
             if (El > 0)
                 {
+                    std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                     available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                         (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
                 }
@@ -857,6 +873,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             //push sat
             if (El > 0)
                 {
+                    std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
                     available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
                         (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
                 }

src/core/receiver/gnss_flowgraph.cc

@@ -1103,11 +1103,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
             acq_channels_count_ = 0;  //all channels are in stanby now
             break;
         case 11:  //request coldstart mode
-            LOG(INFO) << "TC request coldstart";
+            LOG(INFO) << "TC request flowgraph coldstart";
-            //todo: delete all ephemeris and almanac information from maps (also the PVT map queue)
-            //todo: reorder the satellite queues to the receiver default startup order.
-            //This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
-
             //start again the satellite acquisitions
             for (unsigned int i = 0; i < channels_count_; i++)
                 {
@@ -1136,7 +1132,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
                 }
             break;
         case 12:  //request hotstart mode
-            LOG(INFO) << "TC request hotstart";
+            LOG(INFO) << "TC request flowgraph hotstart";
             for (unsigned int i = 0; i < channels_count_; i++)
                 {
                     unsigned int ch_index = (who + i + 1) % channels_count_;
@@ -1164,7 +1160,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
                 }
             break;
         case 13:  //request warmstart mode
-            LOG(INFO) << "TC request warmstart";
+            LOG(INFO) << "TC request flowgraph warmstart";
             //start again the satellite acquisitions
             for (unsigned int i = 0; i < channels_count_; i++)
                 {

src/core/receiver/tcp_cmd_interface.cc

@@ -32,8 +32,13 @@
 #include "tcp_cmd_interface.h"
 #include "control_message_factory.h"
 #include <functional>
+#include <sstream>
 
 
+void TcpCmdInterface::set_pvt(std::shared_ptr<PvtInterface> PVT_sptr)
+{
+    PVT_sptr_ = PVT_sptr;
+}
 time_t TcpCmdInterface::get_utc_time()
 {
     return receiver_utc_time_;
@@ -78,10 +83,51 @@ std::string TcpCmdInterface::standby(const std::vector<std::string> &commandLine
 
 std::string TcpCmdInterface::status(const std::vector<std::string> &commandLine)
 {
-    std::string response;
+    std::stringstream str_stream;
     //todo: implement the receiver status report
-    response = "Not implemented\n";
+
-    return response;
+    //    str_stream << "-------------------------------------------------------\n";
+    //    str_stream << "ch | sys | sig | mode | Tlm | Eph | Doppler | CN0 |\n";
+    //    str_stream << "   |     |     |      |     |     |  [Hz]   | [dB - Hz] |\n";
+    //    str_stream << "-------------------------------------------------------\n";
+    //    int n_ch = 10;
+    //    for (int n = 0; n < n_ch; n++)
+    //        {
+    //            str_stream << n << "GPS | L1CA | TRK | YES | YES | 23412.4 | 44.3 |\n";
+    //        }
+    //    str_stream << "--------------------------------------------------------\n";
+
+    double longitude_deg, latitude_deg, height_m, ground_speed_kmh, course_over_ground_deg;
+    time_t UTC_time;
+    if (PVT_sptr_->get_latest_PVT(&longitude_deg,
+            &latitude_deg,
+            &height_m,
+            &ground_speed_kmh,
+            &course_over_ground_deg,
+            &UTC_time) == true)
+        {
+            struct tm tstruct;
+            char buf1[80];
+            tstruct = *gmtime(&UTC_time);
+            strftime(buf1, sizeof(buf1), "%d/%m/%Y %H:%M:%S", &tstruct);
+            std::string str_time = std::string(buf1);
+            str_stream << "- Receiver UTC Time: " << str_time << std::endl;
+            str_stream << std::setprecision(9);
+            str_stream << "- Receiver Position WGS84 [Lat, Long, H]: "
+                       << latitude_deg << ", "
+                       << longitude_deg << ", ";
+            str_stream << std::setprecision(3);
+            str_stream << height_m << std::endl;
+            str_stream << std::setprecision(1);
+            str_stream << "- Receiver Speed over Ground [km/h]: " << ground_speed_kmh << std::endl;
+            str_stream << "- Receiver Course over ground [deg]: " << course_over_ground_deg << std::endl;
+        }
+    else
+        {
+            str_stream << "No PVT information available.\n";
+        }
+
+    return str_stream.str();
 }
 
 std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine)
@@ -277,6 +323,10 @@ void TcpCmdInterface::run_cmd_server(int tcp_port)
                                                             response = functions[cmd_vector.at(0)](cmd_vector);
                                                         }
                                                 }
+                                            catch (const std::bad_function_call &ex)
+                                                {
+                                                    response = "ERROR: command not found \n ";
+                                                }
                                             catch (const std::exception &ex)
                                                 {
                                                     response = "ERROR: command execution error: " + std::string(ex.what()) + "\n";

src/core/receiver/tcp_cmd_interface.h

@@ -31,6 +31,7 @@
 #ifndef GNSS_SDR_TCPCMDINTERFACE_H_
 #define GNSS_SDR_TCPCMDINTERFACE_H_
 
+#include "pvt_interface.h"
 #include <functional>
 #include <iostream>
 #include <string>
@@ -43,7 +44,7 @@
 #include <gnuradio/message.h>
 #include <gnuradio/msg_queue.h>
 #include <armadillo>
-#include "time.h"
+#include <time.h>
 
 class TcpCmdInterface
 {
@@ -61,6 +62,7 @@ public:
      */
     arma::vec get_LLH();
 
+    void set_pvt(std::shared_ptr<PvtInterface> PVT_sptr);
 
 private:
     std::unordered_map<std::string, std::function<std::string(const std::vector<std::string> &)>>
@@ -83,6 +85,8 @@ private:
     double rx_latitude_;
     double rx_longitude_;
     double rx_altitude_;
+
+    std::shared_ptr<PvtInterface> PVT_sptr_;
 };
 
 #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */