[hamradio-commits] [gnss-sdr] 264/303: Fix cshort version of GPS_L1_CA_DLL_PLL_C_Aid_Tracking
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:36:06 UTC 2017
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch master
in repository gnss-sdr.
commit 72fff7857c6b479439f4d41c58a1da553ebb6d0c
Author: Carles Fernandez <carlesfernandez at gmail.com>
Date: Thu Feb 2 12:03:51 2017 +0100
Fix cshort version of GPS_L1_CA_DLL_PLL_C_Aid_Tracking
---
.../gps_l1_ca_dll_pll_c_aid_tracking_sc.cc | 347 ++++++++++++++-------
.../gps_l1_ca_dll_pll_c_aid_tracking_sc.h | 13 +
2 files changed, 249 insertions(+), 111 deletions(-)
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
index 304d65e..027df53 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
@@ -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
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"));
// initialize internal vars
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);
// Initialize tracking ==========================================
- d_pll_bw_hz=pll_bw_hz;
- d_dll_bw_hz=dll_bw_hz;
+ d_pll_bw_hz = pll_bw_hz;
+ d_dll_bw_hz = dll_bw_hz;
d_pll_bw_narrow_hz = pll_bw_narrow_hz;
d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2);
+ d_extend_correlation_ms = 1; // TODO!
//--- DLL variables --------------------------------------------------------
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;
// sample synchronization
- d_sample_counter = 0;
+ d_sample_counter = 0; //(from trk to tlm)
+
//d_sample_counter_seconds = 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_acc_carrier_phase_cycles = 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_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 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);
}
@@ -268,12 +276,25 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
+ d_enable_extended_integration = false;
+ d_preamble_synchronized = false;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
}
+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()
{
@@ -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();
// 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_secs_Ti = 0.0;
double CURRENT_INTEGRATION_TIME_S;
double CORRECTED_INTEGRATION_TIME_S;
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;
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));
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_sample_counter += samples_offset; // count for the processed samples
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;
}
-
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// 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.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
- CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
-
- // ################## PLL ##########################################################
- // 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)
+ 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);
+
+ // ####### coherent intergration extension
+ // 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
+
+ if (static_cast<int>(d_P_history.size()) > d_extend_correlation_ms)
{
- // fill buffer with prompt correlator output values
- d_Prompt_buffer[d_cn0_estimation_counter] = std::complex<float>(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag()); //prompt
- d_cn0_estimation_counter++;
+ d_E_history.pop_front();
+ d_P_history.pop_front();
+ d_L_history.pop_front();
+ }
+
+ bool enable_dll_pll;
+ if (d_enable_extended_integration == true)
+ {
+ 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));
+ if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0)
+ {
+ // 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
+ {
+ if(d_preamble_synchronized == true)
+ {
+ // continue extended coherent correlation
+ // Compute the next buffer length based on the 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);
+ 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); // 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
{
- 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)
+ // UPDATE INTEGRATION TIME
+ CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
+ enable_dll_pll = true;
+ }
+
+ if (enable_dll_pll == true)
+ {
+ // ################## PLL ##########################################################
+ // Update PLL discriminator [rads/Ti -> Secs/Ti]
+ 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
+
+ // 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)
{
- d_carrier_lock_fail_counter++;
+ // 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
{
- if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
+ 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
+ }
}
- if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
+ // ########### 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
{
- 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
+ current_synchro_data.correlation_length_ms = 1;
}
}
-
- // ########### 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) + old_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;
- current_synchro_data.correlation_length_ms = 1;
- *out[0] = current_synchro_data;
-
+ 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
{
-
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.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
- *out[0] = current_synchro_data;
+ 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);
}
-
+ *out[0] = current_synchro_data;
if(d_dump)
{
// 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));
//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));
//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));
// CN0 and carrier lock test
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.h
index bd0ef5e..ec45b3e 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.h
@@ -135,6 +135,7 @@ private:
double d_rem_code_phase_samples;
double d_rem_code_phase_chips;
double d_rem_carrier_phase_rad;
+ int d_rem_code_phase_integer_samples;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
@@ -156,6 +157,18 @@ private:
double d_acc_carrier_phase_cycles;
double d_code_phase_samples;
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
int d_correlation_length_samples;
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list