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:
commit
382cbfdc2a
@ -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++;
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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
|
||||||
|
@ -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");
|
||||||
|
Loading…
Reference in New Issue
Block a user