mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-09-29 23:41:05 +00:00
immediate call to state 2 (Wide tracking and symbol synchronization) removed from pull-in state.
This commit is contained in:
Marc Majoral
parent
91f509abf9
commit
824dcebec2
@ -33,7 +33,6 @@
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "dll_pll_veml_tracking_fpga.h"
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "GPS_L2C.h"
|
||||
@ -54,6 +53,7 @@
|
||||
#include <pmt/pmt_sugar.h> // for mp
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <algorithm>
|
||||
//#include <chrono>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <cstdlib> // for abs, size_t
|
||||
@ -61,7 +61,6 @@
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
|
||||
|
||||
dll_pll_veml_tracking_fpga_sptr dll_pll_veml_make_tracking_fpga(const Dll_Pll_Conf_Fpga &conf_)
|
||||
{
|
||||
return dll_pll_veml_tracking_fpga_sptr(new dll_pll_veml_tracking_fpga(conf_));
|
||||
@ -272,7 +271,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &
|
||||
|
||||
// Initialize tracking ==========================================
|
||||
d_code_loop_filter = Tracking_loop_filter(d_code_period, trk_parameters.dll_bw_hz, trk_parameters.dll_filter_order, false);
|
||||
printf("trk_parameters.fll_bw_hz = %f trk_parameters.pll_bw_hz = %f trk_parameters.pll_filter_order = %d\n", trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order);
|
||||
//printf("trk_parameters.fll_bw_hz = %f trk_parameters.pll_bw_hz = %f trk_parameters.pll_filter_order = %d\n", trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order);
|
||||
d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order);
|
||||
|
||||
if (d_veml)
|
||||
@ -1382,7 +1381,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
|
||||
*out[0] = *d_acquisition_gnss_synchro;
|
||||
usleep(1000);
|
||||
return 1;
|
||||
break;
|
||||
//break;
|
||||
}
|
||||
case 1: // Pull-in
|
||||
{
|
||||
@ -1453,12 +1452,286 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
|
||||
|
||||
// don't leave the HW module blocking the signal path before the first sample arrives
|
||||
// start the first tracking process
|
||||
run_state_2(current_synchro_data);
|
||||
break;
|
||||
//run_state_2(current_synchro_data);
|
||||
*out[0] = *d_acquisition_gnss_synchro;
|
||||
return 1;
|
||||
//break;
|
||||
}
|
||||
case 2: // Wide tracking and symbol synchronization
|
||||
{
|
||||
run_state_2(current_synchro_data);
|
||||
d_sample_counter = d_sample_counter_next;
|
||||
d_sample_counter_next = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
|
||||
|
||||
// auto start = std::chrono::system_clock::now();
|
||||
do_correlation_step();
|
||||
// auto end = std::chrono::system_clock::now();
|
||||
// std::chrono::duration<double> elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 0: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
// Save single correlation step variables
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
if (d_veml)
|
||||
{
|
||||
d_VE_accu = *d_Very_Early;
|
||||
d_VL_accu = *d_Very_Late;
|
||||
}
|
||||
d_E_accu = *d_Early;
|
||||
d_P_accu = *d_Prompt;
|
||||
d_L_accu = *d_Late;
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 1: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
// Check lock status
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
if (!cn0_and_tracking_lock_status(d_code_period))
|
||||
{
|
||||
clear_tracking_vars();
|
||||
d_state = 0; // loss-of-lock detected
|
||||
}
|
||||
else
|
||||
{
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 2: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
bool next_state = false;
|
||||
// Perform DLL/PLL tracking loop computations. Costas Loop enabled
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
run_dll_pll();
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 3: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
update_tracking_vars();
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 4: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
// enable write dump file this cycle (valid DLL/PLL cycle)
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
log_data(false);
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 5: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
if (d_secondary)
|
||||
{
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
// ####### SECONDARY CODE LOCK #####
|
||||
d_Prompt_circular_buffer.push_back(*d_Prompt);
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 6: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
//d_Prompt_buffer_deque.push_back(*d_Prompt);
|
||||
//if (d_Prompt_buffer_deque.size() == d_secondary_code_length)
|
||||
if (d_Prompt_circular_buffer.size() == d_secondary_code_length)
|
||||
{
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
next_state = acquire_secondary();
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 7: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
if (next_state)
|
||||
{
|
||||
LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
}
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 8: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
//d_Prompt_buffer_deque.pop_front();
|
||||
}
|
||||
}
|
||||
else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change
|
||||
{
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
float current_tracking_time_s = static_cast<float>(d_sample_counter - d_absolute_samples_offset) / trk_parameters.fs_in;
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 6b: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
if (current_tracking_time_s > 10)
|
||||
{
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
d_symbol_history.push_back(d_Prompt->real());
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 7b: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
//******* preamble correlation ********
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
int32_t corr_value = 0;
|
||||
if ((d_symbol_history.size() == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync))
|
||||
{
|
||||
int i = 0;
|
||||
for (const auto &iter : d_symbol_history)
|
||||
{
|
||||
if (iter < 0.0) // symbols clipping
|
||||
{
|
||||
corr_value -= d_preambles_symbols[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_value += d_preambles_symbols[i];
|
||||
}
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 8b: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
if (corr_value == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
|
||||
{
|
||||
LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
next_state = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
next_state = false;
|
||||
}
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 9b: " << elapsed_seconds.count() << "s\n";
|
||||
}
|
||||
else
|
||||
{
|
||||
next_state = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
next_state = true;
|
||||
}
|
||||
|
||||
// start = std::chrono::system_clock::now();
|
||||
|
||||
// ########### Output the tracking results to Telemetry block ##########
|
||||
if (interchange_iq)
|
||||
{
|
||||
if (trk_parameters.track_pilot)
|
||||
{
|
||||
// Note that data and pilot components are in quadrature. I and Q are interchanged
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).imag());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).real());
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).imag());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).real());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (trk_parameters.track_pilot)
|
||||
{
|
||||
// Note that data and pilot components are in quadrature. I and Q are interchanged
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).imag());
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
|
||||
}
|
||||
}
|
||||
|
||||
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
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 = d_correlation_length_ms;
|
||||
|
||||
// end = std::chrono::system_clock::now();
|
||||
// elapsed_seconds = end - start;
|
||||
// std::cout << "elapsed time 10: " << elapsed_seconds.count() << "s\n";
|
||||
|
||||
if (next_state)
|
||||
{ // reset extended correlator
|
||||
d_VE_accu = gr_complex(0.0, 0.0);
|
||||
d_E_accu = gr_complex(0.0, 0.0);
|
||||
d_P_accu = gr_complex(0.0, 0.0);
|
||||
d_L_accu = gr_complex(0.0, 0.0);
|
||||
d_VL_accu = gr_complex(0.0, 0.0);
|
||||
d_Prompt_circular_buffer.clear();
|
||||
d_current_symbol = 0;
|
||||
//d_Prompt_buffer_deque.clear();
|
||||
|
||||
if (d_enable_extended_integration)
|
||||
{
|
||||
// UPDATE INTEGRATION TIME
|
||||
d_extend_correlation_symbols_count = 0;
|
||||
d_current_correlation_time_s = static_cast<float>(trk_parameters.extend_correlation_symbols) * static_cast<float>(d_code_period);
|
||||
|
||||
d_state = 3; // next state is the extended correlator integrator
|
||||
LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
|
||||
<< d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN);
|
||||
std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
|
||||
<< d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
// Set narrow taps delay values [chips]
|
||||
d_code_loop_filter.set_update_interval(d_current_correlation_time_s);
|
||||
d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz);
|
||||
d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order);
|
||||
if (d_veml)
|
||||
{
|
||||
d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
}
|
||||
else
|
||||
{
|
||||
d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_state = 4;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 3: // coherent integration (correlation time extension)
|
||||
@ -1600,183 +1873,183 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
|
||||
}
|
||||
|
||||
|
||||
void dll_pll_veml_tracking_fpga::run_state_2(Gnss_Synchro ¤t_synchro_data)
|
||||
{
|
||||
d_sample_counter = d_sample_counter_next;
|
||||
d_sample_counter_next = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
|
||||
|
||||
do_correlation_step();
|
||||
// Save single correlation step variables
|
||||
if (d_veml)
|
||||
{
|
||||
d_VE_accu = *d_Very_Early;
|
||||
d_VL_accu = *d_Very_Late;
|
||||
}
|
||||
d_E_accu = *d_Early;
|
||||
d_P_accu = *d_Prompt;
|
||||
d_L_accu = *d_Late;
|
||||
|
||||
// Check lock status
|
||||
if (!cn0_and_tracking_lock_status(d_code_period))
|
||||
{
|
||||
clear_tracking_vars();
|
||||
d_state = 0; // loss-of-lock detected
|
||||
}
|
||||
else
|
||||
{
|
||||
bool next_state = false;
|
||||
// Perform DLL/PLL tracking loop computations. Costas Loop enabled
|
||||
run_dll_pll();
|
||||
update_tracking_vars();
|
||||
|
||||
// enable write dump file this cycle (valid DLL/PLL cycle)
|
||||
log_data(false);
|
||||
if (d_secondary)
|
||||
{
|
||||
// ####### SECONDARY CODE LOCK #####
|
||||
d_Prompt_circular_buffer.push_back(*d_Prompt);
|
||||
//d_Prompt_buffer_deque.push_back(*d_Prompt);
|
||||
//if (d_Prompt_buffer_deque.size() == d_secondary_code_length)
|
||||
if (d_Prompt_circular_buffer.size() == d_secondary_code_length)
|
||||
{
|
||||
next_state = acquire_secondary();
|
||||
if (next_state)
|
||||
{
|
||||
LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
}
|
||||
//d_Prompt_buffer_deque.pop_front();
|
||||
}
|
||||
}
|
||||
else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change
|
||||
{
|
||||
float current_tracking_time_s = static_cast<float>(d_sample_counter - d_absolute_samples_offset) / trk_parameters.fs_in;
|
||||
if (current_tracking_time_s > 10)
|
||||
{
|
||||
d_symbol_history.push_back(d_Prompt->real());
|
||||
//******* preamble correlation ********
|
||||
int32_t corr_value = 0;
|
||||
if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) // and (d_make_correlation or !d_flag_frame_sync))
|
||||
{
|
||||
int i = 0;
|
||||
for (const auto &iter : d_symbol_history)
|
||||
{
|
||||
if (iter < 0.0) // symbols clipping
|
||||
{
|
||||
corr_value -= d_preambles_symbols[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_value += d_preambles_symbols[i];
|
||||
}
|
||||
i++;
|
||||
}
|
||||
}
|
||||
if (corr_value == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
|
||||
{
|
||||
LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
next_state = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
next_state = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
next_state = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
next_state = true;
|
||||
}
|
||||
|
||||
// ########### Output the tracking results to Telemetry block ##########
|
||||
if (interchange_iq)
|
||||
{
|
||||
if (trk_parameters.track_pilot)
|
||||
{
|
||||
// Note that data and pilot components are in quadrature. I and Q are interchanged
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).imag());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).real());
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).imag());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).real());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (trk_parameters.track_pilot)
|
||||
{
|
||||
// Note that data and pilot components are in quadrature. I and Q are interchanged
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).imag());
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
|
||||
}
|
||||
}
|
||||
|
||||
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
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 = d_correlation_length_ms;
|
||||
|
||||
if (next_state)
|
||||
{ // reset extended correlator
|
||||
d_VE_accu = gr_complex(0.0, 0.0);
|
||||
d_E_accu = gr_complex(0.0, 0.0);
|
||||
d_P_accu = gr_complex(0.0, 0.0);
|
||||
d_L_accu = gr_complex(0.0, 0.0);
|
||||
d_VL_accu = gr_complex(0.0, 0.0);
|
||||
d_Prompt_circular_buffer.clear();
|
||||
d_current_symbol = 0;
|
||||
//d_Prompt_buffer_deque.clear();
|
||||
|
||||
if (d_enable_extended_integration)
|
||||
{
|
||||
// UPDATE INTEGRATION TIME
|
||||
d_extend_correlation_symbols_count = 0;
|
||||
d_current_correlation_time_s = static_cast<float>(trk_parameters.extend_correlation_symbols) * static_cast<float>(d_code_period);
|
||||
|
||||
d_state = 3; // next state is the extended correlator integrator
|
||||
LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
|
||||
<< d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN);
|
||||
std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
|
||||
<< d_channel
|
||||
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
// Set narrow taps delay values [chips]
|
||||
d_code_loop_filter.set_update_interval(d_current_correlation_time_s);
|
||||
d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz);
|
||||
d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order);
|
||||
if (d_veml)
|
||||
{
|
||||
d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
}
|
||||
else
|
||||
{
|
||||
d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_state = 4;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//void dll_pll_veml_tracking_fpga::run_state_2(Gnss_Synchro ¤t_synchro_data)
|
||||
//{
|
||||
// d_sample_counter = d_sample_counter_next;
|
||||
// d_sample_counter_next = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
|
||||
//
|
||||
// do_correlation_step();
|
||||
// // Save single correlation step variables
|
||||
// if (d_veml)
|
||||
// {
|
||||
// d_VE_accu = *d_Very_Early;
|
||||
// d_VL_accu = *d_Very_Late;
|
||||
// }
|
||||
// d_E_accu = *d_Early;
|
||||
// d_P_accu = *d_Prompt;
|
||||
// d_L_accu = *d_Late;
|
||||
//
|
||||
// // Check lock status
|
||||
// if (!cn0_and_tracking_lock_status(d_code_period))
|
||||
// {
|
||||
// clear_tracking_vars();
|
||||
// d_state = 0; // loss-of-lock detected
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// bool next_state = false;
|
||||
// // Perform DLL/PLL tracking loop computations. Costas Loop enabled
|
||||
// run_dll_pll();
|
||||
// update_tracking_vars();
|
||||
//
|
||||
// // enable write dump file this cycle (valid DLL/PLL cycle)
|
||||
// log_data(false);
|
||||
// if (d_secondary)
|
||||
// {
|
||||
// // ####### SECONDARY CODE LOCK #####
|
||||
// d_Prompt_circular_buffer.push_back(*d_Prompt);
|
||||
// //d_Prompt_buffer_deque.push_back(*d_Prompt);
|
||||
// //if (d_Prompt_buffer_deque.size() == d_secondary_code_length)
|
||||
// if (d_Prompt_circular_buffer.size() == d_secondary_code_length)
|
||||
// {
|
||||
// next_state = acquire_secondary();
|
||||
// if (next_state)
|
||||
// {
|
||||
// LOG(INFO) << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel
|
||||
// << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
// std::cout << systemName << " " << signal_pretty_name << " secondary code locked in channel " << d_channel
|
||||
// << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
// }
|
||||
// //d_Prompt_buffer_deque.pop_front();
|
||||
// }
|
||||
// }
|
||||
// else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change
|
||||
// {
|
||||
// float current_tracking_time_s = static_cast<float>(d_sample_counter - d_absolute_samples_offset) / trk_parameters.fs_in;
|
||||
// if (current_tracking_time_s > 10)
|
||||
// {
|
||||
// d_symbol_history.push_back(d_Prompt->real());
|
||||
// //******* preamble correlation ********
|
||||
// int32_t corr_value = 0;
|
||||
// if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) // and (d_make_correlation or !d_flag_frame_sync))
|
||||
// {
|
||||
// int i = 0;
|
||||
// for (const auto &iter : d_symbol_history)
|
||||
// {
|
||||
// if (iter < 0.0) // symbols clipping
|
||||
// {
|
||||
// corr_value -= d_preambles_symbols[i];
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// corr_value += d_preambles_symbols[i];
|
||||
// }
|
||||
// i++;
|
||||
// }
|
||||
// }
|
||||
// if (corr_value == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
|
||||
// {
|
||||
// LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel
|
||||
// << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
// next_state = true;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// next_state = false;
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// next_state = false;
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// next_state = true;
|
||||
// }
|
||||
//
|
||||
// // ########### Output the tracking results to Telemetry block ##########
|
||||
// if (interchange_iq)
|
||||
// {
|
||||
// if (trk_parameters.track_pilot)
|
||||
// {
|
||||
// // Note that data and pilot components are in quadrature. I and Q are interchanged
|
||||
// current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).imag());
|
||||
// current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).real());
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).imag());
|
||||
// current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).real());
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// if (trk_parameters.track_pilot)
|
||||
// {
|
||||
// // Note that data and pilot components are in quadrature. I and Q are interchanged
|
||||
// current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).real());
|
||||
// current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).imag());
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
|
||||
// current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
|
||||
// current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
// 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 = d_correlation_length_ms;
|
||||
//
|
||||
// if (next_state)
|
||||
// { // reset extended correlator
|
||||
// d_VE_accu = gr_complex(0.0, 0.0);
|
||||
// d_E_accu = gr_complex(0.0, 0.0);
|
||||
// d_P_accu = gr_complex(0.0, 0.0);
|
||||
// d_L_accu = gr_complex(0.0, 0.0);
|
||||
// d_VL_accu = gr_complex(0.0, 0.0);
|
||||
// d_Prompt_circular_buffer.clear();
|
||||
// d_current_symbol = 0;
|
||||
// //d_Prompt_buffer_deque.clear();
|
||||
//
|
||||
// if (d_enable_extended_integration)
|
||||
// {
|
||||
// // UPDATE INTEGRATION TIME
|
||||
// d_extend_correlation_symbols_count = 0;
|
||||
// d_current_correlation_time_s = static_cast<float>(trk_parameters.extend_correlation_symbols) * static_cast<float>(d_code_period);
|
||||
//
|
||||
// d_state = 3; // next state is the extended correlator integrator
|
||||
// LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
|
||||
// << d_channel
|
||||
// << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN);
|
||||
// std::cout << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast<int32_t>(d_code_period * 1000.0) << " ms extended correlator in channel "
|
||||
// << d_channel
|
||||
// << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
// // Set narrow taps delay values [chips]
|
||||
// d_code_loop_filter.set_update_interval(d_current_correlation_time_s);
|
||||
// d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz);
|
||||
// d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order);
|
||||
// if (d_veml)
|
||||
// {
|
||||
// d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
// d_local_code_shift_chips[1] = -trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
// d_local_code_shift_chips[3] = trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
// d_local_code_shift_chips[4] = trk_parameters.very_early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// d_local_code_shift_chips[0] = -trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
// d_local_code_shift_chips[2] = trk_parameters.early_late_space_narrow_chips * static_cast<float>(d_code_samples_per_chip);
|
||||
// }
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// d_state = 4;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
@ -91,7 +91,7 @@ private:
|
||||
void log_data(bool integrating);
|
||||
int32_t save_matfile();
|
||||
|
||||
void run_state_2(Gnss_Synchro ¤t_synchro_data);
|
||||
//void run_state_2(Gnss_Synchro ¤t_synchro_data);
|
||||
|
||||
// tracking configuration vars
|
||||
Dll_Pll_Conf_Fpga trk_parameters;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user