mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-06-24 05:53:16 +00:00
Merge branch 'rinex_fix' of https://github.com/gnss-sdr/gnss-sdr into
rinex_fix # Conflicts: # src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc # src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
This commit is contained in:
Carles Fernandez
commit
886e3d24d0
|
|
@ -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 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;
|
||||
|
||||
// 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;
|
||||
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(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X;
|
||||
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
|
||||
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_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
|
||||
valid_obs++;
|
||||
|
|
|
|||
|
|
@ -238,7 +238,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
|
|||
double T_prn_mod_samples;
|
||||
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);
|
||||
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_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
|
||||
d_pull_in = true;
|
||||
d_enable_tracking = true;
|
||||
d_enable_extended_integration=false;
|
||||
d_preamble_synchronized=false;
|
||||
d_enable_extended_integration = false;
|
||||
d_preamble_synchronized = false;
|
||||
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
|
||||
<< " Code Phase correction [samples]=" << delay_correction_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 CURRENT_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)
|
||||
{
|
||||
// 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_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);
|
||||
|
||||
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_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
|
||||
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
|
||||
// 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[1] = 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_carrier_loop_filter.set_params(10.0, d_pll_bw_narrow_hz,2);
|
||||
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
|
||||
<<" 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)
|
||||
{
|
||||
// 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
|
||||
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_samples = T_prn_seconds * static_cast<double>(d_fs_in);
|
||||
int K_prn_samples = round(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_integer_samples = round(d_rem_code_phase_samples);
|
||||
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete 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;
|
||||
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);
|
||||
//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_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
|
||||
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
|
||||
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 ##########################################################
|
||||
// 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
|
||||
// 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]
|
||||
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]
|
||||
|
|
@ -471,46 +469,34 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
|
|||
|
||||
// ################## DLL ##########################################################
|
||||
// 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
|
||||
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;
|
||||
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 #######################
|
||||
|
||||
// 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
|
||||
T_chip_seconds = 1 / d_code_freq_chips;
|
||||
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
|
||||
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
|
||||
K_prn_samples = round(T_prn_samples);
|
||||
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_samples = T_prn_seconds * static_cast<double>(d_fs_in);
|
||||
double K_prn_samples = round(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 - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
|
||||
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; //round to a discrete 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_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;
|
||||
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
|
||||
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
|
||||
//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);
|
||||
// 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 #################################################
|
||||
//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)
|
||||
{
|
||||
// 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++;
|
||||
}
|
||||
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_Q = static_cast<double>((d_correlator_outs[1]).imag());
|
||||
// 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_Doppler_hz = d_carrier_doppler_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_Q = static_cast<double>((d_correlator_outs[1]).imag());
|
||||
// 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_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler
|
||||
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.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
|
||||
*out[0] = current_synchro_data;
|
||||
|
|
|
|||
|
|
@ -119,7 +119,13 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime)
|
|||
{
|
||||
double dt;
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -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 a;
|
||||
|
|
@ -194,13 +200,13 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
|
|||
// Find satellite's position ----------------------------------------------
|
||||
|
||||
// Restore semi-major axis
|
||||
a = d_sqrt_A*d_sqrt_A;
|
||||
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));
|
||||
n0 = sqrt(GM / (a * a * a));
|
||||
|
||||
// Corrected mean motion
|
||||
n = n0 + d_Delta_n;
|
||||
|
|
@ -209,17 +215,17 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
|
|||
M = d_M_0 + n * tk;
|
||||
|
||||
// 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
|
||||
E = M;
|
||||
|
||||
// --- Iteratively compute eccentric anomaly ----------------------------
|
||||
for (int ii = 1; ii<20; ii++)
|
||||
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);
|
||||
dE = fmod(E - E_old, 2.0 * GPS_PI);
|
||||
if (fabs(dE) < 1e-12)
|
||||
{
|
||||
//Necessary precision is reached, exit from the loop
|
||||
|
|
@ -236,22 +242,22 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
|
|||
phi = nu + d_OMEGA;
|
||||
|
||||
// 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
|
||||
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
|
||||
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
|
||||
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
|
||||
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe;
|
||||
|
||||
// Reduce to between 0 and 2*pi rad
|
||||
Omega = fmod((Omega + 2*GPS_PI), (2*GPS_PI));
|
||||
Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI));
|
||||
|
||||
// --- Compute satellite coordinates in Earth-fixed coordinates
|
||||
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_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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -183,8 +183,9 @@ public:
|
|||
/*!
|
||||
* \brief Compute the ECEF SV coordinates and ECEF velocity
|
||||
* 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
|
||||
|
|
|
|||
|
|
@ -212,7 +212,7 @@ int Obs_Gps_L1_System_Test::configure_receiver()
|
|||
|
||||
const int display_rate_ms = 500;
|
||||
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));
|
||||
|
||||
|
|
@ -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));
|
||||
|
||||
// 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_C_Aid_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.item_type", "gr_complex");
|
||||
config->set_property("Tracking_1C.if", std::to_string(zero));
|
||||
config->set_property("Tracking_1C.dump", "false");
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user