From 588864e19ec26f2822defb668c8b4ea23b58fea7 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 12 Apr 2016 15:28:58 +0200 Subject: [PATCH] Replacing GPS telemetry shared queues and maps with GNURadio asynchronous messages (TLM -> PVT) --- .../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc | 93 ++-- .../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h | 3 + .../adapters/gps_l1_ca_telemetry_decoder.cc | 7 - .../gps_l1_ca_telemetry_decoder_cc.cc | 43 +- .../gps_l1_ca_telemetry_decoder_cc.h | 4 +- .../libs/gps_l1_ca_subframe_fsm.cc | 50 +- .../libs/gps_l1_ca_subframe_fsm.h | 7 +- src/core/receiver/control_thread.cc | 461 +++++++----------- src/core/receiver/control_thread.h | 22 +- src/core/receiver/gnss_flowgraph.cc | 1 + src/main/main.cc | 3 - 11 files changed, 279 insertions(+), 415 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 75d7e51b6..35930d5f2 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -42,10 +42,6 @@ using google::LogMessage; -extern concurrent_map global_gps_ephemeris_map; -extern concurrent_map global_gps_iono_map; -extern concurrent_map global_gps_utc_model_map; - extern concurrent_queue global_sbas_raw_msg_queue; extern concurrent_map global_sbas_iono_map; extern concurrent_map global_sbas_sat_corr_map; @@ -58,6 +54,44 @@ gps_l1_ca_make_pvt_cc(unsigned int nchannels, boost::shared_ptr q } +void gps_l1_ca_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) +{ + try { + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS EPHEMERIS ### + std::shared_ptr gps_eph; + gps_eph= boost::any_cast>(pmt::any_ref(msg)); + LOG(INFO) << "Ephemeris record has arrived from SAT ID " + << gps_eph->i_satellite_PRN << " (Block " + << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" + << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" + << gps_eph->i_GPS_week; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN]=*gps_eph; + }else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### IONO ### + std::shared_ptr gps_iono; + gps_iono= boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_iono=*gps_iono; + LOG(INFO) << "New IONO record has arrived "; + }else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### UTC MODEL ### + std::shared_ptr gps_utc_model; + gps_utc_model= boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_utc_model=*gps_utc_model; + LOG(INFO) << "New UTC record has arrived "; + } + + } + catch(boost::bad_any_cast& e) + { + DLOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; + } +} + gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, @@ -82,6 +116,11 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, d_dump_filename = dump_filename; std::string dump_ls_pvt_filename = dump_filename; + // GPS Ephemeris data message port in + this->message_port_register_in(pmt::mp("telemetry")); + this->set_msg_handler(pmt::mp("telemetry"), + boost::bind(&gps_l1_ca_pvt_cc::msg_handler_telemetry, this, _1)); + //initialize kml_printer std::string kml_dump_filename; kml_dump_filename = d_dump_filename; @@ -171,16 +210,12 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) { d_sample_counter++; - std::map gnss_pseudoranges_map; - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer - //Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //Get the output pointer print_receiver_status(in); - // ############ 1. READ EPHEMERIS FROM GLOBAL MAP #### - d_ls_pvt->gps_ephemeris_map = global_gps_ephemeris_map.get_map_copy(); + // ############ 1. READ EPHEMERIS #### for (unsigned int i = 0; i < d_nchannels; i++) { @@ -197,46 +232,6 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g d_rtcm_printer->lock_time(gps_ephemeris_iter->second, 0.0, in[i][0]); } } - - // ############ READ UTC_MODEL/IONO FROM GLOBAL MAPS #### - if (global_gps_utc_model_map.size() > 0) - { - // UTC MODEL data is shared for all the GPS satellites. Read always at a locked channel - signed int i = 0; - while(true) - { - if (in[i][0].Flag_valid_pseudorange == true) - { - global_gps_utc_model_map.read(i, d_ls_pvt->gps_utc_model); - break; - } - i++; - if (i == (signed int)d_nchannels - 1) - { - break; - } - } - } - - if (global_gps_iono_map.size() > 0) - { - // IONO data is shared for all the GPS satellites. Read always at a locked channel - signed int i = 0; - while(true) - { - if (in[i][0].Flag_valid_pseudorange == true) - { - global_gps_iono_map.read(i, d_ls_pvt->gps_iono); - break; - } - i++; - if (i == (signed int)d_nchannels - 1) - { - break; - } - } - } - // update SBAS data collections if (global_sbas_iono_map.size() > 0) { diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h index c2c8a7b9e..810082c3b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h @@ -96,6 +96,9 @@ private: bool flag_rtcm_server, bool flag_rtcm_tty_port, std::string rtcm_dump_devname); + + void msg_handler_telemetry(pmt::pmt_t msg); + boost::shared_ptr d_queue; bool d_dump; bool b_rinex_header_writen; diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc index 4fa5b1fbd..104460f46 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc @@ -40,10 +40,6 @@ #include "gps_utc_model.h" #include "configuration_interface.h" - -extern concurrent_queue global_gps_ephemeris_queue; -extern concurrent_queue global_gps_iono_queue; -extern concurrent_queue global_gps_utc_model_queue; extern concurrent_queue global_gps_almanac_queue; @@ -68,10 +64,7 @@ GpsL1CaTelemetryDecoder::GpsL1CaTelemetryDecoder(ConfigurationInterface* configu telemetry_decoder_ = gps_l1_ca_make_telemetry_decoder_cc(satellite_, queue_, dump_); // TODO fix me DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")"; // set the navigation msg queue; - telemetry_decoder_->set_ephemeris_queue(&global_gps_ephemeris_queue); - telemetry_decoder_->set_iono_queue(&global_gps_iono_queue); telemetry_decoder_->set_almanac_queue(&global_gps_almanac_queue); - telemetry_decoder_->set_utc_model_queue(&global_gps_utc_model_queue); //decimation factor int decimation_factor = configuration->property(role + ".decimation_factor", 1); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index f7162c58c..28caea364 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -63,6 +63,8 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( { // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); + // Ephemeris data port out + this->message_port_register_out(pmt::mp("telemetry")); // initialize internal vars d_queue = queue; d_dump = dump; @@ -199,7 +201,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ // send asynchronous message to tracking to inform of frame sync and extend correlation time pmt::pmt_t value = pmt::from_double(d_preamble_time_seconds - 0.001); this->message_port_pub(pmt::mp("preamble_timestamp_s"), value); - d_flag_frame_sync = true; if (corr_value < 0) { @@ -271,6 +272,44 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char)*4); d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0; d_GPS_FSM.Event_gps_word_valid(); + // send TLM data to PVT using asynchronous message queues + if (d_GPS_FSM.d_flag_new_subframe==true) + { + switch (d_GPS_FSM.d_subframe_ID) + { + case 3: //we have a new set of ephemeris data for the current SV + if (d_GPS_FSM.d_nav.satellite_validation() == true) + { + // get ephemeris object for this SV (mandatory) + std::shared_ptr tmp_obj= std::make_shared(); + *tmp_obj = d_GPS_FSM.d_nav.get_ephemeris(); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 4: // Possible IONOSPHERE and UTC model update (page 18) + if (d_GPS_FSM.d_nav.flag_iono_valid == true) + { + std::shared_ptr tmp_obj= std::make_shared(); + *tmp_obj = d_GPS_FSM.d_nav.get_iono(); //notice that the read operation will clear the valid flag + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + if (d_GPS_FSM.d_nav.flag_utc_model_valid == true) + { + std::shared_ptr tmp_obj= std::make_shared(); + *tmp_obj = d_GPS_FSM.d_nav.get_utc_model(); //notice that the read operation will clear the valid flag + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 5: + // get almanac (if available) + //TODO: implement almanac reader in navigation_message + break; + default: + break; + } + d_GPS_FSM.clear_flag_new_subframe(); + } + d_flag_parity = true; } else @@ -397,7 +436,7 @@ void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel) LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); } - catch (std::ifstream::failure e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index 67d270b03..1c2892df2 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -70,10 +70,7 @@ public: /*! * \brief Set the satellite data queue */ - void set_ephemeris_queue(concurrent_queue *ephemeris_queue){d_GPS_FSM.d_ephemeris_queue = ephemeris_queue;} //!< Set the ephemeris data queue - void set_iono_queue(concurrent_queue *iono_queue){d_GPS_FSM.d_iono_queue = iono_queue;} //!< Set the iono data queue void set_almanac_queue(concurrent_queue *almanac_queue){d_GPS_FSM.d_almanac_queue = almanac_queue;} //!< Set the almanac data queue - void set_utc_model_queue(concurrent_queue *utc_model_queue){d_GPS_FSM.d_utc_model_queue = utc_model_queue;} //!< Set the UTC model data queue /*! * \brief This is where all signal processing takes place @@ -142,6 +139,7 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; + }; #endif diff --git a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc index 3f662ac21..1f0e7971c 100644 --- a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc +++ b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc @@ -238,11 +238,10 @@ GpsL1CaSubframeFsm::GpsL1CaSubframeFsm() d_nav.reset(); i_channel_ID = 0; i_satellite_PRN = 0; - d_ephemeris_queue = 0; - d_iono_queue = 0; - d_utc_model_queue = 0; d_almanac_queue = 0; d_preamble_time_ms = 0; + d_subframe_ID=0; + d_flag_new_subframe=false; initiate(); //start the FSM } @@ -254,54 +253,25 @@ void GpsL1CaSubframeFsm::gps_word_to_subframe(int position) std::memcpy(&d_subframe[position*GPS_WORD_LENGTH], &d_GPS_frame_4bytes, sizeof(char)*GPS_WORD_LENGTH); } - - +void GpsL1CaSubframeFsm::clear_flag_new_subframe() +{ + d_flag_new_subframe=false; +} void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg() { - int subframe_ID; + //int subframe_ID; // NEW GPS SUBFRAME HAS ARRIVED! - subframe_ID = d_nav.subframe_decoder(this->d_subframe); //decode the subframe + d_subframe_ID = d_nav.subframe_decoder(this->d_subframe); //decode the subframe std::cout << "NAV Message: received subframe " - << subframe_ID << " from satellite " + << d_subframe_ID << " from satellite " << Gnss_Satellite(std::string("GPS"), i_satellite_PRN) << std::endl; - d_nav.i_satellite_PRN = i_satellite_PRN; d_nav.i_channel_ID = i_channel_ID; d_nav.d_subframe_timestamp_ms = this->d_preamble_time_ms; - switch (subframe_ID) - { - case 3: //we have a new set of ephemeris data for the current SV - if (d_nav.satellite_validation() == true) - { - // get ephemeris object for this SV (mandatory) - Gps_Ephemeris ephemeris = d_nav.get_ephemeris(); - d_ephemeris_queue->push(ephemeris); - } - break; - case 4: // Possible IONOSPHERE and UTC model update (page 18) - if (d_nav.flag_iono_valid == true) - { - Gps_Iono iono = d_nav.get_iono(); //notice that the read operation will clear the valid flag - d_iono_queue->push(iono); - } - if (d_nav.flag_utc_model_valid == true) - { - Gps_Utc_Model utc_model = d_nav.get_utc_model(); //notice that the read operation will clear the valid flag - d_utc_model_queue->push(utc_model); - } - break; - case 5: - // get almanac (if available) - //TODO: implement almanac reader in navigation_message - break; - default: - break; - } + d_flag_new_subframe=true; } - - void GpsL1CaSubframeFsm::Event_gps_word_valid() { this->process_event(Ev_gps_word_valid()); diff --git a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h index 1350d9873..2193ceb21 100644 --- a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h +++ b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h @@ -66,14 +66,11 @@ class GpsL1CaSubframeFsm : public sc::state_machine< GpsL1CaSubframeFsm, gps_sub { public: GpsL1CaSubframeFsm(); //!< The constructor starts the Finite State Machine - + void clear_flag_new_subframe(); // channel and satellite info int i_channel_ID; //!< Channel id unsigned int i_satellite_PRN; //!< Satellite PRN number - concurrent_queue *d_ephemeris_queue; //!< Ephemeris queue - concurrent_queue *d_iono_queue; //!< Ionospheric parameters queue - concurrent_queue *d_utc_model_queue; //!< UTC model parameters queue concurrent_queue *d_almanac_queue; //!< Almanac queue Gps_Navigation_Message d_nav; //!< GPS L1 C/A navigation message object @@ -85,6 +82,8 @@ public: Gps_Iono iono; //!< Object that handles ionospheric parameters char d_subframe[GPS_SUBFRAME_LENGTH]; + int d_subframe_ID; + bool d_flag_new_subframe; char d_GPS_frame_4bytes[GPS_WORD_LENGTH]; double d_preamble_time_ms; diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index 846dea475..9f9f33eae 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -56,19 +56,11 @@ #include "file_configuration.h" #include "control_message_factory.h" - - -extern concurrent_map global_gps_ephemeris_map; -extern concurrent_map global_gps_iono_map; -extern concurrent_map global_gps_utc_model_map; extern concurrent_map global_gps_almanac_map; extern concurrent_map global_gps_acq_assist_map; extern concurrent_map global_gps_ref_time_map; extern concurrent_map global_gps_ref_location_map; -extern concurrent_queue global_gps_ephemeris_queue; -extern concurrent_queue global_gps_iono_queue; -extern concurrent_queue global_gps_utc_model_queue; extern concurrent_queue global_gps_almanac_queue; extern concurrent_queue global_gps_acq_assist_queue; extern concurrent_queue global_gps_ref_location_queue; @@ -78,13 +70,11 @@ extern concurrent_map global_galileo_ephemeris_map; extern concurrent_map global_galileo_iono_map; extern concurrent_map global_galileo_utc_model_map; extern concurrent_map global_galileo_almanac_map; -//extern concurrent_map global_gps_acq_assist_map; extern concurrent_queue global_galileo_ephemeris_queue; extern concurrent_queue global_galileo_iono_queue; extern concurrent_queue global_galileo_utc_model_queue; extern concurrent_queue global_galileo_almanac_queue; -//extern concurrent_queue global_gps_acq_assist_queue; using google::LogMessage; @@ -112,7 +102,7 @@ ControlThread::ControlThread(std::shared_ptr configurati ControlThread::~ControlThread() { // save navigation data to files - if (save_assistance_to_XML() == true) {} + // if (save_assistance_to_XML() == true) {} } @@ -154,9 +144,7 @@ void ControlThread::run() keyboard_thread_ = boost::thread(&ControlThread::keyboard_listener, this); //start the GNSS SV data collector thread - gps_ephemeris_data_collector_thread_ = boost::thread(&ControlThread::gps_ephemeris_data_collector, this); - gps_iono_data_collector_thread_ = boost::thread(&ControlThread::gps_iono_data_collector, this); - gps_utc_model_data_collector_thread_ = boost::thread(&ControlThread::gps_utc_model_data_collector, this); + gps_acq_assist_data_collector_thread_= boost::thread(&ControlThread::gps_acq_assist_data_collector, this); gps_ref_location_data_collector_thread_ = boost::thread(&ControlThread::gps_ref_location_data_collector, this); gps_ref_time_data_collector_thread_ = boost::thread(&ControlThread::gps_ref_time_data_collector, this); @@ -176,9 +164,6 @@ void ControlThread::run() flowgraph_->stop(); stop_ = true; // sending empty data to terminate the threads - global_gps_ephemeris_queue.push(Gps_Ephemeris()); - global_gps_iono_queue.push(Gps_Iono()); - global_gps_utc_model_queue.push(Gps_Utc_Model()); global_gps_almanac_queue.push(Gps_Almanac()); global_gps_acq_assist_queue.push(Gps_Acq_Assist()); global_gps_ref_location_queue.push(Gps_Ref_Location()); @@ -189,9 +174,6 @@ void ControlThread::run() global_galileo_almanac_queue.push(Galileo_Almanac()); // Join GPS threads - gps_ephemeris_data_collector_thread_.join(); - gps_iono_data_collector_thread_.join(); - gps_utc_model_data_collector_thread_.join(); gps_acq_assist_data_collector_thread_.join(); gps_ref_location_data_collector_thread_.join(); gps_ref_time_data_collector_thread_.join(); @@ -228,167 +210,167 @@ void ControlThread::set_control_queue(boost::shared_ptr control_q /* * Returns true if reading was successful */ -bool ControlThread::read_assistance_from_XML() -{ - // return variable (true == succeeded) - bool ret = false; - // getting names from the config file, if available - std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); - std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); - std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); - std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); - std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); - - std::cout << "SUPL: Try read GPS ephemeris from XML file " << eph_xml_filename << std::endl; - if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true) - { - std::map::iterator gps_eph_iter; - for(gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.begin(); - gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.end(); - gps_eph_iter++) - { - std::cout << "SUPL: Read XML Ephemeris for GPS SV " << gps_eph_iter->first << std::endl; - global_gps_ephemeris_queue.push(gps_eph_iter->second); - } - ret = true; - } - else - { - std::cout << "ERROR: SUPL client error reading XML" << std::endl; - std::cout << "Disabling SUPL assistance..." << std::endl; - } - // Only look for {utc, iono, ref time, ref location} if SUPL is enabled - bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false); - if (enable_gps_supl_assistance == true) - { - // Try to read UTC model from XML - if (supl_client_acquisition_.load_utc_xml(utc_xml_filename) == true) - { - LOG(INFO) << "SUPL: Read XML UTC model"; - global_gps_utc_model_queue.push(supl_client_acquisition_.gps_utc); - } - else - { - LOG(INFO) << "SUPL: couldn't read UTC model XML"; - } - - // Try to read Iono model from XML - if (supl_client_acquisition_.load_iono_xml(iono_xml_filename) == true) - { - LOG(INFO) << "SUPL: Read XML IONO model"; - global_gps_iono_queue.push(supl_client_acquisition_.gps_iono); - } - else - { - LOG(INFO) << "SUPL: couldn't read IONO model XML"; - } - - // Try to read Ref Time from XML - if (supl_client_acquisition_.load_ref_time_xml(ref_time_xml_filename) == true) - { - LOG(INFO) << "SUPL: Read XML Ref Time"; - global_gps_ref_time_queue.push(supl_client_acquisition_.gps_time); - } - else - { - LOG(INFO) << "SUPL: couldn't read Ref Time XML"; - } - - // Try to read Ref Location from XML - if (supl_client_acquisition_.load_ref_location_xml(ref_location_xml_filename) == true) - { - LOG(INFO) << "SUPL: Read XML Ref Location"; - global_gps_ref_location_queue.push(supl_client_acquisition_.gps_ref_loc); - } - else - { - LOG(INFO) << "SUPL: couldn't read Ref Location XML"; - } - } - - return ret; -} +//bool ControlThread::read_assistance_from_XML() +//{ +// // return variable (true == succeeded) +// bool ret = false; +// // getting names from the config file, if available +// std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); +// std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); +// std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); +// std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); +// std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); +// +// std::cout << "SUPL: Try read GPS ephemeris from XML file " << eph_xml_filename << std::endl; +// if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true) +// { +// std::map::iterator gps_eph_iter; +// for(gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.begin(); +// gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.end(); +// gps_eph_iter++) +// { +// std::cout << "SUPL: Read XML Ephemeris for GPS SV " << gps_eph_iter->first << std::endl; +// global_gps_ephemeris_queue.push(gps_eph_iter->second); +// } +// ret = true; +// } +// else +// { +// std::cout << "ERROR: SUPL client error reading XML" << std::endl; +// std::cout << "Disabling SUPL assistance..." << std::endl; +// } +// // Only look for {utc, iono, ref time, ref location} if SUPL is enabled +// bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false); +// if (enable_gps_supl_assistance == true) +// { +// // Try to read UTC model from XML +// if (supl_client_acquisition_.load_utc_xml(utc_xml_filename) == true) +// { +// LOG(INFO) << "SUPL: Read XML UTC model"; +// global_gps_utc_model_queue.push(supl_client_acquisition_.gps_utc); +// } +// else +// { +// LOG(INFO) << "SUPL: couldn't read UTC model XML"; +// } +// +// // Try to read Iono model from XML +// if (supl_client_acquisition_.load_iono_xml(iono_xml_filename) == true) +// { +// LOG(INFO) << "SUPL: Read XML IONO model"; +// global_gps_iono_queue.push(supl_client_acquisition_.gps_iono); +// } +// else +// { +// LOG(INFO) << "SUPL: couldn't read IONO model XML"; +// } +// +// // Try to read Ref Time from XML +// if (supl_client_acquisition_.load_ref_time_xml(ref_time_xml_filename) == true) +// { +// LOG(INFO) << "SUPL: Read XML Ref Time"; +// global_gps_ref_time_queue.push(supl_client_acquisition_.gps_time); +// } +// else +// { +// LOG(INFO) << "SUPL: couldn't read Ref Time XML"; +// } +// +// // Try to read Ref Location from XML +// if (supl_client_acquisition_.load_ref_location_xml(ref_location_xml_filename) == true) +// { +// LOG(INFO) << "SUPL: Read XML Ref Location"; +// global_gps_ref_location_queue.push(supl_client_acquisition_.gps_ref_loc); +// } +// else +// { +// LOG(INFO) << "SUPL: couldn't read Ref Location XML"; +// } +// } +// +// return ret; +//} // Returns true if reading was successful -bool ControlThread::save_assistance_to_XML() -{ - // return variable (true == succeeded) - bool ret = false; - // getting names from the config file, if available - std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); - std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); - std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); - std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); - std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); - - LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename; - std::map eph_copy = global_gps_ephemeris_map.get_map_copy(); - if (supl_client_ephemeris_.save_ephemeris_map_xml(eph_xml_filename, eph_copy) == true) - { - LOG(INFO) << "SUPL: Successfully saved ephemeris XML file"; - ret = true; - } - else - { - LOG(INFO) << "SUPL: Error while trying to save ephemeris XML file. Maybe an empty map?"; - ret = false; - } - // Only try to save {utc, iono, ref time, ref location} if SUPL is enabled - bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false); - if (enable_gps_supl_assistance == true) - { - // try to save utc model xml file - std::map utc_copy = global_gps_utc_model_map.get_map_copy(); - if (supl_client_acquisition_.save_utc_map_xml(utc_xml_filename, utc_copy) == true) - { - LOG(INFO) << "SUPL: Successfully saved UTC Model XML file"; - //ret = true; - } - else - { - LOG(INFO) << "SUPL: Error while trying to save utc XML file"; - //ret = false; - } - // try to save iono model xml file - std::map iono_copy = global_gps_iono_map.get_map_copy(); - if (supl_client_acquisition_.save_iono_map_xml(iono_xml_filename, iono_copy) == true) - { - LOG(INFO) << "SUPL: Successfully saved IONO Model XML file"; - //ret = true; - } - else - { - LOG(INFO) << "SUPL: Error while trying to save iono XML file"; - //ret = false; - } - // try to save ref time xml file - std::map ref_time_copy = global_gps_ref_time_map.get_map_copy(); - if (supl_client_acquisition_.save_ref_time_map_xml(ref_time_xml_filename, ref_time_copy) == true) - { - LOG(INFO) << "SUPL: Successfully saved Ref Time XML file"; - //ret = true; - } - else - { - LOG(INFO) << "SUPL: Error while trying to save ref time XML file"; - //ref = false; - } - // try to save ref location xml file - std::map ref_location_copy = global_gps_ref_location_map.get_map_copy(); - if (supl_client_acquisition_.save_ref_location_map_xml(ref_location_xml_filename, ref_location_copy) == true) - { - LOG(INFO) << "SUPL: Successfully saved Ref Location XML file"; - //ref = true; - } - else - { - LOG(INFO) << "SUPL: Error while trying to save ref location XML file"; - //ret = false; - } - } - return ret; -} +//bool ControlThread::save_assistance_to_XML() +//{ +// // return variable (true == succeeded) +// bool ret = false; +// // getting names from the config file, if available +// std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); +// std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); +// std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); +// std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); +// std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); +// +// LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename; +// std::map eph_copy = global_gps_ephemeris_map.get_map_copy(); +// if (supl_client_ephemeris_.save_ephemeris_map_xml(eph_xml_filename, eph_copy) == true) +// { +// LOG(INFO) << "SUPL: Successfully saved ephemeris XML file"; +// ret = true; +// } +// else +// { +// LOG(INFO) << "SUPL: Error while trying to save ephemeris XML file. Maybe an empty map?"; +// ret = false; +// } +// // Only try to save {utc, iono, ref time, ref location} if SUPL is enabled +// bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false); +// if (enable_gps_supl_assistance == true) +// { +// // try to save utc model xml file +// std::map utc_copy = global_gps_utc_model_map.get_map_copy(); +// if (supl_client_acquisition_.save_utc_map_xml(utc_xml_filename, utc_copy) == true) +// { +// LOG(INFO) << "SUPL: Successfully saved UTC Model XML file"; +// //ret = true; +// } +// else +// { +// LOG(INFO) << "SUPL: Error while trying to save utc XML file"; +// //ret = false; +// } +// // try to save iono model xml file +// std::map iono_copy = global_gps_iono_map.get_map_copy(); +// if (supl_client_acquisition_.save_iono_map_xml(iono_xml_filename, iono_copy) == true) +// { +// LOG(INFO) << "SUPL: Successfully saved IONO Model XML file"; +// //ret = true; +// } +// else +// { +// LOG(INFO) << "SUPL: Error while trying to save iono XML file"; +// //ret = false; +// } +// // try to save ref time xml file +// std::map ref_time_copy = global_gps_ref_time_map.get_map_copy(); +// if (supl_client_acquisition_.save_ref_time_map_xml(ref_time_xml_filename, ref_time_copy) == true) +// { +// LOG(INFO) << "SUPL: Successfully saved Ref Time XML file"; +// //ret = true; +// } +// else +// { +// LOG(INFO) << "SUPL: Error while trying to save ref time XML file"; +// //ref = false; +// } +// // try to save ref location xml file +// std::map ref_location_copy = global_gps_ref_location_map.get_map_copy(); +// if (supl_client_acquisition_.save_ref_location_map_xml(ref_location_xml_filename, ref_location_copy) == true) +// { +// LOG(INFO) << "SUPL: Successfully saved Ref Location XML file"; +// //ref = true; +// } +// else +// { +// LOG(INFO) << "SUPL: Error while trying to save ref location XML file"; +// //ret = false; +// } +// } +// return ret; +//} void ControlThread::init() @@ -441,7 +423,7 @@ void ControlThread::init() if (SUPL_read_gps_assistance_xml == true) { // read assistance from file - if (read_assistance_from_XML()) {} + //if (read_assistance_from_XML()) {} } else { @@ -458,7 +440,10 @@ void ControlThread::init() gps_eph_iter++) { std::cout << "SUPL: Received Ephemeris for GPS SV " << gps_eph_iter->first << std::endl; - global_gps_ephemeris_map.write(gps_eph_iter->second.i_satellite_PRN, gps_eph_iter->second); + //TODO: Now the ephemeris are transported from telemetry decoder to PVT using msg queues. + // in order to assist the receiver, it is needed to produce a GNURadio message from this thread to the PVT message queue + // global queues or maps are deprecated in this version + //global_gps_ephemeris_map.write(gps_eph_iter->second.i_satellite_PRN, gps_eph_iter->second); } //Save ephemeris to XML file std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); @@ -475,11 +460,11 @@ void ControlThread::init() { std::cout << "ERROR: SUPL client for Ephemeris returned " << error << std::endl; std::cout << "Please check internet connection and SUPL server configuration" << error << std::endl; - std::cout << "Trying to read ephemeris from XML file" << std::endl; - if (read_assistance_from_XML() == false) - { - std::cout << "ERROR: Could not read Ephemeris file: Disabling SUPL assistance.." << std::endl; - } + //std::cout << "Trying to read ephemeris from XML file" << std::endl; + //if (read_assistance_from_XML() == false) + // { + // std::cout << "ERROR: Could not read Ephemeris file: Disabling SUPL assistance.." << std::endl; + // } } // Request almanac , IONO and UTC Model @@ -499,12 +484,14 @@ void ControlThread::init() if (supl_client_ephemeris_.gps_iono.valid == true) { std::cout << "SUPL: Received GPS Iono" << std::endl; - global_gps_iono_map.write(0, supl_client_ephemeris_.gps_iono); + //TODO: use msg queues + //global_gps_iono_map.write(0, supl_client_ephemeris_.gps_iono); } if (supl_client_ephemeris_.gps_utc.valid == true) { std::cout << "SUPL: Received GPS UTC Model" << std::endl; - global_gps_utc_model_map.write(0, supl_client_ephemeris_.gps_utc); + //TODO: use msg queues + //global_gps_utc_model_map.write(0, supl_client_ephemeris_.gps_utc); } } else @@ -635,55 +622,6 @@ void ControlThread::gps_acq_assist_data_collector() } } - -void ControlThread::gps_ephemeris_data_collector() -{ - // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE #################### - Gps_Ephemeris gps_eph; - Gps_Ephemeris gps_eph_old; - while(stop_ == false) - { - global_gps_ephemeris_queue.wait_and_pop(gps_eph); - if(gps_eph.i_satellite_PRN == 0) break; - - // DEBUG MESSAGE - LOG(INFO) << "Ephemeris record has arrived from SAT ID " - << gps_eph.i_satellite_PRN << " (Block " - << gps_eph.satelliteBlock[gps_eph.i_satellite_PRN] << ")"; - // insert new ephemeris record to the global ephemeris map - if (global_gps_ephemeris_map.read(gps_eph.i_satellite_PRN, gps_eph_old)) - { - // Check the EPHEMERIS timestamp. If it is newer, then update the ephemeris - if (gps_eph.i_GPS_week > gps_eph_old.i_GPS_week) - { - std::cout << "Ephemeris record updated (GPS week=" << gps_eph.i_GPS_week << std::endl; - global_gps_ephemeris_map.write(gps_eph.i_satellite_PRN, gps_eph); - } - else - { - if (gps_eph.d_Toe > gps_eph_old.d_Toe) - { - LOG(INFO) << "Ephemeris record updated (Toe=" << gps_eph.d_Toe; - global_gps_ephemeris_map.write(gps_eph.i_satellite_PRN, gps_eph); - } - else - { - LOG(INFO) << "Not updating the existing ephemeris"; - } - } - } - else - { - // insert new ephemeris record - LOG(INFO) << "New Ephemeris record inserted with Toe=" - << gps_eph.d_Toe<<" and GPS Week=" - << gps_eph.i_GPS_week; - global_gps_ephemeris_map.write(gps_eph.i_satellite_PRN, gps_eph); - } - } -} - - void ControlThread::galileo_ephemeris_data_collector() { // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE #################### @@ -737,23 +675,6 @@ void ControlThread::galileo_ephemeris_data_collector() } -void ControlThread::gps_iono_data_collector() -{ - // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE #################### - Gps_Iono gps_iono; - while(stop_ == false) - { - global_gps_iono_queue.wait_and_pop(gps_iono); - if(gps_iono.valid == true) - { - LOG(INFO) << "New IONO record has arrived "; - } - // there is no timestamp for the iono data, new entries must always be added - global_gps_iono_map.write(0, gps_iono); - } -} - - void ControlThread::galileo_almanac_data_collector() { // ############ 1.bis READ ALMANAC QUEUE #################### @@ -817,40 +738,6 @@ void ControlThread::galileo_iono_data_collector() } -void ControlThread::gps_utc_model_data_collector() -{ - // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE #################### - Gps_Utc_Model gps_utc; - Gps_Utc_Model gps_utc_old; - while(stop_ == false) - { - global_gps_utc_model_queue.wait_and_pop(gps_utc); - LOG(INFO) << "New UTC MODEL record has arrived with A0=" << gps_utc.d_A0; - // insert new utc record to the global utc model map - if (global_gps_utc_model_map.read(0, gps_utc_old)) - { - if (gps_utc.i_WN_T > gps_utc_old.i_WN_T) - { - global_gps_utc_model_map.write(0, gps_utc); - } - else if ((gps_utc.i_WN_T == gps_utc_old.i_WN_T) and (gps_utc.d_t_OT > gps_utc_old.d_t_OT)) - { - global_gps_utc_model_map.write(0, gps_utc); - } - else - { - LOG(INFO) << "Not updating the existing utc model"; - } - } - else - { - // insert new utc model record - global_gps_utc_model_map.write(0, gps_utc); - } - } -} - - void ControlThread::gps_ref_location_data_collector() { // ############ READ REF LOCATION #################### diff --git a/src/core/receiver/control_thread.h b/src/core/receiver/control_thread.h index 43fd4b6fa..0f45300c8 100644 --- a/src/core/receiver/control_thread.h +++ b/src/core/receiver/control_thread.h @@ -124,25 +124,15 @@ private: void init(); // Read {ephemeris, iono, utc, ref loc, ref time} assistance from a local XML file previously recorded - bool read_assistance_from_XML(); + //bool read_assistance_from_XML(); // Save {ephemeris, iono, utc, ref loc, ref time} assistance to a local XML file - bool save_assistance_to_XML(); + //bool save_assistance_to_XML(); void read_control_messages(); void process_control_messages(); - /* - * Blocking function that reads the GPS ephemeris queue and updates the shared ephemeris map, accessible from the PVT block - */ - void gps_ephemeris_data_collector(); - - /* - * Blocking function that reads the UTC model queue and updates the shared map, accessible from the PVT block - */ - void gps_utc_model_data_collector(); - /* * \brief Blocking function that reads the ref location queue and updates the shared map */ @@ -153,11 +143,6 @@ private: */ void gps_ref_time_data_collector(); - /* - * Blocking function that reads the iono model queue and updates the shared map, accessible from the PVT block - */ - void gps_iono_data_collector(); - /* * Blocking function that reads the GPS assistance queue */ @@ -195,9 +180,6 @@ private: unsigned int applied_actions_; boost::thread keyboard_thread_; - boost::thread gps_ephemeris_data_collector_thread_; - boost::thread gps_iono_data_collector_thread_; - boost::thread gps_utc_model_data_collector_thread_; boost::thread gps_acq_assist_data_collector_thread_; boost::thread gps_ref_location_data_collector_thread_; boost::thread gps_ref_time_data_collector_thread_; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index b34277193..0879e219e 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -325,6 +325,7 @@ void GNSSFlowgraph::connect() for (unsigned int i = 0; i < channels_count_; i++) { top_block_->connect(observables_->get_right_block(), i, pvt_->get_left_block(), i); + top_block_->msg_connect(channels_.at(i)->get_right_block(),pmt::mp("telemetry"),pvt_->get_left_block(),pmt::mp("telemetry")); } } catch (std::exception& e) diff --git a/src/main/main.cc b/src/main/main.cc index 1053c8813..9fbd1bf36 100644 --- a/src/main/main.cc +++ b/src/main/main.cc @@ -88,9 +88,6 @@ DECLARE_string(log_dir); */ // For GPS NAVIGATION (L1) -concurrent_queue global_gps_ephemeris_queue; -concurrent_queue global_gps_iono_queue; -concurrent_queue global_gps_utc_model_queue; concurrent_queue global_gps_almanac_queue; concurrent_queue global_gps_acq_assist_queue; concurrent_queue global_gps_ref_location_queue;