mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-10 04:03:02 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/mmajoral/gnss-sdr into fpga_extended_coherent_integration

Browse Source
This commit is contained in:
Marc Majoral
2019-07-04 12:55:36 +02:00
parent feccc62c04 620191f818
commit c8ccd5ef91
273 changed files with 9234 additions and 5518 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/tracking/adapters/CMakeLists.txt
View File

@@ -108,7 +108,7 @@ target_include_directories(tracking_adapters
${CMAKE_SOURCE_DIR}/src/core/interfaces
)
if(ENABLE_CUDA)
if(ENABLE_CUDA AND NOT CMAKE_VERSION VERSION_GREATER 3.11)
target_link_libraries(tracking_adapters
PUBLIC
${CUDA_LIBRARIES}

10
src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc
View File

@@ -59,14 +59,20 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking(
bool dump = configuration->property(role + ".dump", false);
trk_param.dump = dump;
float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
if (FLAGS_pll_bw_hz != 0.0)
{
pll_bw_hz = static_cast<float>(FLAGS_pll_bw_hz);
}
trk_param.pll_bw_hz = pll_bw_hz;
float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0);
trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz;
float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0);
trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz;
float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
if (FLAGS_dll_bw_hz != 0.0)
{
dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
}
trk_param.dll_bw_hz = dll_bw_hz;
int dll_filter_order = configuration->property(role + ".dll_filter_order", 2);

20
src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc
View File

@@ -227,17 +227,17 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga(
for (uint32_t PRN = 1; PRN <= GALILEO_E1_NUMBER_OF_CODES; PRN++)
{
char data_signal[3] = "1B";
std::array<char, 3> data_signal = {'1', 'B', '\0'};
if (d_track_pilot)
{
char pilot_signal[3] = "1C";
galileo_e1_code_gen_sinboc11_float(ca_codes_f, pilot_signal, PRN);
galileo_e1_code_gen_sinboc11_float(data_codes_f, data_signal, PRN);
std::array<char, 3> pilot_signal = {'1', 'C', '\0'};
galileo_e1_code_gen_sinboc11_float(gsl::span<float>(ca_codes_f, static_cast<int32_t>(GALILEO_E1_B_CODE_LENGTH_CHIPS) * code_samples_per_chip), pilot_signal, PRN);
galileo_e1_code_gen_sinboc11_float(gsl::span<float>(data_codes_f, static_cast<uint32_t>(GALILEO_E1_B_CODE_LENGTH_CHIPS) * code_samples_per_chip), data_signal, PRN);
// The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA
for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++)
{
int32_t tmp_value = static_cast<int32_t>(ca_codes_f[s]);
auto tmp_value = static_cast<int32_t>(ca_codes_f[s]);
if (tmp_value < 0)
{
tmp_value = 0;
@@ -255,12 +255,12 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga(
}
else
{
galileo_e1_code_gen_sinboc11_float(ca_codes_f, data_signal, PRN);
galileo_e1_code_gen_sinboc11_float(gsl::span<float>(ca_codes_f, static_cast<int32_t>(GALILEO_E1_B_CODE_LENGTH_CHIPS) * code_samples_per_chip), data_signal, PRN);
// The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA
for (uint32_t s = 0; s < 2 * GALILEO_E1_B_CODE_LENGTH_CHIPS; s++)
{
int32_t tmp_value = static_cast<int32_t>(ca_codes_f[s]);
auto tmp_value = static_cast<int32_t>(ca_codes_f[s]);
if (tmp_value < 0)
{
tmp_value = 0;
@@ -271,7 +271,7 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga(
}
}
delete[] ca_codes_f;
volk_gnsssdr_free(ca_codes_f);
if (d_track_pilot)
{
volk_gnsssdr_free(data_codes_f);
@@ -292,10 +292,10 @@ GalileoE1DllPllVemlTrackingFpga::GalileoE1DllPllVemlTrackingFpga(
GalileoE1DllPllVemlTrackingFpga::~GalileoE1DllPllVemlTrackingFpga()
{
delete[] d_ca_codes;
volk_gnsssdr_free(d_ca_codes);
if (d_track_pilot)
{
delete[] d_data_codes;
volk_gnsssdr_free(d_data_codes);
}
}

11
src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc
View File

@@ -217,14 +217,15 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga(
for (uint32_t PRN = 1; PRN <= GALILEO_E5A_NUMBER_OF_CODES; PRN++)
{
galileo_e5_a_code_gen_complex_primary(aux_code, PRN, const_cast<char *>(sig_));
std::array<char, 3> sig_a = {'5', 'X', '\0'};
galileo_e5_a_code_gen_complex_primary(gsl::span<gr_complex>(aux_code, code_length_chips * code_samples_per_chip), PRN, sig_a);
if (trk_param_fpga.track_pilot)
{
// The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA
for (uint32_t s = 0; s < code_length_chips; s++)
{
int32_t tmp_value = static_cast<int32_t>(aux_code[s].imag());
auto tmp_value = static_cast<int32_t>(aux_code[s].imag());
if (tmp_value < 0)
{
tmp_value = 0;
@@ -246,7 +247,7 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga(
// The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA
for (uint32_t s = 0; s < code_length_chips; s++)
{
int32_t tmp_value = static_cast<int32_t>(aux_code[s].real());
auto tmp_value = static_cast<int32_t>(aux_code[s].real());
if (tmp_value < 0)
{
tmp_value = 0;
@@ -303,10 +304,10 @@ GalileoE5aDllPllTrackingFpga::GalileoE5aDllPllTrackingFpga(
GalileoE5aDllPllTrackingFpga::~GalileoE5aDllPllTrackingFpga()
{
delete[] d_ca_codes;
volk_gnsssdr_free(d_ca_codes);
if (d_track_pilot)
{
delete[] d_data_codes;
volk_gnsssdr_free(d_data_codes);
}
}

4
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc
View File

@@ -213,7 +213,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga(
d_ca_codes = static_cast<int32_t*>(volk_gnsssdr_malloc(static_cast<int32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
for (uint32_t PRN = 1; PRN <= NUM_PRNs; PRN++)
{
gps_l1_ca_code_gen_int(&d_ca_codes[(int32_t(GPS_L1_CA_CODE_LENGTH_CHIPS)) * (PRN - 1)], PRN, 0);
gps_l1_ca_code_gen_int(gsl::span<int32_t>(&d_ca_codes[static_cast<int32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS) * (PRN - 1)], &d_ca_codes[static_cast<int32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS) * (PRN)]), PRN, 0);
// The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA
for (uint32_t k = 0; k < GPS_L1_CA_CODE_LENGTH_CHIPS; k++)
@@ -257,7 +257,7 @@ GpsL1CaDllPllTrackingFpga::GpsL1CaDllPllTrackingFpga(
GpsL1CaDllPllTrackingFpga::~GpsL1CaDllPllTrackingFpga()
{
delete[] d_ca_codes;
volk_gnsssdr_free(d_ca_codes);
}

2
src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc
View File

@@ -128,7 +128,7 @@ GpsL2MDllPllTrackingFpga::GpsL2MDllPllTrackingFpga(
d_ca_codes = static_cast<int*>(volk_gnsssdr_malloc(static_cast<int>(GPS_L2_M_CODE_LENGTH_CHIPS * NUM_PRNs) * sizeof(int), volk_gnsssdr_get_alignment()));
for (unsigned int PRN = 1; PRN <= NUM_PRNs; PRN++)
{
gps_l2c_m_code_gen_float(ca_codes_f, PRN);
gps_l2c_m_code_gen_float(gsl::span<float>(ca_codes_f, static_cast<unsigned int>(GPS_L2_M_CODE_LENGTH_CHIPS)), PRN);
for (unsigned int s = 0; s < 2 * static_cast<unsigned int>(GPS_L2_M_CODE_LENGTH_CHIPS); s++)
{
d_ca_codes[static_cast<int>(GPS_L2_M_CODE_LENGTH_CHIPS) * (PRN - 1) + s] = static_cast<int>(ca_codes_f[s]);

10
src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc
View File

@@ -236,13 +236,13 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga(
{
if (track_pilot)
{
gps_l5q_code_gen_float(tracking_code, PRN);
gps_l5i_code_gen_float(data_code, PRN);
gps_l5q_code_gen_float(gsl::span<float>(tracking_code, code_length_chips), PRN);
gps_l5i_code_gen_float(gsl::span<float>(data_code, code_length_chips), PRN);
// The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA
for (uint32_t s = 0; s < code_length_chips; s++)
{
int32_t tmp_value = static_cast<int32_t>(tracking_code[s]);
auto tmp_value = static_cast<int32_t>(tracking_code[s]);
if (tmp_value < 0)
{
tmp_value = 0;
@@ -261,12 +261,12 @@ GpsL5DllPllTrackingFpga::GpsL5DllPllTrackingFpga(
}
else
{
gps_l5i_code_gen_float(tracking_code, PRN);
gps_l5i_code_gen_float(gsl::span<float>(tracking_code, code_length_chips), PRN);
// The code is generated as a series of 1s and -1s. In order to store the values using only one bit, a -1 is stored as a 0 in the FPGA
for (uint32_t s = 0; s < code_length_chips; s++)
{
int32_t tmp_value = static_cast<int32_t>(tracking_code[s]);
auto tmp_value = static_cast<int32_t>(tracking_code[s]);
if (tmp_value < 0)
{
tmp_value = 0;

2
src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt
View File

@@ -97,7 +97,7 @@ target_link_libraries(tracking_gr_blocks
tracking_libs
)
if(ENABLE_CUDA)
if(ENABLE_CUDA AND NOT CMAKE_VERSION VERSION_GREATER 3.11)
target_link_libraries(tracking_gr_blocks
PUBLIC
${CUDA_LIBRARIES}

171
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@@ -63,9 +63,11 @@
#include <algorithm> // for fill_n
#include <cmath> // for fmod, round, floor
#include <exception> // for exception
#include <iostream> // for cout, cerr
#include <gsl/gsl>
#include <iostream> // for cout, cerr
#include <map>
#include <numeric>
#include <vector>
#if HAS_STD_FILESYSTEM
#if HAS_STD_FILESYSTEM_EXPERIMENTAL
@@ -447,7 +449,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[trk_parameters.cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(trk_parameters.cn0_samples);
d_carrier_lock_test = 1.0;
d_CN0_SNV_dB_Hz = 0.0;
d_carrier_lock_fail_counter = 0;
@@ -587,48 +589,52 @@ void dll_pll_veml_tracking::start_tracking()
d_carrier_phase_rate_step_rad = 0.0;
d_carr_ph_history.clear();
d_code_ph_history.clear();
std::array<char, 3> Signal_;
std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3);
if (systemName == "GPS" and signal_type == "1C")
{
gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0);
gps_l1_ca_code_gen_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0);
}
else if (systemName == "GPS" and signal_type == "2S")
{
gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN);
gps_l2c_m_code_gen_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN);
}
else if (systemName == "GPS" and signal_type == "L5")
{
if (trk_parameters.track_pilot)
{
gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN);
gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN);
gps_l5q_code_gen_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN);
gps_l5i_code_gen_float(gsl::span<float>(d_data_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN);
d_Prompt_Data[0] = gr_complex(0.0, 0.0);
correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift);
}
else
{
gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN);
gps_l5i_code_gen_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN);
}
}
else if (systemName == "Galileo" and signal_type == "1B")
{
if (trk_parameters.track_pilot)
{
char pilot_signal[3] = "1C";
galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN);
galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN);
std::array<char, 3> pilot_signal = {{'1', 'C', '\0'}};
galileo_e1_code_gen_sinboc11_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), pilot_signal, d_acquisition_gnss_synchro->PRN);
galileo_e1_code_gen_sinboc11_float(gsl::span<float>(d_data_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN);
d_Prompt_Data[0] = gr_complex(0.0, 0.0);
correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift);
}
else
{
galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN);
galileo_e1_code_gen_sinboc11_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN);
}
}
else if (systemName == "Galileo" and signal_type == "5X")
{
auto *aux_code = static_cast<gr_complex *>(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment()));
galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast<char *>(signal_type.c_str()));
std::array<char, 3> signal_type_ = {{'5', 'X', '\0'}};
galileo_e5_a_code_gen_complex_primary(gsl::span<gr_complex>(aux_code, d_code_length_chips), d_acquisition_gnss_synchro->PRN, signal_type_);
if (trk_parameters.track_pilot)
{
d_secondary_code_string = const_cast<std::string *>(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]);
@@ -651,7 +657,7 @@ void dll_pll_veml_tracking::start_tracking()
}
else if (systemName == "Beidou" and signal_type == "B1")
{
beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0);
beidou_b1i_code_gen_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0);
// Update secondary code settings for geo satellites
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
@@ -691,7 +697,7 @@ void dll_pll_veml_tracking::start_tracking()
else if (systemName == "Beidou" and signal_type == "B3")
{
beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0);
beidou_b3i_code_gen_float(gsl::span<float>(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0);
// Update secondary code settings for geo satellites
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
@@ -818,7 +824,6 @@ dll_pll_veml_tracking::~dll_pll_veml_tracking()
volk_gnsssdr_free(d_data_code);
correlator_data_cpu.free();
}
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
@@ -881,10 +886,10 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_
d_Prompt_buffer[d_cn0_estimation_counter % trk_parameters.cn0_samples] = d_P_accu;
d_cn0_estimation_counter++;
// Code lock indicator
float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s));
float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s));
d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw);
// Carrier lock indicator
d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer, 1));
d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1));
//d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples);
// Loss of lock detection
if (!d_pull_in_transitory)
@@ -1356,28 +1361,28 @@ int32_t dll_pll_veml_tracking::save_matfile()
{
return 1;
}
auto *abs_VE = new float[num_epoch];
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *abs_VL = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new float[num_epoch];
auto *carrier_doppler_hz = new float[num_epoch];
auto *carrier_doppler_rate_hz = new float[num_epoch];
auto *code_freq_chips = new float[num_epoch];
auto *code_freq_rate_chips = new float[num_epoch];
auto *carr_error_hz = new float[num_epoch];
auto *carr_error_filt_hz = new float[num_epoch];
auto *code_error_chips = new float[num_epoch];
auto *code_error_filt_chips = new float[num_epoch];
auto *CN0_SNV_dB_Hz = new float[num_epoch];
auto *carrier_lock_test = new float[num_epoch];
auto *aux1 = new float[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_VE = std::vector<float>(num_epoch);
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto abs_VL = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<float>(num_epoch);
auto carrier_doppler_hz = std::vector<float>(num_epoch);
auto carrier_doppler_rate_hz = std::vector<float>(num_epoch);
auto code_freq_chips = std::vector<float>(num_epoch);
auto code_freq_rate_chips = std::vector<float>(num_epoch);
auto carr_error_hz = std::vector<float>(num_epoch);
auto carr_error_filt_hz = std::vector<float>(num_epoch);
auto code_error_chips = std::vector<float>(num_epoch);
auto code_error_filt_chips = std::vector<float>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<float>(num_epoch);
auto carrier_lock_test = std::vector<float>(num_epoch);
auto aux1 = std::vector<float>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -1414,28 +1419,6 @@ int32_t dll_pll_veml_tracking::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_doppler_rate_hz;
delete[] code_freq_chips;
delete[] code_freq_rate_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -1449,117 +1432,95 @@ int32_t dll_pll_veml_tracking::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0);
matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0);
matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_doppler_rate_hz;
delete[] code_freq_chips;
delete[] code_freq_rate_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}

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

@@ -46,6 +46,7 @@
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <utility> // for pair
#include <vector>
class Gnss_Synchro;
class dll_pll_veml_tracking;
@@ -201,7 +202,7 @@ private:
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
gr_complex *d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
Exponential_Smoother d_cn0_smoother;
Exponential_Smoother d_carrier_lock_test_smoother;
// file dump

141
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc
View File

@@ -58,6 +58,7 @@
#include <exception>
#include <iostream>
#include <map>
#include <vector>
#if HAS_STD_FILESYSTEM
#if HAS_STD_FILESYSTEM_EXPERIMENTAL
@@ -475,7 +476,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[trk_parameters.cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(trk_parameters.cn0_samples);
d_carrier_lock_test = 1.0;
d_CN0_SNV_dB_Hz = 0.0;
d_cn0_smoother = Exponential_Smoother();
@@ -659,7 +660,6 @@ dll_pll_veml_tracking_fpga::~dll_pll_veml_tracking_fpga()
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_Prompt_Data);
delete[] d_Prompt_buffer;
multicorrelator_fpga->free();
}
catch (const std::exception &ex)
@@ -724,10 +724,10 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra
{
d_cn0_estimation_counter = 0;
// Code lock indicator
float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer, trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s));
float d_CN0_SNV_dB_Hz_raw = cn0_svn_estimator(d_Prompt_buffer.data(), trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s));
d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), trk_parameters.cn0_samples);
// Loss of lock detection
if (!d_pull_in_transitory)
{
@@ -858,7 +858,6 @@ void dll_pll_veml_tracking_fpga::clear_tracking_vars()
d_code_error_filt_chips = 0.0;
d_current_symbol = 0;
d_Prompt_circular_buffer.clear();
//d_Prompt_buffer_deque.clear();
d_carrier_phase_rate_step_rad = 0.0;
d_code_phase_rate_step_chips = 0.0;
d_carr_ph_history.clear();
@@ -1442,28 +1441,28 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile()
{
return 1;
}
auto *abs_VE = new float[num_epoch];
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *abs_VL = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new float[num_epoch];
auto *carrier_doppler_hz = new float[num_epoch];
auto *carrier_doppler_rate_hz = new float[num_epoch];
auto *code_freq_chips = new float[num_epoch];
auto *code_freq_rate_chips = new float[num_epoch];
auto *carr_error_hz = new float[num_epoch];
auto *carr_error_filt_hz = new float[num_epoch];
auto *code_error_chips = new float[num_epoch];
auto *code_error_filt_chips = new float[num_epoch];
auto *CN0_SNV_dB_Hz = new float[num_epoch];
auto *carrier_lock_test = new float[num_epoch];
auto *aux1 = new float[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_VE = std::vector<float>(num_epoch);
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto abs_VL = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<float>(num_epoch);
auto carrier_doppler_hz = std::vector<float>(num_epoch);
auto carrier_doppler_rate_hz = std::vector<float>(num_epoch);
auto code_freq_chips = std::vector<float>(num_epoch);
auto code_freq_rate_chips = std::vector<float>(num_epoch);
auto carr_error_hz = std::vector<float>(num_epoch);
auto carr_error_filt_hz = std::vector<float>(num_epoch);
auto code_error_chips = std::vector<float>(num_epoch);
auto code_error_filt_chips = std::vector<float>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<float>(num_epoch);
auto carrier_lock_test = std::vector<float>(num_epoch);
auto aux1 = std::vector<float>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -1500,28 +1499,6 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_doppler_rate_hz;
delete[] code_freq_chips;
delete[] code_freq_rate_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -1535,117 +1512,95 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0);
matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0);
matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_doppler_rate_hz;
delete[] code_freq_chips;
delete[] code_freq_rate_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}

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

@@ -48,6 +48,7 @@
#include <memory> // for shared_ptr
#include <string>
#include <utility> // for pair
#include <vector>
class Fpga_Multicorrelator_8sc;
class Gnss_Synchro;
@@ -213,7 +214,7 @@ private:
double d_carrier_lock_threshold;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
//std::deque<gr_complex> d_Prompt_buffer_deque;
gr_complex *d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
Exponential_Smoother d_cn0_smoother;

13
src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
View File

@@ -163,7 +163,7 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -188,10 +188,12 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking()
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
std::array<char, 3> Signal_;
std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3);
// generate local reference ALWAYS starting at chip 1 (2 samples per chip)
galileo_e1_code_gen_complex_sampled(d_ca_code,
d_acquisition_gnss_synchro->Signal,
galileo_e1_code_gen_complex_sampled(gsl::span<gr_complex>(d_ca_code, (2 * GALILEO_E1_B_CODE_LENGTH_CHIPS)),
Signal_,
false,
d_acquisition_gnss_synchro->PRN,
2 * GALILEO_E1_CODE_CHIP_RATE_HZ,
@@ -245,7 +247,6 @@ Galileo_E1_Tcp_Connector_Tracking_cc::~Galileo_E1_Tcp_Connector_Tracking_cc()
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
delete[] d_Prompt_buffer;
d_tcp_com.close_tcp_connection(d_port);
multicorrelator_cpu.free();
}
@@ -413,10 +414,10 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GALILEO_E1_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GALILEO_E1_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)

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

@@ -47,6 +47,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class Galileo_E1_Tcp_Connector_Tracking_cc;
@@ -166,7 +167,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex *d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
float d_carrier_lock_test;
float d_CN0_SNV_dB_Hz;
float d_carrier_lock_threshold;

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

@@ -175,7 +175,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -272,7 +272,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
glonass_l1_ca_code_gen_complex(d_ca_code, 0);
glonass_l1_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, GLONASS_L1_CA_CODE_LENGTH_CHIPS), 0);
multicorrelator_cpu.set_local_code_and_taps(static_cast<int32_t>(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int32_t n = 0; n < d_n_correlator_taps; n++)
@@ -345,7 +345,6 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::~glonass_l1_ca_dll_pll_c_aid_tracking_c
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
@@ -386,24 +385,24 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_cc::save_matfile()
{
return 1;
}
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new double[num_epoch];
auto *carrier_doppler_hz = new double[num_epoch];
auto *code_freq_chips = new double[num_epoch];
auto *carr_error_hz = new double[num_epoch];
auto *carr_error_filt_hz = new double[num_epoch];
auto *code_error_chips = new double[num_epoch];
auto *code_error_filt_chips = new double[num_epoch];
auto *CN0_SNV_dB_Hz = new double[num_epoch];
auto *carrier_lock_test = new double[num_epoch];
auto *aux1 = new double[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<double>(num_epoch);
auto carrier_doppler_hz = std::vector<double>(num_epoch);
auto code_freq_chips = std::vector<double>(num_epoch);
auto carr_error_hz = std::vector<double>(num_epoch);
auto carr_error_filt_hz = std::vector<double>(num_epoch);
auto code_error_chips = std::vector<double>(num_epoch);
auto code_error_filt_chips = std::vector<double>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<double>(num_epoch);
auto carrier_lock_test = std::vector<double>(num_epoch);
auto aux1 = std::vector<double>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -436,24 +435,6 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_cc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -467,97 +448,79 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_cc::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
@@ -798,9 +761,9 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{

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

@@ -50,6 +50,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class glonass_l1_ca_dll_pll_c_aid_tracking_cc;
@@ -180,7 +181,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

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

@@ -173,7 +173,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::glonass_l1_ca_dll_pll_c_aid_tracking_sc
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -266,7 +266,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
glonass_l1_ca_code_gen_complex(d_ca_code, 0);
glonass_l1_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, GLONASS_L1_CA_CODE_LENGTH_CHIPS), 0);
volk_gnsssdr_32fc_convert_16ic(d_ca_code_16sc, d_ca_code, static_cast<int32_t>(GLONASS_L1_CA_CODE_LENGTH_CHIPS));
multicorrelator_cpu_16sc.set_local_code_and_taps(static_cast<int32_t>(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips);
@@ -333,24 +333,24 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_sc::save_matfile()
{
return 1;
}
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new double[num_epoch];
auto *carrier_doppler_hz = new double[num_epoch];
auto *code_freq_chips = new double[num_epoch];
auto *carr_error_hz = new double[num_epoch];
auto *carr_error_filt_hz = new double[num_epoch];
auto *code_error_chips = new double[num_epoch];
auto *code_error_filt_chips = new double[num_epoch];
auto *CN0_SNV_dB_Hz = new double[num_epoch];
auto *carrier_lock_test = new double[num_epoch];
auto *aux1 = new double[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<double>(num_epoch);
auto carrier_doppler_hz = std::vector<double>(num_epoch);
auto code_freq_chips = std::vector<double>(num_epoch);
auto carr_error_hz = std::vector<double>(num_epoch);
auto carr_error_filt_hz = std::vector<double>(num_epoch);
auto code_error_chips = std::vector<double>(num_epoch);
auto code_error_filt_chips = std::vector<double>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<double>(num_epoch);
auto carrier_lock_test = std::vector<double>(num_epoch);
auto aux1 = std::vector<double>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -383,24 +383,6 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_sc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -414,97 +396,79 @@ int32_t glonass_l1_ca_dll_pll_c_aid_tracking_sc::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
@@ -549,7 +513,6 @@ glonass_l1_ca_dll_pll_c_aid_tracking_sc::~glonass_l1_ca_dll_pll_c_aid_tracking_s
volk_gnsssdr_free(d_ca_code_16sc);
volk_gnsssdr_free(d_correlator_outs_16sc);
delete[] d_Prompt_buffer;
multicorrelator_cpu_16sc.free();
}
@@ -788,9 +751,9 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{

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

@@ -51,6 +51,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class glonass_l1_ca_dll_pll_c_aid_tracking_sc;
@@ -183,7 +184,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

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

@@ -52,6 +52,7 @@
#include <memory>
#include <sstream>
#include <utility>
#include <vector>
#define CN0_ESTIMATION_SAMPLES 10
@@ -145,7 +146,7 @@ Glonass_L1_Ca_Dll_Pll_Tracking_cc::Glonass_L1_Ca_Dll_Pll_Tracking_cc(
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -226,7 +227,7 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
glonass_l1_ca_code_gen_complex(d_ca_code, 0);
glonass_l1_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, GLONASS_L1_CA_CODE_LENGTH_CHIPS), 0);
multicorrelator_cpu.set_local_code_and_taps(static_cast<int32_t>(GLONASS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int32_t n = 0; n < d_n_correlator_taps; n++)
@@ -297,7 +298,6 @@ Glonass_L1_Ca_Dll_Pll_Tracking_cc::~Glonass_L1_Ca_Dll_Pll_Tracking_cc()
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
@@ -338,24 +338,24 @@ int32_t Glonass_L1_Ca_Dll_Pll_Tracking_cc::save_matfile()
{
return 1;
}
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new double[num_epoch];
auto *carrier_doppler_hz = new double[num_epoch];
auto *code_freq_chips = new double[num_epoch];
auto *carr_error_hz = new double[num_epoch];
auto *carr_error_filt_hz = new double[num_epoch];
auto *code_error_chips = new double[num_epoch];
auto *code_error_filt_chips = new double[num_epoch];
auto *CN0_SNV_dB_Hz = new double[num_epoch];
auto *carrier_lock_test = new double[num_epoch];
auto *aux1 = new double[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<double>(num_epoch);
auto carrier_doppler_hz = std::vector<double>(num_epoch);
auto code_freq_chips = std::vector<double>(num_epoch);
auto carr_error_hz = std::vector<double>(num_epoch);
auto carr_error_filt_hz = std::vector<double>(num_epoch);
auto code_error_chips = std::vector<double>(num_epoch);
auto code_error_filt_chips = std::vector<double>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<double>(num_epoch);
auto carrier_lock_test = std::vector<double>(num_epoch);
auto aux1 = std::vector<double>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -388,24 +388,6 @@ int32_t Glonass_L1_Ca_Dll_Pll_Tracking_cc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -419,97 +401,79 @@ int32_t Glonass_L1_Ca_Dll_Pll_Tracking_cc::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
@@ -656,9 +620,9 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L1_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{

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

@@ -47,6 +47,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class Glonass_L1_Ca_Dll_Pll_Tracking_cc;
@@ -148,7 +149,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

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

@@ -52,6 +52,7 @@
#include <memory>
#include <sstream>
#include <utility>
#include <vector>
#define CN0_ESTIMATION_SAMPLES 10
@@ -172,7 +173,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -269,7 +270,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
glonass_l2_ca_code_gen_complex(d_ca_code, 0);
glonass_l2_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, static_cast<int32_t>(GLONASS_L2_CA_CODE_LENGTH_CHIPS)), 0);
multicorrelator_cpu.set_local_code_and_taps(static_cast<int32_t>(GLONASS_L2_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int32_t n = 0; n < d_n_correlator_taps; n++)
@@ -343,7 +344,6 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::~glonass_l2_ca_dll_pll_c_aid_tracking_c
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
@@ -384,24 +384,24 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_cc::save_matfile()
{
return 1;
}
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new double[num_epoch];
auto *carrier_doppler_hz = new double[num_epoch];
auto *code_freq_chips = new double[num_epoch];
auto *carr_error_hz = new double[num_epoch];
auto *carr_error_filt_hz = new double[num_epoch];
auto *code_error_chips = new double[num_epoch];
auto *code_error_filt_chips = new double[num_epoch];
auto *CN0_SNV_dB_Hz = new double[num_epoch];
auto *carrier_lock_test = new double[num_epoch];
auto *aux1 = new double[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<double>(num_epoch);
auto carrier_doppler_hz = std::vector<double>(num_epoch);
auto code_freq_chips = std::vector<double>(num_epoch);
auto carr_error_hz = std::vector<double>(num_epoch);
auto carr_error_filt_hz = std::vector<double>(num_epoch);
auto code_error_chips = std::vector<double>(num_epoch);
auto code_error_filt_chips = std::vector<double>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<double>(num_epoch);
auto carrier_lock_test = std::vector<double>(num_epoch);
auto aux1 = std::vector<double>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -434,24 +434,6 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_cc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -465,97 +447,79 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_cc::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
@@ -796,9 +760,9 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{

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

@@ -48,6 +48,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class glonass_l2_ca_dll_pll_c_aid_tracking_cc;
@@ -178,7 +179,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

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

@@ -172,7 +172,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::glonass_l2_ca_dll_pll_c_aid_tracking_sc
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -265,7 +265,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
glonass_l2_ca_code_gen_complex(d_ca_code, 0);
glonass_l2_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, static_cast<int32_t>(GLONASS_L2_CA_CODE_LENGTH_CHIPS)), 0);
volk_gnsssdr_32fc_convert_16ic(d_ca_code_16sc, d_ca_code, static_cast<int32_t>(GLONASS_L2_CA_CODE_LENGTH_CHIPS));
multicorrelator_cpu_16sc.set_local_code_and_taps(static_cast<int32_t>(GLONASS_L2_CA_CODE_LENGTH_CHIPS), d_ca_code_16sc, d_local_code_shift_chips);
@@ -332,24 +332,24 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_sc::save_matfile()
{
return 1;
}
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new double[num_epoch];
auto *carrier_doppler_hz = new double[num_epoch];
auto *code_freq_chips = new double[num_epoch];
auto *carr_error_hz = new double[num_epoch];
auto *carr_error_filt_hz = new double[num_epoch];
auto *code_error_chips = new double[num_epoch];
auto *code_error_filt_chips = new double[num_epoch];
auto *CN0_SNV_dB_Hz = new double[num_epoch];
auto *carrier_lock_test = new double[num_epoch];
auto *aux1 = new double[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<double>(num_epoch);
auto carrier_doppler_hz = std::vector<double>(num_epoch);
auto code_freq_chips = std::vector<double>(num_epoch);
auto carr_error_hz = std::vector<double>(num_epoch);
auto carr_error_filt_hz = std::vector<double>(num_epoch);
auto code_error_chips = std::vector<double>(num_epoch);
auto code_error_filt_chips = std::vector<double>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<double>(num_epoch);
auto carrier_lock_test = std::vector<double>(num_epoch);
auto aux1 = std::vector<double>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -382,24 +382,6 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_sc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -413,97 +395,79 @@ int32_t glonass_l2_ca_dll_pll_c_aid_tracking_sc::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
@@ -548,7 +512,6 @@ glonass_l2_ca_dll_pll_c_aid_tracking_sc::~glonass_l2_ca_dll_pll_c_aid_tracking_s
volk_gnsssdr_free(d_ca_code_16sc);
volk_gnsssdr_free(d_correlator_outs_16sc);
delete[] d_Prompt_buffer;
multicorrelator_cpu_16sc.free();
}
@@ -787,9 +750,9 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{

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

@@ -49,6 +49,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class glonass_l2_ca_dll_pll_c_aid_tracking_sc;
@@ -181,7 +182,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

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

@@ -145,7 +145,7 @@ Glonass_L2_Ca_Dll_Pll_Tracking_cc::Glonass_L2_Ca_Dll_Pll_Tracking_cc(
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -226,7 +226,7 @@ void Glonass_L2_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
glonass_l2_ca_code_gen_complex(d_ca_code, 0);
glonass_l2_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, static_cast<int32_t>(GLONASS_L2_CA_CODE_LENGTH_CHIPS)), 0);
multicorrelator_cpu.set_local_code_and_taps(static_cast<int32_t>(GLONASS_L2_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int32_t n = 0; n < d_n_correlator_taps; n++)
@@ -297,7 +297,6 @@ Glonass_L2_Ca_Dll_Pll_Tracking_cc::~Glonass_L2_Ca_Dll_Pll_Tracking_cc()
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
@@ -338,24 +337,24 @@ int32_t Glonass_L2_Ca_Dll_Pll_Tracking_cc::save_matfile()
{
return 1;
}
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new double[num_epoch];
auto *carrier_doppler_hz = new double[num_epoch];
auto *code_freq_chips = new double[num_epoch];
auto *carr_error_hz = new double[num_epoch];
auto *carr_error_filt_hz = new double[num_epoch];
auto *code_error_chips = new double[num_epoch];
auto *code_error_filt_chips = new double[num_epoch];
auto *CN0_SNV_dB_Hz = new double[num_epoch];
auto *carrier_lock_test = new double[num_epoch];
auto *aux1 = new double[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<double>(num_epoch);
auto carrier_doppler_hz = std::vector<double>(num_epoch);
auto code_freq_chips = std::vector<double>(num_epoch);
auto carr_error_hz = std::vector<double>(num_epoch);
auto carr_error_filt_hz = std::vector<double>(num_epoch);
auto code_error_chips = std::vector<double>(num_epoch);
auto code_error_filt_chips = std::vector<double>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<double>(num_epoch);
auto carrier_lock_test = std::vector<double>(num_epoch);
auto aux1 = std::vector<double>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<uint32_t>(num_epoch);
try
{
@@ -388,24 +387,6 @@ int32_t Glonass_L2_Ca_Dll_Pll_Tracking_cc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -419,97 +400,79 @@ int32_t Glonass_L2_Ca_Dll_Pll_Tracking_cc::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
@@ -656,9 +619,9 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES, GLONASS_L2_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{

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

@@ -45,6 +45,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class Glonass_L2_Ca_Dll_Pll_Tracking_cc;
@@ -146,7 +147,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

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

@@ -138,7 +138,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -217,7 +217,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0);
gps_l1_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, static_cast<int32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS)), d_acquisition_gnss_synchro->PRN, 0);
multicorrelator_gpu->set_local_code_and_taps(static_cast<int32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips, d_n_correlator_taps);
@@ -270,7 +270,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc()
cudaFreeHost(d_correlator_outs);
cudaFreeHost(d_local_code_shift_chips);
cudaFreeHost(d_ca_code);
delete[] d_Prompt_buffer;
multicorrelator_gpu->free_cuda();
delete (multicorrelator_gpu);
}
@@ -327,11 +326,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
double code_error_chips_Ti = 0.0;
double code_error_filt_chips = 0.0;
double code_error_filt_secs_Ti = 0.0;
double CURRENT_INTEGRATION_TIME_S;
double CORRECTED_INTEGRATION_TIME_S;
double CURRENT_INTEGRATION_TIME_S = 0.001;
double CORRECTED_INTEGRATION_TIME_S = 0.001;
double dll_code_error_secs_Ti = 0.0;
double carr_phase_error_secs_Ti = 0.0;
double old_d_rem_code_phase_samples;
if (d_enable_tracking == true)
{
// Fill the acquisition data
@@ -406,8 +404,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(K_blk_samples); //round to a discrete samples
old_d_rem_code_phase_samples = d_rem_code_phase_samples;
d_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
// UPDATE REMNANT CARRIER PHASE
@@ -439,9 +436,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{
@@ -524,14 +521,14 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// PLL commands
tmp_float = 1.0 / (d_carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S);
tmp_float = 1.0 / (carr_phase_error_secs_Ti * CURRENT_INTEGRATION_TIME_S);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = 1.0 / (d_code_error_filt_chips_Ti * CURRENT_INTEGRATION_TIME_S);
tmp_float = 1.0 / (code_error_filt_secs_Ti * CURRENT_INTEGRATION_TIME_S);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// DLL commands
tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S;
tmp_float = code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_code_error_filt_chips_Ti;
tmp_float = code_error_filt_secs_Ti;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// CN0 and carrier lock test
tmp_float = d_CN0_SNV_dB_Hz;
@@ -539,7 +536,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
tmp_float = d_carrier_lock_test;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// AUX vars (for debug purposes)
tmp_float = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S;
tmp_float = code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
double tmp_double = static_cast<double>(d_sample_counter + d_correlation_length_samples);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));

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

@@ -44,6 +44,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc;
@@ -157,7 +158,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex *d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

143
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc
View File

@@ -49,10 +49,12 @@
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <cmath>
#include <exception>
#include <gsl/gsl>
#include <iostream>
#include <memory>
#include <sstream>
#include <utility>
#include <vector>
gps_l1_ca_kf_tracking_cc_sptr
@@ -161,7 +163,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -320,7 +322,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0);
gps_l1_ca_code_gen_float(gsl::span<float>(d_ca_code, static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float)), d_acquisition_gnss_synchro->PRN, 0);
multicorrelator_cpu.set_local_code_and_taps(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int32_t n = 0; n < d_n_correlator_taps; n++)
@@ -392,7 +394,6 @@ Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc()
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
@@ -433,28 +434,28 @@ int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
{
return 1;
}
auto *abs_VE = new float[num_epoch];
auto *abs_E = new float[num_epoch];
auto *abs_P = new float[num_epoch];
auto *abs_L = new float[num_epoch];
auto *abs_VL = new float[num_epoch];
auto *Prompt_I = new float[num_epoch];
auto *Prompt_Q = new float[num_epoch];
auto *PRN_start_sample_count = new uint64_t[num_epoch];
auto *acc_carrier_phase_rad = new float[num_epoch];
auto *carrier_doppler_hz = new float[num_epoch];
auto *carrier_dopplerrate_hz2 = new float[num_epoch];
auto *code_freq_chips = new float[num_epoch];
auto *carr_error_hz = new float[num_epoch];
auto *carr_noise_sigma2 = new float[num_epoch];
auto *carr_error_filt_hz = new float[num_epoch];
auto *code_error_chips = new float[num_epoch];
auto *code_error_filt_chips = new float[num_epoch];
auto *CN0_SNV_dB_Hz = new float[num_epoch];
auto *carrier_lock_test = new float[num_epoch];
auto *aux1 = new float[num_epoch];
auto *aux2 = new double[num_epoch];
auto *PRN = new uint32_t[num_epoch];
auto abs_VE = std::vector<float>(num_epoch);
auto abs_E = std::vector<float>(num_epoch);
auto abs_P = std::vector<float>(num_epoch);
auto abs_L = std::vector<float>(num_epoch);
auto abs_VL = std::vector<float>(num_epoch);
auto Prompt_I = std::vector<float>(num_epoch);
auto Prompt_Q = std::vector<float>(num_epoch);
auto PRN_start_sample_count = std::vector<uint64_t>(num_epoch);
auto acc_carrier_phase_rad = std::vector<float>(num_epoch);
auto carrier_doppler_hz = std::vector<float>(num_epoch);
auto carrier_dopplerrate_hz2 = std::vector<float>(num_epoch);
auto code_freq_chips = std::vector<float>(num_epoch);
auto carr_error_hz = std::vector<float>(num_epoch);
auto carr_noise_sigma2 = std::vector<float>(num_epoch);
auto carr_error_filt_hz = std::vector<float>(num_epoch);
auto code_error_chips = std::vector<float>(num_epoch);
auto code_error_filt_chips = std::vector<float>(num_epoch);
auto CN0_SNV_dB_Hz = std::vector<float>(num_epoch);
auto carrier_lock_test = std::vector<float>(num_epoch);
auto aux1 = std::vector<float>(num_epoch);
auto aux2 = std::vector<double>(num_epoch);
auto PRN = std::vector<int32_t>(num_epoch);
try
{
@@ -491,28 +492,6 @@ int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_dopplerrate_hz2;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_noise_sigma2;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
@@ -526,117 +505,95 @@ int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0);
matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2, 0);
matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2, 0);
matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0);
matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0);
matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_dopplerrate_hz2;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_noise_sigma2;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
@@ -835,9 +792,9 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{

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

@@ -50,6 +50,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class Gps_L1_Ca_Kf_Tracking_cc;
@@ -198,7 +199,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;

9
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc
View File

@@ -149,7 +149,7 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_Prompt_buffer = std::vector<gr_complex>(FLAGS_cn0_samples);
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
@@ -223,7 +223,7 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::start_tracking()
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0);
gps_l1_ca_code_gen_complex(gsl::span<gr_complex>(d_ca_code, (GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex)), d_acquisition_gnss_synchro->PRN, 0);
multicorrelator_cpu.set_local_code_and_taps(static_cast<int32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int32_t n = 0; n < d_n_correlator_taps; n++)
@@ -276,7 +276,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::~Gps_L1_Ca_Tcp_Connector_Tracking_cc()
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
d_tcp_com.close_tcp_connection(d_port);
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
@@ -450,8 +449,8 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attrib
else
{
d_cn0_estimation_counter = 0;
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer.data(), FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer.data(), FLAGS_cn0_samples);
// ###### TRACKING UNLOCK NOTIFICATION #####
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)

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

@@ -44,6 +44,7 @@
#include <fstream>
#include <map>
#include <string>
#include <vector>
class Gps_L1_Ca_Tcp_Connector_Tracking_cc;
@@ -152,7 +153,7 @@ private:
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex *d_Prompt_buffer;
std::vector<gr_complex> d_Prompt_buffer;
float d_carrier_lock_test;
float d_CN0_SNV_dB_Hz;
float d_carrier_lock_threshold;

42
src/algorithms/tracking/libs/CMakeLists.txt
View File

@@ -17,18 +17,6 @@
#
if(ENABLE_CUDA)
# Append current NVCC flags by something, eg comput capability
# set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} --gpu-architecture sm_30)
list(APPEND CUDA_NVCC_FLAGS "-gencode arch=compute_30,code=sm_30; -std=c++11;-O3; -use_fast_math -default-stream per-thread")
set(CUDA_PROPAGATE_HOST_FLAGS OFF)
cuda_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
set(LIB_TYPE STATIC) #set the lib type
cuda_add_library(CUDA_CORRELATOR_LIB ${LIB_TYPE} cuda_multicorrelator.h cuda_multicorrelator.cu)
set(OPT_TRACKING_LIBRARIES ${OPT_TRACKING_LIBRARIES} CUDA_CORRELATOR_LIB)
set(OPT_TRACKING_INCLUDES ${OPT_TRACKING_INCLUDES} ${CUDA_INCLUDE_DIRS})
endif()
set(TRACKING_LIB_SOURCES
cpu_multicorrelator.cc
cpu_multicorrelator_real_codes.cc
@@ -63,6 +51,22 @@ set(TRACKING_LIB_HEADERS
exponential_smoother.h
)
if(ENABLE_CUDA)
if(CMAKE_VERSION VERSION_GREATER 3.11)
set(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} cuda_multicorrelator.cu)
set(TRACKING_LIB_HEADERS ${TRACKING_LIB_HEADERS} cuda_multicorrelator.h)
else()
list(APPEND CUDA_NVCC_FLAGS "-gencode arch=compute_30,code=sm_30; -std=c++11;-O3; -use_fast_math -default-stream per-thread")
set(CUDA_PROPAGATE_HOST_FLAGS OFF)
cuda_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
set(LIB_TYPE STATIC) #set the lib type
cuda_add_library(CUDA_CORRELATOR_LIB ${LIB_TYPE} cuda_multicorrelator.h cuda_multicorrelator.cu)
set(OPT_TRACKING_LIBRARIES ${OPT_TRACKING_LIBRARIES} CUDA_CORRELATOR_LIB)
set(OPT_TRACKING_INCLUDES ${OPT_TRACKING_INCLUDES} ${CUDA_INCLUDE_DIRS})
endif()
endif()
if(ARMADILLO_VERSION_STRING VERSION_GREATER 7.400)
# sqrtmat_sympd() requires 7.400
set(TRACKING_LIB_SOURCES ${TRACKING_LIB_SOURCES} nonlinear_tracking.cc)
@@ -95,10 +99,16 @@ target_link_libraries(tracking_libs
Glog::glog
)
target_include_directories(tracking_libs
PUBLIC
${OPT_TRACKING_INCLUDES}
)
if(NOT CMAKE_VERSION VERSION_GREATER 3.11)
target_link_libraries(tracking_libs
PUBLIC
${OPT_TRACKING_LIBRARIES}
)
target_include_directories(tracking_libs
PUBLIC
${OPT_TRACKING_INCLUDES}
)
endif()
if(Boost_VERSION VERSION_GREATER "106599")
target_compile_definitions(tracking_libs

18
src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h
View File

@@ -47,6 +47,15 @@
*/
class Tracking_2nd_DLL_filter
{
public:
void set_DLL_BW(float dll_bw_hz); //!< Set DLL filter bandwidth [Hz]
void set_pdi(float pdi_code); //!< Set Summation interval for code [s]
void initialize(); //!< Start tracking with acquisition information
float get_code_nco(float DLL_discriminator); //!< Numerically controlled oscillator
Tracking_2nd_DLL_filter(float pdi_code);
Tracking_2nd_DLL_filter();
~Tracking_2nd_DLL_filter();
private:
// PLL filter parameters
float d_tau1_code = 0.0;
@@ -57,15 +66,6 @@ private:
float d_old_code_error = 0.0;
float d_old_code_nco = 0.0;
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k);
public:
void set_DLL_BW(float dll_bw_hz); //! Set DLL filter bandwidth [Hz]
void set_pdi(float pdi_code); //! Set Summation interval for code [s]
void initialize(); //! Start tracking with acquisition information
float get_code_nco(float DLL_discriminator); //! Numerically controlled oscillator
Tracking_2nd_DLL_filter(float pdi_code);
Tracking_2nd_DLL_filter();
~Tracking_2nd_DLL_filter();
};
#endif

29
src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h
View File

@@ -46,28 +46,25 @@
*/
class Tracking_2nd_PLL_filter
{
private:
// PLL filter parameters
float d_tau1_carr = 0.0;
float d_tau2_carr = 0.0;
float d_pdi_carr = 0.0;
float d_pllnoisebandwidth = 0.0;
float d_plldampingratio = 0.0;
float d_old_carr_error = 0.0;
float d_old_carr_nco = 0.0;
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k);
public:
void set_PLL_BW(float pll_bw_hz); //! Set PLL loop bandwidth [Hz]
void set_pdi(float pdi_carr); //! Set Summation interval for code [s]
void set_PLL_BW(float pll_bw_hz); //!< Set PLL loop bandwidth [Hz]
void set_pdi(float pdi_carr); //!< Set Summation interval for code [s]
void initialize();
float get_carrier_nco(float PLL_discriminator);
Tracking_2nd_PLL_filter(float pdi_carr);
Tracking_2nd_PLL_filter();
~Tracking_2nd_PLL_filter();
private:
// PLL filter parameters
float d_tau1_carr = 0.0;
float d_tau2_carr = 0.0;
float d_pdi_carr = 0.0;
float d_pllnoisebandwidth = 0.0;
float d_plldampingratio = 0.0;
float d_old_carr_error = 0.0;
float d_old_carr_nco = 0.0;
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k);
};
#endif

14
src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h
View File

@@ -36,6 +36,13 @@
*/
class Tracking_FLL_PLL_filter
{
public:
void set_params(float fll_bw_hz, float pll_bw_hz, int order);
void initialize(float d_acq_carrier_doppler_hz);
float get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s);
Tracking_FLL_PLL_filter();
~Tracking_FLL_PLL_filter();
private:
// FLL + PLL filter parameters
int d_order;
@@ -49,13 +56,6 @@ private:
float d_pll_w0p2;
float d_pll_b3;
float d_pll_w0p;
public:
void set_params(float fll_bw_hz, float pll_bw_hz, int order);
void initialize(float d_acq_carrier_doppler_hz);
float get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s);
Tracking_FLL_PLL_filter();
~Tracking_FLL_PLL_filter();
};
#endif

42
src/algorithms/tracking/libs/tracking_loop_filter.h
View File

@@ -43,6 +43,27 @@
*/
class Tracking_loop_filter
{
public:
float get_noise_bandwidth(void) const;
float get_update_interval(void) const;
bool get_include_last_integrator(void) const;
int get_order(void) const;
void set_noise_bandwidth(float noise_bandwidth);
void set_update_interval(float update_interval);
void set_include_last_integrator(bool include_last_integrator);
void set_order(int loop_order);
void initialize(float initial_output = 0.0);
float apply(float current_input);
Tracking_loop_filter(float update_interval, float noise_bandwidth,
int loop_order = 2,
bool include_last_integrator = false);
Tracking_loop_filter();
~Tracking_loop_filter();
private:
// Store the last inputs and outputs:
std::vector<float> d_inputs;
@@ -71,27 +92,6 @@ private:
// Compute the filter coefficients:
void update_coefficients(void);
public:
float get_noise_bandwidth(void) const;
float get_update_interval(void) const;
bool get_include_last_integrator(void) const;
int get_order(void) const;
void set_noise_bandwidth(float noise_bandwidth);
void set_update_interval(float update_interval);
void set_include_last_integrator(bool include_last_integrator);
void set_order(int loop_order);
void initialize(float initial_output = 0.0);
float apply(float current_input);
Tracking_loop_filter(float update_interval, float noise_bandwidth,
int loop_order = 2,
bool include_last_integrator = false);
Tracking_loop_filter();
~Tracking_loop_filter();
};
#endif