From 01a1954b155b5d9a72d0c2e05ddfb5f03fda67a1 Mon Sep 17 00:00:00 2001 From: Javier Date: Mon, 5 Nov 2018 17:53:53 +0100 Subject: [PATCH] Adding the Hotstart telecommand implementation to predict visible satellites for GPS and Galileo constellations --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 22 +++ src/algorithms/PVT/adapters/rtklib_pvt.h | 7 + .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 23 ++- .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 19 ++- src/algorithms/channel/libs/channel_fsm.cc | 4 +- src/algorithms/libs/CMakeLists.txt | 1 + .../libs/geofunctions.cc | 0 .../libs/geofunctions.h | 0 .../libs/rtklib/rtklib_conversions.cc | 58 ++++++- .../libs/rtklib/rtklib_conversions.h | 6 + src/core/interfaces/pvt_interface.h | 9 ++ src/core/receiver/control_thread.cc | 147 ++++++++++++++++++ src/core/receiver/control_thread.h | 13 +- src/core/receiver/gnss_flowgraph.cc | 64 ++++++-- src/core/receiver/gnss_flowgraph.h | 22 ++- src/core/receiver/tcp_cmd_interface.cc | 87 +++++++++-- src/core/receiver/tcp_cmd_interface.h | 17 +- src/main/CMakeLists.txt | 1 + src/tests/system-tests/libs/CMakeLists.txt | 1 - 19 files changed, 458 insertions(+), 43 deletions(-) rename src/{tests/system-tests => algorithms}/libs/geofunctions.cc (100%) rename src/{tests/system-tests => algorithms}/libs/geofunctions.h (100%) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 74b2f4c5d..c5532dea2 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -48,6 +48,12 @@ namespace bc = boost::integer; using google::LogMessage; + +void RtklibPvt::clear_ephemeris() +{ + pvt_->clear_ephemeris(); +} + RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, std::string role, unsigned int in_streams, @@ -515,6 +521,22 @@ RtklibPvt::~RtklibPvt() rtkfree(&rtk); } +std::map RtklibPvt::get_gps_ephemeris() +{ + return pvt_->get_gps_ephemeris_map(); +} +std::map RtklibPvt::get_galileo_ephemeris() +{ + return pvt_->get_galileo_ephemeris_map(); +} +std::map RtklibPvt::get_gps_almanac() +{ + return pvt_->get_gps_almanac_map(); +} +std::map RtklibPvt::get_galileo_almanac() +{ + return pvt_->get_galileo_almanac_map(); +} void RtklibPvt::connect(gr::top_block_sptr top_block) { diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index c79519581..75e9c6c6d 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -63,6 +63,13 @@ public: return "RTKLIB_PVT"; } + void clear_ephemeris(); + std::map get_gps_ephemeris(); + std::map get_galileo_ephemeris(); + std::map get_gps_almanac(); + std::map get_galileo_almanac(); + + void connect(gr::top_block_sptr top_block) override; void disconnect(gr::top_block_sptr top_block) override; gr::basic_block_sptr get_left_block() override; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index fe2cce6e1..69cc62264 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -234,11 +234,32 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) } -std::map rtklib_pvt_cc::get_GPS_L1_ephemeris_map() +std::map rtklib_pvt_cc::get_gps_ephemeris_map() { return d_ls_pvt->gps_ephemeris_map; } +std::map rtklib_pvt_cc::get_gps_almanac_map() +{ + return d_ls_pvt->gps_almanac_map; +} + +std::map rtklib_pvt_cc::get_galileo_ephemeris_map() +{ + return d_ls_pvt->galileo_ephemeris_map; +} +std::map rtklib_pvt_cc::get_galileo_almanac_map() +{ + return d_ls_pvt->galileo_almanac_map; +} + +void rtklib_pvt_cc::clear_ephemeris() +{ + d_ls_pvt->gps_ephemeris_map.clear(); + d_ls_pvt->gps_almanac_map.clear(); + d_ls_pvt->galileo_ephemeris_map.clear(); + d_ls_pvt->galileo_almanac_map.clear(); +} rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, const Pvt_Conf& conf_, diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 427936704..f063431d1 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -31,7 +31,7 @@ #ifndef GNSS_SDR_RTKLIB_PVT_CC_H #define GNSS_SDR_RTKLIB_PVT_CC_H - +#include "gps_ephemeris.h" #include "nmea_printer.h" #include "kml_printer.h" #include "gpx_printer.h" @@ -142,11 +142,22 @@ public: rtk_t& rtk); /*! - * \brief Get latest set of GPS L1 ephemeris from PVT block + * \brief Get latest set of ephemeris from PVT block * - * It is used to save the assistance data at the receiver shutdown */ - std::map get_GPS_L1_ephemeris_map(); + std::map get_gps_ephemeris_map(); + + std::map get_gps_almanac_map(); + + std::map get_galileo_ephemeris_map(); + + std::map get_galileo_almanac_map(); + + /*! + * \brief Clear all ephemeris information and the almanacs for GPS and Galileo + * + */ + void clear_ephemeris(); ~rtklib_pvt_cc(); //!< Default destructor diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 5b212193a..14a4e6b12 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -59,7 +59,6 @@ bool ChannelFsm::Event_stop_channel() switch (d_state) { case 0: //already in stanby - return true; break; case 1: //acquisition d_state = 0; @@ -70,8 +69,9 @@ bool ChannelFsm::Event_stop_channel() stop_tracking(); break; default: - return true; + break; } + return true; } bool ChannelFsm::Event_start_acquisition() diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index 18dd400b0..ce4560e6f 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -39,6 +39,7 @@ set(GNSS_SPLIBS_SOURCES conjugate_sc.cc conjugate_ic.cc gnss_sdr_create_directory.cc + geofunctions.cc ) set(GNSS_SPLIBS_HEADERS diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/algorithms/libs/geofunctions.cc similarity index 100% rename from src/tests/system-tests/libs/geofunctions.cc rename to src/algorithms/libs/geofunctions.cc diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/algorithms/libs/geofunctions.h similarity index 100% rename from src/tests/system-tests/libs/geofunctions.h rename to src/algorithms/libs/geofunctions.h diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 7953509a2..22b8af3b9 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -116,7 +116,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glona eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO; rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; @@ -174,7 +174,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; rtklib_sat.sat = gps_eph.i_satellite_PRN; rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; rtklib_sat.M0 = gps_eph.d_M_0; @@ -231,7 +231,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; @@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) return rtklib_sat; } + +alm_t alm_to_rtklib(const Gps_Almanac& gps_alm) +{ + alm_t rtklib_alm; + + rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + rtklib_alm.sat = gps_alm.i_satellite_PRN; + rtklib_alm.svh = gps_alm.i_SV_health; + rtklib_alm.svconf = gps_alm.i_AS_status; + rtklib_alm.week = gps_alm.i_WNa; + rtklib_alm.toa = gpst2time(gps_alm.i_WNa, gps_alm.d_Toa); + rtklib_alm.A = gps_alm.d_sqrt_A * gps_alm.d_sqrt_A; + rtklib_alm.e = gps_alm.d_e_eccentricity; + rtklib_alm.i0 = gps_alm.d_Delta_i + 0.3; + rtklib_alm.OMG0 = gps_alm.d_OMEGA0; + rtklib_alm.OMGd = gps_alm.d_OMEGA_DOT; + rtklib_alm.omg = gps_alm.d_OMEGA; + rtklib_alm.M0 = gps_alm.d_M_0; + rtklib_alm.f0 = gps_alm.d_A_f0; + rtklib_alm.f1 = gps_alm.d_A_f1; + rtklib_alm.toas = gps_alm.d_Toa; + + + return rtklib_alm; +} +alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm) +{ + alm_t rtklib_alm; + + rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + rtklib_alm.sat = gal_alm.i_satellite_PRN; + rtklib_alm.svh = gal_alm.E1B_HS; + rtklib_alm.svconf = gal_alm.E1B_HS; + rtklib_alm.week = gal_alm.d_WNa; + rtklib_alm.toa = gpst2time(gal_alm.d_WNa, gal_alm.d_Toa); + rtklib_alm.A = 5440.588203494 + gal_alm.d_Delta_sqrt_A; + rtklib_alm.A = rtklib_alm.A * rtklib_alm.A; + rtklib_alm.e = gal_alm.d_e_eccentricity; + rtklib_alm.i0 = gal_alm.d_Delta_i + 0.31111; + rtklib_alm.OMG0 = gal_alm.d_OMEGA0; + rtklib_alm.OMGd = gal_alm.d_OMEGA_DOT; + rtklib_alm.omg = gal_alm.d_OMEGA; + rtklib_alm.M0 = gal_alm.d_M_0; + rtklib_alm.f0 = gal_alm.d_A_f0; + rtklib_alm.f1 = gal_alm.d_A_f1; + rtklib_alm.toas = gal_alm.d_Toa; + + + return rtklib_alm; +} diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index 11f4208d1..dca5ec7be 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -38,10 +38,16 @@ #include "gps_cnav_ephemeris.h" #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_utc_model.h" +#include "gps_almanac.h" +#include "galileo_almanac.h" eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph); eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph); eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph); + +alm_t alm_to_rtklib(const Gps_Almanac& gps_alm); +alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm); + /*! * \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart * \param glonass_gnav_eph GLONASS GNAV Ephemeris structure diff --git a/src/core/interfaces/pvt_interface.h b/src/core/interfaces/pvt_interface.h index 69b23f541..c9a2db162 100644 --- a/src/core/interfaces/pvt_interface.h +++ b/src/core/interfaces/pvt_interface.h @@ -38,6 +38,10 @@ #define GNSS_SDR_PVT_INTERFACE_H_ #include "gnss_block_interface.h" +#include "gps_ephemeris.h" +#include "galileo_ephemeris.h" +#include "gps_almanac.h" +#include "galileo_almanac.h" /*! * \brief This class represents an interface to a PVT block. @@ -52,6 +56,11 @@ class PvtInterface : public GNSSBlockInterface { public: virtual void reset() = 0; + virtual void clear_ephemeris() = 0; + virtual std::map get_gps_ephemeris() = 0; + virtual std::map get_galileo_ephemeris() = 0; + virtual std::map get_gps_almanac() = 0; + virtual std::map get_galileo_almanac() = 0; }; #endif /* GNSS_SDR_PVT_INTERFACE_H_ */ diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index a73d2b446..c95973797 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -35,6 +35,7 @@ #include "control_thread.h" #include "concurrent_queue.h" #include "concurrent_map.h" +#include "pvt_interface.h" #include "control_message_factory.h" #include "file_configuration.h" #include "gnss_flowgraph.h" @@ -49,6 +50,10 @@ #include "gps_almanac.h" #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_utc_model.h" +#include "geofunctions.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_conversions.h" +#include "rtklib_ephemeris.h" #include #include #include @@ -693,6 +698,10 @@ void ControlThread::process_control_messages() } else { + if (control_messages_->at(i)->who == 300) //some TC commands require also actions from controlthread + { + apply_action(control_messages_->at(i)->what); + } flowgraph_->apply_action(control_messages_->at(i)->who, control_messages_->at(i)->what); } processed_control_messages_++; @@ -704,6 +713,8 @@ void ControlThread::process_control_messages() void ControlThread::apply_action(unsigned int what) { + std::shared_ptr pvt_ptr; + std::vector> visible_satellites; switch (what) { case 0: @@ -717,12 +728,148 @@ void ControlThread::apply_action(unsigned int what) restart_ = true; applied_actions_++; break; + case 12: + DLOG(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"; + //delete all ephemeris and almanac information from maps (also the PVT map queue) + pvt_ptr = flowgraph_->get_pvt(); + pvt_ptr->clear_ephemeris(); + //load the ephemeris and the almanac from XML files (receiver assistance) + read_assistance_from_XML(); + //call here the function that computes the set of visible satellites and its elevation + //for the date and time specified by the warmstart command and the assisted position + 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; default: DLOG(INFO) << "Unrecognized action."; break; } } +std::vector> ControlThread::get_visible_sats(time_t rx_utc_time, arma::vec LLH) +{ + //1. Compute rx ECEF position from LLH WGS84 + arma::vec LLH_rad = arma::vec{degtorad(LLH(0)), degtorad(LLH(1)), LLH(2)}; + arma::mat C_tmp = arma::zeros(3, 3); + arma::vec r_eb_e = arma::zeros(3, 1); + arma::vec v_eb_e = arma::zeros(3, 1); + Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp); + + //2. Compute rx GPS time from UTC time + gtime_t utc_gtime; + utc_gtime.time = rx_utc_time; + utc_gtime.sec = 0; + gtime_t gps_gtime = utc2gpst(utc_gtime); + + //2. loop throught all the available ephemeris or almanac and compute satellite positions and elevations + // store visible satellites in a vector of pairs to associate an elevation to the each satellite + std::vector> available_satellites; + + std::shared_ptr pvt_ptr = flowgraph_->get_pvt(); + + 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) + { + eph_t rtklib_eph = eph_to_rtklib(it->second); + double r_sat[3]; + double clock_bias_s; + double sat_pos_variance_m2; + eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s, + &sat_pos_variance_m2); + double Az, El, dist_m; + 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) + { + available_satellites.push_back(std::pair(floor(El), + (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); + } + } + + std::map gal_eph_map = pvt_ptr->get_galileo_ephemeris(); + for (std::map::iterator it = gal_eph_map.begin(); it != gal_eph_map.end(); ++it) + { + eph_t rtklib_eph = eph_to_rtklib(it->second); + double r_sat[3]; + double clock_bias_s; + double sat_pos_variance_m2; + eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s, + &sat_pos_variance_m2); + double Az, El, dist_m; + 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) + { + available_satellites.push_back(std::pair(floor(El), + (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN)))); + } + } + + std::map gps_alm_map = pvt_ptr->get_gps_almanac(); + for (std::map::iterator it = gps_alm_map.begin(); it != gps_alm_map.end(); ++it) + { + alm_t rtklib_alm = alm_to_rtklib(it->second); + double r_sat[3]; + double clock_bias_s; + double sat_pos_variance_m2; + alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s); + double Az, El, dist_m; + 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) + { + available_satellites.push_back(std::pair(floor(El), + (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); + } + } + + std::map gal_alm_map = pvt_ptr->get_galileo_almanac(); + for (std::map::iterator it = gal_alm_map.begin(); it != gal_alm_map.end(); ++it) + { + alm_t rtklib_alm = alm_to_rtklib(it->second); + double r_sat[3]; + double clock_bias_s; + double sat_pos_variance_m2; + alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s); + double Az, El, dist_m; + 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 Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; + //push sat + if (El > 0) + { + available_satellites.push_back(std::pair(floor(El), + (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN)))); + } + } + + //sort the visible satellites in ascending order of elevation + std::sort(available_satellites.begin(), available_satellites.end(), [](const std::pair &a, const std::pair &b) { // use lambda. Cleaner and easier to read + return a.first < b.first; + }); + //std::reverse(available_satellites.begin(), available_satellites.end()); + return available_satellites; +} + void ControlThread::gps_acq_assist_data_collector() { diff --git a/src/core/receiver/control_thread.h b/src/core/receiver/control_thread.h index ffc2ffa39..c62006eee 100644 --- a/src/core/receiver/control_thread.h +++ b/src/core/receiver/control_thread.h @@ -35,16 +35,17 @@ #ifndef GNSS_SDR_CONTROL_THREAD_H_ #define GNSS_SDR_CONTROL_THREAD_H_ +#include "gnss_satellite.h" #include "control_message_factory.h" #include "gnss_sdr_supl_client.h" #include "tcp_cmd_interface.h" +#include "gnss_flowgraph.h" +#include "configuration_interface.h" #include #include #include #include - -class GNSSFlowgraph; -class ConfigurationInterface; +#include /*! @@ -143,6 +144,12 @@ private: */ void gps_acq_assist_data_collector(); + /* + * Compute elevations for the specified time and position for all the available satellites in ephemeris and almanac queues + * returns a vector filled with the available satellites ordered from high elevation to low elevation angle. + */ + + std::vector> get_visible_sats(time_t rx_utc_time, arma::vec LLH); /* * Read initial GNSS assistance from SUPL server or local XML files */ diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index dbeabf004..527696d00 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1137,10 +1137,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) break; case 12: //request hotstart mode LOG(INFO) << "TC request hotstart"; - //todo: call here the function that computes the set of visible satellites and its elevation - //for the date and time specified by the hotstart command and the last available PVT - //todo: reorder the satellite queue to acquire first those visible satellites - //start again the satellite acquisitions for (unsigned int i = 0; i < channels_count_; i++) { unsigned int ch_index = (who + i + 1) % channels_count_; @@ -1169,12 +1165,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) break; case 13: //request warmstart mode LOG(INFO) << "TC request warmstart"; - //todo: delete all ephemeris and almanac information from maps (also the PVT map queue) - //todo: load the ephemeris and the almanac from XML files (receiver assistance) - //todo: call here the function that computes the set of visible satellites and its elevation - //for the date and time specified by the warmstart command and the assisted position - //todo: reorder the satellite queue to acquire first those visible satellites - //start again the satellite acquisitions for (unsigned int i = 0; i < channels_count_; i++) { @@ -1209,6 +1199,60 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } +void GNSSFlowgraph::priorize_satellites(std::vector> visible_satellites) +{ + size_t old_size; + Gnss_Signal gs; + for (std::vector>::iterator it = visible_satellites.begin(); it != visible_satellites.end(); ++it) + { + if (it->second.get_system() == "GPS") + { + gs = Gnss_Signal(it->second, "1C"); + old_size = available_GPS_1C_signals_.size(); + available_GPS_1C_signals_.remove(gs); + if (old_size > available_GPS_1C_signals_.size()) + { + available_GPS_1C_signals_.push_front(gs); + } + + gs = Gnss_Signal(it->second, "2S"); + old_size = available_GPS_2S_signals_.size(); + available_GPS_2S_signals_.remove(gs); + if (old_size > available_GPS_2S_signals_.size()) + { + available_GPS_2S_signals_.push_front(gs); + } + + gs = Gnss_Signal(it->second, "L5"); + old_size = available_GPS_L5_signals_.size(); + available_GPS_L5_signals_.remove(gs); + if (old_size > available_GPS_L5_signals_.size()) + { + available_GPS_L5_signals_.push_front(gs); + } + } + else if (it->second.get_system() == "Galileo") + { + gs = Gnss_Signal(it->second, "1B"); + old_size = available_GAL_1B_signals_.size(); + available_GAL_1B_signals_.remove(gs); + if (old_size > available_GAL_1B_signals_.size()) + { + available_GAL_1B_signals_.push_front(gs); + } + + gs = Gnss_Signal(it->second, "5X"); + old_size = available_GAL_5X_signals_.size(); + available_GAL_5X_signals_.remove(gs); + if (old_size > available_GAL_5X_signals_.size()) + { + available_GAL_5X_signals_.push_front(gs); + } + } + } +} + + void GNSSFlowgraph::set_configuration(std::shared_ptr configuration) { if (running_) diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index 1f1d07ada..d5e25543a 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -41,6 +41,11 @@ #include "gnss_signal.h" #include "gnss_sdr_sample_counter.h" #include "gnss_synchro_monitor.h" +#include "gnss_block_interface.h" +#include "pvt_interface.h" +#include "channel_interface.h" +#include "configuration_interface.h" +#include "gnss_block_factory.h" #include #include #include @@ -55,10 +60,6 @@ #include "gnss_sdr_fpga_sample_counter.h" #endif -class GNSSBlockInterface; -class ChannelInterface; -class ConfigurationInterface; -class GNSSBlockFactory; /*! \brief This class represents a GNSS flow graph. * @@ -129,6 +130,19 @@ public: */ bool send_telemetry_msg(pmt::pmt_t msg); + /*! + * \brief Returns a smart pointer to the PVT object + */ + std::shared_ptr get_pvt() + { + return std::dynamic_pointer_cast(pvt_); + } + + /*! + * \brief Priorize visible satellites in the specified vector + */ + void priorize_satellites(std::vector> visible_satellites); + private: void init(); // Populates the SV PRN list available for acquisition and tracking void set_signals_list(); diff --git a/src/core/receiver/tcp_cmd_interface.cc b/src/core/receiver/tcp_cmd_interface.cc index 175d96859..b857ee204 100644 --- a/src/core/receiver/tcp_cmd_interface.cc +++ b/src/core/receiver/tcp_cmd_interface.cc @@ -34,6 +34,15 @@ #include +time_t TcpCmdInterface::get_utc_time() +{ + return receiver_utc_time_; +} + +arma::vec TcpCmdInterface::get_LLH() +{ + return arma::vec{rx_latitude_, rx_longitude_, rx_altitude_}; +} std::string TcpCmdInterface::reset(const std::vector &commandLine) { std::string response; @@ -78,17 +87,40 @@ std::string TcpCmdInterface::status(const std::vector &commandLine) std::string TcpCmdInterface::hotstart(const std::vector &commandLine) { std::string response; - //todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS - //todo: store it in a member variable - std::unique_ptr cmf(new ControlMessageFactory()); - if (control_queue_ != nullptr) + if (commandLine.size() > 5) { - control_queue_->handle(cmf->GetQueueMessage(300, 12)); //send the standby message (who=300,what=12) - response = "OK\n"; + //read commandline time parameter + struct tm tm; + strptime(commandLine.at(1).c_str(), "%d/%m/%Y %H:%M:%S", &tm); + receiver_utc_time_ = timegm(&tm); + + + //read latitude, longitude, and height + rx_latitude_ = std::stod(commandLine.at(3).c_str()); + rx_longitude_ = std::stod(commandLine.at(4).c_str()); + rx_altitude_ = std::stod(commandLine.at(5).c_str()); + + if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_)) + { + response = "ERROR: position malformed\n"; + } + else + { + std::unique_ptr cmf(new ControlMessageFactory()); + if (control_queue_ != nullptr) + { + control_queue_->handle(cmf->GetQueueMessage(300, 12)); //send the standby message (who=300,what=12) + response = "OK\n"; + } + else + { + response = "ERROR\n"; + } + } } else { - response = "ERROR\n"; + response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n"; } return response; } @@ -96,17 +128,40 @@ std::string TcpCmdInterface::hotstart(const std::vector &commandLin std::string TcpCmdInterface::warmstart(const std::vector &commandLine) { std::string response; - //todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS - //todo: store it in a member variable - std::unique_ptr cmf(new ControlMessageFactory()); - if (control_queue_ != nullptr) + if (commandLine.size() > 5) { - control_queue_->handle(cmf->GetQueueMessage(300, 13)); //send the standby message (who=300,what=13) - response = "OK\n"; + std::string tmp_str; + //read commandline time parameter + struct tm tm; + tmp_str = commandLine.at(1) + commandLine.at(2); + strptime(tmp_str.c_str(), "%d/%m/%Y %H:%M:%S", &tm); + receiver_utc_time_ = timegm(&tm); + + //read latitude, longitude, and height + rx_latitude_ = std::stod(commandLine.at(3).c_str()); + rx_longitude_ = std::stod(commandLine.at(4).c_str()); + rx_altitude_ = std::stod(commandLine.at(5).c_str()); + if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_)) + { + response = "ERROR: position malformed\n"; + } + else + { + std::unique_ptr cmf(new ControlMessageFactory()); + if (control_queue_ != nullptr) + { + control_queue_->handle(cmf->GetQueueMessage(300, 13)); //send the standby message (who=300,what=13) + response = "OK\n"; + } + else + { + response = "ERROR\n"; + } + } } else { - response = "ERROR\n"; + response = "ERROR: time parameter not found, please use warmstart %d/%m/%Y %H:%M:%S Lat Long H\n"; } return response; } @@ -154,6 +209,10 @@ TcpCmdInterface::TcpCmdInterface() register_functions(); keep_running_ = true; control_queue_ = nullptr; + rx_latitude_ = 0; + rx_longitude_ = 0; + rx_altitude_ = 0; + receiver_utc_time_ = 0; } diff --git a/src/core/receiver/tcp_cmd_interface.h b/src/core/receiver/tcp_cmd_interface.h index 6071e32bd..7da156fe0 100644 --- a/src/core/receiver/tcp_cmd_interface.h +++ b/src/core/receiver/tcp_cmd_interface.h @@ -42,7 +42,8 @@ #include #include #include - +#include +#include "time.h" class TcpCmdInterface { @@ -51,6 +52,14 @@ public: virtual ~TcpCmdInterface(); void run_cmd_server(int tcp_port); void set_msg_queue(gr::msg_queue::sptr control_queue); + /*! + * \brief gets the UTC time parsed from the last TC command issued + */ + time_t get_utc_time(); + /*! + * \brief gets the Latitude, Longitude and Altitude vector from the last TC command issued + */ + arma::vec get_LLH(); private: @@ -68,6 +77,12 @@ private: gr::msg_queue::sptr control_queue_; bool keep_running_; + + time_t receiver_utc_time_; + + double rx_latitude_; + double rx_longitude_; + double rx_altitude_; }; #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */ diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 7b41ccfbd..701f8ca95 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -50,6 +50,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/system_parameters ${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/receiver + ${CMAKE_SOURCE_DIR}/src/core/monitor ${CMAKE_SOURCE_DIR}/src/core/libs ${CMAKE_SOURCE_DIR}/src/core/libs/supl ${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp diff --git a/src/tests/system-tests/libs/CMakeLists.txt b/src/tests/system-tests/libs/CMakeLists.txt index 4742e80fc..89b6970c3 100644 --- a/src/tests/system-tests/libs/CMakeLists.txt +++ b/src/tests/system-tests/libs/CMakeLists.txt @@ -18,7 +18,6 @@ set(SYSTEM_TESTING_LIB_SOURCES - geofunctions.cc spirent_motion_csv_dump_reader.cc rtklib_solver_dump_reader.cc )