From 72fff7857c6b479439f4d41c58a1da553ebb6d0c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 2 Feb 2017 12:03:51 +0100 Subject: [PATCH] Fix cshort version of GPS_L1_CA_DLL_PLL_C_Aid_Tracking --- .../gps_l1_ca_dll_pll_c_aid_tracking_sc.cc | 345 ++++++++++++------ .../gps_l1_ca_dll_pll_c_aid_tracking_sc.h | 13 + 2 files changed, 248 insertions(+), 110 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 304d65ee2..027df538b 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(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(acq_to_trk_delay_samples), static_cast(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(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(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); + 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(d_correlation_length_samples) / static_cast(d_fs_in); + // ####### 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 - // ################## PLL ########################################################## - // Update PLL discriminator [rads/Ti -> Secs/Ti] - carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex(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(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag()), std::complex(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(d_fs_in); - K_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast(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(d_correlation_length_samples); //rounding error < 1 sample - - // UPDATE REMNANT CARRIER PHASE - CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(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(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(d_fs_in); - //remnant code phase [chips] - d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); - - // ####### CN0 ESTIMATION AND LOCK DETECTORS ####################################### - if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + if (static_cast(d_P_history.size()) > d_extend_correlation_ms) { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = std::complex(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(); } - else + + bool enable_dll_pll; + if (d_enable_extended_integration == true) { - 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) + long int symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); + if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) { - d_carrier_lock_fail_counter++; + // compute coherent integration and enable tracking loop + // perform coherent integration using correlator output history + // std::cout<<"##### RESET COHERENT INTEGRATION ####"<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(d_extend_correlation_ms) * GPS_L1_CA_CODE_PERIOD; + enable_dll_pll = true; } 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_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(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(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(d_fs_in)); + d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast(d_correlation_length_samples), GPS_TWO_PI); + + // UPDATE ACCUMULATED CARRIER PHASE + CORRECTED_INTEGRATION_TIME_S = (static_cast(d_correlation_length_samples) / static_cast(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(d_correlation_length_samples) / static_cast(d_fs_in); + enable_dll_pll = true; + } } } + else + { + // UPDATE INTEGRATION TIME + CURRENT_INTEGRATION_TIME_S = static_cast(d_correlation_length_samples) / static_cast(d_fs_in); + enable_dll_pll = true; + } - // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); - current_synchro_data.Prompt_Q = static_cast((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(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast(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; + 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(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(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag()), std::complex(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(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(d_fs_in); //(code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti) * static_cast(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(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(d_correlation_length_samples) / static_cast(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(d_fs_in); + //remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(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(d_correlator_outs_16sc[1].real()), static_cast(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((d_correlator_outs_16sc[1]).real()); + current_synchro_data.Prompt_Q = static_cast((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(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); + current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(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((d_correlator_outs_16sc[1]).real()); + current_synchro_data.Prompt_Q = static_cast((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(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); + current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(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(d_sample_counter) / static_cast(d_fs_in); - *out[0] = current_synchro_data; + current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(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(&d_code_freq_chips), sizeof(double)); //PLL commands - d_dump_file.write(reinterpret_cast(&carr_phase_error_secs_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carr_phase_error_secs_Ti), sizeof(double)); d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_error_chips_Ti), sizeof(double)); d_dump_file.write(reinterpret_cast(&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 bd0ef5e1c..ec45b3eff 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 d_E_history; + std::deque d_P_history; + std::deque d_L_history; //Integration period in samples int d_correlation_length_samples;