mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-02-19 04:20:08 +00:00
Replacing GPS telemetry shared queues and maps with GNURadio
asynchronous messages (TLM -> PVT)
This commit is contained in:
Javier Arribas
parent
414eaddb42
commit
588864e19e
@ -42,10 +42,6 @@
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
extern concurrent_map<Gps_Ephemeris> global_gps_ephemeris_map;
|
||||
extern concurrent_map<Gps_Iono> global_gps_iono_map;
|
||||
extern concurrent_map<Gps_Utc_Model> global_gps_utc_model_map;
|
||||
|
||||
extern concurrent_queue<Sbas_Raw_Msg> global_sbas_raw_msg_queue;
|
||||
extern concurrent_map<Sbas_Ionosphere_Correction> global_sbas_iono_map;
|
||||
extern concurrent_map<Sbas_Satellite_Correction> global_sbas_sat_corr_map;
|
||||
@ -58,6 +54,44 @@ gps_l1_ca_make_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> 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>) )
|
||||
{
|
||||
// ### GPS EPHEMERIS ###
|
||||
std::shared_ptr<Gps_Ephemeris> gps_eph;
|
||||
gps_eph= boost::any_cast<std::shared_ptr<Gps_Ephemeris>>(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<Gps_Iono>) )
|
||||
{
|
||||
// ### IONO ###
|
||||
std::shared_ptr<Gps_Iono> gps_iono;
|
||||
gps_iono= boost::any_cast<std::shared_ptr<Gps_Iono>>(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<Gps_Utc_Model>) )
|
||||
{
|
||||
// ### UTC MODEL ###
|
||||
std::shared_ptr<Gps_Utc_Model> gps_utc_model;
|
||||
gps_utc_model= boost::any_cast<std::shared_ptr<Gps_Utc_Model>>(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<gr::msg_queue> 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<int,Gnss_Synchro> 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)
|
||||
{
|
||||
|
||||
@ -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<gr::msg_queue> d_queue;
|
||||
bool d_dump;
|
||||
bool b_rinex_header_writen;
|
||||
|
||||
@ -40,10 +40,6 @@
|
||||
#include "gps_utc_model.h"
|
||||
#include "configuration_interface.h"
|
||||
|
||||
|
||||
extern concurrent_queue<Gps_Ephemeris> global_gps_ephemeris_queue;
|
||||
extern concurrent_queue<Gps_Iono> global_gps_iono_queue;
|
||||
extern concurrent_queue<Gps_Utc_Model> global_gps_utc_model_queue;
|
||||
extern concurrent_queue<Gps_Almanac> 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);
|
||||
|
||||
@ -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<Gps_Ephemeris> tmp_obj= std::make_shared<Gps_Ephemeris>();
|
||||
*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<Gps_Iono> tmp_obj= std::make_shared<Gps_Iono>();
|
||||
*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<Gps_Utc_Model> tmp_obj= std::make_shared<Gps_Utc_Model>();
|
||||
*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();
|
||||
}
|
||||
|
||||
@ -70,10 +70,7 @@ public:
|
||||
/*!
|
||||
* \brief Set the satellite data queue
|
||||
*/
|
||||
void set_ephemeris_queue(concurrent_queue<Gps_Ephemeris> *ephemeris_queue){d_GPS_FSM.d_ephemeris_queue = ephemeris_queue;} //!< Set the ephemeris data queue
|
||||
void set_iono_queue(concurrent_queue<Gps_Iono> *iono_queue){d_GPS_FSM.d_iono_queue = iono_queue;} //!< Set the iono data queue
|
||||
void set_almanac_queue(concurrent_queue<Gps_Almanac> *almanac_queue){d_GPS_FSM.d_almanac_queue = almanac_queue;} //!< Set the almanac data queue
|
||||
void set_utc_model_queue(concurrent_queue<Gps_Utc_Model> *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
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -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<Gps_Ephemeris> *d_ephemeris_queue; //!< Ephemeris queue
|
||||
concurrent_queue<Gps_Iono> *d_iono_queue; //!< Ionospheric parameters queue
|
||||
concurrent_queue<Gps_Utc_Model> *d_utc_model_queue; //!< UTC model parameters queue
|
||||
concurrent_queue<Gps_Almanac> *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;
|
||||
|
||||
|
||||
@ -56,19 +56,11 @@
|
||||
#include "file_configuration.h"
|
||||
#include "control_message_factory.h"
|
||||
|
||||
|
||||
|
||||
extern concurrent_map<Gps_Ephemeris> global_gps_ephemeris_map;
|
||||
extern concurrent_map<Gps_Iono> global_gps_iono_map;
|
||||
extern concurrent_map<Gps_Utc_Model> global_gps_utc_model_map;
|
||||
extern concurrent_map<Gps_Almanac> global_gps_almanac_map;
|
||||
extern concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
|
||||
extern concurrent_map<Gps_Ref_Time> global_gps_ref_time_map;
|
||||
extern concurrent_map<Gps_Ref_Location> global_gps_ref_location_map;
|
||||
|
||||
extern concurrent_queue<Gps_Ephemeris> global_gps_ephemeris_queue;
|
||||
extern concurrent_queue<Gps_Iono> global_gps_iono_queue;
|
||||
extern concurrent_queue<Gps_Utc_Model> global_gps_utc_model_queue;
|
||||
extern concurrent_queue<Gps_Almanac> global_gps_almanac_queue;
|
||||
extern concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
|
||||
extern concurrent_queue<Gps_Ref_Location> global_gps_ref_location_queue;
|
||||
@ -78,13 +70,11 @@ extern concurrent_map<Galileo_Ephemeris> global_galileo_ephemeris_map;
|
||||
extern concurrent_map<Galileo_Iono> global_galileo_iono_map;
|
||||
extern concurrent_map<Galileo_Utc_Model> global_galileo_utc_model_map;
|
||||
extern concurrent_map<Galileo_Almanac> global_galileo_almanac_map;
|
||||
//extern concurrent_map<Galileo_Acq_Assist> global_gps_acq_assist_map;
|
||||
|
||||
extern concurrent_queue<Galileo_Ephemeris> global_galileo_ephemeris_queue;
|
||||
extern concurrent_queue<Galileo_Iono> global_galileo_iono_queue;
|
||||
extern concurrent_queue<Galileo_Utc_Model> global_galileo_utc_model_queue;
|
||||
extern concurrent_queue<Galileo_Almanac> global_galileo_almanac_queue;
|
||||
//extern concurrent_queue<Galileo_Acq_Assist> global_gps_acq_assist_queue;
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
@ -112,7 +102,7 @@ ControlThread::ControlThread(std::shared_ptr<ConfigurationInterface> 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<gr::msg_queue> 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<int,Gps_Ephemeris>::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<int,Gps_Ephemeris>::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<int, Gps_Ephemeris> 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<int, Gps_Utc_Model> 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<int, Gps_Iono> 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<int, Gps_Ref_Time> 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<int, Gps_Ref_Location> 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<int, Gps_Ephemeris> 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<int, Gps_Utc_Model> 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<int, Gps_Iono> 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<int, Gps_Ref_Time> 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<int, Gps_Ref_Location> 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 ####################
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -88,9 +88,6 @@ DECLARE_string(log_dir);
|
||||
*/
|
||||
|
||||
// For GPS NAVIGATION (L1)
|
||||
concurrent_queue<Gps_Ephemeris> global_gps_ephemeris_queue;
|
||||
concurrent_queue<Gps_Iono> global_gps_iono_queue;
|
||||
concurrent_queue<Gps_Utc_Model> global_gps_utc_model_queue;
|
||||
concurrent_queue<Gps_Almanac> global_gps_almanac_queue;
|
||||
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
|
||||
concurrent_queue<Gps_Ref_Location> global_gps_ref_location_queue;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user