mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 12:10:34 +00:00
Major update:
- 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
parent
09c6ac095a
commit
3e87d1f204
@ -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)
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
* 1.2 Assume no valid pseudoranges
|
||||
*/
|
||||
for (unsigned int i=0; i<d_nchannels ; i++)
|
||||
{
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* 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]
|
||||
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].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;
|
||||
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;
|
||||
}
|
||||
//}
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}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)
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
/*!
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user