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

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

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
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 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++;

69
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -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);
@ -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;
}
@ -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]
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
// 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;
@ -461,7 +460,6 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
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]
@ -476,41 +474,29 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
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;
// 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);
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);
//################### DLL COMMANDS #################################################
//code phase step (Code resampler phase increment per sample) [chips/sample]
@ -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;

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

@ -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;
@ -197,7 +203,7 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
a = d_sqrt_A * d_sqrt_A;
// Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toc);
tk = check_t(transmitTime - d_Toe);
// Computed mean motion
n0 = sqrt(GM / (a * a * a));
@ -209,7 +215,7 @@ 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;
@ -219,7 +225,7 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
{
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;
}

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

@ -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

6
src/tests/system-tests/obs_gps_l1_system_test.cc
View File

@ -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");