From 59a991b884d2fb79731c1e966a5e4ad7de62d2fe Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 19 Mar 2018 11:19:55 +0100 Subject: [PATCH] Add adaptive Cloop-4quadrant discriminator --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 44 +++-- .../gnuradio_blocks/dll_pll_veml_tracking.h | 5 +- src/utils/matlab/dll_pll_veml_plot_sample.m | 84 ++++++++++ .../libs/dll_pll_veml_read_tracking_dump.m | 153 ++++++++++++++++++ src/utils/matlab/libs/plotVEMLTracking.m | 24 +-- 5 files changed, 282 insertions(+), 28 deletions(-) create mode 100644 src/utils/matlab/dll_pll_veml_plot_sample.m create mode 100644 src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 00c01ff95..9a0c97c18 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -126,6 +126,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // initialize internal vars d_dump = dump; d_veml = false; + d_cloop = true; d_synchonizing = false; d_track_pilot = track_pilot; d_fs_in = fs_in; @@ -535,6 +536,7 @@ void dll_pll_veml_tracking::start_tracking() // enable tracking pull-in d_state = 1; d_synchonizing = false; + d_cloop = true; d_Prompt_buffer_deque.clear(); d_last_prompt = gr_complex(0.0, 0.0); LOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz @@ -703,11 +705,11 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) } -void dll_pll_veml_tracking::run_dll_pll(bool enable_costas_loop) +void dll_pll_veml_tracking::run_dll_pll() { // ################## PLL ########################################################## // PLL discriminator - if (enable_costas_loop) + if (d_cloop) { // Costas loop discriminator, insensitive to 180 deg phase transitions d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; @@ -823,11 +825,18 @@ void dll_pll_veml_tracking::save_correlation_results() d_E_accu += *d_Early; d_P_accu += *d_Prompt; d_L_accu += *d_Late; + d_current_symbol++; + d_current_symbol %= d_symbols_per_bit; } + // If tracking pilot, disable Costas loop + if (d_track_pilot) + d_cloop = false; + else + d_cloop = true; } -void dll_pll_veml_tracking::log_data() +void dll_pll_veml_tracking::log_data(bool integrating) { if (d_dump) { @@ -876,6 +885,16 @@ void dll_pll_veml_tracking::log_data() tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); + if (integrating) + { + // It compensates the amplitude difference while integrating + float scale_factor = static_cast(d_extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); + tmp_VE *= scale_factor; + tmp_E *= scale_factor; + tmp_P *= scale_factor; + tmp_L *= scale_factor; + tmp_VL *= scale_factor; + } try { @@ -987,11 +1006,11 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { bool next_state = false; // Perform DLL/PLL tracking loop computations. Costas Loop enabled - run_dll_pll(true); + run_dll_pll(); update_tracking_vars(); // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); + log_data(false); if (d_secondary) { // ####### SECONDARY CODE LOCK ##### @@ -1139,6 +1158,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_extend_correlation_symbols_count = 0; d_state = 4; } + log_data(true); break; } case 4: // narrow tracking @@ -1158,17 +1178,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } else { - run_dll_pll(!d_secondary); - //EQUIVALENT TO THE LINE ABOVE if (d_secondary) - //{ - // //If secondary code is locked, disable Costas Loop - // run_dll_pll(false); - //} - //else - //{ - // //The signal does not have secondary code, enable Costas Loop - // run_dll_pll(true); - //} + run_dll_pll(); update_tracking_vars(); // ########### Output the tracking results to Telemetry block ########## @@ -1207,7 +1217,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = d_correlation_length_ms; // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); + log_data(false); // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index cf17cdf56..6d3ddef62 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -112,16 +112,17 @@ private: bool cn0_and_tracking_lock_status(); bool acquire_secondary(); void do_correlation_step(const gr_complex *input_samples); - void run_dll_pll(bool disable_costas_loop); + void run_dll_pll(); void update_tracking_vars(); void clear_tracking_vars(); void save_correlation_results(); - void log_data(); + void log_data(bool integrating); int save_matfile(); // tracking configuration vars bool d_dump; bool d_veml; + bool d_cloop; unsigned int d_vector_length; unsigned int d_channel; double d_fs_in; diff --git a/src/utils/matlab/dll_pll_veml_plot_sample.m b/src/utils/matlab/dll_pll_veml_plot_sample.m new file mode 100644 index 000000000..15b86f098 --- /dev/null +++ b/src/utils/matlab/dll_pll_veml_plot_sample.m @@ -0,0 +1,84 @@ +% /*! +% * \file dll_pll_vml_plot_sample.m +% * \brief Read GNSS-SDR Tracking dump binary file using the provided +% function and plot some internal variables +% * \author Javier Arribas, 2011. jarribas(at)cttc.es +% * \author Antonio Ramos, 2018. antonio.ramos(at)cttc.es +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% * +% * GNSS-SDR is a software defined Global Navigation +% * Satellite Systems receiver +% * +% * This file is part of GNSS-SDR. +% * +% * GNSS-SDR is free software: you can redistribute it and/or modify +% * it under the terms of the GNU General Public License as published by +% * the Free Software Foundation, either version 3 of the License, or +% * at your option) any later version. +% * +% * GNSS-SDR is distributed in the hope that it will be useful, +% * but WITHOUT ANY WARRANTY; without even the implied warranty of +% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% * GNU General Public License for more details. +% * +% * You should have received a copy of the GNU General Public License +% * along with GNSS-SDR. If not, see . +% * +% * ------------------------------------------------------------------------- +% */ +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m','file') + addpath('./libs') +end + +samplingFreq = 5000000; %[Hz] +coherent_integration_time_ms = 20; %[ms] +channels = 5; % Number of channels +first_channel = 0; % Number of the first channel + +path = '/dump_dir/'; %% CHANGE THIS PATH + +for N=1:1:channels + tracking_log_path = [path 'track_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch BY YOUR dump_filename + GNSS_tracking(N)= dll_pll_veml_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).P.'; + trackResults(N).Q_P = zeros(1,length(GNSS_tracking(N).P)); + + trackResults(N).I_VE = GNSS_tracking(N).VE.'; + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).I_VL = GNSS_tracking(N).VL.'; + trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE)); + trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L)); + trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); + trackResults(N).data_I = GNSS_tracking(N).prompt_I.'; + trackResults(N).data_Q = GNSS_tracking(N).prompt_Q.'; + trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E)*coherent_integration_time_ms; + plotVEMLTracking(N,trackResults,settings) +end + + + diff --git a/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m new file mode 100644 index 000000000..daa2324dd --- /dev/null +++ b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m @@ -0,0 +1,153 @@ +% /*! +% * \file dll_pll_veml_read_tracking_dump.m +% * \brief Read GNSS-SDR Tracking dump binary file into MATLAB. +% * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) +% * +% * GNSS-SDR is a software defined Global Navigation +% * Satellite Systems receiver +% * +% * This file is part of GNSS-SDR. +% * +% * GNSS-SDR is free software: you can redistribute it and/or modify +% * it under the terms of the GNU General Public License as published by +% * the Free Software Foundation, either version 3 of the License, or +% * at your option) any later version. +% * +% * GNSS-SDR is distributed in the hope that it will be useful, +% * but WITHOUT ANY WARRANTY; without even the implied warranty of +% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% * GNU General Public License for more details. +% * +% * You should have received a copy of the GNU General Public License +% * along with GNSS-SDR. If not, see . +% * +% * ------------------------------------------------------------------------- +% */ + +function [GNSS_tracking] = dll_pll_veml_read_tracking_dump (filename, count) + %% usage: dll_pll_veml_read_tracking_dump (filename, [count]) + %% + %% open GNSS-SDR tracking binary log file .dat and return the contents + %% + + m = nargchk (1,2,nargin); + + num_float_vars = 17; + num_unsigned_long_int_vars = 1; + num_double_vars = 1; + num_unsigned_int_vars = 1; + + if(~isempty(strfind(computer('arch'), '64'))) + % 64-bit computer + double_size_bytes = 8; + unsigned_long_int_size_bytes = 8; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; + else + double_size_bytes = 8; + unsigned_long_int_size_bytes = 4; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; + end + + skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ... + double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes; + + bytes_shift = 0; + + if (m) + usage (m); + end + + if (nargin < 2) + count = Inf; + end + %loops_counter = fread (f, count, 'uint32',4*12); + f = fopen (filename, 'rb'); + if (f < 0) + else + v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes); + bytes_shift = bytes_shift + unsigned_long_int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v18 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next double + v19 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); + bytes_shift = bytes_shift + double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next unsigned int + v20 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); + fclose (f); + + GNSS_tracking.VE = v1; + GNSS_tracking.E = v2; + GNSS_tracking.P = v3; + GNSS_tracking.L = v4; + GNSS_tracking.VL = v5; + GNSS_tracking.prompt_I = v6; + GNSS_tracking.prompt_Q = v7; + GNSS_tracking.PRN_start_sample = v8; + GNSS_tracking.acc_carrier_phase_rad = v9; + GNSS_tracking.carrier_doppler_hz = v10; + GNSS_tracking.code_freq_hz = v11; + GNSS_tracking.carr_error = v12; + GNSS_tracking.carr_nco = v13; + GNSS_tracking.code_error = v14; + GNSS_tracking.code_nco = v15; + GNSS_tracking.CN0_SNV_dB_Hz = v16; + GNSS_tracking.carrier_lock_test = v17; + GNSS_tracking.var1 = v18; + GNSS_tracking.var2 = v19; + GNSS_tracking.PRN = v20; + end + diff --git a/src/utils/matlab/libs/plotVEMLTracking.m b/src/utils/matlab/libs/plotVEMLTracking.m index 8af2225f9..f3846e164 100644 --- a/src/utils/matlab/libs/plotVEMLTracking.m +++ b/src/utils/matlab/libs/plotVEMLTracking.m @@ -69,8 +69,8 @@ for channelNr = channelList timeAxisInSeconds = (1:4:settings.msToProcess)/1000; %----- Discrete-Time Scatter Plot --------------------------------- - plot(handles(1, 1), trackResults(channelNr).I_P,... - trackResults(channelNr).Q_P, ... + plot(handles(1, 1), trackResults(channelNr).data_I,... + trackResults(channelNr).data_Q, ... '.'); grid (handles(1, 1)); @@ -80,8 +80,9 @@ for channelNr = channelList ylabel(handles(1, 1), 'Q prompt'); %----- Nav bits --------------------------------------------------- - plot (handles(1, 2), timeAxisInSeconds, ... - trackResults(channelNr).I_P); + t = (1:length(trackResults(channelNr).data_I)); + plot (handles(1, 2), t, ... + trackResults(channelNr).data_I); grid (handles(1, 2)); title (handles(1, 2), 'Bits of the navigation message'); @@ -89,7 +90,8 @@ for channelNr = channelList axis (handles(1, 2), 'tight'); %----- PLL discriminator unfiltered-------------------------------- - plot (handles(2, 1), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).pllDiscr)); + plot (handles(2, 1), t, ... trackResults(channelNr).pllDiscr, 'r'); grid (handles(2, 1)); @@ -99,7 +101,8 @@ for channelNr = channelList title (handles(2, 1), 'Raw PLL discriminator'); %----- Correlation ------------------------------------------------ - plot(handles(2, 2), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).I_VE)); + plot(handles(2, 2), t, ... [sqrt(trackResults(channelNr).I_VE.^2 + ... trackResults(channelNr).Q_VE.^2)', ... sqrt(trackResults(channelNr).I_E.^2 + ... @@ -127,7 +130,8 @@ for channelNr = channelList set(hLegend, 'Interpreter', 'Latex'); %----- PLL discriminator filtered---------------------------------- - plot (handles(3, 1), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).pllDiscrFilt)); + plot (handles(3, 1), t, ... trackResults(channelNr).pllDiscrFilt, 'b'); grid (handles(3, 1)); @@ -137,7 +141,8 @@ for channelNr = channelList title (handles(3, 1), 'Filtered PLL discriminator'); %----- DLL discriminator unfiltered-------------------------------- - plot (handles(3, 2), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).dllDiscr)); + plot (handles(3, 2), t, ... trackResults(channelNr).dllDiscr, 'r'); grid (handles(3, 2)); @@ -147,7 +152,8 @@ for channelNr = channelList title (handles(3, 2), 'Raw DLL discriminator'); %----- DLL discriminator filtered---------------------------------- - plot (handles(3, 3), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).dllDiscrFilt)); + plot (handles(3, 3), t, ... trackResults(channelNr).dllDiscrFilt, 'b'); grid (handles(3, 3));