mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 12:10:34 +00:00
Code Issues Releases Wiki Activity

Major update:

Browse Source 
- The GNSS-SDR code-based Observable generation algorithm for GPS L1 C/A was migrated from common TX time to common RX time. One of the main reasons is that the common TX algorithm was incompatible with the standard RINEX observables, wich requires the oservables at the same time of reception.
- Using common Rx time, the code was simplified and now requires less memory due to the lack of symbol buffer

- Now it is possible to use standard RINEX post processing software (gpstk and rtklib for instance) to get PVT and the obtained precision is comparable to the internal Least Squares solver. It is possible to use also the carrier phase observable, but is still experimental.

- The default RINEX version output was set to 2.11 in this revision.  RINEX 3.00 is under verification now.


git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@364 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas 2013-06-12 15:19:32 +00:00
parent 09c6ac095a
commit 3e87d1f204
12 changed files with 170 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -89,7 +89,7 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
d_sample_counter = 0;
d_last_sample_nav_output=0;
d_tx_time=0.0;
d_rx_time=0.0;
b_rinex_header_writen = false;
rp = new Rinex_Printer();
@ -139,8 +139,6 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
d_sample_counter++;
std::map<int,Gnss_Synchro> gnss_pseudoranges_map;
std::map<int,double> pseudoranges;
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
@ -149,20 +147,10 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
if (in[i][0].Flag_valid_pseudorange == true)
{
gnss_pseudoranges_map.insert(std::pair<int,Gnss_Synchro>(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map
d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges)
}
}
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
{
double pr = gnss_pseudoranges_iter->second.Pseudorange_m;
pseudoranges[gnss_pseudoranges_iter->first] = pr;
}
// 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);
// ############ 1. READ EPHEMERIS/UTC_MODE/IONO FROM GLOBAL MAPS ####
d_ls_pvt->gps_ephemeris_map=global_gps_ephemeris_map.get_map_copy();
@ -184,13 +172,13 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{
// 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);
pvt_result=d_ls_pvt->get_PVT(gnss_pseudoranges_map,d_rx_time,d_flag_averaging);
if (pvt_result==true)
{
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
@ -202,7 +190,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
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_obs_header(rp->obsFile, gps_ephemeris_iter->second,d_rx_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
}
@ -221,7 +209,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
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, gnss_pseudoranges_map);
rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map);
}
}
}
@ -249,9 +237,9 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{
tmp_double = in[i][0].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = in[i][0].Pseudorange_symbol_shift;
tmp_double = 0;
d_dump_file.write((char*)&tmp_double, sizeof(double));
d_dump_file.write((char*)&d_tx_time, sizeof(double));
d_dump_file.write((char*)&d_rx_time, sizeof(double));
}
}
catch (std::ifstream::failure e)

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

@ -77,7 +77,7 @@ private:
long unsigned int d_last_sample_nav_output;
Kml_Printer d_kml_dump;
Nmea_Printer *d_nmea_printer;
double d_tx_time;
double d_rx_time;
gps_l1_ca_ls_pvt *d_ls_pvt;
public:

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

@ -203,17 +203,15 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
A(i,3) = 1.0;
}
// These lines allow the code to exit gracefully in case of any errors
//if (rank(A) != 4) {
// pos.clear();
// return pos;
//}
//--- Find position update ---------------------------------------------
x = arma::solve(w*A, w*omc); // Armadillo
//--- Apply position update --------------------------------------------
pos = pos + x;
if (arma::norm(x,2)<1e-4)
{
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
}
}
try{
@ -249,8 +247,12 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
arma::mat satpos = arma::zeros(3,valid_pseudoranges); //satellite positions matrix
int GPS_week = 0;
double GPS_corrected_time = 0;
//double GPS_corrected_time = 0;
double utc = 0;
double SV_clock_drift_s = 0;
double SV_relativistic_clock_corr_s=0;
double TX_time_corrected_s;
double SV_clock_bias_s=0;
d_flag_averaging = flag_averaging;
@ -272,19 +274,32 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(obs_counter,obs_counter) = 1;
// 2- compute the clock error including relativistic effects for this SV
GPS_corrected_time = gps_ephemeris_iter->second.sv_clock_correction(GPS_current_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);
// 4- compute the current ECEF position for this SV
gps_ephemeris_iter->second.satellitePosition(GPS_corrected_time);
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time=GPS_current_time;
double Tx_time=Rx_time-gnss_pseudoranges_iter->second.Pseudorange_m/GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_drift_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the relativistic clock drift using the clock model (broadcast) for this SV
SV_relativistic_clock_corr_s = gps_ephemeris_iter->second.sv_clock_relativistic_term(Tx_time);
// 4- compute the current ECEF position for this SV using corrected TX time
SV_clock_bias_s=SV_clock_drift_s+SV_relativistic_clock_corr_s-gps_ephemeris_iter->second.d_TGD;
TX_time_corrected_s=Tx_time-SV_clock_bias_s;
gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
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;
obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s*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++;
@ -294,6 +309,11 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
<< " [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;
// compute the UTC time for this SV (just to print the asociated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
utc = gps_utc_model.utc_time(TX_time_corrected_s,GPS_week);
}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
@ -301,7 +321,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
DLOG(INFO)<<"No ephemeris data for SV "<<gnss_pseudoranges_iter->first<<std::endl;
}
obs_counter++;
}
// ********************************************************************************
@ -326,7 +345,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// 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) << "(new)Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d

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

@ -54,7 +54,7 @@
using google::LogMessage;
DEFINE_string(RINEX_version, "3.01", "Specifies the RINEX version (2.11 or 3.01)");
DEFINE_string(RINEX_version, "2.11", "Specifies the RINEX version (2.11 or 3.01)");
Rinex_Printer::Rinex_Printer()
@ -1067,7 +1067,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
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=eph.sv_clock_correction(obs_time);
double gps_t=obs_time;
std::string month (timestring, 4, 2);
std::string day (timestring, 6, 2);
@ -1265,7 +1265,7 @@ boost::posix_time::ptime Rinex_Printer::compute_UTC_time(Gps_Navigation_Message
{
// if we are processing a file -> wait to leap second to resolve the ambiguity else take the week from the local system time
//: idea resolve the ambiguity with the leap second http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm
double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW));
double utc_t = nav_msg.utc_time(nav_msg.d_TOW);
boost::posix_time::time_duration t = boost::posix_time::millisec((utc_t + 604800*(double)(nav_msg.i_GPS_week))*1000);
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
return p_time;
@ -1277,7 +1277,7 @@ boost::posix_time::ptime Rinex_Printer::compute_GPS_time(Gps_Ephemeris eph, doub
// (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 = eph.sv_clock_correction(obs_time);
double gps_t = obs_time;
//double gps_t=obs_time;
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);

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

@ -62,7 +62,6 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
d_dump = dump;
d_nchannels = nchannels;
d_output_rate_ms = output_rate_ms;
d_history_gnss_synchro_deque = new std::deque<Gnss_Synchro>[d_nchannels];
d_dump_filename = dump_filename;
d_flag_averaging = flag_averaging;
@ -90,51 +89,20 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
{
d_dump_file.close();
delete[] d_history_gnss_synchro_deque;
}
bool pairCompare_gnss_synchro_Prn_delay_ms( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
}
bool pairCompare_gnss_synchro_preamble_symbol_count( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
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)
{
return (a.second.Preamble_timestamp_ms) < (b.second.Preamble_timestamp_ms);
}
bool pairCompare_double( std::pair<int,double> a, std::pair<int,double> b)
{
return (a.second) < (b.second);
}
void clearQueue( std::deque<double> &q )
{
std::deque<double> empty;
std::swap(q, empty);
}
int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -142,14 +110,10 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //Get the output pointer
Gnss_Synchro current_gnss_synchro[d_nchannels];
// Gnss_Synchro current_gnss_synchro[cd_channels];
std::map<int,Gnss_Synchro> current_gnss_synchro_map;
std::map<int,Gnss_Synchro> gnss_synchro_aligned_map;
std::map<int,Gnss_Synchro>::iterator gnss_synchro_iter;
double traveltime_ms;
double pseudorange_m;
d_sample_counter++; //count for the processed samples
bool flag_history_ok = true; //flag to indicate that all the queues have filled their GNSS SYNCHRO history
/*
* 1. Read the GNSS SYNCHRO objects from available channels
*/
@ -157,102 +121,54 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{
//Copy the telemetry decoder data to local copy
current_gnss_synchro[i] = in[i][0];
if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word
{
//record the word structure in a map for pseudoranges
current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
// RECORD PRN start timestamps history
if (d_history_gnss_synchro_deque[i].size() < MAX_TOA_DELAY_MS)
{
d_history_gnss_synchro_deque[i].push_front(current_gnss_synchro[i]);
flag_history_ok = false; // at least one channel need more samples
}
else
{
//clearQueue(d_history_prn_delay_ms[i]); //clear the queue as the preamble arrives
d_history_gnss_synchro_deque[i].pop_back();
d_history_gnss_synchro_deque[i].push_front(current_gnss_synchro[i]);
}
}
}
/*
* 1.2 Assume no satellites in tracking
*/
for (unsigned int i=0; i<d_nchannels ; i++)
{
/*
* 1.2 Assume no valid pseudoranges
*/
current_gnss_synchro[i].Flag_valid_pseudorange = false;
current_gnss_synchro[i].Pseudorange_m = 0.0;
current_gnss_synchro[i].Pseudorange_symbol_shift = 0.0;
if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word
{
//record the word structure in a map for pseudorange computation
current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
}
}
/*
* 2. Compute RAW pseudoranges: Use only the valid channels (channels that are tracking a satellite)
* 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite)
*/
if(current_gnss_synchro_map.size() > 0 and flag_history_ok == true)
if(current_gnss_synchro_map.size() > 0)
{
/*
* 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)
* 2.1 Use CURRENT set of measurements and find the nearest satellite
* common RX time algorithm
*/
int reference_channel;
int history_shift;
Gnss_Synchro 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);
//;
// what is the most recent symbol TOW in the current set? -> this will be the reference symbol
gnss_synchro_iter = max_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
double d_ref_PRN_rx_time_ms=gnss_synchro_iter->second.Prn_timestamp_ms;
//int reference_channel= gnss_synchro_iter->second.Channel_ID;
// Now compute RX time differences due to the PRN alignement in the correlators
double traveltime_ms;
double pseudorange_m;
double delta_rx_time_ms;
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));
}
}
// compute the required symbol history shift in order to match the reference symbol
delta_rx_time_ms=gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms;
//std::cout<<"delta_rx_time_ms="<<delta_rx_time_ms<<std::endl;
//compute the pseudorange
traveltime_ms=(d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0+delta_rx_time_ms+GPS_STARTOFFSET_ms;
pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
// 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].Flag_valid_pseudorange = true;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol=round(d_TOW_reference*1000)/1000+GPS_STARTOFFSET_ms/1000.0;
}
/*
* 3 Compute the pseudoranges using the aligned data map
*/
double min_symbol_timestamp_ms;
double max_symbol_timestamp_ms;
gnss_synchro_iter = min_element(gnss_synchro_aligned_map.begin(), gnss_synchro_aligned_map.end(), pairCompare_gnss_synchro_Prn_delay_ms);
min_symbol_timestamp_ms = gnss_synchro_iter->second.Prn_timestamp_ms; //[ms]
gnss_synchro_iter = max_element(gnss_synchro_aligned_map.begin(), gnss_synchro_aligned_map.end(), pairCompare_gnss_synchro_Prn_delay_ms);
max_symbol_timestamp_ms = gnss_synchro_iter->second.Prn_timestamp_ms; //[ms]
// check again if this is a valid set of observations
//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]
pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
// 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 = 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)
{
// MULTIPLEXED FILE RECORDING - Record results to file
@ -261,13 +177,13 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
tmp_double = current_gnss_synchro[i].Preamble_timestamp_ms;
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_symbol_shift;
tmp_double = 0;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double));
@ -280,19 +196,11 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
}
consume_each(1); //one by one
// mod 8/4/2012: always make the observables output
//if ((d_sample_counter % d_output_rate_ms) == 0)
// {
for (unsigned int i=0; i<d_nchannels ; i++)
{
*out[i] = current_gnss_synchro[i];
}
return 1; //Output the observables
// }
//else
// {
// return 0; //hold on
// }
}

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

@ -76,7 +76,6 @@ private:
int d_output_rate_ms;
std::string d_dump_filename;
std::ofstream d_dump_file;
std::deque<Gnss_Synchro> *d_history_gnss_synchro_deque;
};
#endif

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

@ -118,7 +118,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
}
}
d_sample_counter = 0;
d_preamble_code_phase_seconds = 0;
d_stat = 0;
d_preamble_index = 0;
d_symbol_accumulator=0;
@ -220,7 +219,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
d_flag_preamble = true;
d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P)
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs;// - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble
d_preamble_code_phase_seconds = in[0][0].Code_phase_secs;
if (!d_flag_frame_sync)
{
@ -306,18 +304,18 @@ 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
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
d_TOW_at_current_symbol=d_TOW_at_Preamble+GPS_CA_PREAMBLE_LENGTH_BITS/GPS_CA_TELEMETRY_RATE_BITS_SECOND;
Prn_timestamp_at_preamble_ms=in[0][0].Tracking_timestamp_secs * 1000.0;
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;
flag_TOW_set=true;
}
}else{
d_TOW_at_current_symbol=d_TOW_at_current_symbol+GPS_L1_CA_CODE_PERIOD;
}
@ -325,20 +323,20 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
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 = 0;//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.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
tmp_double = current_synchro_data.Preamble_timestamp_ms;
tmp_double = d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_synchro_data.Prn_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_synchro_data.Preamble_symbol_counter;
tmp_double = d_TOW_at_Preamble;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e)

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

@ -120,10 +120,10 @@ private:
//std::deque<double> d_prn_start_sample_history;
double d_preamble_time_seconds;
double d_preamble_code_phase_seconds;
double d_TOW_at_Preamble;
double d_TOW_at_current_symbol;
double Prn_timestamp_at_preamble_ms;
bool flag_TOW_set;
std::string d_dump_filename;

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

@ -55,24 +55,27 @@ public:
//Tracking
double Prompt_I; //!< Set by Tracking processing block
double Prompt_Q; //!< Set by Tracking processing block
double Carrier_phase_rads; //!< Set by Tracking processing block
double Code_phase_secs; //!< Set by Tracking processing block
double Tracking_timestamp_secs; //!< Set by Tracking processing block
double CN0_dB_hz; //!< Set by Tracking processing block
double Carrier_Doppler_hz; //!< Set by Tracking processing block
double Carrier_phase_rads; //!< Set by Tracking processing block
//old
double Code_phase_secs; //!< Set by Tracking processing block
double Tracking_timestamp_secs; //!< Set by Tracking processing block
//new
unsigned long int PRN_start_sample; //!< Set by Tracking processing block
bool Flag_valid_tracking;
//Telemetry Decoder
double Preamble_timestamp_ms; //!< Set by Telemetry Decoder processing block
//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
double Prn_timestamp_at_preamble_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;
double Pseudorange_timestamp_ms;
bool Flag_valid_pseudorange;
};

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

@ -136,16 +136,62 @@ double Gps_Ephemeris::check_t(double time)
// 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
double Gps_Ephemeris::sv_clock_correction(double transmitTime)
//clkDrift= af0 + af1.*(tTX - toc) + af2.*(tTX - toc).^2;
double Gps_Ephemeris::sv_clock_drift(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;
d_satClkDrift = d_A_f0 + d_A_f1*dt + (d_A_f2 * dt)*(d_A_f2 * dt);
return d_satClkDrift;
}
// compute the relativistic correction term
double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
{
double tk;
double a;
double n;
double n0;
double E;
double E_old;
double dE;
double M;
// 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);
return d_dtr;
}
void Gps_Ephemeris::satellitePosition(double transmitTime)
{
@ -200,8 +246,8 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
}
}
// Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
// Compute relativistic correction term (now is in sepparated function)
//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);

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

@ -115,7 +115,7 @@ public:
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_satClkDrift; // GPS clock error
double d_dtr; // relativistic clock correction term
// satellite positions
@ -188,10 +188,16 @@ public:
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)
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200E, 20.3.3.3.3.1)
*/
double sv_clock_correction(double transmitTime);
double sv_clock_drift(double transmitTime);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200E, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
/*!

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

@ -493,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-6; // Set transmission time
d_TOW = d_TOW_SF2-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);