mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Major changes: 

- Gps telemetry decoder and PVT now uses independent queues for ephemeris, iono, utc_model and almanac. The old gps_navigation_queue is now deprecated and it is removed from the implementation. This affect almost all the PVT and Rinex printer classes.
- Rinex printer class updated to use the new ephemeris, iono, utc_model objects
Bug fixes and correction:
- The pseudorange generation now is based on the computed TOW for each received symbol. The History_Shift computation become simplier in this way. This modification is also a solution for a bug where the pseudoranges became temporally invalid when the TOW is updated (that happen every 6 secons!) 

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@343 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas 2013-03-14 12:52:32 +00:00
parent 175a143c61
commit 8b2d21c1c9
27 changed files with 840 additions and 500 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
conf/gnss-sdr_optim_tracking.conf
View File

@ -17,7 +17,7 @@ ControlThread.wait_for_flowgraph=false
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=/media/DATALOGGER_/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
SignalSource.filename=/media/DATALOGGER/spirent scenario 1/data/sc1_d16.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=gr_complex

22
conf/master.conf
View File

@ -409,23 +409,35 @@ Observables.dump_filename=./observables.dat
PVT.implementation=GPS_L1_CA_PVT
;#averaging_depth: Number of PVT observations in the moving average algorithm
PVT.averaging_depth=100
PVT.averaging_depth=10
;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false]
PVT.flag_averaging=true
;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms]
PVT.output_rate_ms=100;
PVT.output_rate_ms=100
;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms.
PVT.display_rate_ms=500;
PVT.display_rate_ms=500
;#dump: Enable or disable the PVT internal binary data file logging [true] or [false]
PVT.dump=false
;# RINEX, KML, and NMEA output configuration
;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump.
PVT.dump_filename=./PVT
;#nmea_dump_filename: NMEA log path and filename
PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea;
;#flag_nmea_tty_port: Enable or disable the NMEA log to a serial TTY port (Can be used with real hardware or virtual one)
PVT.flag_nmea_tty_port=true;
;#nmea_dump_devname: serial device descriptor for NMEA logging
PVT.nmea_dump_devname=/dev/pts/4
;#dump: Enable or disable the PVT internal binary data file logging [true] or [false]
PVT.dump=false
;######### OUTPUT_FILTER CONFIG ############
;# Receiver output filter: Leave this block disabled in this version
OutputFilter.implementation=Null_Sink_Output_Filter

10
src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc
View File

@ -38,7 +38,10 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
extern concurrent_queue<Gps_Navigation_Message> global_gps_nav_msg_queue;
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;
using google::LogMessage;
@ -81,7 +84,10 @@ GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration,
pvt_ = gps_l1_ca_make_pvt_cc(in_streams_, queue_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms,flag_nmea_tty_port,nmea_dump_filename,nmea_dump_devname);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
// set the navigation msg queue;
pvt_->set_navigation_queue(&global_gps_nav_msg_queue);
pvt_->set_ephemeris_queue(&global_gps_ephemeris_queue);
pvt_->set_iono_queue(&global_gps_iono_queue);
//pvt_->set_almanac_queue(&global_gps_almanac_queue);
pvt_->set_utc_model_queue(&global_gps_utc_model_queue);
DLOG(INFO) << "global navigation message queue assigned to pvt (" << pvt_->unique_id() << ")";
}

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

@ -82,7 +82,6 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
d_ls_pvt = new gps_l1_ca_ls_pvt(nchannels,dump_ls_pvt_filename,d_dump);
d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_ephemeris_clock_s = 0.0;
d_sample_counter = 0;
d_last_sample_nav_output=0;
@ -91,11 +90,6 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
b_rinex_header_writen = false;
rp = new Rinex_Printer();
for (unsigned int i=0; i<nchannels; i++)
{
nav_data_map[i] = Gps_Navigation_Message();
}
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
@ -166,107 +160,120 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
// find the minimum index (nearest satellite, will be the reference)
gnss_pseudoranges_iter = std::min_element(gnss_pseudoranges_map.begin(), gnss_pseudoranges_map.end(), pseudoranges_pairCompare_min);
Gps_Navigation_Message nav_msg;
while (d_nav_queue->try_pop(nav_msg) == true)
{
std::cout << "New ephemeris record has arrived from SAT ID "
<< nav_msg.i_satellite_PRN << " (Block "
<< nav_msg.satelliteBlock[nav_msg.i_satellite_PRN]
<< ")" << std::endl;
d_last_nav_msg = nav_msg;
if (nav_msg.b_valid_ephemeris_set_flag == true)
{
d_ls_pvt->d_ephemeris[nav_msg.i_channel_ID] = nav_msg;
nav_data_map[nav_msg.i_channel_ID] = nav_msg;
}
// **** update pseudoranges clock ****
if (nav_msg.i_satellite_PRN == gnss_pseudoranges_iter->second.PRN)
{
d_ephemeris_clock_s = d_last_nav_msg.d_TOW;
d_ephemeris_timestamp_ms = d_last_nav_msg.d_subframe_timestamp_ms;
}
}
// ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################
Gps_Ephemeris gps_eph;
while (d_gps_eph_queue->try_pop(gps_eph) == true)
{
// DEBUG MESSAGE
std::cout << "New ephemeris record has arrived from SAT ID "
<< gps_eph.i_satellite_PRN << " (Block "
<< gps_eph.satelliteBlock[gps_eph.i_satellite_PRN]
<< ")" << std::endl;
d_ls_pvt->gps_ephemeris_map[gps_eph.i_satellite_PRN] = gps_eph;
std::cout<<"SV "<<gps_eph.i_satellite_PRN<<"d_TOW="<<gps_eph.d_TOW<<std::endl;
}
Gps_Utc_Model gps_utc_model;
while (d_gps_utc_model_queue->try_pop(gps_utc_model) == true)
{
// DEBUG MESSAGE
std::cout << "New UTC model record has arrived "<< std::endl;
// ############ 2. COMPUTE THE PVT ################################
// write the pseudoranges to RINEX OBS file
// 1- need a valid clock
if (d_ephemeris_clock_s > 0 and d_last_nav_msg.i_satellite_PRN > 0 and d_last_nav_msg.b_valid_ephemeris_set_flag == true)
{
double clock_error;
double satellite_tx_time_using_timestamps;
//for GPS L1 C/A: t_tx = TOW + N_symbols_from_TOW*T_symbol
//Notice that the TOW is decoded AFTER processing the subframe -> we need to add ONE subframe duration to t_tx
d_tx_time = d_ephemeris_clock_s + gnss_pseudoranges_iter->second.Pseudorange_symbol_shift/(double)GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND + GPS_SUBFRAME_SECONDS;
//Perform an extra check to verify the TOW update (the ephemeris queue is ASYNCHRONOUS to the GNU Radio Gnss_Synchro sample stream)
//-> compute the t_tx_timestamps using the symbols timestamp (it is affected by code Doppler, but it is not wrapped like N_symbols_from_TOW)
satellite_tx_time_using_timestamps = d_ephemeris_clock_s + (gnss_pseudoranges_iter->second.Pseudorange_timestamp_ms - d_ephemeris_timestamp_ms)/1000.0;
//->compute the absolute error between both T_tx
clock_error = std::abs(d_tx_time - satellite_tx_time_using_timestamps);
// -> The symbol counter N_symbols_from_TOW will be reset every new received telemetry word, if the TOW is not updated, both t_tx and t_tx_timestamps times will difer by more than 1 seconds.
if (clock_error < 1)
{
// compute on the fly PVT solution
//mod 8/4/2012 Set the PVT computation rate in this block
if ((d_sample_counter % d_output_rate_ms) == 0)
{
if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,d_tx_time,d_flag_averaging) == true)
{
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
d_ls_pvt->gps_utc_model=gps_utc_model;
}
if (!b_rinex_header_writen) // & we have utc data in nav message!
{
rp->rinex_nav_header(rp->navFile, d_last_nav_msg);
rp->rinex_obs_header(rp->obsFile, d_last_nav_msg);
b_rinex_header_writen = true; // do not write header anymore
}
if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s)
{
// Limit the RINEX navigation output rate to 1/6 seg
// Notice that d_sample_counter period is 1ms (for GPS correlators)
Gps_Iono gps_iono;
while (d_gps_iono_queue->try_pop(gps_iono) == true)
{
// DEBUG MESSAGE
std::cout << "New IONO record has arrived "<< std::endl;
if ((d_sample_counter-d_last_sample_nav_output)>=6000)
{
rp->log_rinex_nav(rp->navFile, nav_data_map);
d_last_sample_nav_output=d_sample_counter;
}
rp->log_rinex_obs(rp->obsFile, d_last_nav_msg, d_tx_time, pseudoranges);
}
}
}
d_ls_pvt->gps_iono=gps_iono;
}
if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true)
{
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " is HDOP = " << d_ls_pvt->d_HDOP << " and VDOP = " << d_ls_pvt->d_VDOP << std::endl;
}
// ############ 2.bis COMPUTE THE PVT ################################
if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() >0)
{
// The GPS TX time is directly the Time of Week (TOW) associated to the current symbol of the reference channel
// It is used to compute the SV positions at the TX instant
d_tx_time = gnss_pseudoranges_iter->second.d_TOW_at_current_symbol;
// compute on the fly PVT solution
//mod 8/4/2012 Set the PVT computation rate in this block
if ((d_sample_counter % d_output_rate_ms) == 0)
{
bool pvt_result;
pvt_result=d_ls_pvt->get_PVT(gnss_pseudoranges_map,d_tx_time,d_flag_averaging);
if (pvt_result==true)
{
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
if (!b_rinex_header_writen) // & we have utc data in nav message!
{
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
{
rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second,d_tx_time);
rp->rinex_nav_header(rp->navFile,d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model);
b_rinex_header_writen = true; // do not write header anymore
}
}
if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s)
{
// Limit the RINEX navigation output rate to 1/6 seg
// Notice that d_sample_counter period is 1ms (for GPS correlators)
if ((d_sample_counter-d_last_sample_nav_output)>=6000)
{
rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map);
d_last_sample_nav_output=d_sample_counter;
}
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
{
rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_tx_time, pseudoranges);
}
}
}
}
// DEBUG MESSAGE: Display position in console output
if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true)
{
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " is HDOP = " << d_ls_pvt->d_HDOP << " and VDOP = " << d_ls_pvt->d_VDOP << std::endl;
}
// MULTIPLEXED FILE RECORDING - Record results to file
if(d_dump == true)
{
try
{
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
tmp_double = in[i][0].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = in[i][0].Pseudorange_symbol_shift;
d_dump_file.write((char*)&tmp_double, sizeof(double));
d_dump_file.write((char*)&d_tx_time, sizeof(double));
}
}
catch (std::ifstream::failure e)
{
std::cout << "Exception writing observables dump file " << e.what() << std::endl;
}
}
}
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
tmp_double = in[i][0].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = in[i][0].Pseudorange_symbol_shift;
d_dump_file.write((char*)&tmp_double, sizeof(double));
d_dump_file.write((char*)&d_tx_time, sizeof(double));
}
}
catch (std::ifstream::failure e)
{
std::cout << "Exception writing observables dump file " << e.what() << std::endl;
}
}
}
}
consume_each(1); //one by one
return 0;
}

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

@ -38,6 +38,9 @@
#include <boost/thread/thread.hpp>
#include "concurrent_queue.h"
#include "gps_navigation_message.h"
#include "gps_ephemeris.h"
#include "gps_utc_model.h"
#include "gps_iono.h"
#include "nmea_printer.h"
#include "kml_printer.h"
#include "rinex_printer.h"
@ -75,20 +78,23 @@ private:
long unsigned int d_last_sample_nav_output;
Kml_Printer d_kml_dump;
Nmea_Printer *d_nmea_printer;
concurrent_queue<Gps_Navigation_Message> *d_nav_queue; // Navigation ephemeris queue
Gps_Navigation_Message d_last_nav_msg; // Last navigation message
double d_ephemeris_clock_s;
double d_ephemeris_timestamp_ms;
concurrent_queue<Gps_Ephemeris> *d_gps_eph_queue; // Navigation ephemeris queue
concurrent_queue<Gps_Utc_Model> *d_gps_utc_model_queue; // Navigation UTC model queue
concurrent_queue<Gps_Iono> *d_gps_iono_queue; // Navigation UTC model queue
double d_tx_time;
gps_l1_ca_ls_pvt *d_ls_pvt;
std::map<int,Gps_Navigation_Message> nav_data_map;
public:
~gps_l1_ca_pvt_cc (); //!< Default destructor
/*!
* \brief Set the queue for getting navigation messages from the GpsL1CaTelemetryDecoder
*/
void set_navigation_queue(concurrent_queue<Gps_Navigation_Message> *nav_queue){d_nav_queue=nav_queue;}
void set_ephemeris_queue(concurrent_queue<Gps_Ephemeris> *eph_queue){d_gps_eph_queue=eph_queue;}
void set_utc_model_queue(concurrent_queue<Gps_Utc_Model> *utc_model_queue){d_gps_utc_model_queue=utc_model_queue;}
void set_iono_queue(concurrent_queue<Gps_Iono> *iono_queue){d_gps_iono_queue=iono_queue;}
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing
};

353
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@ -240,197 +240,204 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging)
{
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
arma::mat W = arma::eye(d_nchannels,d_nchannels); //channels weights matrix
arma::vec obs = arma::zeros(d_nchannels); // pseudoranges observation vector
arma::mat satpos = arma::zeros(3,d_nchannels); //satellite positions matrix
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
int GPS_week = 0;
double GPS_corrected_time = 0;
double utc = 0;
int valid_pseudoranges=gnss_pseudoranges_map.size();
d_flag_averaging = flag_averaging;
arma::mat W = arma::eye(valid_pseudoranges,valid_pseudoranges); //channels weights matrix
arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
arma::mat satpos = arma::zeros(3,valid_pseudoranges); //satellite positions matrix
int valid_obs = 0; //valid observations counter
for (int i=0; i<d_nchannels; i++)
int GPS_week = 0;
double GPS_corrected_time = 0;
double utc = 0;
d_flag_averaging = flag_averaging;
// ********************************************************************************
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// ********************************************************************************
int valid_obs = 0; //valid observations counter
int obs_counter=0;
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
{
if (d_ephemeris[i].satellite_validation() == true)
{
gnss_pseudoranges_iter = gnss_pseudoranges_map.find(d_ephemeris[i].i_satellite_PRN);
if (gnss_pseudoranges_iter != gnss_pseudoranges_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(i,i) = 1;
// compute the GPS master clock
// d_ephemeris[i].master_clock(GPS_current_time); ?????
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first);
if (gps_ephemeris_iter != gps_ephemeris_map.end())
{
// Ephemeris available in the ephemeris map
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(obs_counter,obs_counter) = 1;
// compute the clock error including relativistic effects
GPS_corrected_time = d_ephemeris[i].sv_clock_correction(GPS_current_time);
GPS_week = d_ephemeris[i].i_GPS_week;
// 2- compute the clock error including relativistic effects for this SV
GPS_corrected_time = gps_ephemeris_iter->second.sv_clock_correction(GPS_current_time);
//DLOG(INFO)<<"pseudorranges SV "<<gps_ephemeris_iter->second.i_satellite_PRN
// << " tx_time-d_TOW="<<GPS_corrected_time-gnss_pseudoranges_iter->second.d_TOW<<std::endl;
//DLOG(INFO)<<"pseudorranges SV "<<gps_ephemeris_iter->second.i_satellite_PRN
// << " d_TOW_current_symbol="<<gnss_pseudoranges_iter->second.d_TOW_at_current_symbol<<std::endl;
utc = d_ephemeris[i].utc_time(GPS_corrected_time);
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
// 3- compute the UTC time for this SV
//TODO: check if the utc time model is valid (or it is empty (not received yet!) and no corrections are available)
utc = gps_utc_model.utc_time(GPS_corrected_time,GPS_week);
//DLOG(INFO)<<"pseudorranges SV "<<gps_ephemeris_iter->second.i_satellite_PRN
// << " symbol_shift_from_preamble="<<gnss_pseudoranges_iter->second.Pseudorange_symbol_shift<<std::endl;
// compute the satellite current ECEF position
d_ephemeris[i].satellitePosition(GPS_corrected_time);
// 4- compute the current ECEF position for this SV
gps_ephemeris_iter->second.satellitePosition(GPS_corrected_time);
satpos(0,obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1,obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2,obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
// 5- fill the observations vector with the corrected pseudorranges
obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + gps_ephemeris_iter->second.d_satClkCorr*GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] =gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++;
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs="<<obs(obs_counter)<<"[m]"<< std::endl;
}else{ // the ephemeris are not available for this SV
// no valid pseudorange for the current SV
W(obs_counter,obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO)<<"No ephemeris data for SV "<<gnss_pseudoranges_iter->first<<std::endl;
}
obs_counter++;
satpos(0,i) = d_ephemeris[i].d_satpos_X;
satpos(1,i) = d_ephemeris[i].d_satpos_Y;
satpos(2,i) = d_ephemeris[i].d_satpos_Z;
DLOG(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN
<< " X=" << d_ephemeris[i].d_satpos_X
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y
<< " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
obs(i) = gnss_pseudoranges_iter->second.Pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = d_ephemeris[i].i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] =gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++;
}
else
{
// no valid pseudorange for the current channel
W(i,i) = 0; // channel de-activated
obs(i) = 1; // to avoid algorithm problems (divide by zero)
}
}
else
{
// no valid ephemeris for the current channel
W(i,i) = 0; // channel de-activated
obs(i) = 1; // to avoid algorithm problems (divide by zero)
}
}
d_valid_observations = valid_obs;
DLOG(INFO) << "PVT: valid observations=" << valid_obs << std::endl;
// ********************************************************************************
// ****** SOLVE LEAST SQUARES******************************************************
// ********************************************************************************
d_valid_observations = valid_obs;
DLOG(INFO) << "(new)PVT: valid observations=" << valid_obs << std::endl;
if (valid_obs >= 4)
{
arma::vec mypos;
mypos=leastSquarePos(satpos,obs,W);
DLOG(INFO) << "Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4);
if (valid_obs >= 4)
{
arma::vec mypos;
DLOG(INFO)<<"satpos="<<satpos<<std::endl;
DLOG(INFO)<<"obs="<<obs<<std::endl;
DLOG(INFO)<<"W="<<W<<std::endl;
mypos=leastSquarePos(satpos,obs,W);
DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4);
// Compute UTC time and print PVT solution
double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek*(double)GPS_week);
// 22 August 1999 last GPS time roll over
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
GPS_current_time = GPS_corrected_time;
// Compute UTC time and print PVT solution
double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek*(double)GPS_week);
// 22 August 1999 last GPS time roll over
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
GPS_current_time = GPS_corrected_time;
DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl;
DLOG(INFO) << "(new)Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl;
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
tmp_double = GPS_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double = mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double = mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double = mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e)
{
std::cout << "Exception writing PVT LS dump file "<< e.what() << std::endl;
}
}
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
tmp_double = GPS_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double = mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double = mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double = mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e)
{
std::cout << "Exception writing PVT LS dump file "<< e.what() << std::endl;
}
}
// MOVING AVERAGE PVT
if (flag_averaging == true)
{
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
// MOVING AVERAGE PVT
if (flag_averaging == true)
{
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = 0;
d_avg_longitude_d = 0;
d_avg_height_m = 0;
for (unsigned int i=0; i<d_hist_longitude_d.size(); i++)
{
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
}
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
b_valid_position = true;
return true; //indicates that the returned position is valid
}
else
{
//int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = 0;
d_avg_longitude_d = 0;
d_avg_height_m = 0;
for (unsigned int i=0; i<d_hist_longitude_d.size(); i++)
{
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
}
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
b_valid_position = true;
return true; //indicates that the returned position is valid
}
else
{
//int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m;
b_valid_position = false;
return false; //indicates that the returned position is not valid yet
// output the average, although it will not have the full historic available
// d_avg_latitude_d=0;
// d_avg_longitude_d=0;
// d_avg_height_m=0;
// for (unsigned int i=0;i<d_hist_longitude_d.size();i++)
// {
// d_avg_latitude_d=d_avg_latitude_d+d_hist_latitude_d.at(i);
// d_avg_longitude_d=d_avg_longitude_d+d_hist_longitude_d.at(i);
// d_avg_height_m=d_avg_height_m+d_hist_height_m.at(i);
// }
// d_avg_latitude_d=d_avg_latitude_d/(double)current_depth;
// d_avg_longitude_d=d_avg_longitude_d/(double)current_depth;
// d_avg_height_m=d_avg_height_m/(double)current_depth;
}
}
else
{
b_valid_position = true;
return true; //indicates that the returned position is valid
}
}
else
{
b_valid_position = false;
return false;
}
d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m;
b_valid_position = false;
return false; //indicates that the returned position is not valid yet
}
}
else
{
b_valid_position = true;
return true; //indicates that the returned position is valid
}
}
else
{
b_valid_position = false;
return false;
}
}

12
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
View File

@ -47,6 +47,8 @@
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
#include "gps_ephemeris.h"
#include "gps_utc_model.h"
#define PVT_MAX_CHANNELS 24
@ -70,6 +72,14 @@ public:
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites
Gps_Navigation_Message* d_ephemeris;
// new ephemeris storage
std::map<int,Gps_Ephemeris> gps_ephemeris_map;
// new utc_model storage
Gps_Utc_Model gps_utc_model;
// new iono storage
Gps_Iono gps_iono;
double d_GPS_current_time;
boost::posix_time::ptime d_position_UTC_time;
@ -112,7 +122,7 @@ public:
gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~gps_l1_ca_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> pseudoranges,double GPS_current_time,bool flag_averaging);
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical

206
src/algorithms/PVT/libs/rinex_printer.cc
View File

@ -30,6 +30,10 @@
#include "rinex_printer.h"
#include "gps_navigation_message.h"
#include "gps_ephemeris.h"
#include "gps_iono.h"
#include "gps_utc_model.h"
#include "GPS_L1_CA.h"
#include <ostream>
#include <fstream>
@ -336,7 +340,7 @@ std::string Rinex_Printer::getLocalTime()
}
void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message nav_msg)
void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Iono iono, Gps_Utc_Model utc_model)
{
std::string line;
@ -394,10 +398,10 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
if (version == 2)
{
line += std::string(2, ' ');
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha3, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha3, 10, 2), 12);
line += std::string(10, ' ');
line += Rinex_Printer::leftJustify("ION ALPHA", 20);
}
@ -405,10 +409,10 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
{
line += std::string("GPSA");
line += std::string(1, ' ');
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_alpha3, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha3, 10, 2), 12);
line += std::string(7, ' ');
line += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20);
}
@ -420,10 +424,10 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
if (version == 2)
{
line += std::string(2, ' ');
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta3, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta3, 10, 2), 12);
line += std::string(10, ' ');
line += Rinex_Printer::leftJustify("ION BETA", 20);
}
@ -432,10 +436,10 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
{
line += std::string("GPSB");
line += std::string(1, ' ');
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_beta3, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta0, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta1, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta2, 10, 2), 12);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta3, 10, 2), 12);
line += std::string(7, ' ');
line += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20);
}
@ -447,10 +451,10 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
if (version == 2)
{
line += std::string(3, ' ');
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_A0, 18, 2), 19);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_A1, 18, 2), 19);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.d_t_OT), 9);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.i_WN_T + 1024), 9); // valid until 2019
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A0, 18, 2), 19);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A1, 18, 2), 19);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.d_t_OT), 9);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.i_WN_T + 1024), 9); // valid until 2019
line += std::string(1, ' ');
line += Rinex_Printer::leftJustify("DELTA-UTC: A0,A1,T,W", 20);
}
@ -458,10 +462,10 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
if (version == 3)
{
line += std::string("GPUT");
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_A0, 16, 2), 18);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(nav_msg.d_A1, 15, 2), 16);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.d_t_OT), 7);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.i_WN_T + 1024), 5); // valid until 2019
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A0, 16, 2), 18);
line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A1, 15, 2), 16);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.d_t_OT), 7);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.i_WN_T + 1024), 5); // valid until 2019
/* if ( SBAS )
{
line += string(1, ' ');
@ -481,16 +485,16 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
// -------- Line 6 leap seconds
// For leap second information, see http://www.endruntechnologies.com/leap.htm
line.clear();
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.d_DeltaT_LS), 6);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.d_DeltaT_LS), 6);
if (version == 2)
{
line += std::string(54, ' ');
}
if (version == 3)
{
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.d_DeltaT_LSF), 6);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.i_WN_LSF), 6);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.i_DN), 6);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.d_DeltaT_LSF), 6);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.i_WN_LSF), 6);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(utc_model.i_DN), 6);
line += std::string(36, ' ');
}
line += Rinex_Printer::leftJustify("LEAP SECONDS", 20);
@ -510,13 +514,17 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Navigation_Message
void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigation_Message> nav_msg)
void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris> eph_map)
{
std::string line;
for (int i=0; i < (int)nav_msg.size(); i++)
{
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
for(gps_ephemeris_iter = eph_map.begin();
gps_ephemeris_iter != eph_map.end();
gps_ephemeris_iter++)
{
// -------- SV / EPOCH / SV CLK
boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_GPS_time(nav_msg[i],nav_msg[i].d_TOW);
boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_GPS_time(gps_ephemeris_iter->second,gps_ephemeris_iter->second.d_TOW);
std::string timestring = boost::posix_time::to_iso_string(p_utc_time);
std::string month (timestring, 4, 2);
std::string day (timestring, 6, 2);
@ -525,7 +533,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
std::string seconds (timestring, 13, 2);
if (version == 2)
{
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg[i].i_satellite_PRN), 2);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(gps_ephemeris_iter->second.i_satellite_PRN), 2);
line += std::string(1, ' ');
std::string year (timestring, 2, 2);
line += year;
@ -547,18 +555,18 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
}
line += decimal;
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_A_f0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f1, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_A_f1, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f2, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_A_f2, 18, 2);
line += std::string(1, ' ');
}
if (version == 3)
{
line += satelliteSystem["GPS"];
if (nav_msg[i].i_satellite_PRN < 10) line += std::string("0");
line += boost::lexical_cast<std::string>(nav_msg[i].i_satellite_PRN);
if (gps_ephemeris_iter->second.i_satellite_PRN < 10) line += std::string("0");
line += boost::lexical_cast<std::string>(gps_ephemeris_iter->second.i_satellite_PRN);
std::string year (timestring, 0, 4);
line += std::string(1, ' ');
line += year;
@ -573,11 +581,11 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
line += std::string(1, ' ');
line += seconds;
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_A_f0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f1, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_A_f1, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f2, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_A_f2, 18, 2);
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
@ -596,20 +604,24 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
{
line += std::string(5, ' ');
}
if (nav_msg[i].d_IODE_SF2 == nav_msg[i].d_IODE_SF3)
{
line += Rinex_Printer::doub2for(nav_msg[i].d_IODE_SF2, 18, 2);
}
else
{
LOG_AT_LEVEL(ERROR) << "Discontinued reception of Frame 2 and 3 " << std::endl;
}
// IODE is not present in ephemeris data
// If there is a discontinued reception the ephemeris is not validated
//if (gps_ephemeris_iter->second.d_IODE_SF2 == gps_ephemeris_iter->second.d_IODE_SF3)
// {
// line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_IODE_SF2, 18, 2);
// }
//else
// {
// LOG_AT_LEVEL(ERROR) << "Discontinued reception of Frame 2 and 3 " << std::endl;
// }
double d_IODE_SF2=0;
line += Rinex_Printer::doub2for(d_IODE_SF2, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_Crs, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Crs, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_Delta_n, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Delta_n, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_M_0, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_M_0, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
@ -628,13 +640,13 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_Cuc, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Cuc, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_e_eccentricity, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_e_eccentricity, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_Cus, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Cus, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_sqrt_A, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_sqrt_A, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
@ -654,13 +666,13 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_Toe, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Toe, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_Cic, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Cic, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_OMEGA0, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_OMEGA0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_Cis, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Cis, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
@ -681,13 +693,13 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_i_0, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_i_0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_Crc, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Crc, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_OMEGA, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_OMEGA, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_OMEGA_DOT, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_OMEGA_DOT, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
@ -707,14 +719,14 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_IDOT, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_IDOT, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_code_on_L2), 18, 2);
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_code_on_L2), 18, 2);
line += std::string(1, ' ');
double GPS_week_continuous_number = (double)(nav_msg[i].i_GPS_week + 1024); // valid until April 7, 2019 (check http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm)
double GPS_week_continuous_number = (double)(gps_ephemeris_iter->second.i_GPS_week + 1024); // valid until April 7, 2019 (check http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm)
line += Rinex_Printer::doub2for(GPS_week_continuous_number, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_code_on_L2), 18, 2);
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_code_on_L2), 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
@ -733,13 +745,13 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_SV_accuracy), 18, 2);
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_SV_accuracy), 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_SV_health), 18, 2);
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_SV_health), 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_TGD, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_TGD, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_IODC, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_IODC, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
@ -758,30 +770,30 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_TOW, 18, 2);
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_TOW, 18, 2);
line += std::string(1, ' ');
double curve_fit_interval = 4;
if (nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIA"))
if (gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIA"))
{
// Block II/IIA (Table 20-XI IS-GPS-200E )
if ( (nav_msg[i].d_IODC > 239) && (nav_msg[i].d_IODC < 248) ) curve_fit_interval = 8;
if ( ( (nav_msg[i].d_IODC > 247) && (nav_msg[i].d_IODC < 256) ) || (nav_msg[i].d_IODC == 496) ) curve_fit_interval = 14;
if ( (nav_msg[i].d_IODC > 496) && (nav_msg[i].d_IODC < 504) ) curve_fit_interval = 26;
if ( (nav_msg[i].d_IODC > 503) && (nav_msg[i].d_IODC < 511) ) curve_fit_interval = 50;
if ( ( (nav_msg[i].d_IODC > 751) && (nav_msg[i].d_IODC < 757) ) || (nav_msg[i].d_IODC == 511) ) curve_fit_interval = 74;
if ( nav_msg[i].d_IODC == 757 ) curve_fit_interval = 98;
if ( (gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248) ) curve_fit_interval = 8;
if ( ( (gps_ephemeris_iter->second.d_IODC > 247) && (gps_ephemeris_iter->second.d_IODC < 256) ) || (gps_ephemeris_iter->second.d_IODC == 496) ) curve_fit_interval = 14;
if ( (gps_ephemeris_iter->second.d_IODC > 496) && (gps_ephemeris_iter->second.d_IODC < 504) ) curve_fit_interval = 26;
if ( (gps_ephemeris_iter->second.d_IODC > 503) && (gps_ephemeris_iter->second.d_IODC < 511) ) curve_fit_interval = 50;
if ( ( (gps_ephemeris_iter->second.d_IODC > 751) && (gps_ephemeris_iter->second.d_IODC < 757) ) || (gps_ephemeris_iter->second.d_IODC == 511) ) curve_fit_interval = 74;
if ( gps_ephemeris_iter->second.d_IODC == 757 ) curve_fit_interval = 98;
}
if ((nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIR") == 0) ||
(nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIR-M") == 0) ||
(nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIF") == 0) ||
(nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIIA") == 0) )
if ((gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIR") == 0) ||
(gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIR-M") == 0) ||
(gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIF") == 0) ||
(gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIIA") == 0) )
{
// Block IIR/IIR-M/IIF/IIIA (Table 20-XII IS-GPS-200E )
if ( (nav_msg[i].d_IODC > 239) && (nav_msg[i].d_IODC < 248)) curve_fit_interval = 8;
if ( ( (nav_msg[i].d_IODC > 247) && (nav_msg[i].d_IODC < 256)) || (nav_msg[i].d_IODC == 496) ) curve_fit_interval = 14;
if ( ( (nav_msg[i].d_IODC > 496) && (nav_msg[i].d_IODC < 504)) || ( (nav_msg[i].d_IODC > 1020) && (nav_msg[i].d_IODC < 1024) ) ) curve_fit_interval = 26;
if ( (gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248)) curve_fit_interval = 8;
if ( ( (gps_ephemeris_iter->second.d_IODC > 247) && (gps_ephemeris_iter->second.d_IODC < 256)) || (gps_ephemeris_iter->second.d_IODC == 496) ) curve_fit_interval = 14;
if ( ( (gps_ephemeris_iter->second.d_IODC > 496) && (gps_ephemeris_iter->second.d_IODC < 504)) || ( (gps_ephemeris_iter->second.d_IODC > 1020) && (gps_ephemeris_iter->second.d_IODC < 1024) ) ) curve_fit_interval = 26;
}
line += Rinex_Printer::doub2for(curve_fit_interval, 18, 2);
line += std::string(1, ' ');
@ -801,7 +813,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigatio
void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Navigation_Message nav_msg)
void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, double d_TOW_first_observation)
{
std::string line;
@ -957,14 +969,14 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Navigation_Message
// -------- TIME OF FIRST OBS
line.clear();
boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(nav_msg,nav_msg.d_TOW);
boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph,d_TOW_first_observation);
std::string timestring=boost::posix_time::to_iso_string(p_gps_time);
std::string year (timestring, 0, 4);
std::string month (timestring, 4, 2);
std::string day (timestring, 6, 2);
std::string hour (timestring, 9, 2);
std::string minutes (timestring, 11, 2);
double gps_t =nav_msg.d_TOW;
double gps_t =d_TOW_first_observation;
double seconds = fmod(gps_t, 60);
line += Rinex_Printer::rightJustify(year, 6);
line += Rinex_Printer::rightJustify(month, 6);
@ -991,16 +1003,16 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Navigation_Message
void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav_msg, double obs_time, std::map<int,double> pseudoranges)
void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double obs_time, std::map<int,double> pseudoranges)
{
// RINEX observations timestamps are GPS timestamps.
std::string line;
boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(nav_msg,obs_time);
boost::posix_time::ptime p_gps_time= Rinex_Printer::compute_GPS_time(eph,obs_time);
std::string timestring=boost::posix_time::to_iso_string(p_gps_time);
//double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time));
double gps_t=nav_msg.sv_clock_correction(obs_time);
double gps_t=eph.sv_clock_correction(obs_time);
std::string month (timestring, 4, 2);
std::string day (timestring, 6, 2);
@ -1206,15 +1218,15 @@ boost::posix_time::ptime Rinex_Printer::compute_UTC_time(Gps_Navigation_Message
return p_time;
}
boost::posix_time::ptime Rinex_Printer::compute_GPS_time(Gps_Navigation_Message nav_msg, double obs_time)
boost::posix_time::ptime Rinex_Printer::compute_GPS_time(Gps_Ephemeris eph, double obs_time)
{
// The RINEX v2.11 v3.00 format uses GPS time for the observations epoch, not UTC time, thus, no leap seconds needed here.
// (see Section 3 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex211.txt)
// (see Pag. 17 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex300.pdf)
// --??? No time correction here, since it will be done in the RINEX processor
double gps_t = nav_msg.sv_clock_correction(obs_time);
double gps_t = eph.sv_clock_correction(obs_time);
//double gps_t=obs_time;
boost::posix_time::time_duration t = boost::posix_time::millisec((gps_t + 604800*(double)(nav_msg.i_GPS_week%1024))*1000);
boost::posix_time::time_duration t = boost::posix_time::millisec((gps_t + 604800*(double)(eph.i_GPS_week%1024))*1000);
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
return p_time;
}

10
src/algorithms/PVT/libs/rinex_printer.h
View File

@ -83,12 +83,12 @@ public:
/*!
* \brief Generates the Navigation Data header
*/
void rinex_nav_header(std::ofstream& out, Gps_Navigation_Message nav);
void rinex_nav_header(std::ofstream& out, Gps_Iono iono, Gps_Utc_Model utc_model);
/*!
* \brief Generates the Observation data header
*/
void rinex_obs_header(std::ofstream& out, Gps_Navigation_Message nav);
void rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, double d_TOW_first_observation);
/*!
* \brief Computes the UTC time and returns a boost::posix_time::ptime object
@ -98,18 +98,18 @@ public:
/*!
* \brief Computes the GPS time and returns a boost::posix_time::ptime object
*/
boost::posix_time::ptime compute_GPS_time(Gps_Navigation_Message nav_msg, double obs_time);
boost::posix_time::ptime compute_GPS_time(Gps_Ephemeris eph, double obs_time);
/*!
* \brief Writes data from the navigation message into the RINEX file
*/
void log_rinex_nav(std::ofstream& out, std::map<int,Gps_Navigation_Message> nav_msg);
void log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris> eph_map);
/*!
* \brief Writes observables into the RINEX file
*/
void log_rinex_obs(std::ofstream& out,Gps_Navigation_Message nav_msg, double obs_time, std::map<int,double> pseudoranges);
void log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double obs_time, std::map<int,double> pseudoranges);
std::map<std::string,std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass
std::map<std::string,std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH
std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors

5
src/algorithms/observables/adapters/gps_l1_ca_observables.cc
View File

@ -39,8 +39,6 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
extern concurrent_queue<Gps_Navigation_Message> global_gps_nav_msg_queue;
using google::LogMessage;
GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
@ -65,9 +63,6 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
observables_ = gps_l1_ca_make_observables_cc(in_streams_, queue_, dump_, dump_filename_, output_rate_ms, flag_averaging);
observables_->set_fs_in(fs_in_);
DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")";
// set the navigation msg queue;
observables_->set_navigation_queue(&global_gps_nav_msg_queue);
DLOG(INFO) << "global navigation message queue assigned to observables ("<< observables_->unique_id() << ")";
}

71
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@ -107,6 +107,10 @@ bool pairCompare_gnss_synchro_preamble_symbol_count( std::pair<int,Gnss_Synchro>
return (a.second.Preamble_symbol_counter) < (b.second.Preamble_symbol_counter);
}
bool pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.d_TOW_at_current_symbol) < (b.second.d_TOW_at_current_symbol);
}
bool pairCompare_gnss_synchro_preamble_delay_ms( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
@ -190,44 +194,32 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
* 2.1 Find the correct symbol timestamp in the gnss_synchro history: we have to compare timestamps between channels on the SAME symbol
* (common TX time algorithm)
*/
double min_preamble_delay_ms;
double max_preamble_delay_ms;
int current_symbol = 0;
int reference_channel;
int history_shift;
Gnss_Synchro tmp_gnss_synchro;
gnss_synchro_iter = min_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_preamble_delay_ms);
min_preamble_delay_ms = gnss_synchro_iter->second.Preamble_timestamp_ms; //[ms]
gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_preamble_delay_ms);
max_preamble_delay_ms = gnss_synchro_iter->second.Preamble_timestamp_ms; //[ms]
if ((max_preamble_delay_ms - min_preamble_delay_ms) < MAX_TOA_DELAY_MS)
{
// we have a valid information set. Its time to align the symbols information
// what is the most delayed symbol in the current set? -> this will be the reference symbol
gnss_synchro_iter = min_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_preamble_symbol_count);
current_symbol = gnss_synchro_iter->second.Preamble_symbol_counter;
reference_channel = gnss_synchro_iter->second.Channel_ID;
// save it in the aligned symbols map
gnss_synchro_aligned_map.insert(std::pair<int,Gnss_Synchro>(gnss_synchro_iter->second.Channel_ID, gnss_synchro_iter->second));
// Now find where the same symbols were in the rest of the channels searching in the symbol history
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{
//TODO: Replace the loop using current current_symbol-Preamble_symbol_counter
if (reference_channel != gnss_synchro_iter->second.Channel_ID)
{
// compute the required symbol history shift in order to match the reference symbol
history_shift = gnss_synchro_iter->second.Preamble_symbol_counter - current_symbol;
if (history_shift < (int)MAX_TOA_DELAY_MS ) // and history_shift>=0)
{
tmp_gnss_synchro= d_history_gnss_synchro_deque[gnss_synchro_iter->second.Channel_ID][history_shift];
gnss_synchro_aligned_map.insert(std::pair<int,Gnss_Synchro>(gnss_synchro_iter->second.Channel_ID, tmp_gnss_synchro));
}
}
}
}
// we have a valid information set. Its time to align the symbols information
// what is the most delayed symbol in the current set? -> this will be the reference symbol
gnss_synchro_iter = min_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_d_TOW_at_current_symbol);
double d_TOW_reference=gnss_synchro_iter->second.d_TOW_at_current_symbol;
reference_channel = gnss_synchro_iter->second.Channel_ID;
// save it in the aligned symbols map
gnss_synchro_aligned_map.insert(std::pair<int,Gnss_Synchro>(gnss_synchro_iter->second.Channel_ID, gnss_synchro_iter->second));
// Now find where the same symbols were in the rest of the channels searching in the symbol history
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{
//TODO: Replace the loop using current current_symbol-Preamble_symbol_counter
if (reference_channel != gnss_synchro_iter->second.Channel_ID)
{
// compute the required symbol history shift in order to match the reference symbol
history_shift=round((gnss_synchro_iter->second.d_TOW_at_current_symbol-d_TOW_reference)*1000);
//check if this is a valid set of observations
if (history_shift < (int)MAX_TOA_DELAY_MS )
{
tmp_gnss_synchro= d_history_gnss_synchro_deque[gnss_synchro_iter->second.Channel_ID][history_shift];
gnss_synchro_aligned_map.insert(std::pair<int,Gnss_Synchro>(gnss_synchro_iter->second.Channel_ID, tmp_gnss_synchro));
}
}
}
/*
* 3 Compute the pseudoranges using the aligned data map
@ -241,11 +233,12 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
max_symbol_timestamp_ms = gnss_synchro_iter->second.Prn_timestamp_ms; //[ms]
// check again if this is a valid set of observations
if ((max_symbol_timestamp_ms - min_symbol_timestamp_ms) < MAX_TOA_DELAY_MS)
//not necesary
//if ((max_symbol_timestamp_ms - min_symbol_timestamp_ms) < MAX_TOA_DELAY_MS)
/*
* 2.3 compute the pseudoranges
*/
{
//{
for(gnss_synchro_iter = gnss_synchro_aligned_map.begin(); gnss_synchro_iter != gnss_synchro_aligned_map.end(); gnss_synchro_iter++)
{
traveltime_ms = gnss_synchro_iter->second.Prn_timestamp_ms - min_symbol_timestamp_ms + GPS_STARTOFFSET_ms; //[ms]
@ -253,11 +246,11 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
// update the pseudorange object
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_symbol_shift = (double)current_symbol; // number of symbols shifted from preamble start symbol
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_symbol_shift = 0;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_timestamp_ms = max_symbol_timestamp_ms;
}
}
//}
}
if(d_dump == true)

3
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
View File

@ -57,7 +57,6 @@ class gps_l1_ca_observables_cc : public gr_block
{
public:
~gps_l1_ca_observables_cc ();
void set_navigation_queue(concurrent_queue<Gps_Navigation_Message> *nav_queue){d_nav_queue = nav_queue;}
void set_fs_in(unsigned long int fs_in) {d_fs_in = fs_in;};
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
@ -77,9 +76,7 @@ private:
int d_output_rate_ms;
std::string d_dump_filename;
std::ofstream d_dump_file;
//std::deque<double> *d_history_prn_delay_ms;
std::deque<Gnss_Synchro> *d_history_gnss_synchro_deque;
concurrent_queue<Gps_Navigation_Message> *d_nav_queue; // Navigation ephemeris queue
};
#endif

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

@ -42,7 +42,6 @@
#include "gps_iono.h"
#include "gps_utc_model.h"
extern concurrent_queue<Gps_Navigation_Message> global_gps_nav_msg_queue;
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;
@ -74,7 +73,6 @@ GpsL1CaTelemetryDecoder::GpsL1CaTelemetryDecoder(ConfigurationInterface* configu
telemetry_decoder_ = gps_l1_ca_make_telemetry_decoder_cc(satellite_, 0, (long)fs_in, vector_length_, queue_, dump_); // TODO fix me
DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";
// set the navigation msg queue;
telemetry_decoder_->set_navigation_queue(&global_gps_nav_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);

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

@ -129,6 +129,10 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes = 0;
d_flag_parity = false;
d_TOW_at_Preamble=0;
d_TOW_at_current_symbol=0;
flag_TOW_set=false;
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
}
@ -236,6 +240,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
std::cout << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff << std::endl;
d_stat = 0; //lost of frame sync
d_flag_frame_sync = false;
flag_TOW_set=false;
}
}
}
@ -301,11 +306,28 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
//1. Copy the current tracking output
current_synchro_data=in[0][0];
//2. Add the telemetry decoder information
current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_flag_parity == true);
if (flag_TOW_set==true)
{
d_TOW_at_current_symbol=d_TOW_at_current_symbol+GPS_L1_CA_CODE_PERIOD;
}
if (this->d_flag_preamble==true and d_GPS_FSM.d_nav.d_TOW>0) //update TOW at the preamble instant (todo: check for valid d_TOW)
{
d_TOW_at_Preamble=d_GPS_FSM.d_nav.d_TOW+GPS_SUBFRAME_SECONDS; //we decoded the current TOW when the last word of the subframe arrive, so, we have a lag of ONE SUBFRAME
if (flag_TOW_set==false)
{
d_TOW_at_current_symbol=d_TOW_at_Preamble+GPS_L1_CA_CODE_PERIOD*GPS_CA_PREAMBLE_LENGTH_BITS;
flag_TOW_set=true;
}
}
current_synchro_data.d_TOW=d_TOW_at_Preamble;
current_synchro_data.d_TOW_at_current_symbol=d_TOW_at_current_symbol;
current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set==true);
current_synchro_data.Flag_preamble = d_flag_preamble;
current_synchro_data.Preamble_timestamp_ms = d_preamble_time_seconds * 1000.0;
current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
current_synchro_data.Preamble_symbol_counter = fmod((double)(d_sample_counter - d_preamble_index), 6000); //not corrected the preamble correlation lag! -> to be taken into account in TX Time
current_synchro_data.Preamble_symbol_counter = 0;//fmod((double)(d_sample_counter - d_preamble_index), 6000); //not corrected the preamble correlation lag! -> to be taken into account in TX Time
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file

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

@ -63,9 +63,8 @@ public:
void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN
void set_channel(int channel); //!< Set receiver's channel
/*!
* \brief Set the navigation queue
* \brief Set the satellite data queue
*/
void set_navigation_queue(concurrent_queue<Gps_Navigation_Message> *nav_queue){d_GPS_FSM.d_nav_queue=nav_queue;}
void set_ephemeris_queue(concurrent_queue<Gps_Ephemeris> *ephemeris_queue){d_GPS_FSM.d_ephemeris_queue=ephemeris_queue;}
void set_iono_queue(concurrent_queue<Gps_Iono> *iono_queue){d_GPS_FSM.d_iono_queue=iono_queue;}
void set_almanac_queue(concurrent_queue<Gps_Almanac> *almanac_queue){d_GPS_FSM.d_almanac_queue=almanac_queue;}
@ -123,6 +122,10 @@ private:
double d_preamble_time_seconds;
double d_preamble_code_phase_seconds;
double d_TOW_at_Preamble;
double d_TOW_at_current_symbol;
bool flag_TOW_set;
std::string d_dump_filename;
std::ofstream d_dump_file;
};

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

@ -232,19 +232,12 @@ GpsL1CaSubframeFsm::GpsL1CaSubframeFsm()
initiate(); //start the FSM
}
void GpsL1CaSubframeFsm::gps_word_to_subframe(int position)
{
// insert the word in the correct position of the subframe
std::memcpy(&d_subframe[position*GPS_WORD_LENGTH], &d_GPS_frame_4bytes, sizeof(char)*GPS_WORD_LENGTH);
}
void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
{
int subframe_ID;
@ -254,36 +247,36 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
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;
d_nav.b_update_tow_flag = true;
/*!
* \todo change satellite validation to subframe 5 because it will have a complete set of ephemeris parameters
*/
if (d_nav.satellite_validation()==true)
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);
//
// // get ionospheric parameters (if available)
// if (d_nav.flag_iono_valid==true)
// {
// Gps_Iono iono=d_nav.get_iono();
// d_iono_queue->push(iono);
// }
//
// // get almanac (if available)
// //TODO: implement almanac reader in navigation_message
//
// // get UTC model
// if (d_nav.flag_utc_model_valid==true)
// {
// Gps_Utc_Model utc_model=d_nav.get_utc_model();
// d_utc_model_queue->push(utc_model);
//
// }
// old nav queue
d_nav_queue->push(d_nav);
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;
}
}
void GpsL1CaSubframeFsm::Event_gps_word_valid()

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

@ -82,8 +82,7 @@ public:
concurrent_queue<Gps_Utc_Model> *d_utc_model_queue;
// Almanac queue
concurrent_queue<Gps_Almanac> *d_almanac_queue;
// Old navigation message queue
concurrent_queue<Gps_Navigation_Message> *d_nav_queue;
// navigation message class
Gps_Navigation_Message d_nav;

12
src/core/system_parameters/gnss_synchro.h
View File

@ -61,11 +61,13 @@ public:
double CN0_dB_hz; //!< Set by Tracking processing block
bool Flag_valid_tracking;
//Telemetry Decoder
double Preamble_timestamp_ms;
double Prn_timestamp_ms;
int Preamble_symbol_counter; //n_pre
bool Flag_valid_word;
bool Flag_preamble;
double Preamble_timestamp_ms; //!< Set by Telemetry Decoder processing block
double Prn_timestamp_ms;//!< Set by Telemetry Decoder processing block
int Preamble_symbol_counter; //!< Set by Telemetry Decoder processing block
bool Flag_valid_word;//!< Set by Telemetry Decoder processing block
bool Flag_preamble;//!< Set by Telemetry Decoder processing block
double d_TOW; //!< Set by Telemetry Decoder processing block
double d_TOW_at_current_symbol;
// Pseudorange
double Pseudorange_m;
double Pseudorange_symbol_shift;

164
src/core/system_parameters/gps_ephemeris.cc
View File

@ -1,6 +1,6 @@
/*!
* \file gps_ephemeris.cc
* \brief Interface of a GPS EPHEMERIS storage
* \brief Interface of a GPS EPHEMERIS storage and orbital model functions
*
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
* \author Javier Arribas, 2013. jarribas(at)cttc.es
@ -35,5 +35,167 @@
Gps_Ephemeris::Gps_Ephemeris()
{
//Plane A (info from http://www.navcen.uscg.gov/?Do=constellationStatus)
satelliteBlock[9] = "IIA";
satelliteBlock[31] = "IIR-M";
satelliteBlock[8] = "IIA";
satelliteBlock[7] = "IIR-M";
satelliteBlock[27] = "IIA";
//Plane B
satelliteBlock[16] = "IIR";
satelliteBlock[25] = "IIF";
satelliteBlock[28] = "IIR";
satelliteBlock[12] = "IIR-M";
satelliteBlock[30] = "IIA";
//Plane C
satelliteBlock[29] = "IIR-M";
satelliteBlock[3] = "IIA";
satelliteBlock[19] = "IIR";
satelliteBlock[17] = "IIR-M";
satelliteBlock[6] = "IIA";
//Plane D
satelliteBlock[2] = "IIR";
satelliteBlock[1] = "IIF";
satelliteBlock[21] = "IIR";
satelliteBlock[4] = "IIA";
satelliteBlock[11] = "IIR";
satelliteBlock[24] = "IIA"; // Decommissioned from active service on 04 Nov 2011
//Plane E
satelliteBlock[20] = "IIR";
satelliteBlock[22] = "IIR";
satelliteBlock[5] = "IIR-M";
satelliteBlock[18] = "IIR";
satelliteBlock[32] = "IIA";
satelliteBlock[10] = "IIA";
//Plane F
satelliteBlock[14] = "IIR";
satelliteBlock[15] = "IIR-M";
satelliteBlock[13] = "IIR";
satelliteBlock[23] = "IIR";
satelliteBlock[26] = "IIA";
}
double Gps_Ephemeris::check_t(double time)
{
double corrTime;
double half_week = 302400; // seconds
corrTime = time;
if (time > half_week)
{
corrTime = time - 2*half_week;
}
else if (time < -half_week)
{
corrTime = time + 2*half_week;
}
return corrTime;
}
// 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
double Gps_Ephemeris::sv_clock_correction(double transmitTime)
{
double dt;
dt = check_t(transmitTime - d_Toc);
d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr;
double correctedTime = transmitTime - d_satClkCorr;
return correctedTime;
}
void Gps_Ephemeris::satellitePosition(double transmitTime)
{
double tk;
double a;
double n;
double n0;
double M;
double E;
double E_old;
double dE;
double nu;
double phi;
double u;
double r;
double i;
double Omega;
// Find satellite's position ----------------------------------------------
// Restore semi-major axis
a = d_sqrt_A*d_sqrt_A;
// Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe);
// Computed mean motion
n0 = sqrt(GM / (a*a*a));
// Corrected mean motion
n = n0 + d_Delta_n;
// Mean anomaly
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2*GPS_PI), (2*GPS_PI));
// Initial guess of eccentric anomaly
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1; ii<20; ii++)
{
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2*GPS_PI);
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
break;
}
}
// Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
// Compute the true anomaly
double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E);
double tmp_X = cos(E) - d_e_eccentricity;
nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude)
phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2*GPS_PI));
// Correct argument of latitude
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi);
// Correct radius
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi);
// Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi);
// Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2*GPS_PI), (2*GPS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega);
d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT;
d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i);
}

42
src/core/system_parameters/gps_ephemeris.h
View File

@ -40,7 +40,7 @@
/*!
* \brief This class is a storage for the GPS SV ephemeris data as described in IS-GPS-200E
* \brief This class is a storage and orbital model functions for the GPS SV ephemeris data as described in IS-GPS-200E
*
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
*/
@ -48,6 +48,16 @@ class Gps_Ephemeris
{
private:
/*
* Accounts for the beginning or end of week crossover
*
* See paragraph 20.3.3.3.3.1 (IS-GPS-200E)
* \param[in] - time in seconds
* \param[out] - corrected time, in seconds
*/
double check_t(double time);
public:
unsigned int i_satellite_PRN; // SV PRN NUMBER
@ -103,6 +113,36 @@ public:
bool b_alert_flag; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
bool b_antispoofing_flag; //!< If true, the AntiSpoofing mode is ON in that SV
// clock terms derived from ephemeris data
double d_satClkCorr; // GPS clock error
double d_dtr; // relativistic clock correction term
// satellite positions
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
// Satellite velocity
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m]
std::map<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200E)
*/
void satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction
* and returns the corrected clock (IS-GPS-200E, 20.3.3.3.3.1)
*/
double sv_clock_correction(double transmitTime);
/*!
* Default constructor

17
src/core/system_parameters/gps_navigation_message.cc
View File

@ -37,7 +37,6 @@
void Gps_Navigation_Message::reset()
{
b_update_tow_flag = false;
b_valid_ephemeris_set_flag = false;
d_TOW = 0;
d_TOW_SF1 = 0;
@ -468,7 +467,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
d_TOW_SF1 = (double)read_navigation_unsigned(subframe_bits, TOW);
//we are in the first subframe (the transmitted TOW is the start time of the next subframe) !
d_TOW_SF1 = d_TOW_SF1*6;
d_TOW = d_TOW_SF5; // Set transmission time
d_TOW = d_TOW_SF1-6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -494,7 +493,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
case 2: //--- It is subframe 2 -------------------
d_TOW_SF2 = (double)read_navigation_unsigned(subframe_bits, TOW);
d_TOW_SF2 = d_TOW_SF2*6;
d_TOW = d_TOW_SF1; // Set transmission time
d_TOW = d_TOW_SF1-6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -524,7 +523,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
case 3: // --- It is subframe 3 -------------------------------------
d_TOW_SF3 = (double)read_navigation_unsigned(subframe_bits, TOW);
d_TOW_SF3 = d_TOW_SF3*6;
d_TOW = d_TOW_SF2; // Set transmission time
d_TOW = d_TOW_SF3-6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -551,7 +550,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32)
d_TOW_SF4 = (double)read_navigation_unsigned(subframe_bits, TOW);
d_TOW_SF4 = d_TOW_SF4*6;
d_TOW = d_TOW_SF3; // Set transmission time
d_TOW = d_TOW_SF4-6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -616,7 +615,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
case 5://--- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time.
d_TOW_SF5 = (double)read_navigation_unsigned(subframe_bits, TOW);
d_TOW_SF5 = d_TOW_SF5*6;
d_TOW = d_TOW_SF4; // Set transmission time
d_TOW = d_TOW_SF5-6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -790,7 +789,8 @@ Gps_Iono Gps_Navigation_Message::get_iono()
iono.d_beta2=d_beta2;
iono.d_beta3=d_beta3;
iono.valid=flag_iono_valid;
// TODO: Clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
//WARNING: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
flag_iono_valid=false;
return iono;
}
@ -808,7 +808,8 @@ Gps_Utc_Model Gps_Navigation_Message::get_utc_model()
utc_model.i_WN_LSF=i_WN_LSF;
utc_model.i_DN=i_DN;
utc_model.d_DeltaT_LSF=d_DeltaT_LSF;
// TODO: Clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
// warning: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
d_DeltaT_LSF=false;
return utc_model;
}
bool Gps_Navigation_Message::satellite_validation()

1
src/core/system_parameters/gps_navigation_message.h
View File

@ -70,7 +70,6 @@ private:
double check_t(double time);
public:
bool b_update_tow_flag; // flag indicating that this ephemeris set have an updated TOW
bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check
//broadcast orbit 1
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]

73
src/core/system_parameters/gps_utc_model.cc
View File

@ -33,6 +33,79 @@
Gps_Utc_Model::Gps_Utc_Model()
{
valid=false;
d_A1 = 0;
d_A0 = 0;
d_t_OT = 0;
i_WN_T = 0;
d_DeltaT_LS = 0;
i_WN_LSF = 0;
i_DN = 0;
d_DeltaT_LSF= 0;
}
double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
{
double t_utc;
double t_utc_daytime;
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T));
// Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week;
if ((weeksToLeapSecondEvent) >= 0) // is not in the past
{
//Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
int secondOfLeapSecondEvent = i_DN * 24 * 60 * 60;
if (weeksToLeapSecondEvent > 0)
{
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
else //we are in the same week than the leap second event
{
if (abs(gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{
/* 20.3.3.5.2.4a
* Whenever the effectivity time indicated by the WN_LSF and the DN values
* is not in the past (relative to the user's present time), and the user's
* present time does not fall in the time span which starts at six hours prior
* to the effectivity time and ends at six hours after the effectivity time,
* the UTC/GPS-time relationship is given by
*/
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
else
{
/* 20.3.3.5.2.4b
* Whenever the user's current time falls within the time span of six hours
* prior to the effectivity time to six hours after the effectivity time,
* proper accommodation of the leap second event with a possible week number
* transition is provided by the following expression for UTC:
*/
int W = fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400) + 43200;
t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS);
//implement something to handle a leap second event!
}
if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800*(double)(i_GPS_week - i_WN_T));
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
}
}
else // the effectivity time is in the past
{
/* 20.3.3.5.2.4c
* Whenever the effectivity time of the leap second event, as indicated by the
* WNLSF and DN values, is in the "past" (relative to the user's current time),
* and the user<EFBFBD>s current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T));
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
double secondsOfWeekBeforeToday = 43200 * floor(gpstime_corrected / 43200);
t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc;
}

6
src/core/system_parameters/gps_utc_model.h
View File

@ -61,6 +61,12 @@ public:
* Default constructor
*/
Gps_Utc_Model();
/*!
* \brief Computes the Coordinated Universal Time (UTC) and
* returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4)
*/
double utc_time(double gpstime_corrected, int i_GPS_week);
};
#endif

2
src/main/main.cc
View File

@ -65,7 +65,7 @@ DECLARE_string(log_dir);
* Concurrent queue that communicates the Telemetry Decoder
* to the Observables modules
*/
concurrent_queue<Gps_Navigation_Message> global_gps_nav_msg_queue;
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;

1
src/tests/single_test_main.cc
View File

@ -42,7 +42,6 @@
#include "concurrent_queue.h"
#include "gps_navigation_message.h"
concurrent_queue<Gps_Navigation_Message> global_gps_nav_msg_queue;
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;

2
src/tests/test_main.cc
View File

@ -68,8 +68,6 @@
#include "string_converter/string_converter_test.cc"
concurrent_queue<Gps_Navigation_Message> global_gps_nav_msg_queue;
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;