1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 20:20:35 +00:00

Merge branch 'rinex_fix' of https://github.com/carlesfernandez/gnss-sdr into rinex_fix

This commit is contained in:
Carles Fernandez 2017-01-31 09:34:35 +01:00
commit 382cbfdc2a
5 changed files with 78 additions and 70 deletions

View File

@ -115,12 +115,14 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
double Rx_time = GPS_current_time; double Rx_time = GPS_current_time;
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GPS_C_m_s; 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, including relativistic effect // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD; SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
// 3- compute the current ECEF position for this SV using corrected TX time // 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
TX_time_corrected_s = Tx_time - SV_clock_bias_s; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y; satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y;
@ -128,7 +130,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// 4- fill the observations vector with the corrected pseudoranges // 4- fill the observations vector with the corrected pseudoranges
obs.resize(valid_obs + 1, 1); obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s; obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + dtr * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN; 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; d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++; valid_obs++;

View File

@ -238,7 +238,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
double T_prn_mod_samples; double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in); d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1/d_code_freq_chips; T_chip_mod_seconds = 1.0 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in); T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
@ -293,8 +293,8 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
// enable tracking // enable tracking
d_pull_in = true; d_pull_in = true;
d_enable_tracking = true; d_enable_tracking = true;
d_enable_extended_integration=false; d_enable_extended_integration = false;
d_preamble_synchronized=false; d_preamble_synchronized = false;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples << " Code Phase correction [samples]=" << delay_correction_samples
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
@ -329,8 +329,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
double code_error_filt_secs_Ti = 0.0; double code_error_filt_secs_Ti = 0.0;
double CURRENT_INTEGRATION_TIME_S = 0.0; double CURRENT_INTEGRATION_TIME_S = 0.0;
double CORRECTED_INTEGRATION_TIME_S = 0.0; double CORRECTED_INTEGRATION_TIME_S = 0.0;
double dll_code_error_secs_Ti = 0.0;
double old_d_rem_code_phase_samples;
if (d_enable_tracking == true) if (d_enable_tracking == true)
{ {
// Fill the acquisition data // Fill the acquisition data
@ -344,11 +343,13 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_correlation_length_samples)); acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_correlation_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in); current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
*out[0] = current_synchro_data;
d_sample_counter += samples_offset; //count for the processed samples d_sample_counter += samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GPS_TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GPS_TWO_PI;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
*out[0] = current_synchro_data;
consume_each(samples_offset); //shift input to perform alignment with local replica consume_each(samples_offset); //shift input to perform alignment with local replica
return 1; return 1;
} }
@ -383,7 +384,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
{ {
// compute coherent integration and enable tracking loop // compute coherent integration and enable tracking loop
// perform coherent integration using correlator output history // perform coherent integration using correlator output history
//std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl; // std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
d_correlator_outs[0] = gr_complex(0.0,0.0); d_correlator_outs[0] = gr_complex(0.0,0.0);
d_correlator_outs[1] = gr_complex(0.0,0.0); d_correlator_outs[1] = gr_complex(0.0,0.0);
d_correlator_outs[2] = gr_complex(0.0,0.0); d_correlator_outs[2] = gr_complex(0.0,0.0);
@ -399,7 +400,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_narrow_hz,2); d_carrier_loop_filter.set_params(10.0, d_pll_bw_narrow_hz,2);
d_preamble_synchronized = true; d_preamble_synchronized = true;
std::cout << "Enabled "<<d_extend_correlation_ms<<" [ms] extended correlator for CH "<< d_channel <<" : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) std::cout << "Enabled " << d_extend_correlation_ms << " [ms] extended correlator for CH "<< d_channel << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<<" pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl <<" pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl
<<" dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; <<" dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl;
} }
@ -412,28 +413,26 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
if(d_preamble_synchronized == true) if(d_preamble_synchronized == true)
{ {
// continue extended coherent correlation // continue extended coherent correlation
//remnant carrier phase [rads]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GPS_TWO_PI);
// Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation // Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation
double T_chip_seconds = 1 / d_code_freq_chips; double T_chip_seconds = 1.0 / d_code_freq_chips;
double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in); double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
int K_prn_samples = round(T_prn_samples); int K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples; double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples; d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples;
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples; d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
//code phase step (Code resampler phase increment per sample) [chips/sample] // code phase step (Code resampler phase increment per sample) [chips/sample]
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in); d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
//remnant code phase [chips] // remnant code phase [chips]
d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in)); d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GPS_TWO_PI);
// UPDATE ACCUMULATED CARRIER PHASE // UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in)); CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S; d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI;
// disable tracking loop and inform telemetry decoder // disable tracking loop and inform telemetry decoder
enable_dll_pll = false; enable_dll_pll = false;
@ -458,10 +457,9 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
{ {
// ################## PLL ########################################################## // ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti] // Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; //prompt output d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output
// Carrier discriminator filter // Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
//d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_phase_error_filt_secs_ti/INTEGRATION_TIME;
// Input [s/Ti] -> output [Hz] // Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S); d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
// PLL to DLL assistance [Secs/Ti] // PLL to DLL assistance [Secs/Ti]
@ -471,46 +469,34 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
// ################## DLL ########################################################## // ################## DLL ##########################################################
// DLL discriminator // DLL discriminator
d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] //early and late d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late
// Code discriminator filter // Code discriminator filter
d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); //input [chips/Ti] -> output [chips/second] d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second]
d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S; d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S;
code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti] code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti]
// DLL code error estimation [s/Ti]
// PLL to DLL assistance is disable due to the use of a fractional resampler that allows the correction of the code Doppler effect.
dll_code_error_secs_Ti = - code_error_filt_secs_Ti; // + d_pll_to_dll_assist_secs_Ti;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer // keep alignment parameters for the next input buffer
double T_chip_seconds;
double T_prn_seconds;
double T_prn_samples;
double K_prn_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
T_chip_seconds = 1 / d_code_freq_chips; double T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in); double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_prn_samples = round(T_prn_samples); double K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples; double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
old_d_rem_code_phase_samples = d_rem_code_phase_samples; d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples + code_error_filt_secs_Ti * static_cast<double>(d_fs_in); //(code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti) * static_cast<double>(d_fs_in);
d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in); d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples;
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples
d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples; d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
//################### PLL COMMANDS #################################################
//carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE // UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in)); CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
//remnant carrier phase [rad] //remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI); d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
// UPDATE CARRIER PHASE ACCUULATOR
//carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
//################### PLL COMMANDS #################################################
//carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
//################### DLL COMMANDS ################################################# //################### DLL COMMANDS #################################################
//code phase step (Code resampler phase increment per sample) [chips/sample] //code phase step (Code resampler phase increment per sample) [chips/sample]
@ -522,7 +508,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{ {
// fill buffer with prompt correlator output values // fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; // prompt
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
} }
else else
@ -554,7 +540,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real()); current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag()); current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in); current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
@ -573,7 +560,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real()); current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag()); current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in); current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
@ -587,7 +575,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
} }
current_synchro_data.System = {'G'}; current_synchro_data.System = {'G'};
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in); current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_length_samples + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(d_fs_in);
} }
//assign the GNURadio block output data //assign the GNURadio block output data
*out[0] = current_synchro_data; *out[0] = current_synchro_data;

View File

@ -119,7 +119,13 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime)
{ {
double dt; double dt;
dt = check_t(transmitTime - d_Toc); dt = check_t(transmitTime - d_Toc);
d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime);
for (int i = 0; i < 2; i++)
{
dt -= d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt);
}
d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt);
return d_satClkDrift; return d_satClkDrift;
} }
@ -174,7 +180,7 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
} }
void Gps_Ephemeris::satellitePosition(double transmitTime) double Gps_Ephemeris::satellitePosition(double transmitTime)
{ {
double tk; double tk;
double a; double a;
@ -194,13 +200,13 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
// Find satellite's position ---------------------------------------------- // Find satellite's position ----------------------------------------------
// Restore semi-major axis // Restore semi-major axis
a = d_sqrt_A*d_sqrt_A; a = d_sqrt_A * d_sqrt_A;
// Time from ephemeris reference epoch // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toc); tk = check_t(transmitTime - d_Toe);
// Computed mean motion // Computed mean motion
n0 = sqrt(GM / (a*a*a)); n0 = sqrt(GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; n = n0 + d_Delta_n;
@ -209,17 +215,17 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2*GPS_PI), (2*GPS_PI)); M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; E = M;
// --- Iteratively compute eccentric anomaly ---------------------------- // --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1; ii<20; ii++) for (int ii = 1; ii < 20; ii++)
{ {
E_old = E; E_old = E;
E = M + d_e_eccentricity * sin(E); E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2*GPS_PI); dE = fmod(E - E_old, 2.0 * GPS_PI);
if (fabs(dE) < 1e-12) if (fabs(dE) < 1e-12)
{ {
//Necessary precision is reached, exit from the loop //Necessary precision is reached, exit from the loop
@ -236,22 +242,22 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
phi = nu + d_OMEGA; phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2*GPS_PI)); phi = fmod((phi), (2.0 * GPS_PI));
// Correct argument of latitude // Correct argument of latitude
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi); u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi);
// Correct radius // Correct radius
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi); r = a * (1.0 - d_e_eccentricity*cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi);
// Correct inclination // Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi); i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi);
// Compute the angle between the ascending node and the Greenwich meridian // 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; Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2*GPS_PI), (2*GPS_PI)); Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -263,4 +269,14 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
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_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_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); d_satvel_Z = d_satpos_Y * sin(i);
// Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc);
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */
dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
return dtr_s;
} }

View File

@ -183,8 +183,9 @@ public:
/*! /*!
* \brief Compute the ECEF SV coordinates and ECEF velocity * \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200E) * Implementation of Table 20-IV (IS-GPS-200E)
* and compute the clock bias term including relativistic effect (return value)
*/ */
void satellitePosition(double transmitTime); double satellitePosition(double transmitTime);
/*! /*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction

View File

@ -212,7 +212,7 @@ int Obs_Gps_L1_System_Test::configure_receiver()
const int display_rate_ms = 500; const int display_rate_ms = 500;
const int output_rate_ms = 1000; const int output_rate_ms = 1000;
const int averaging_depth = 10; const int averaging_depth = 1;
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(sampling_rate_internal)); config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(sampling_rate_internal));
@ -285,8 +285,8 @@ int Obs_Gps_L1_System_Test::configure_receiver()
config->set_property("Acquisition_1C.tong_max_dwells", std::to_string(tong_max_dwells)); config->set_property("Acquisition_1C.tong_max_dwells", std::to_string(tong_max_dwells));
// Set Tracking // Set Tracking
config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking"); //config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking");
//config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking"); config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking");
config->set_property("Tracking_1C.item_type", "gr_complex"); config->set_property("Tracking_1C.item_type", "gr_complex");
config->set_property("Tracking_1C.if", std::to_string(zero)); config->set_property("Tracking_1C.if", std::to_string(zero));
config->set_property("Tracking_1C.dump", "false"); config->set_property("Tracking_1C.dump", "false");