From ea3db59fd720b4493902538580e63f9e95131b15 Mon Sep 17 00:00:00 2001 From: Javier Date: Tue, 6 Nov 2018 14:39:57 +0100 Subject: [PATCH] Adding Tele Command status for reporting the Position, Speed and Course over ground and Time --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 15 +++++ src/algorithms/PVT/adapters/rtklib_pvt.h | 7 +++ .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 28 ++++++++++ .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 13 ++++- src/algorithms/PVT/libs/pvt_solution.cc | 22 ++++++++ src/algorithms/PVT/libs/pvt_solution.h | 14 ++++- src/algorithms/PVT/libs/rtklib_solver.cc | 16 ++++++ src/core/interfaces/pvt_interface.h | 7 +++ src/core/receiver/control_thread.cc | 33 ++++++++--- src/core/receiver/gnss_flowgraph.cc | 10 +--- src/core/receiver/tcp_cmd_interface.cc | 56 ++++++++++++++++++- src/core/receiver/tcp_cmd_interface.h | 6 +- 12 files changed, 204 insertions(+), 23 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index c5532dea2..126813e26 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/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(); diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index 75e9c6c6d..93f238733 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/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; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 69cc62264..98202bb68 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -47,6 +47,7 @@ #include #include #include +#include #if OLD_BOOST #include 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; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index f063431d1..bf53bfd3d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/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, diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index e71030085..0ecbd15eb 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/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 { diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 958bf4668..585703d68 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/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] diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 12d316196..a4e1251e9 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -827,6 +827,22 @@ bool rtklib_solver::get_PVT(const std::map &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] diff --git a/src/core/interfaces/pvt_interface.h b/src/core/interfaces/pvt_interface.h index c9a2db162..db37d9afd 100644 --- a/src/core/interfaces/pvt_interface.h +++ b/src/core/interfaces/pvt_interface.h @@ -61,6 +61,13 @@ public: virtual std::map get_galileo_ephemeris() = 0; virtual std::map get_gps_almanac() = 0; virtual std::map 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_ */ diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index c95973797..b8bf6e345 100644 --- a/src/core/receiver/control_thread.cc +++ b/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> ControlThread::get_visible_sats(time std::vector> available_satellites; std::shared_ptr 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 gps_eph_map = pvt_ptr->get_gps_ephemeris(); for (std::map::iterator it = gps_eph_map.begin(); it != gps_eph_map.end(); ++it) @@ -789,10 +805,10 @@ std::vector> 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(floor(El), (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); } @@ -811,10 +827,10 @@ std::vector> 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(floor(El), (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN)))); } @@ -832,10 +848,10 @@ std::vector> 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(floor(El), (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); } @@ -857,6 +873,7 @@ std::vector> 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(floor(El), (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN)))); } diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 527696d00..ab15b1084 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/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"; - //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 - + LOG(INFO) << "TC request flowgraph coldstart"; //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++) { diff --git a/src/core/receiver/tcp_cmd_interface.cc b/src/core/receiver/tcp_cmd_interface.cc index b857ee204..5913034b5 100644 --- a/src/core/receiver/tcp_cmd_interface.cc +++ b/src/core/receiver/tcp_cmd_interface.cc @@ -32,8 +32,13 @@ #include "tcp_cmd_interface.h" #include "control_message_factory.h" #include +#include +void TcpCmdInterface::set_pvt(std::shared_ptr 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 &commandLine std::string TcpCmdInterface::status(const std::vector &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 &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"; diff --git a/src/core/receiver/tcp_cmd_interface.h b/src/core/receiver/tcp_cmd_interface.h index 7da156fe0..fa039116a 100644 --- a/src/core/receiver/tcp_cmd_interface.h +++ b/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 #include #include @@ -43,7 +44,7 @@ #include #include #include -#include "time.h" +#include class TcpCmdInterface { @@ -61,6 +62,7 @@ public: */ arma::vec get_LLH(); + void set_pvt(std::shared_ptr PVT_sptr); private: std::unordered_map &)>> @@ -83,6 +85,8 @@ private: double rx_latitude_; double rx_longitude_; double rx_altitude_; + + std::shared_ptr PVT_sptr_; }; #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */