mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-30 17:03:15 +00:00
Code Issues Releases Wiki Activity

Fix cshort version of GPS_L1_CA_DLL_PLL_C_Aid_Tracking

Browse Source
This commit is contained in:
Carles Fernandez 2017-02-02 12:03:51 +01:00
parent 92f1f90935
commit 72fff7857c
2 changed files with 248 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

345
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -102,6 +102,8 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
{ {
// Telemetry bit synchronization message port input // Telemetry bit synchronization message port input
this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
this->set_msg_handler(pmt::mp("preamble_timestamp_s"),
boost::bind(&gps_l1_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index, this, _1));
this->message_port_register_out(pmt::mp("events")); this->message_port_register_out(pmt::mp("events"));
// initialize internal vars // initialize internal vars
d_dump = dump; d_dump = dump;
@ -112,12 +114,13 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
d_correlation_length_samples = static_cast<int>(d_vector_length); d_correlation_length_samples = static_cast<int>(d_vector_length);
// Initialize tracking ========================================== // Initialize tracking ==========================================
d_pll_bw_hz=pll_bw_hz; d_pll_bw_hz = pll_bw_hz;
d_dll_bw_hz=dll_bw_hz; d_dll_bw_hz = dll_bw_hz;
d_pll_bw_narrow_hz = pll_bw_narrow_hz; d_pll_bw_narrow_hz = pll_bw_narrow_hz;
d_dll_bw_narrow_hz = dll_bw_narrow_hz; d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2); d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2);
d_extend_correlation_ms = 1; // TODO!
//--- DLL variables -------------------------------------------------------- //--- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
@ -153,7 +156,8 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
d_rem_carrier_phase_rad = 0.0; d_rem_carrier_phase_rad = 0.0;
// sample synchronization // sample synchronization
d_sample_counter = 0; d_sample_counter = 0; //(from trk to tlm)
//d_sample_counter_seconds = 0; //d_sample_counter_seconds = 0;
d_acq_sample_stamp = 0; d_acq_sample_stamp = 0;
@ -180,11 +184,15 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
d_carrier_doppler_hz = 0.0; d_carrier_doppler_hz = 0.0;
d_acc_carrier_phase_cycles = 0.0; d_acc_carrier_phase_cycles = 0.0;
d_code_phase_samples = 0.0; d_code_phase_samples = 0.0;
d_rem_code_phase_integer_samples = 0;
d_pll_to_dll_assist_secs_Ti = 0.0; d_pll_to_dll_assist_secs_Ti = 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;
d_carrier_phase_step_rad = 0.0; d_carrier_phase_step_rad = 0.0;
d_code_error_filt_chips_s = 0.0;
d_code_error_filt_chips_Ti = 0.0;
d_preamble_timestamp_s = 0.0;
//set_min_output_buffer((long int)300); //set_min_output_buffer((long int)300);
} }
@ -268,12 +276,25 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::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_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;
} }
void gps_l1_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index(pmt::pmt_t msg)
{
//pmt::print(msg);
DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN);
if (d_enable_extended_integration == false) //avoid re-setting preamble indicator
{
d_preamble_timestamp_s = pmt::to_double(msg);
d_enable_extended_integration = true;
d_preamble_synchronized = false;
}
}
gps_l1_ca_dll_pll_c_aid_tracking_sc::~gps_l1_ca_dll_pll_c_aid_tracking_sc() gps_l1_ca_dll_pll_c_aid_tracking_sc::~gps_l1_ca_dll_pll_c_aid_tracking_sc()
{ {
@ -301,13 +322,13 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri
Gnss_Synchro current_synchro_data = Gnss_Synchro(); Gnss_Synchro current_synchro_data = Gnss_Synchro();
// process vars // process vars
double code_error_chips_Ti = 0.0; double d_code_error_chips_Ti = 0.0;
double code_error_filt_chips = 0.0; double code_error_filt_chips = 0.0;
double code_error_filt_secs_Ti = 0.0; double code_error_filt_secs_Ti = 0.0;
double CURRENT_INTEGRATION_TIME_S; double CURRENT_INTEGRATION_TIME_S;
double CORRECTED_INTEGRATION_TIME_S; double CORRECTED_INTEGRATION_TIME_S;
double dll_code_error_secs_Ti = 0.0; double dll_code_error_secs_Ti = 0.0;
double carr_phase_error_secs_Ti = 0.0; double d_carr_phase_error_secs_Ti = 0.0;
double old_d_rem_code_phase_samples; double old_d_rem_code_phase_samples;
if (d_enable_tracking == true) if (d_enable_tracking == true)
{ {
@ -323,138 +344,242 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri
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;
consume_each(samples_offset); //shift input to perform alignment with local replica 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; return 1;
} }
// ################# CARRIER WIPEOFF AND CORRELATORS ############################## // ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation // perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu_16sc.set_input_output_vectors(d_correlator_outs_16sc, in); multicorrelator_cpu_16sc.set_input_output_vectors(d_correlator_outs_16sc, in);
multicorrelator_cpu_16sc.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad, d_carrier_phase_step_rad, d_rem_code_phase_chips, d_code_phase_step_chips, d_correlation_length_samples); multicorrelator_cpu_16sc.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad,
d_carrier_phase_step_rad,
d_rem_code_phase_chips,
d_code_phase_step_chips,
d_correlation_length_samples);
// UPDATE INTEGRATION TIME // ####### coherent intergration extension
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in); // keep the last symbols
d_E_history.push_back(d_correlator_outs_16sc[0]); // save early output
d_P_history.push_back(d_correlator_outs_16sc[1]); // save prompt output
d_L_history.push_back(d_correlator_outs_16sc[2]); // save late output
// ################## PLL ########################################################## if (static_cast<int>(d_P_history.size()) > d_extend_correlation_ms)
// Update PLL discriminator [rads/Ti -> Secs/Ti]
carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())) / 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, carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / GPS_L1_FREQ_HZ;
// code Doppler frequency update
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
// ################## DLL ##########################################################
// DLL discriminator
code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex<float>(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag()), std::complex<float>(d_correlator_outs_16sc[2].real(),d_correlator_outs_16sc[2].imag())); //[chips/Ti] //early and late
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips_Ti); //input [chips/Ti] -> output [chips/second]
code_error_filt_secs_Ti = code_error_filt_chips*CURRENT_INTEGRATION_TIME_S/d_code_freq_chips; // [s/Ti]
// DLL code error estimation [s/Ti]
// TODO: PLL carrier aid to DLL is disabled. Re-enable it and measure performance
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_blk_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_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(K_blk_samples); //round to a discrete samples
old_d_rem_code_phase_samples = d_rem_code_phase_samples;
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_correlation_length_samples); //rounding error < 1 sample
// UPDATE REMNANT 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]
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));
// ####### CN0 ESTIMATION AND LOCK DETECTORS #######################################
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{ {
// fill buffer with prompt correlator output values d_E_history.pop_front();
d_Prompt_buffer[d_cn0_estimation_counter] = std::complex<float>(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag()); //prompt d_P_history.pop_front();
d_cn0_estimation_counter++; d_L_history.pop_front();
} }
else
bool enable_dll_pll;
if (d_enable_extended_integration == true)
{ {
d_cn0_estimation_counter = 0; long int symbol_diff = round(1000.0 * ((static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in) - d_preamble_timestamp_s));
// Code lock indicator if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0)
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
{ {
d_carrier_lock_fail_counter++; // compute coherent integration and enable tracking loop
// perform coherent integration using correlator output history
// std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
d_correlator_outs_16sc[0] = lv_cmake(0,0);
d_correlator_outs_16sc[1] = lv_cmake(0,0);
d_correlator_outs_16sc[2] = lv_cmake(0,0);
for (int n = 0; n < d_extend_correlation_ms; n++)
{
d_correlator_outs_16sc[0] += d_E_history.at(n);
d_correlator_outs_16sc[1] += d_P_history.at(n);
d_correlator_outs_16sc[2] += d_L_history.at(n);
}
if (d_preamble_synchronized == false)
{
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)
<< " 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;
}
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_extend_correlation_ms) * GPS_L1_CA_CODE_PERIOD;
enable_dll_pll = true;
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if(d_preamble_synchronized == true)
} {
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) // continue extended coherent correlation
{ // Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; double T_chip_seconds = 1.0 / d_code_freq_chips;
LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
d_carrier_lock_fail_counter = 0; int K_prn_samples = round(T_prn_samples);
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine 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); // 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_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI;
// disable tracking loop and inform telemetry decoder
enable_dll_pll = false;
}
else
{
// perform basic (1ms) correlation
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
enable_dll_pll = true;
}
} }
} }
else
{
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
enable_dll_pll = true;
}
// ########### Output the tracking data to navigation and PVT ########## if (enable_dll_pll == true)
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real()); {
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[1]).imag()); // ################## PLL ##########################################################
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) // Update PLL discriminator [rads/Ti -> Secs/Ti]
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in); d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())) / GPS_TWO_PI; //prompt output
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;
current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
// 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]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / GPS_L1_FREQ_HZ;
// code Doppler frequency update
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
// ################## DLL ##########################################################
// DLL discriminator
d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex<float>(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag()), std::complex<float>(d_correlator_outs_16sc[2].real(),d_correlator_outs_16sc[2].imag())); // [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_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]
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
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;
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);
//################### DLL COMMANDS #################################################
//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));
// ####### CN0 ESTIMATION AND LOCK DETECTORS #######################################
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = lv_cmake(static_cast<float>(d_correlator_outs_16sc[1].real()), static_cast<float>(d_correlator_outs_16sc[1].imag()) ); // prompt
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator( d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
{
d_carrier_lock_fail_counter++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[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_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;
current_synchro_data.Flag_valid_symbol_output = true;
if (d_preamble_synchronized == true)
{
current_synchro_data.correlation_length_ms = d_extend_correlation_ms;
}
else
{
current_synchro_data.correlation_length_ms = 1;
}
}
else
{
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[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_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;
}
} }
else else
{ {
for (int n = 0; n < d_n_correlator_taps; n++) for (int n = 0; n < d_n_correlator_taps; n++)
{ {
d_correlator_outs_16sc[n] = lv_16sc_t(0,0); d_correlator_outs_16sc[n] = lv_cmake(0,0);
} }
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_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);
*out[0] = current_synchro_data; current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(d_fs_in);
} }
*out[0] = current_synchro_data;
if(d_dump) if(d_dump)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
@ -487,11 +612,11 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
//PLL commands //PLL commands
d_dump_file.write(reinterpret_cast<char*>(&carr_phase_error_secs_Ti), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&d_carr_phase_error_secs_Ti), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
//DLL commands //DLL commands
d_dump_file.write(reinterpret_cast<char*>(&code_error_chips_Ti), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&d_code_error_chips_Ti), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
// CN0 and carrier lock test // CN0 and carrier lock test

13
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.h
View File

@ -135,6 +135,7 @@ private:
double d_rem_code_phase_samples; double d_rem_code_phase_samples;
double d_rem_code_phase_chips; double d_rem_code_phase_chips;
double d_rem_carrier_phase_rad; double d_rem_carrier_phase_rad;
int d_rem_code_phase_integer_samples;
// PLL and DLL filter library // PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_DLL_filter d_code_loop_filter;
@ -156,6 +157,18 @@ private:
double d_acc_carrier_phase_cycles; double d_acc_carrier_phase_cycles;
double d_code_phase_samples; double d_code_phase_samples;
double d_pll_to_dll_assist_secs_Ti; double d_pll_to_dll_assist_secs_Ti;
double d_preamble_timestamp_s;
int d_extend_correlation_ms;
bool d_enable_extended_integration;
bool d_preamble_synchronized;
double d_code_error_filt_chips_s;
double d_code_error_filt_chips_Ti;
void msg_handler_preamble_index(pmt::pmt_t msg);
// symbol history to detect bit transition
std::deque<lv_16sc_t> d_E_history;
std::deque<lv_16sc_t> d_P_history;
std::deque<lv_16sc_t> d_L_history;
//Integration period in samples //Integration period in samples
int d_correlation_length_samples; int d_correlation_length_samples;