mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-17 07:37:16 +00:00
Code Issues Releases Wiki Activity

Replacing GPS telemetry shared queues and maps with GNURadio

Browse Source 
asynchronous messages (TLM -> PVT)
This commit is contained in:
Javier Arribas
2016-04-12 15:28:58 +02:00
parent 414eaddb42
commit 588864e19e
11 changed files with 279 additions and 415 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
View File

@@ -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)
{

3
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h
View File

@@ -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;

7
src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc
View File

@@ -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);

43
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@@ -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();
}

4
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@@ -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

50
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc
View File

@@ -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());

7
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h
View File

@@ -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;