mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-25 06:23:18 +00:00
Code Issues Releases Wiki Activity

Add phase coherence check to Glonass tracking blocks

Browse Source 
Improve const correctness
This commit is contained in:
Carles Fernandez 2020-07-20 11:27:39 +02:00
parent 3e6931200e
commit 3ddcfc786e
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
14 changed files with 311 additions and 255 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -211,6 +211,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc
d_glonass_freq_ch = 0;
d_acc_carrier_phase_initialized = false;
// set_min_output_buffer((int64_t)300);
}
@ -224,31 +225,26 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
// Doppler effect
// Fd=(C/(C+Vr))*F
d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (DFRQ1_GLO * static_cast<double>(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)));
double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GLONASS_L1_CA_CODE_RATE_CPS;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1.0 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
const double T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_CPS;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
const double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_CPS;
const double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples;
double delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
@ -267,7 +263,6 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(static_cast<float>(d_carrier_frequency_hz)); // The carrier loop filter implements the Doppler accumulator
d_code_loop_filter.initialize(); // initialize the code filter
@ -286,8 +281,9 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_pll_to_dll_assist_secs_Ti = 0.0;
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
const std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
d_acc_carrier_phase_initialized = false;
// DEBUG OUTPUT
std::cout << "Tracking of GLONASS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << '\n';
@ -353,10 +349,10 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_cc::save_matfile() const
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 11;
int32_t number_of_float_vars = 5;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 11;
const int32_t number_of_float_vars = 5;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@ -552,6 +548,16 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::set_gnss_synchro(Gnss_Synchro *p_g
}
void glonass_l1_ca_dll_pll_c_aid_tracking_cc::check_carrier_phase_coherent_initialization()
{
if (d_acc_carrier_phase_initialized == false)
{
d_acc_carrier_phase_cycles = -d_rem_carrier_phase_rad / TWO_PI;
d_acc_carrier_phase_initialized = true;
}
}
int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -574,12 +580,9 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// Receiver signal alignment
if (d_pull_in == true)
{
int32_t samples_offset;
double acq_trk_shif_correction_samples;
int32_t acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
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);
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
const double 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));
const int32_t samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
@ -653,11 +656,11 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
{
// 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 * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
int32_t K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
const double T_chip_seconds = 1.0 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const int32_t K_prn_samples = round(T_prn_samples);
const 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
@ -778,6 +781,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
check_carrier_phase_coherent_initialization();
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());

4
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h
View File

@ -110,6 +110,9 @@ private:
float early_late_space_chips);
void msg_handler_preamble_index(const pmt::pmt_t& msg);
void check_carrier_phase_coherent_initialization();
int32_t save_matfile() const;
volk_gnsssdr::vector<gr_complex> d_ca_code;
@ -197,6 +200,7 @@ private:
// control vars
bool d_enable_tracking;
bool d_pull_in;
bool d_acc_carrier_phase_initialized;
bool d_dump;
};

78
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -208,6 +208,8 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc
d_carrier_doppler_old_hz = 0.0;
d_glonass_freq_ch = 0;
d_acc_carrier_phase_initialized = false;
// set_min_output_buffer((int64_t)300);
}
@ -221,31 +223,26 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
// Doppler effect
// Fd=(C/(C+Vr))*F
d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GLONASS_L1_CA_CODE_RATE_CPS;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1.0 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
const double T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_CPS;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
const double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_CPS;
const double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples;
double delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
@ -285,8 +282,9 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_pll_to_dll_assist_secs_Ti = 0.0;
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
const std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
d_acc_carrier_phase_initialized = false;
// DEBUG OUTPUT
std::cout << "Tracking of GLONASS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << '\n';
@ -308,10 +306,10 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_sc::save_matfile() const
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 11;
int32_t number_of_float_vars = 5;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 11;
const int32_t number_of_float_vars = 5;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@ -553,6 +551,16 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::set_gnss_synchro(Gnss_Synchro *p_g
}
void glonass_l1_ca_dll_pll_c_aid_tracking_sc::check_carrier_phase_coherent_initialization()
{
if (d_acc_carrier_phase_initialized == false)
{
d_acc_carrier_phase_cycles = -d_rem_carrier_phase_rad / TWO_PI;
d_acc_carrier_phase_initialized = true;
}
}
int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -575,12 +583,9 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// Receiver signal alignment
if (d_pull_in == true)
{
int32_t samples_offset;
double acq_trk_shif_correction_samples;
int32_t acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
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);
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
const double 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));
const int32_t samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
@ -653,11 +658,11 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
{
// 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 * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
int32_t K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
const double T_chip_seconds = 1.0 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const int32_t K_prn_samples = round(T_prn_samples);
const 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
@ -717,11 +722,11 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// 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 * GLONASS_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;
const double T_chip_seconds = 1.0 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const double K_prn_samples = round(T_prn_samples);
const 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
@ -777,6 +782,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
check_carrier_phase_coherent_initialization();
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real());

4
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.h
View File

@ -109,6 +109,9 @@ private:
float early_late_space_chips);
void msg_handler_preamble_index(const pmt::pmt_t& msg);
void check_carrier_phase_coherent_initialization();
int32_t save_matfile() const;
volk_gnsssdr::vector<gr_complex> d_ca_code;
@ -196,6 +199,7 @@ private:
// control vars
bool d_enable_tracking;
bool d_pull_in;
bool d_acc_carrier_phase_initialized;
bool d_dump;
};

67
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc
View File

@ -156,6 +156,7 @@ Glonass_L1_Ca_Dll_Pll_Tracking_cc::Glonass_L1_Ca_Dll_Pll_Tracking_cc(
d_glonass_freq_ch = 0;
d_acc_carrier_phase_initialized = false;
set_relative_rate(1.0 / static_cast<double>(d_vector_length));
}
@ -169,31 +170,26 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
const double acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
// Doppler effect
// Fd=(C/(C+Vr))*F
d_glonass_freq_ch = GLONASS_L1_CA_FREQ_HZ + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GLONASS_L1_CA_CODE_RATE_CPS;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
const double T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_CPS;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
const double T_prn_true_seconds = GLONASS_L1_CA_CODE_LENGTH_CHIPS / GLONASS_L1_CA_CODE_RATE_CPS;
const double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples;
double delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
@ -228,8 +224,9 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
const std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
d_acc_carrier_phase_initialized = false;
// DEBUG OUTPUT
std::cout << "Tracking of GLONASS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << '\n';
@ -293,10 +290,10 @@ int32_t Glonass_L1_Ca_Dll_Pll_Tracking_cc::save_matfile() const
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 11;
int32_t number_of_float_vars = 5;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 11;
const int32_t number_of_float_vars = 5;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@ -492,6 +489,16 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_sy
}
void Glonass_L1_Ca_Dll_Pll_Tracking_cc::check_carrier_phase_coherent_initialization()
{
if (d_acc_carrier_phase_initialized == false)
{
d_acc_carrier_phase_rad = -d_rem_carr_phase_rad;
d_acc_carrier_phase_initialized = true;
}
}
int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -515,12 +522,9 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// Receiver signal alignment
if (d_pull_in == true)
{
int32_t samples_offset;
double acq_trk_shif_correction_samples;
int32_t acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
const double acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
const int32_t samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
@ -560,9 +564,9 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], static_cast<float>(d_early_late_spc_chips), 1.0); // [chips/Ti] //early and late
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(static_cast<float>(code_error_chips)); // [chips/second]
double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds]
const double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
const double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
const double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds]
// double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GLONASS_L1_CA_CODE_RATE_CPS; // [seconds]
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
@ -570,8 +574,8 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// 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 / static_cast<double>(d_code_freq_chips);
// double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples
// ################### PLL COMMANDS #################################################
@ -625,6 +629,7 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
check_carrier_phase_coherent_initialization();
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());

3
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.h
View File

@ -96,6 +96,8 @@ private:
float dll_bw_hz,
float early_late_space_chips);
void check_carrier_phase_coherent_initialization();
int32_t save_matfile() const;
volk_gnsssdr::vector<gr_complex> d_ca_code;
@ -164,6 +166,7 @@ private:
// control vars
bool d_enable_tracking;
bool d_pull_in;
bool d_acc_carrier_phase_initialized;
bool d_dump;
};

80
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -208,6 +208,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc
d_glonass_freq_ch = 0;
d_acc_carrier_phase_initialized = false;
// set_min_output_buffer((int64_t)300);
}
@ -221,31 +222,26 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
// Doppler effect
// Fd=(C/(C+Vr))*F
d_glonass_freq_ch = GLONASS_L2_CA_FREQ_HZ + (DFRQ2_GLO * static_cast<double>(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)));
double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GLONASS_L2_CA_CODE_RATE_CPS;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1.0 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GLONASS_L2_CA_CODE_LENGTH_CHIPS / GLONASS_L2_CA_CODE_RATE_CPS;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
const double T_prn_true_seconds = GLONASS_L2_CA_CODE_LENGTH_CHIPS / GLONASS_L2_CA_CODE_RATE_CPS;
const double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples;
double delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
@ -282,7 +278,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_pll_to_dll_assist_secs_Ti = 0.0;
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
const std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
// DEBUG OUTPUT
@ -294,6 +290,8 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_enable_tracking = true;
d_enable_extended_integration = false;
d_preamble_synchronized = false;
d_acc_carrier_phase_initialized = 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;
@ -350,10 +348,10 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_cc::save_matfile() const
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 11;
int32_t number_of_float_vars = 5;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 11;
const int32_t number_of_float_vars = 5;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@ -549,6 +547,16 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::set_gnss_synchro(Gnss_Synchro *p_g
}
void glonass_l2_ca_dll_pll_c_aid_tracking_cc::check_carrier_phase_coherent_initialization()
{
if (d_acc_carrier_phase_initialized == false)
{
d_acc_carrier_phase_cycles = -d_rem_carrier_phase_rad / TWO_PI;
d_acc_carrier_phase_initialized = true;
}
}
int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -571,12 +579,9 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// Receiver signal alignment
if (d_pull_in == true)
{
int32_t samples_offset;
double acq_trk_shif_correction_samples;
int32_t acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
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);
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
const double 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));
const int32_t samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
@ -614,7 +619,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
bool enable_dll_pll;
if (d_enable_extended_integration == true)
{
int64_t 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));
const int64_t 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
@ -650,11 +655,11 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
{
// 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 * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
int32_t K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
const double T_chip_seconds = 1.0 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const int32_t K_prn_samples = round(T_prn_samples);
const 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
@ -715,11 +720,11 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// 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 * GLONASS_L2_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;
const double T_chip_seconds = 1.0 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const double K_prn_samples = round(T_prn_samples);
const 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
@ -775,6 +780,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
check_carrier_phase_coherent_initialization();
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());

3
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h
View File

@ -107,6 +107,8 @@ private:
void msg_handler_preamble_index(const pmt::pmt_t& msg);
void check_carrier_phase_coherent_initialization();
int32_t save_matfile() const;
volk_gnsssdr::vector<gr_complex> d_ca_code;
@ -194,6 +196,7 @@ private:
// control vars
bool d_enable_tracking;
bool d_pull_in;
bool d_acc_carrier_phase_initialized;
bool d_dump;
};

80
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -206,6 +206,8 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::glonass_l2_ca_dll_pll_c_aid_tracking_sc
d_carrier_doppler_old_hz = 0.0;
d_glonass_freq_ch = 0;
d_acc_carrier_phase_initialized = false;
// set_min_output_buffer((int64_t)300);
}
@ -219,31 +221,26 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
// Doppler effect
// Fd=(C/(C+Vr))*F
d_glonass_freq_ch = GLONASS_L2_CA_FREQ_HZ + (GLONASS_L2_CA_FREQ_HZ * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GLONASS_L2_CA_CODE_RATE_CPS;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1.0 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GLONASS_L2_CA_CODE_LENGTH_CHIPS / GLONASS_L2_CA_CODE_RATE_CPS;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
const double T_prn_true_seconds = GLONASS_L2_CA_CODE_LENGTH_CHIPS / GLONASS_L2_CA_CODE_RATE_CPS;
const double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples;
double delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
@ -283,7 +280,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_pll_to_dll_assist_secs_Ti = 0.0;
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
const std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
// DEBUG OUTPUT
@ -295,6 +292,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_enable_tracking = true;
d_enable_extended_integration = true;
d_preamble_synchronized = true;
d_acc_carrier_phase_initialized = false;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
@ -306,10 +304,10 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_sc::save_matfile() const
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 11;
int32_t number_of_float_vars = 5;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 11;
const int32_t number_of_float_vars = 5;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@ -551,6 +549,16 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::set_gnss_synchro(Gnss_Synchro *p_g
}
void glonass_l2_ca_dll_pll_c_aid_tracking_sc::check_carrier_phase_coherent_initialization()
{
if (d_acc_carrier_phase_initialized == false)
{
d_acc_carrier_phase_cycles = -d_rem_carrier_phase_rad / TWO_PI;
d_acc_carrier_phase_initialized = true;
}
}
int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -573,12 +581,9 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// Receiver signal alignment
if (d_pull_in == true)
{
int32_t samples_offset;
double acq_trk_shif_correction_samples;
int32_t acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
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);
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
const double 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));
const int32_t samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
@ -616,7 +621,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
bool enable_dll_pll;
if (d_enable_extended_integration == true)
{
int64_t 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));
const int64_t 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
@ -651,11 +656,11 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
{
// 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 * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
int32_t K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
const double T_chip_seconds = 1.0 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const int32_t K_prn_samples = round(T_prn_samples);
const 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
@ -715,11 +720,11 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// 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 * GLONASS_L2_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;
const double T_chip_seconds = 1.0 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const double K_prn_samples = round(T_prn_samples);
const 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
@ -775,6 +780,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
check_carrier_phase_coherent_initialization();
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real());

3
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.h
View File

@ -108,6 +108,8 @@ private:
void msg_handler_preamble_index(const pmt::pmt_t& msg);
void check_carrier_phase_coherent_initialization();
int32_t save_matfile() const;
volk_gnsssdr::vector<gr_complex> d_ca_code;
@ -194,6 +196,7 @@ private:
// control vars
bool d_enable_tracking;
bool d_pull_in;
bool d_acc_carrier_phase_initialized;
bool d_dump;
};

68
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc
View File

@ -157,6 +157,8 @@ Glonass_L2_Ca_Dll_Pll_Tracking_cc::Glonass_L2_Ca_Dll_Pll_Tracking_cc(
d_glonass_freq_ch = 0;
d_acc_carrier_phase_initialized = false;
set_relative_rate(1.0 / static_cast<double>(d_vector_length));
}
@ -170,31 +172,26 @@ void Glonass_L2_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
const double acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
// Doppler effect
// Fd=(C/(C+Vr))*F
d_glonass_freq_ch = GLONASS_L2_CA_FREQ_HZ + (DFRQ2_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
const double radial_velocity = (d_glonass_freq_ch + d_acq_carrier_doppler_hz) / d_glonass_freq_ch;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GLONASS_L2_CA_CODE_RATE_CPS;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GLONASS_L2_CA_CODE_LENGTH_CHIPS / GLONASS_L2_CA_CODE_RATE_CPS;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
const double T_prn_true_seconds = GLONASS_L2_CA_CODE_LENGTH_CHIPS / GLONASS_L2_CA_CODE_RATE_CPS;
const double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples;
double delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
@ -229,7 +226,7 @@ void Glonass_L2_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
const std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
// DEBUG OUTPUT
@ -239,6 +236,7 @@ void Glonass_L2_Ca_Dll_Pll_Tracking_cc::start_tracking()
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
d_acc_carrier_phase_initialized = false;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_frequency_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
@ -294,10 +292,10 @@ int32_t Glonass_L2_Ca_Dll_Pll_Tracking_cc::save_matfile() const
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 11;
int32_t number_of_float_vars = 5;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 11;
const int32_t number_of_float_vars = 5;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@ -493,6 +491,16 @@ void Glonass_L2_Ca_Dll_Pll_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_sy
}
void Glonass_L2_Ca_Dll_Pll_Tracking_cc::check_carrier_phase_coherent_initialization()
{
if (d_acc_carrier_phase_initialized == false)
{
d_acc_carrier_phase_rad = -d_rem_carr_phase_rad;
d_acc_carrier_phase_initialized = true;
}
}
int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -516,12 +524,9 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// Receiver signal alignment
if (d_pull_in == true)
{
int32_t samples_offset;
double acq_trk_shif_correction_samples;
int32_t acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
const double acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
const int32_t samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
@ -561,9 +566,9 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2], static_cast<float>(d_early_late_spc_chips), 1.0); // [chips/Ti] //early and late
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(static_cast<float>(code_error_chips)); // [chips/second]
double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
double T_prn_seconds = T_chip_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds]
const double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
const double T_prn_seconds = T_chip_seconds * GLONASS_L2_CA_CODE_LENGTH_CHIPS;
const double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds]
// double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GLONASS_L1_CA_CODE_RATE_CPS; // [seconds]
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
@ -571,8 +576,8 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// 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 / static_cast<double>(d_code_freq_chips);
// double T_prn_seconds = T_chip_seconds * GLONASS_L1_CA_CODE_LENGTH_CHIPS;
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples
// ################### PLL COMMANDS #################################################
@ -626,6 +631,7 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
check_carrier_phase_coherent_initialization();
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());

3
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.h
View File

@ -94,6 +94,8 @@ private:
float dll_bw_hz,
float early_late_space_chips);
void check_carrier_phase_coherent_initialization();
int32_t save_matfile() const;
volk_gnsssdr::vector<gr_complex> d_ca_code;
@ -162,6 +164,7 @@ private:
// control vars
bool d_enable_tracking;
bool d_pull_in;
bool d_acc_carrier_phase_initialized;
bool d_dump;
};

62
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
View File

@ -151,6 +151,8 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
d_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 0.0;
d_carrier_phase_step_rad = 0.0;
d_acc_carrier_phase_initialized = false;
// set_min_output_buffer((int64_t)300);
}
@ -164,30 +166,25 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); // -d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
// doppler effect
// Fd=(C/(C+Vr))*F
double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
const double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_CPS;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
const double T_chip_mod_seconds = 1 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_CPS;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
const double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_CPS;
const double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
const double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
const double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
@ -224,7 +221,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
d_pll_to_dll_assist_secs_Ti = 0.0;
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
const std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
// DEBUG OUTPUT
@ -234,6 +231,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
d_acc_carrier_phase_initialized = false;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
@ -302,6 +300,16 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_sy
}
void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::check_carrier_phase_coherent_initialization()
{
if (d_acc_carrier_phase_initialized == false)
{
d_acc_carrier_phase_cycles = -d_rem_carrier_phase_rad / TWO_PI;
d_acc_carrier_phase_initialized = true;
}
}
int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -327,12 +335,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
// Receiver signal alignment
if (d_pull_in == true)
{
int32_t samples_offset;
double acq_trk_shif_correction_samples;
int32_t acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
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);
const int32_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
const double 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));
const int32_t samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
current_synchro_data.fs = d_fs_in;
current_synchro_data.correlation_length_ms = 1;
@ -384,15 +389,11 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// 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);
const double T_chip_seconds = 1 / d_code_freq_chips;
const double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
const double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
const double 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
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_correlation_length_samples); // rounding error < 1 sample
@ -446,6 +447,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
check_carrier_phase_coherent_initialization();
}
// ########### Output the tracking data to navigation and PVT ##########

43
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h
View File

@ -99,26 +99,37 @@ private:
float early_late_space_chips);
void update_local_code();
void update_local_carrier();
void check_carrier_phase_coherent_initialization();
// tracking configuration vars
uint32_t d_vector_length;
bool d_dump;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_FLL_PLL_filter d_carrier_loop_filter;
Gnss_Synchro *d_acquisition_gnss_synchro;
uint32_t d_channel;
std::vector<gr_complex> d_Prompt_buffer;
// file dump
std::string d_dump_filename;
std::ofstream d_dump_file;
std::map<std::string, std::string> systemName;
std::string sys;
// tracking configuration vars
int64_t d_if_freq;
int64_t d_fs_in;
double d_early_late_spc_chips;
uint32_t d_vector_length;
uint32_t d_channel;
int32_t d_n_correlator_taps;
// GPU HOST PINNED MEMORY IN/OUT VECTORS
gr_complex *in_gpu;
float *d_local_code_shift_chips;
gr_complex *d_correlator_outs;
cuda_multicorrelator *multicorrelator_gpu;
gr_complex *in_gpu;
gr_complex *d_correlator_outs;
gr_complex *d_ca_code;
float *d_local_code_shift_chips;
gr_complex *d_Early;
gr_complex *d_Prompt;
@ -129,10 +140,6 @@ private:
double d_rem_code_phase_chips;
double d_rem_carrier_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_FLL_PLL_filter d_carrier_loop_filter;
// acquisition
double d_acq_code_phase_samples;
double d_acq_carrier_doppler_hz;
@ -154,23 +161,17 @@ private:
uint64_t d_acq_sample_stamp;
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;
int32_t d_carrier_lock_fail_counter;
int32_t d_cn0_estimation_counter;
// control vars
bool d_acc_carrier_phase_initialized;
bool d_enable_tracking;
bool d_pull_in;
// file dump
std::string d_dump_filename;
std::ofstream d_dump_file;
std::map<std::string, std::string> systemName;
std::string sys;
bool d_dump;
};
#endif // GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_GPU_CC_H