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

Fix L2C tracking

Browse Source
This commit is contained in:
Carles Fernandez 2017-01-29 20:51:58 +01:00
parent bf34702f9d
commit f6be75ecf7
2 changed files with 30 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
View File

@ -179,7 +179,6 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc(
d_carrier_doppler_hz = 0.0; d_carrier_doppler_hz = 0.0;
d_acc_carrier_phase_rad = 0.0; d_acc_carrier_phase_rad = 0.0;
d_code_phase_samples = 0.0; d_code_phase_samples = 0.0;
d_acc_code_phase_secs = 0.0;
d_rem_code_phase_chips = 0.0; d_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 0.0; d_code_phase_step_chips = 0.0;
@ -203,7 +202,7 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length; acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in); acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect // Doppler effect
// Fd=(C/(C+Vr))*F // Fd=(C/(C+Vr))*F
double radial_velocity = (GPS_L2_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L2_FREQ_HZ; double radial_velocity = (GPS_L2_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L2_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler // new chip and prn sequence periods based on acq Doppler
@ -253,7 +252,6 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
d_rem_carr_phase_rad = 0; d_rem_carr_phase_rad = 0;
d_rem_code_phase_chips = 0.0; d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0; d_acc_carrier_phase_rad = 0;
d_acc_code_phase_secs = 0;
d_code_phase_samples = d_acq_code_phase_samples; d_code_phase_samples = d_acq_code_phase_samples;
@ -318,8 +316,11 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
acq_trk_shif_correction_samples = -fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples)); acq_trk_shif_correction_samples = -fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);//+(1.5*(d_fs_in/GPS_L2_M_CODE_RATE_HZ))); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);//+(1.5*(d_fs_in/GPS_L2_M_CODE_RATE_HZ)));
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 = d_sample_counter + samples_offset; //count for the processed samples d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
*out[0] = current_synchro_data;
d_pull_in = false; d_pull_in = false;
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;
@ -343,45 +344,39 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz;
// New code Doppler frequency estimation // New code Doppler frequency estimation
d_code_freq_chips = GPS_L2_M_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L2_M_CODE_RATE_HZ) / GPS_L2_FREQ_HZ); d_code_freq_chips = GPS_L2_M_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L2_M_CODE_RATE_HZ) / GPS_L2_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad -= GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L2_TWO_PI);
// ################## DLL ########################################################## // ################## DLL ##########################################################
// DLL discriminator // DLL discriminator
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti]
// Code discriminator filter // Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator //Code phase accumulator
double code_error_filt_secs; double code_error_filt_secs = (GPS_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_CODE_RATE_HZ; //[seconds]
code_error_filt_secs = (GPS_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_CODE_RATE_HZ; //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
// ################## 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_blk_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.0 / d_code_freq_chips; double T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * GPS_L2_M_CODE_LENGTH_CHIPS; double T_prn_seconds = T_chip_seconds * GPS_L2_M_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_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in); double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples
//################### PLL COMMANDS ################################################# //################### PLL COMMANDS #################################################
//carrier phase step (NCO phase increment per sample) [rads/sample] // carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in); d_carrier_phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L2_TWO_PI);
// carrier phase accumulator
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples;
//################### 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]
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_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample
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_code_freq_chips * (d_rem_code_phase_samples / static_cast<double>(d_fs_in));
// ####### CN0 ESTIMATION AND LOCK DETECTORS ###### // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES) if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES)
@ -418,17 +413,13 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
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
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??) current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter + d_current_prn_length_samples) + 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_rem_code_phase_samples) / static_cast<double>(d_fs_in);
//compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
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;
current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms=20; current_synchro_data.correlation_length_ms = 20;
} }
else else
@ -437,7 +428,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
{ {
d_correlator_outs[n] = gr_complex(0,0); d_correlator_outs[n] = gr_complex(0,0);
} }
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_current_prn_length_samples) + static_cast<double>(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;
@ -497,8 +488,8 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
} }
} }
consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
d_sample_counter += d_current_prn_length_samples; //count for the processed samples d_sample_counter += d_current_prn_length_samples; // count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false
} }

5
src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h
View File

@ -136,12 +136,11 @@ private:
double d_carrier_phase_step_rad; double d_carrier_phase_step_rad;
double d_acc_carrier_phase_rad; double d_acc_carrier_phase_rad;
double d_code_phase_samples; double d_code_phase_samples;
double d_acc_code_phase_secs;
//PRN period in samples // PRN period in samples
int d_current_prn_length_samples; int d_current_prn_length_samples;
//processing samples counters // processing samples counters
unsigned long int d_sample_counter; unsigned long int d_sample_counter;
unsigned long int d_acq_sample_stamp; unsigned long int d_acq_sample_stamp;