[hamradio-commits] [gnss-sdr] 77/126: Carrier phase observable bug fix for GPS L1 CA trackings, new GPS L1 carrier aided tracking using nex multitap correlator library, and some CUDA multitap correlator performance improvements
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Sat Dec 26 18:38:03 UTC 2015
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch next
in repository gnss-sdr.
commit 8c22e5846d7449e6bf5e67401929813e1e55c15b
Author: Javier Arribas <jarribas at cttc.es>
Date: Mon Nov 30 11:41:42 2015 +0100
Carrier phase observable bug fix for GPS L1 CA trackings, new GPS L1 carrier aided tracking using nex multitap correlator library, and some CUDA multitap correlator performance improvements
---
conf/gnss-sdr_Galileo_E1_nsr.conf | 8 +-
conf/gnss-sdr_Hybrid_byte_sim.conf | 346 ++++++++++++++
src/algorithms/PVT/libs/rinex_printer.cc | 2 +-
.../gnuradio_blocks/galileo_e1_observables_cc.cc | 95 +++-
.../gnuradio_blocks/galileo_e1_observables_cc.h | 6 +
.../gnuradio_blocks/gps_l1_ca_observables_cc.cc | 86 +++-
.../gnuradio_blocks/gps_l1_ca_observables_cc.h | 10 +
.../gps_l1_ca_telemetry_decoder_cc.cc | 16 +-
.../gps_l1_ca_telemetry_decoder_cc.h | 7 +
src/algorithms/tracking/adapters/CMakeLists.txt | 1 +
.../adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc | 159 +++++++
.../adapters/gps_l1_ca_dll_pll_c_aid_tracking.h | 114 +++++
.../tracking/gnuradio_blocks/CMakeLists.txt | 1 +
.../galileo_e1_dll_pll_veml_tracking_cc.cc | 67 +--
.../galileo_e1_dll_pll_veml_tracking_cc.h | 18 +-
.../galileo_e1_tcp_connector_tracking_cc.cc | 2 +-
.../galileo_e5a_dll_pll_tracking_cc.cc | 121 ++---
.../galileo_e5a_dll_pll_tracking_cc.h | 34 +-
.../galileo_volk_e1_dll_pll_veml_tracking_cc.cc | 61 ++-
.../galileo_volk_e1_dll_pll_veml_tracking_cc.h | 18 +-
.../gps_l1_ca_dll_fll_pll_tracking_cc.cc | 5 +-
...c.cc => gps_l1_ca_dll_pll_c_aid_tracking_cc.cc} | 355 +++++++-------
..._cc.h => gps_l1_ca_dll_pll_c_aid_tracking_cc.h} | 78 ++-
.../gps_l1_ca_dll_pll_optim_tracking_cc.cc | 63 +--
.../gps_l1_ca_dll_pll_optim_tracking_cc.h | 20 +-
.../gps_l1_ca_dll_pll_tracking_cc.cc | 114 ++---
.../gps_l1_ca_dll_pll_tracking_cc.h | 20 +-
.../gps_l1_ca_dll_pll_tracking_gpu_cc.cc | 100 ++--
.../gps_l1_ca_dll_pll_tracking_gpu_cc.h | 4 -
.../gps_l2_m_dll_pll_tracking_cc.cc | 116 ++---
.../gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h | 20 +-
src/algorithms/tracking/libs/CMakeLists.txt | 1 +
.../tracking/libs/cpu_multicorrelator.cc | 167 +++++++
src/algorithms/tracking/libs/cpu_multicorrelator.h | 98 ++++
.../tracking/libs/cuda_multicorrelator.cu | 523 +++++++--------------
.../tracking/libs/cuda_multicorrelator.h | 24 +-
.../tracking/libs/tracking_discriminators.cc | 18 +-
.../tracking/libs/tracking_discriminators.h | 10 +-
src/core/receiver/gnss_block_factory.cc | 13 +
src/core/system_parameters/GPS_L1_CA.h | 4 +
src/core/system_parameters/Galileo_E1.h | 5 +
41 files changed, 1904 insertions(+), 1026 deletions(-)
diff --git a/conf/gnss-sdr_Galileo_E1_nsr.conf b/conf/gnss-sdr_Galileo_E1_nsr.conf
index 4764608..c9b3572 100644
--- a/conf/gnss-sdr_Galileo_E1_nsr.conf
+++ b/conf/gnss-sdr_Galileo_E1_nsr.conf
@@ -16,7 +16,7 @@ ControlThread.wait_for_flowgraph=false
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=Nsr_File_Signal_Source
-SignalSource.filename=/datalogger/signals/ifen/E1L1_FE0_Band0.stream
+SignalSource.filename=/Users/javier/signals/ifen/E1L1_FE0_Band0.stream
SignalSource.item_type=byte
SignalSource.sampling_frequency=20480000
SignalSource.freq=1575420000
@@ -110,15 +110,15 @@ TelemetryDecoder_1B.dump=false
;######### OBSERVABLES CONFIG ############
Observables.implementation=Galileo_E1B_Observables
-Observables.dump=false
+Observables.dump=true
Observables.dump_filename=./observables.dat
;######### PVT CONFIG ############
PVT.implementation=GALILEO_E1_PVT
-PVT.averaging_depth=10
+PVT.averaging_depth=1
PVT.flag_averaging=false
-PVT.output_rate_ms=10
+PVT.output_rate_ms=100
PVT.display_rate_ms=500
PVT.dump=true
PVT.dump_filename=./PVT
diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf
new file mode 100644
index 0000000..aedc605
--- /dev/null
+++ b/conf/gnss-sdr_Hybrid_byte_sim.conf
@@ -0,0 +1,346 @@
+; Default configuration file
+; You can define your own receiver and invoke it by doing
+; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf
+;
+
+[GNSS-SDR]
+
+;######### GLOBAL OPTIONS ##################
+;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz].
+GNSS-SDR.internal_fs_hz=2600000
+
+;######### CONTROL_THREAD CONFIG ############
+ControlThread.wait_for_flowgraph=false
+
+;######### SIGNAL_SOURCE CONFIG ############
+;#implementation: Use [File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental)
+SignalSource.implementation=File_Signal_Source
+
+;#filename: path to file with the captured GNSS signal samples to be processed
+SignalSource.filename=/Users/javier/gnss/gnss-simulator/build/signal_out.bin
+
+;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
+SignalSource.item_type=byte
+
+;#sampling_frequency: Original Signal sampling frequency in [Hz]
+SignalSource.sampling_frequency=2600000
+
+;#freq: RF front-end center frequency in [Hz]
+SignalSource.freq=1575420000
+
+;#samples: Number of samples to be processed. Notice that 0 indicates the entire file.
+SignalSource.samples=0
+
+;#repeat: Repeat the processing file. Disable this option in this version
+SignalSource.repeat=false
+
+;#dump: Dump the Signal source data to a file. Disable this option in this version
+SignalSource.dump=false
+
+SignalSource.dump_filename=../data/signal_source.dat
+
+
+;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing.
+; it helps to not overload the CPU, but the processing time will be longer.
+SignalSource.enable_throttle_control=false
+
+
+;######### SIGNAL_CONDITIONER CONFIG ############
+;## It holds blocks to change data type, filter and resample input data.
+
+;#implementation: Use [Pass_Through] or [Signal_Conditioner]
+;#[Pass_Through] disables this block and the [DataTypeAdapter], [InputFilter] and [Resampler] blocks
+;#[Signal_Conditioner] enables this block. Then you have to configure [DataTypeAdapter], [InputFilter] and [Resampler] blocks
+SignalConditioner.implementation=Signal_Conditioner
+
+;######### DATA_TYPE_ADAPTER CONFIG ############
+;## Changes the type of input data. Please disable it in this version.
+;#implementation: [Pass_Through] disables this block
+DataTypeAdapter.implementation=Ibyte_To_Complex
+
+;######### INPUT_FILTER CONFIG ############
+;## Filter the input data. Can be combined with frequency translation for IF signals
+
+;#implementation: Use [Pass_Through] or [Fir_Filter] or [Freq_Xlating_Fir_Filter]
+;#[Pass_Through] disables this block
+;#[Fir_Filter] enables a FIR Filter
+;#[Freq_Xlating_Fir_Filter] enables FIR filter and a composite frequency translation that shifts IF down to zero Hz.
+
+;InputFilter.implementation=Fir_Filter
+;InputFilter.implementation=Freq_Xlating_Fir_Filter
+InputFilter.implementation=Pass_Through
+
+;#dump: Dump the filtered data to a file.
+InputFilter.dump=false
+
+;#dump_filename: Log path and filename.
+InputFilter.dump_filename=../data/input_filter.dat
+
+;#The following options are used in the filter design of Fir_Filter and Freq_Xlating_Fir_Filter implementation.
+;#These options are based on parameters of gnuradio's function: gr_remez.
+;#These function calculates the optimal (in the Chebyshev/minimax sense) FIR filter inpulse reponse given a set of band edges, the desired reponse on those bands, and the weight given to the error in those bands.
+
+;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version.
+InputFilter.input_item_type=gr_complex
+
+;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version.
+InputFilter.output_item_type=gr_complex
+
+;#taps_item_type: Type and resolution for the taps of the filter. Use only float in this version.
+InputFilter.taps_item_type=float
+
+;#number_of_taps: Number of taps in the filter. Increasing this parameter increases the processing time
+InputFilter.number_of_taps=5
+
+;#number_of _bands: Number of frequency bands in the filter.
+InputFilter.number_of_bands=2
+
+;#bands: frequency at the band edges [ b1 e1 b2 e2 b3 e3 ...].
+;#Frequency is in the range [0, 1], with 1 being the Nyquist frequency (Fs/2)
+;#The number of band_begin and band_end elements must match the number of bands
+
+InputFilter.band1_begin=0.0
+InputFilter.band1_end=0.45
+InputFilter.band2_begin=0.55
+InputFilter.band2_end=1.0
+
+;#ampl: desired amplitude at the band edges [ a(b1) a(e1) a(b2) a(e2) ...].
+;#The number of ampl_begin and ampl_end elements must match the number of bands
+
+InputFilter.ampl1_begin=1.0
+InputFilter.ampl1_end=1.0
+InputFilter.ampl2_begin=0.0
+InputFilter.ampl2_end=0.0
+
+;#band_error: weighting applied to each band (usually 1).
+;#The number of band_error elements must match the number of bands
+InputFilter.band1_error=1.0
+InputFilter.band2_error=1.0
+
+;#filter_type: one of "bandpass", "hilbert" or "differentiator"
+InputFilter.filter_type=bandpass
+
+;#grid_density: determines how accurately the filter will be constructed.
+;The minimum value is 16; higher values are slower to compute the filter.
+InputFilter.grid_density=16
+
+;#The following options are used only in Freq_Xlating_Fir_Filter implementation.
+;#InputFilter.IF is the intermediate frequency (in Hz) shifted down to zero Hz
+
+InputFilter.sampling_frequency=2600000
+InputFilter.IF=0
+
+
+
+;######### RESAMPLER CONFIG ############
+;## Resamples the input data.
+
+;#implementation: Use [Pass_Through] or [Direct_Resampler]
+;#[Pass_Through] disables this block
+;#[Direct_Resampler] enables a resampler that implements a nearest neigbourhood interpolation
+;Resampler.implementation=Direct_Resampler
+Resampler.implementation=Pass_Through
+
+;#dump: Dump the resamplered data to a file.
+Resampler.dump=false
+;#dump_filename: Log path and filename.
+Resampler.dump_filename=../data/resampler.dat
+
+;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
+Resampler.item_type=gr_complex
+
+;#sample_freq_in: the sample frequency of the input signal
+Resampler.sample_freq_in=2600000
+
+;#sample_freq_out: the desired sample frequency of the output signal
+Resampler.sample_freq_out=2600000
+
+
+;######### CHANNELS GLOBAL CONFIG ############
+;#count: Number of available GPS satellite channels.
+Channels_1C.count=8
+;#count: Number of available Galileo satellite channels.
+Channels_1B.count=0
+;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver
+Channels.in_acquisition=1
+
+;#signal:
+;#if the option is disabled by default is assigned "1C" GPS L1 C/A
+Channel1.signal=1C
+Channel2.signal=1C
+Channel3.signal=1C
+Channel4.signal=1C
+Channel5.signal=1C
+Channel6.signal=1C
+Channel7.signal=1C
+Channel8.signal=1C
+Channel9.signal=1C
+Channel10.signal=1C
+Channel11.signal=1C
+Channel12.signal=1C
+Channel13.signal=1B
+Channel14.signal=1B
+Channel15.signal=1B
+
+
+;######### GPS ACQUISITION CONFIG ############
+
+;#dump: Enable or disable the acquisition internal data file logging [true] or [false]
+Acquisition_1C.dump=false
+;#filename: Log path and filename
+Acquisition_1C.dump_filename=./acq_dump.dat
+;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
+Acquisition_1C.item_type=gr_complex
+;#if: Signal intermediate frequency in [Hz]
+Acquisition_1C.if=0
+;#sampled_ms: Signal block duration for the acquisition signal detection [ms]
+Acquisition_1C.sampled_ms=1
+;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition]
+Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
+;#threshold: Acquisition threshold
+Acquisition_1C.threshold=0.035
+;#pfa: Acquisition false alarm probability. This option overrides the threshold option. Only use with implementations: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition]
+;Acquisition_1C.pfa=0.01
+;#doppler_max: Maximum expected Doppler shift [Hz]
+Acquisition_1C.doppler_max=6000
+;#doppler_max: Doppler step in the grid search [Hz]
+Acquisition_1C.doppler_step=100
+
+
+;######### GALILEO ACQUISITION CONFIG ############
+
+;#dump: Enable or disable the acquisition internal data file logging [true] or [false]
+Acquisition_1B.dump=false
+;#filename: Log path and filename
+Acquisition_1B.dump_filename=./acq_dump.dat
+;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
+Acquisition_1B.item_type=gr_complex
+;#if: Signal intermediate frequency in [Hz]
+Acquisition_1B.if=0
+;#sampled_ms: Signal block duration for the acquisition signal detection [ms]
+Acquisition_1B.sampled_ms=4
+;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition]
+Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition
+;#threshold: Acquisition threshold
+;Acquisition_1B.threshold=0
+;#pfa: Acquisition false alarm probability. This option overrides the threshold option. Only use with implementations: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition]
+Acquisition_1B.pfa=0.0000008
+;#doppler_max: Maximum expected Doppler shift [Hz]
+Acquisition_1B.doppler_max=15000
+;#doppler_max: Doppler step in the grid search [Hz]
+Acquisition_1B.doppler_step=125
+
+;######### TRACKING GPS CONFIG ############
+
+;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking]
+Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking
+;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version.
+Tracking_1C.item_type=gr_complex
+
+;#sampling_frequency: Signal Intermediate Frequency in [Hz]
+Tracking_1C.if=0
+
+;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false]
+Tracking_1C.dump=true
+
+;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number.
+Tracking_1C.dump_filename=../data/epl_tracking_ch_
+
+;#pll_bw_hz: PLL loop filter bandwidth [Hz]
+Tracking_1C.pll_bw_hz=20.0;
+
+;#dll_bw_hz: DLL loop filter bandwidth [Hz]
+Tracking_1C.dll_bw_hz=1.5;
+
+;#fll_bw_hz: FLL loop filter bandwidth [Hz]
+Tracking_1C.fll_bw_hz=2.0;
+
+;#order: PLL/DLL loop filter order [2] or [3]
+Tracking_1C.order=3;
+
+;######### TRACKING GALILEO CONFIG ############
+
+;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking]
+Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking
+;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version.
+Tracking_1B.item_type=gr_complex
+
+;#sampling_frequency: Signal Intermediate Frequency in [Hz]
+Tracking_1B.if=0
+
+;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false]
+Tracking_1B.dump=false
+
+;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number.
+Tracking_1B.dump_filename=../data/veml_tracking_ch_
+
+;#pll_bw_hz: PLL loop filter bandwidth [Hz]
+Tracking_1B.pll_bw_hz=15.0;
+
+;#dll_bw_hz: DLL loop filter bandwidth [Hz]
+Tracking_1B.dll_bw_hz=2.0;
+
+;#fll_bw_hz: FLL loop filter bandwidth [Hz]
+Tracking_1B.fll_bw_hz=10.0;
+
+;#order: PLL/DLL loop filter order [2] or [3]
+Tracking_1B.order=3;
+
+;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo
+Tracking_1B.early_late_space_chips=0.15;
+
+;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6]
+Tracking_1B.very_early_late_space_chips=0.6;
+
+
+;######### TELEMETRY DECODER GPS CONFIG ############
+;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A
+TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
+TelemetryDecoder_1C.dump=false
+;#decimation factor
+TelemetryDecoder_1C.decimation_factor=1;
+
+;######### TELEMETRY DECODER GALILEO CONFIG ############
+;#implementation: Use [Galileo_E1B_Telemetry_Decoder] for Galileo E1B
+TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder
+TelemetryDecoder_1B.dump=false
+TelemetryDecoder_1B.decimation_factor=1;
+
+;######### OBSERVABLES CONFIG ############
+;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A.
+Observables.implementation=GPS_L1_CA_Observables
+
+;#dump: Enable or disable the Observables internal binary data file logging [true] or [false]
+Observables.dump=true
+
+;#dump_filename: Log path and filename.
+Observables.dump_filename=./observables.dat
+
+
+;######### PVT CONFIG ############
+;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version.
+PVT.implementation=GPS_L1_CA_PVT
+
+;#averaging_depth: Number of PVT observations in the moving average algorithm
+PVT.averaging_depth=10
+
+;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false]
+PVT.flag_averaging=false
+
+;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms]
+PVT.output_rate_ms=100;
+
+;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms.
+PVT.display_rate_ms=500;
+
+;#dump: Enable or disable the PVT internal binary data file logging [true] or [false]
+PVT.dump=false
+
+;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump.
+PVT.dump_filename=./PVT
+
+;######### OUTPUT_FILTER CONFIG ############
+;# Receiver output filter: Leave this block disabled in this version
+OutputFilter.implementation=Null_Sink_Output_Filter
+OutputFilter.filename=data/gnss-sdr.dat
+OutputFilter.item_type=gr_complex
diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc
index a65142a..1f2ede3 100644
--- a/src/algorithms/PVT/libs/rinex_printer.cc
+++ b/src/algorithms/PVT/libs/rinex_printer.cc
@@ -2992,7 +2992,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int>(ssi), 1);
// Galileo E1B PHASE
- lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Carrier_phase_rads / (2 * GALILEO_PI), 3), 14);
+ lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
index ea542db..7dd8207 100644
--- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
@@ -41,6 +41,7 @@
#include <glog/logging.h>
#include "control_message_factory.h"
#include "gnss_synchro.h"
+#include "Galileo_E1.h"
using google::LogMessage;
@@ -65,6 +66,13 @@ galileo_e1_observables_cc::galileo_e1_observables_cc(unsigned int nchannels, boo
d_dump_filename = dump_filename;
d_flag_averaging = flag_averaging;
+ for (int i=0;i<d_nchannels;i++)
+ {
+ d_acc_carrier_phase_queue_rads.push_back(std::deque<double>(d_nchannels));
+ d_carrier_doppler_queue_hz.push_back(std::deque<double>(d_nchannels));
+ d_symbol_TOW_queue_s.push_back(std::deque<double>(d_nchannels));
+ }
+
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
@@ -129,11 +137,39 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
*/
current_gnss_synchro[i].Flag_valid_pseudorange = false;
current_gnss_synchro[i].Pseudorange_m = 0.0;
- if (current_gnss_synchro[i].Flag_valid_word)
- {
- //record the word structure in a map for pseudorange computation
- current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
- }
+
+ if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word
+ {
+ //record the word structure in a map for pseudorange computation
+ current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
+
+ //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE #######
+ d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz);
+ d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads);
+ // save TOW history
+ d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol);
+
+ if (d_carrier_doppler_queue_hz[i].size()>GALILEO_E1_HISTORY_DEEP)
+ {
+ d_carrier_doppler_queue_hz[i].pop_front();
+ }
+ if (d_acc_carrier_phase_queue_rads[i].size()>GALILEO_E1_HISTORY_DEEP)
+ {
+ d_acc_carrier_phase_queue_rads[i].pop_front();
+ }
+ if (d_symbol_TOW_queue_s[i].size()>GALILEO_E1_HISTORY_DEEP)
+ {
+ d_symbol_TOW_queue_s[i].pop_front();
+ }
+ }else{
+ // Clear the observables history for this channel
+ if (d_symbol_TOW_queue_s[i].size()>0)
+ {
+ d_symbol_TOW_queue_s[i].clear();
+ d_carrier_doppler_queue_hz[i].clear();
+ d_acc_carrier_phase_queue_rads[i].clear();
+ }
+ }
}
/*
@@ -155,18 +191,47 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
double traveltime_ms;
double pseudorange_m;
double delta_rx_time_ms;
+ arma::vec symbol_TOW_vec_s;
+ arma::vec dopper_vec_hz;
+ arma::vec dopper_vec_interp_hz;
+ arma::vec acc_phase_vec_rads;
+ arma::vec acc_phase_vec_interp_rads;
+ arma::vec desired_symbol_TOW(1);
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{
- // compute the required symbol history shift in order to match the reference symbol
- delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms;
- //compute the pseudorange
- traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms;
- pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m]
- // update the pseudorange object
- //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GALILEO_STARTOFFSET_ms/1000.0;
+ // compute the required symbol history shift in order to match the reference symbol
+ delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms;
+ //compute the pseudorange
+ traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms;
+ pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
+ // update the pseudorange object
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GALILEO_STARTOFFSET_ms/1000.0;
+
+ if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size()>=GPS_L1_CA_HISTORY_DEEP)
+ {
+ // compute interpolated observation values for Doppler and Accumulate carrier phase
+ symbol_TOW_vec_s=arma::vec(std::vector<double>(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end()));
+ acc_phase_vec_rads=arma::vec(std::vector<double>(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end()));
+ dopper_vec_hz=arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
+ desired_symbol_TOW[0]=symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]+delta_rx_time_ms/1000.0;
+ // Curve fitting to cuadratic function
+ arma::mat A=arma::ones<arma::mat> (GPS_L1_CA_HISTORY_DEEP,2);
+ A.col(1)=symbol_TOW_vec_s;
+ //A.col(2)=symbol_TOW_vec_s % symbol_TOW_vec_s;
+ arma::mat coef_acc_phase(1,3);
+ coef_acc_phase=arma::pinv(A.t()*A)*A.t()*acc_phase_vec_rads;
+ arma::mat coef_doppler(1,3);
+ coef_doppler=arma::pinv(A.t()*A)*A.t()*dopper_vec_hz;
+ arma::vec acc_phase_lin;
+ arma::vec carrier_doppler_lin;
+ acc_phase_lin=coef_acc_phase[0]+coef_acc_phase[1]*desired_symbol_TOW[0];//+coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
+ carrier_doppler_lin=coef_doppler[0]+coef_doppler[1]*desired_symbol_TOW[0];//+coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads =acc_phase_lin[0];
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz =carrier_doppler_lin[0];
+ }
}
}
diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h
index 131d76f..e626d25 100644
--- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h
+++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h
@@ -41,6 +41,7 @@
#include <boost/thread/thread.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
+#include <armadillo>
#include "concurrent_queue.h"
#include "galileo_navigation_message.h"
#include "rinex_printer.h"
@@ -70,6 +71,11 @@ private:
galileo_e1_make_observables_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging);
galileo_e1_observables_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging);
+ //Tracking observable history
+ std::vector<std::deque<double>> d_acc_carrier_phase_queue_rads;
+ std::vector<std::deque<double>> d_carrier_doppler_queue_hz;
+ std::vector<std::deque<double>> d_symbol_TOW_queue_s;
+
// class private vars
boost::shared_ptr<gr::msg_queue> d_queue;
bool d_dump;
diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
index 8df2646..192fc03 100644
--- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
@@ -63,6 +63,13 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, boost
d_dump_filename = dump_filename;
d_flag_averaging = flag_averaging;
+ for (int i=0;i<d_nchannels;i++)
+ {
+ d_acc_carrier_phase_queue_rads.push_back(std::deque<double>(d_nchannels));
+ d_carrier_doppler_queue_hz.push_back(std::deque<double>(d_nchannels));
+ d_symbol_TOW_queue_s.push_back(std::deque<double>(d_nchannels));
+ }
+
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
@@ -128,6 +135,33 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{
//record the word structure in a map for pseudorange computation
current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
+
+ //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE #######
+ d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz);
+ d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads);
+ // save TOW history
+ d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol);
+
+ if (d_carrier_doppler_queue_hz[i].size()>GPS_L1_CA_HISTORY_DEEP)
+ {
+ d_carrier_doppler_queue_hz[i].pop_front();
+ }
+ if (d_acc_carrier_phase_queue_rads[i].size()>GPS_L1_CA_HISTORY_DEEP)
+ {
+ d_acc_carrier_phase_queue_rads[i].pop_front();
+ }
+ if (d_symbol_TOW_queue_s[i].size()>GPS_L1_CA_HISTORY_DEEP)
+ {
+ d_symbol_TOW_queue_s[i].pop_front();
+ }
+ }else{
+ // Clear the observables history for this channel
+ if (d_symbol_TOW_queue_s[i].size()>0)
+ {
+ d_symbol_TOW_queue_s[i].clear();
+ d_carrier_doppler_queue_hz[i].clear();
+ d_acc_carrier_phase_queue_rads[i].clear();
+ }
}
}
@@ -150,6 +184,12 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
double traveltime_ms;
double pseudorange_m;
double delta_rx_time_ms;
+ arma::vec symbol_TOW_vec_s;
+ arma::vec dopper_vec_hz;
+ arma::vec dopper_vec_interp_hz;
+ arma::vec acc_phase_vec_rads;
+ arma::vec acc_phase_vec_interp_rads;
+ arma::vec desired_symbol_TOW(1);
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{
// compute the required symbol history shift in order to match the reference symbol
@@ -160,8 +200,45 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
// update the pseudorange object
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GPS_STARTOFFSET_ms/1000.0;
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GPS_STARTOFFSET_ms/1000.0;
+
+ if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size()>=GPS_L1_CA_HISTORY_DEEP)
+ {
+ // compute interpolated observation values for Doppler and Accumulate carrier phase
+ symbol_TOW_vec_s=arma::vec(std::vector<double>(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end()));
+ acc_phase_vec_rads=arma::vec(std::vector<double>(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end()));
+ dopper_vec_hz=arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
+
+ //std::cout<<"symbol_TOW_vec_s[0]="<<symbol_TOW_vec_s[0]<<std::endl;
+ //std::cout<<"symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]="<<symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]<<std::endl;
+ //std::cout<<"acc_phase_vec_rads="<<acc_phase_vec_rads<<std::endl;
+ //std::cout<<"dopper_vec_hz="<<dopper_vec_hz<<std::endl;
+
+ desired_symbol_TOW[0]=symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]+delta_rx_time_ms/1000.0;
+ //std::cout<<"desired_symbol_TOW="<<desired_symbol_TOW[0]<<std::endl;
+
+// arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz);
+// arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads);
+
+ // Curve fitting to cuadratic function
+ arma::mat A=arma::ones<arma::mat> (GPS_L1_CA_HISTORY_DEEP,2);
+ A.col(1)=symbol_TOW_vec_s;
+ //A.col(2)=symbol_TOW_vec_s % symbol_TOW_vec_s;
+ arma::mat coef_acc_phase(1,3);
+ coef_acc_phase=arma::pinv(A.t()*A)*A.t()*acc_phase_vec_rads;
+ arma::mat coef_doppler(1,3);
+ coef_doppler=arma::pinv(A.t()*A)*A.t()*dopper_vec_hz;
+ arma::vec acc_phase_lin;
+ arma::vec carrier_doppler_lin;
+ acc_phase_lin=coef_acc_phase[0]+coef_acc_phase[1]*desired_symbol_TOW[0];//+coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
+ carrier_doppler_lin=coef_doppler[0]+coef_doppler[1]*desired_symbol_TOW[0];//+coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
+ //std::cout<<"acc_phase_vec_interp_rads="<<acc_phase_vec_interp_rads[0]<<std::endl;
+ //std::cout<<"dopper_vec_interp_hz="<<dopper_vec_interp_hz[0]<<std::endl;
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads =acc_phase_lin[0];
+ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz =carrier_doppler_lin[0];
+ }
+
}
}
@@ -175,7 +252,10 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double));
- tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
+ //tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
+ tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz;
+ d_dump_file.write((char*)&tmp_double, sizeof(double));
+ tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
index 7f7dacb..56cfd6d 100644
--- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
+++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
@@ -33,12 +33,16 @@
#include <fstream>
#include <queue>
+#include <deque>
+#include <vector>
#include <string>
#include <utility>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
+#include <boost/shared_ptr.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
+#include <armadillo>
#include "concurrent_queue.h"
#include "gps_navigation_message.h"
#include "rinex_printer.h"
@@ -68,6 +72,12 @@ private:
gps_l1_ca_make_observables_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging);
gps_l1_ca_observables_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging);
+
+ //Tracking observable history
+ std::vector<std::deque<double>> d_acc_carrier_phase_queue_rads;
+ std::vector<std::deque<double>> d_carrier_doppler_queue_hz;
+ std::vector<std::deque<double>> d_symbol_TOW_queue_s;
+
// class private vars
boost::shared_ptr<gr::msg_queue> d_queue;
bool d_dump;
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
index 260f6de..604379d 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
@@ -135,6 +135,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_decimation_output_factor = 1;
d_channel = 0;
Prn_timestamp_at_preamble_ms = 0.0;
+ flag_PLL_180_deg_phase_locked=false;
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
}
@@ -224,6 +225,13 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
+ if (corr_value<0)
+ {
+ flag_PLL_180_deg_phase_locked=true; //PLL is locked to opposite phase!
+ std::cout<<"PLL in opposite phase for Sat "<<this->d_satellite.get_PRN()<<std::endl;
+ }else{
+ flag_PLL_180_deg_phase_locked=false;
+ }
LOG(INFO) <<" Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
}
}
@@ -313,7 +321,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
// Sice we detected the preable, then, we are in the last symbol of that preamble, or just at the start of the first subframe symbol.
{
d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_SUBFRAME_SECONDS; //we decoded the current TOW when the last word of the subframe arrive, so, we have a lag of ONE SUBFRAME
- d_TOW_at_current_symbol = d_TOW_at_Preamble;//GPS_L1_CA_CODE_PERIOD;// + (double)GPS_CA_PREAMBLE_LENGTH_BITS/(double)GPS_CA_TELEMETRY_RATE_BITS_SECOND;
+ d_TOW_at_current_symbol = d_TOW_at_Preamble;
Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
if (flag_TOW_set == false)
{
@@ -327,13 +335,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
current_synchro_data.d_TOW = d_TOW_at_Preamble;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
-
current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol; // to be used in the hybrid configuration
current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set == true);
current_synchro_data.Flag_preamble = d_flag_preamble;
current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;
+ if (flag_PLL_180_deg_phase_locked==true)
+ {
+ //correct the accumulated phase for the costas loop phase shift, if required
+ current_synchro_data.Carrier_phase_rads+=GPS_PI;
+ }
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
index f9c5957..10f305c 100644
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
@@ -35,6 +35,7 @@
#include <string>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
+#include <deque>
#include "GPS_L1_CA.h"
#include "gps_l1_ca_subframe_fsm.h"
#include "concurrent_queue.h"
@@ -142,8 +143,14 @@ private:
double d_TOW_at_Preamble;
double d_TOW_at_current_symbol;
+ std::deque<double> d_symbol_TOW_queue_s;
+ // Doppler and Phase accumulator queue for interpolation in Observables
+ std::deque<double> d_carrier_doppler_queue_hz;
+ std::deque<double> d_acc_carrier_phase_queue_rads;
+
double Prn_timestamp_at_preamble_ms;
bool flag_TOW_set;
+ bool flag_PLL_180_deg_phase_locked;
std::string d_dump_filename;
std::ofstream d_dump_file;
diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt
index 55029b0..993c578 100644
--- a/src/algorithms/tracking/adapters/CMakeLists.txt
+++ b/src/algorithms/tracking/adapters/CMakeLists.txt
@@ -28,6 +28,7 @@ set(TRACKING_ADAPTER_SOURCES
gps_l1_ca_dll_fll_pll_tracking.cc
gps_l1_ca_dll_pll_optim_tracking.cc
gps_l1_ca_dll_pll_tracking.cc
+ gps_l1_ca_dll_pll_c_aid_tracking.cc
gps_l1_ca_tcp_connector_tracking.cc
galileo_e5a_dll_pll_tracking.cc
gps_l2_m_dll_pll_tracking.cc
diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc
new file mode 100644
index 0000000..08e24f8
--- /dev/null
+++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc
@@ -0,0 +1,159 @@
+/*!
+ * \file gps_l1_ca_dll_pll_c_aid_tracking.cc
+ * \brief Implementation of an adapter of a DLL+PLL tracking loop block
+ * for GPS L1 C/A to a TrackingInterface
+ * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
+ * Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Code DLL + carrier PLL according to the algorithms described in:
+ * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
+ * A Software-Defined GPS and Galileo Receiver. A Single-Frequency
+ * Approach, Birkhauser, 2007
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+
+#include "gps_l1_ca_dll_pll_c_aid_tracking.h"
+#include <glog/logging.h>
+#include "GPS_L1_CA.h"
+#include "configuration_interface.h"
+
+
+using google::LogMessage;
+
+GpsL1CaDllPllCAidTracking::GpsL1CaDllPllCAidTracking(
+ ConfigurationInterface* configuration, std::string role,
+ unsigned int in_streams, unsigned int out_streams,
+ boost::shared_ptr<gr::msg_queue> queue) :
+ role_(role), in_streams_(in_streams), out_streams_(out_streams),
+ queue_(queue)
+{
+ DLOG(INFO) << "role " << role;
+ //################# CONFIGURATION PARAMETERS ########################
+ int fs_in;
+ int vector_length;
+ int f_if;
+ bool dump;
+ std::string dump_filename;
+ std::string item_type;
+ std::string default_item_type = "gr_complex";
+ float pll_bw_hz;
+ float dll_bw_hz;
+ float early_late_space_chips;
+ item_type = configuration->property(role + ".item_type", default_item_type);
+ //vector_length = configuration->property(role + ".vector_length", 2048);
+ fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
+ f_if = configuration->property(role + ".if", 0);
+ dump = configuration->property(role + ".dump", false);
+ pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
+ dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
+ early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
+ std::string default_dump_filename = "./track_ch";
+ dump_filename = configuration->property(role + ".dump_filename",
+ default_dump_filename); //unused!
+ vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
+
+ //################# MAKE TRACKING GNURadio object ###################
+ if (item_type.compare("gr_complex") == 0)
+ {
+ item_size_ = sizeof(gr_complex);
+ tracking_ = gps_l1_ca_dll_pll_c_aid_make_tracking_cc(
+ f_if,
+ fs_in,
+ vector_length,
+ queue_,
+ dump,
+ dump_filename,
+ pll_bw_hz,
+ dll_bw_hz,
+ early_late_space_chips);
+ }
+ else
+ {
+ item_size_ = sizeof(gr_complex);
+ LOG(WARNING) << item_type << " unknown tracking item type.";
+ }
+ channel_ = 0;
+ channel_internal_queue_ = 0;
+ DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
+}
+
+
+GpsL1CaDllPllCAidTracking::~GpsL1CaDllPllCAidTracking()
+{}
+
+
+void GpsL1CaDllPllCAidTracking::start_tracking()
+{
+ tracking_->start_tracking();
+}
+
+/*
+ * Set tracking channel unique ID
+ */
+void GpsL1CaDllPllCAidTracking::set_channel(unsigned int channel)
+{
+ channel_ = channel;
+ tracking_->set_channel(channel);
+}
+
+/*
+ * Set tracking channel internal queue
+ */
+void GpsL1CaDllPllCAidTracking::set_channel_queue(
+ concurrent_queue<int> *channel_internal_queue)
+{
+ channel_internal_queue_ = channel_internal_queue;
+ tracking_->set_channel_queue(channel_internal_queue_);
+}
+
+void GpsL1CaDllPllCAidTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
+{
+ tracking_->set_gnss_synchro(p_gnss_synchro);
+}
+
+void GpsL1CaDllPllCAidTracking::connect(gr::top_block_sptr top_block)
+{
+ if(top_block) { /* top_block is not null */};
+ //nothing to connect, now the tracking uses gr_sync_decimator
+}
+
+void GpsL1CaDllPllCAidTracking::disconnect(gr::top_block_sptr top_block)
+{
+ if(top_block) { /* top_block is not null */};
+ //nothing to disconnect, now the tracking uses gr_sync_decimator
+}
+
+gr::basic_block_sptr GpsL1CaDllPllCAidTracking::get_left_block()
+{
+ return tracking_;
+}
+
+gr::basic_block_sptr GpsL1CaDllPllCAidTracking::get_right_block()
+{
+ return tracking_;
+}
+
diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.h
new file mode 100644
index 0000000..d643df5
--- /dev/null
+++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.h
@@ -0,0 +1,114 @@
+/*!
+ * \file gps_l1_ca_dll_pll_c_aid_tracking.h
+ * \brief Interface of an adapter of a DLL+PLL tracking loop block
+ * for GPS L1 C/A to a TrackingInterface
+ * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
+ * Javier Arribas, 2011. jarribas(at)cttc.es
+ *
+ * Code DLL + carrier PLL according to the algorithms described in:
+ * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
+ * A Software-Defined GPS and Galileo Receiver. A Single-Frequency
+ * Approach, Birkha user, 2007
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_H_
+#define GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_H_
+
+#include <string>
+#include <gnuradio/msg_queue.h>
+#include "tracking_interface.h"
+#include "gps_l1_ca_dll_pll_c_aid_tracking_cc.h"
+
+
+class ConfigurationInterface;
+
+/*!
+ * \brief This class implements a code DLL + carrier PLL tracking loop
+ */
+class GpsL1CaDllPllCAidTracking : public TrackingInterface
+{
+public:
+
+ GpsL1CaDllPllCAidTracking(ConfigurationInterface* configuration,
+ std::string role,
+ unsigned int in_streams,
+ unsigned int out_streams,
+ boost::shared_ptr<gr::msg_queue> queue);
+
+ virtual ~GpsL1CaDllPllCAidTracking();
+
+ std::string role()
+ {
+ return role_;
+ }
+
+ //! Returns "gps_l1_ca_dll_pll_c_aid_tracking"
+ std::string implementation()
+ {
+ return "gps_l1_ca_dll_pll_c_aid_tracking";
+ }
+ size_t item_size()
+ {
+ return item_size_;
+ }
+
+ void connect(gr::top_block_sptr top_block);
+ void disconnect(gr::top_block_sptr top_block);
+ gr::basic_block_sptr get_left_block();
+ gr::basic_block_sptr get_right_block();
+
+
+ /*!
+ * \brief Set tracking channel unique ID
+ */
+ void set_channel(unsigned int channel);
+
+ /*!
+ * \brief Set acquisition/tracking common Gnss_Synchro object pointer
+ * to efficiently exchange synchronization data between acquisition and tracking blocks
+ */
+ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
+
+ /*!
+ * \brief Set tracking channel internal queue
+ */
+ void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
+
+ void start_tracking();
+
+private:
+ gps_l1_ca_dll_pll_c_aid_tracking_cc_sptr tracking_;
+ size_t item_size_;
+ unsigned int channel_;
+ std::string role_;
+ unsigned int in_streams_;
+ unsigned int out_streams_;
+ boost::shared_ptr<gr::msg_queue> queue_;
+ concurrent_queue<int> *channel_internal_queue_;
+};
+
+#endif // GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_H_
diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt
index cadba3e..bd6bb71 100644
--- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt
+++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt
@@ -33,6 +33,7 @@ set(TRACKING_GR_BLOCKS_SOURCES
gps_l1_ca_tcp_connector_tracking_cc.cc
galileo_e5a_dll_pll_tracking_cc.cc
gps_l2_m_dll_pll_tracking_cc.cc
+ gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
${OPT_TRACKING_BLOCKS}
)
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
index 0ea9258..e98d99c 100755
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
@@ -236,7 +236,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
{
double tcode_half_chips;
- float rem_code_phase_half_chips;
+ double rem_code_phase_half_chips;
int associated_chip_index;
int code_length_half_chips = static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS) * 2;
double code_phase_step_chips;
@@ -246,11 +246,11 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
int epl_loop_length_samples;
// unified loop for VE, E, P, L, VL code vectors
- code_phase_step_chips = (static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
- code_phase_step_half_chips = (2.0 * static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
+ code_phase_step_chips = d_code_freq_chips / (static_cast<double>(d_fs_in));
+ code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast<double>(d_fs_in));
rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in);
- tcode_half_chips = - static_cast<double>(rem_code_phase_half_chips);
+ tcode_half_chips = - rem_code_phase_half_chips;
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips);
@@ -310,10 +310,14 @@ galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc()
int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
- float carr_error_hz;
- float carr_error_filt_hz;
- float code_error_chips;
- float code_error_filt_chips;
+ double carr_error_hz;
+ carr_error_hz=0.0;
+ double carr_error_filt_hz;
+ carr_error_filt_hz=0.0;
+ double code_error_chips;
+ code_error_chips=0.0;
+ double code_error_filt_chips;
+ code_error_filt_chips=0.0;
if (d_enable_tracking == true)
{
@@ -323,7 +327,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
* Signal alignment (skip samples until the incoming signal is aligned with local replica)
*/
int samples_offset;
- float acq_trk_shif_correction_samples;
+ double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
@@ -364,7 +368,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// ################## PLL ##########################################################
// PLL discriminator
- carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
+ carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GALILEO_TWO_PI;
// Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation
@@ -372,10 +376,10 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// New code Doppler frequency estimation
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ);
//carrier phase accumulator for (K) Doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
+ d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * d_current_prn_length_samples/static_cast<double>(d_fs_in);
//remnant carrier phase to prevent overflow in the code NCO
- d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
- d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
+ d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * d_current_prn_length_samples/static_cast<double>(d_fs_in);
+ d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI);
// ################## DLL ##########################################################
// DLL discriminator
@@ -383,7 +387,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator
- float code_error_filt_secs;
+ double code_error_filt_secs;
code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds]
//code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*static_cast<float>(d_fs_in); //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
@@ -395,7 +399,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
double T_prn_samples;
double K_blk_samples;
// Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation
- T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
+ T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
@@ -460,9 +464,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
- current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
- current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
- current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
+ current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_pseudorange = false;
*out[0] = current_synchro_data;
@@ -547,19 +551,28 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
+ tmp_float=d_acc_carrier_phase_rad;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
+ tmp_float=d_carrier_doppler_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ tmp_float=d_code_freq_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
+ tmp_float=carr_error_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ tmp_float=carr_error_filt_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
+ tmp_float=code_error_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ tmp_float=code_error_filt_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
+ tmp_float=d_CN0_SNV_dB_Hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ 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_rem_code_phase_samples;
d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h
index 10b0738..c42eb04 100755
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h
@@ -126,8 +126,8 @@ private:
long d_if_freq;
long d_fs_in;
- float d_early_late_spc_chips;
- float d_very_early_late_spc_chips;
+ double d_early_late_spc_chips;
+ double d_very_early_late_spc_chips;
gr_complex* d_ca_code;
@@ -146,22 +146,22 @@ private:
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ double d_rem_carr_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
- float d_acq_code_phase_samples;
- float d_acq_carrier_doppler_hz;
+ double d_acq_code_phase_samples;
+ double d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
- float d_carrier_doppler_hz;
+ double d_carrier_doppler_hz;
double d_acc_carrier_phase_rad;
double d_acc_code_phase_secs;
@@ -175,9 +175,9 @@ private:
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
- float d_carrier_lock_test;
- float d_CN0_SNV_dB_Hz;
- float d_carrier_lock_threshold;
+ double d_carrier_lock_test;
+ double d_CN0_SNV_dB_Hz;
+ double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
index 74a399d..ec5119e 100644
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
@@ -387,7 +387,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
// New code Doppler frequency estimation
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD;
+ d_acc_carrier_phase_rad -= GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD;
//remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
index 0a988a1..fccccfb 100644
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
@@ -217,18 +217,18 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
long int acq_trk_diff_samples;
- float acq_trk_diff_seconds;
+ double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect
// Fd=(C/(C+Vr))*F
- float radial_velocity;
+ double radial_velocity;
radial_velocity = (Galileo_E5a_FREQ_HZ + d_acq_carrier_doppler_hz)/Galileo_E5a_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
- float T_chip_mod_seconds;
- float T_prn_mod_seconds;
- float T_prn_mod_samples;
+ double T_chip_mod_seconds;
+ double T_prn_mod_seconds;
+ double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * Galileo_E5a_CODE_CHIP_RATE_HZ;
T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E5a_CODE_LENGTH_CHIPS;
@@ -236,13 +236,13 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking()
d_current_prn_length_samples = round(T_prn_mod_samples);
- float T_prn_true_seconds = Galileo_E5a_CODE_LENGTH_CHIPS / Galileo_E5a_CODE_CHIP_RATE_HZ;
- float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
- float T_prn_diff_seconds;
+ double T_prn_true_seconds = Galileo_E5a_CODE_LENGTH_CHIPS / Galileo_E5a_CODE_CHIP_RATE_HZ;
+ double T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
+ double T_prn_diff_seconds;
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
- float N_prn_diff;
+ double N_prn_diff;
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
- float corrected_acq_phase_samples, delay_correction_samples;
+ double corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
@@ -358,7 +358,7 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_code()
int epl_loop_length_samples;
// unified loop for E, P, L code vectors
- code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
+ code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
tcode_chips = -rem_code_phase_chips;
@@ -383,7 +383,7 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_code()
void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_carrier()
{
float sin_f, cos_f;
- float phase_step_rad = static_cast<float>(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
+ float phase_step_rad = static_cast<float>(2.0 * GALILEO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in));
int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
@@ -400,10 +400,10 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
- float carr_error_hz;
- float carr_error_filt_hz;
- float code_error_chips;
- float code_error_filt_chips;
+ double carr_error_hz;
+ double carr_error_filt_hz;
+ double code_error_chips;
+ double code_error_filt_chips;
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
@@ -451,7 +451,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
case 1:
{
int samples_offset;
- float acq_trk_shif_correction_samples;
+ double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
@@ -561,11 +561,11 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
{
if (d_secondary_lock == true)
{
- carr_error_hz = pll_four_quadrant_atan(d_Prompt) / static_cast<float>(GALILEO_PI) * 2;
+ carr_error_hz = pll_four_quadrant_atan(d_Prompt) / GALILEO_PI * 2.0;
}
else
{
- carr_error_hz = pll_cloop_two_quadrant_atan(d_Prompt) / static_cast<float>(GALILEO_PI) * 2;
+ carr_error_hz = pll_cloop_two_quadrant_atan(d_Prompt) / GALILEO_PI * 2.0;
}
// Carrier discriminator filter
@@ -576,10 +576,10 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
d_code_freq_chips = Galileo_E5a_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E5a_CODE_CHIP_RATE_HZ) / Galileo_E5a_FREQ_HZ);
}
//carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD;
+ d_acc_carrier_phase_rad -= 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
- d_rem_carr_phase_rad = d_rem_carr_phase_rad + 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD;
- d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, 2*GALILEO_PI);
+ d_rem_carr_phase_rad = d_rem_carr_phase_rad + 2.0*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD;
+ d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, 2.0*GALILEO_PI);
// ################## DLL ##########################################################
if (d_integration_counter == d_current_ti_ms)
@@ -600,7 +600,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
double T_prn_samples;
double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
- T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
+ T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * Galileo_E5a_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + d_code_error_filt_secs * static_cast<double>(d_fs_in);
@@ -694,9 +694,9 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
- current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
- current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
- current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
+ current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_tracking = false;
@@ -781,39 +781,42 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
}
try
{
- // EPR
- d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
- // PROMPT I and Q (to analyze navigation symbols)
- d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
- // PRN start sample stamp
- d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
- // accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
-
- // carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
-
- //PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
-
- //DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
-
- // CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
-
- // AUX vars (for debug purposes)
- tmp_float = d_rem_code_phase_samples;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
- d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+
+ // EPR
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
+ // PROMPT I and Q (to analyze navigation symbols)
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
+ // PRN start sample stamp
+ //tmp_float=(float)d_sample_counter;
+ d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
+ // accumulated carrier phase
+ d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
+
+ // carrier and code frequency
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
+
+ //PLL commands
+ d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(double));
+
+ //DLL commands
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
+
+ // CN0 and carrier lock test
+ d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
+
+ // AUX vars (for debug purposes)
+ tmp_double = d_rem_code_phase_samples;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+ tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+
}
catch (std::ifstream::failure e)
{
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h
index 5b40a7c..7257f80 100644
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h
@@ -137,10 +137,10 @@ private:
long d_fs_in;
double d_early_late_spc_chips;
- float d_dll_bw_hz;
- float d_pll_bw_hz;
- float d_dll_bw_init_hz;
- float d_pll_bw_init_hz;
+ double d_dll_bw_hz;
+ double d_pll_bw_hz;
+ double d_dll_bw_init_hz;
+ double d_pll_bw_init_hz;
gr_complex* d_codeQ;
gr_complex* d_codeI;
@@ -160,26 +160,26 @@ private:
float tmp_P;
float tmp_L;
// remaining code phase and carrier phase between tracking loops
- float d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ double d_rem_code_phase_samples;
+ double d_rem_carr_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
- float d_acq_code_phase_samples;
- float d_acq_carrier_doppler_hz;
+ double d_acq_code_phase_samples;
+ double d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
- float d_code_freq_chips;
- float d_carrier_doppler_hz;
- float d_acc_carrier_phase_rad;
- float d_code_phase_samples;
- float d_acc_code_phase_secs;
- float d_code_error_filt_secs;
+ double d_code_freq_chips;
+ double d_carrier_doppler_hz;
+ double d_acc_carrier_phase_rad;
+ double d_code_phase_samples;
+ double d_acc_code_phase_secs;
+ double d_code_error_filt_secs;
//PRN period in samples
int d_current_prn_length_samples;
@@ -191,9 +191,9 @@ private:
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
- float d_carrier_lock_test;
- float d_CN0_SNV_dB_Hz;
- float d_carrier_lock_threshold;
+ double d_carrier_lock_test;
+ double d_CN0_SNV_dB_Hz;
+ double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc
index d9ae8bd..820b998 100644
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc
@@ -253,7 +253,7 @@ void galileo_volk_e1_dll_pll_veml_tracking_cc::start_tracking()
void galileo_volk_e1_dll_pll_veml_tracking_cc::update_local_code()
{
double tcode_half_chips;
- float rem_code_phase_half_chips;
+ double rem_code_phase_half_chips;
int code_length_half_chips = static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS) * 2;
double code_phase_step_chips;
double code_phase_step_half_chips;
@@ -262,11 +262,11 @@ void galileo_volk_e1_dll_pll_veml_tracking_cc::update_local_code()
int epl_loop_length_samples;
// unified loop for VE, E, P, L, VL code vectors
- code_phase_step_chips = (static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
- code_phase_step_half_chips = (2.0 * static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
+ code_phase_step_chips = (d_code_freq_chips) / (static_cast<double>(d_fs_in));
+ code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast<double>(d_fs_in));
rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in);
- tcode_half_chips = - static_cast<double>(rem_code_phase_half_chips);
+ tcode_half_chips = - rem_code_phase_half_chips;
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips);
@@ -287,9 +287,9 @@ void galileo_volk_e1_dll_pll_veml_tracking_cc::update_local_carrier()
{
float phase_rad, phase_step_rad;
// Compute the carrier phase step for the K-1 carrier doppler estimation
- phase_step_rad = static_cast<float>(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
+ phase_step_rad = static_cast<float> (GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in));
// Initialize the carrier phase with the remanent carrier phase of the K-2 loop
- phase_rad = d_rem_carr_phase_rad;
+ phase_rad = static_cast<float> (d_rem_carr_phase_rad);
//HERE YOU CAN CHOOSE THE DESIRED VOLK IMPLEMENTATION
//volk_gnsssdr_s32f_x2_update_local_carrier_32fc_manual(d_carr_sign, phase_rad, phase_step_rad, d_current_prn_length_samples, "generic");
@@ -340,10 +340,10 @@ galileo_volk_e1_dll_pll_veml_tracking_cc::~galileo_volk_e1_dll_pll_veml_tracking
int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
- float carr_error_hz;
- float carr_error_filt_hz;
- float code_error_chips;
- float code_error_filt_chips;
+ double carr_error_hz;
+ double carr_error_filt_hz;
+ double code_error_chips;
+ double code_error_filt_chips;
if (d_enable_tracking == true)
{
@@ -353,7 +353,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr
* Signal alignment (skip samples until the incoming signal is aligned with local replica)
*/
int samples_offset;
- float acq_trk_shif_correction_samples;
+ double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
@@ -419,7 +419,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr
// New code Doppler frequency estimation
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ);
//carrier phase accumulator for (K) Doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
+ d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
//remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
@@ -430,7 +430,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator
- float code_error_filt_secs;
+ double code_error_filt_secs;
code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds]
//code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*static_cast<float>(d_fs_in); //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
@@ -442,7 +442,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr
double T_prn_samples;
double K_blk_samples;
// Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation
- T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
+ T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
@@ -507,9 +507,9 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
- current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
- current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
- current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
+ current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_pseudorange = false;
*out[0] = current_synchro_data;
@@ -594,19 +594,28 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
+ tmp_float=d_acc_carrier_phase_rad;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
+ tmp_float=d_carrier_doppler_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ tmp_float=d_code_freq_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
+ tmp_float=carr_error_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ tmp_float=carr_error_filt_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
+ tmp_float=code_error_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ tmp_float=code_error_filt_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
+ tmp_float=d_CN0_SNV_dB_Hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ 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_rem_code_phase_samples;
d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h
index 7f118ab..ead8d05 100644
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h
@@ -126,8 +126,8 @@ private:
long d_if_freq;
long d_fs_in;
- float d_early_late_spc_chips;
- float d_very_early_late_spc_chips;
+ double d_early_late_spc_chips;
+ double d_very_early_late_spc_chips;
gr_complex* d_ca_code;
@@ -162,22 +162,22 @@ private:
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ double d_rem_carr_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
- float d_acq_code_phase_samples;
- float d_acq_carrier_doppler_hz;
+ double d_acq_code_phase_samples;
+ double d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
- float d_carrier_doppler_hz;
+ double d_carrier_doppler_hz;
double d_acc_carrier_phase_rad;
double d_acc_code_phase_secs;
@@ -191,9 +191,9 @@ private:
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
- float d_carrier_lock_test;
- float d_CN0_SNV_dB_Hz;
- float d_carrier_lock_threshold;
+ double d_carrier_lock_test;
+ double d_CN0_SNV_dB_Hz;
+ double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
index fec174e..6db9452 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
@@ -315,7 +315,7 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier()
phase += phase_step;
}
d_rem_carr_phase = fmod(phase, GPS_TWO_PI);
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + phase;
+ d_acc_carrier_phase_rad -= d_acc_carrier_phase_rad + phase;
}
@@ -439,6 +439,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
if (d_FLL_wait == 1)
{
d_Prompt_prev = *d_Prompt;
+ d_FLL_discriminator_hz=0.0;
d_FLL_wait = 0;
}
else
@@ -532,7 +533,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
T_prn_samples = T_prn_seconds * d_fs_in;
float code_error_filt_samples;
- code_error_filt_samples = T_prn_seconds * code_error_filt_chips * T_chip_seconds * static_cast<double>(d_fs_in); //[seconds]
+ code_error_filt_samples = GPS_L1_CA_CODE_PERIOD * code_error_filt_chips * GPS_L1_CA_CHIP_PERIOD * static_cast<double>(d_fs_in); //[seconds]
d_acc_code_phase_samples = d_acc_code_phase_samples + code_error_filt_samples;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_samples;
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
similarity index 63%
copy from src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
copy to src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
index fea305f..72b4b86 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
@@ -1,13 +1,8 @@
/*!
- * \file gps_l1_ca_dll_pll_tracking_gpu_cc.cc
- * \brief Implementation of a code DLL + carrier PLL tracking block, GPU ACCELERATED
+ * \file gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
+ * \brief Implementation of a code DLL + carrier PLL tracking block
* \author Javier Arribas, 2015. jarribas(at)cttc.es
*
- * Code DLL + carrier PLL according to the algorithms described in:
- * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
- * A Software-Defined GPS and Galileo Receiver. A Single-Frequency
- * Approach, Birkhauser, 2007
- *
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
@@ -33,13 +28,14 @@
* -------------------------------------------------------------------------
*/
-#include "gps_l1_ca_dll_pll_tracking_gpu_cc.h"
+#include "gps_l1_ca_dll_pll_c_aid_tracking_cc.h"
#include <cmath>
#include <iostream>
#include <memory>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
+#include <volk/volk.h>
#include <glog/logging.h>
#include "gnss_synchro.h"
#include "gps_sdr_signal_processing.h"
@@ -47,8 +43,6 @@
#include "lock_detectors.h"
#include "GPS_L1_CA.h"
#include "control_message_factory.h"
-#include <volk/volk.h> // volk_alignment
-#include <cuda_profiler_api.h>
/*!
@@ -62,8 +56,8 @@
using google::LogMessage;
-gps_l1_ca_dll_pll_tracking_gpu_cc_sptr
-gps_l1_ca_dll_pll_make_tracking_gpu_cc(
+gps_l1_ca_dll_pll_c_aid_tracking_cc_sptr
+gps_l1_ca_dll_pll_c_aid_make_tracking_cc(
long if_freq,
long fs_in,
unsigned int vector_length,
@@ -74,12 +68,13 @@ gps_l1_ca_dll_pll_make_tracking_gpu_cc(
float dll_bw_hz,
float early_late_space_chips)
{
- return gps_l1_ca_dll_pll_tracking_gpu_cc_sptr(new Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(if_freq,
+ return gps_l1_ca_dll_pll_c_aid_tracking_cc_sptr(new gps_l1_ca_dll_pll_c_aid_tracking_cc(if_freq,
fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips));
}
-void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::forecast (int noutput_items,
+
+void gps_l1_ca_dll_pll_c_aid_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required)
{
ninput_items_required[0] = static_cast<int>(d_vector_length) * 2; //set the required available samples in each call
@@ -87,7 +82,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::forecast (int noutput_items,
-Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
+gps_l1_ca_dll_pll_c_aid_tracking_cc::gps_l1_ca_dll_pll_c_aid_tracking_cc(
long if_freq,
long fs_in,
unsigned int vector_length,
@@ -97,7 +92,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips) :
- gr::block("Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
+ gr::block("gps_l1_ca_dll_pll_c_aid_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
// initialize internal vars
@@ -107,47 +102,33 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
d_fs_in = fs_in;
d_vector_length = vector_length;
d_dump_filename = dump_filename;
+ d_correlation_length_samples = static_cast<int>(d_vector_length);
// Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
- d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
+ d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2);
//--- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
// Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip
- //d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
- d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment()));
-
- multicorrelator_gpu = new cuda_multicorrelator();
- int N_CORRELATORS = 3;
- //local code resampler on CPU (old)
- //multicorrelator_gpu->init_cuda(0, NULL, 2 * d_vector_length , 2 * d_vector_length , N_CORRELATORS);
-
- //local code resampler on GPU (new)
- multicorrelator_gpu->init_cuda_integrated_resampler(0, NULL, 2 * d_vector_length , GPS_L1_CA_CODE_LENGTH_CHIPS , N_CORRELATORS);
-
- // Get space for the resampled early / prompt / late local replicas
- cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped );
-
- //allocate host memory
- //pinned memory mode - use special function to get OS-pinned memory
- cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped );
-
- //old local codes vector
- // (cudaHostAlloc((void**)&d_local_codes_gpu, (V_LEN * sizeof(gr_complex))*N_CORRELATORS, cudaHostAllocWriteCombined ));
-
- //new integrated shifts
- // (cudaHostAlloc((void**)&d_local_codes_gpu, (2 * d_vector_length * sizeof(gr_complex)), cudaHostAllocWriteCombined ));
+ d_ca_code = static_cast<gr_complex*>(volk_malloc(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment()));
// correlator outputs (scalar)
- cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocWriteCombined );
+ d_n_correlator_taps=3; // Early, Prompt, and Late
+ d_correlator_outs = static_cast<gr_complex*>(volk_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_get_alignment()));
+ for (int n=0;n<d_n_correlator_taps;n++)
+ {
+ d_correlator_outs[n] = gr_complex(0,0);
+ }
+ d_local_code_shift_chips = static_cast<float*>(volk_malloc(d_n_correlator_taps*sizeof(float), volk_get_alignment()));
+ // Set TAPs delay values [chips]
+ d_local_code_shift_chips[0]=-d_early_late_spc_chips;
+ d_local_code_shift_chips[1]=0.0;
+ d_local_code_shift_chips[2]=d_early_late_spc_chips;
- //map to EPL pointers
- d_Early = &d_corr_outs_gpu[0];
- d_Prompt = &d_corr_outs_gpu[1];
- d_Late = &d_corr_outs_gpu[2];
+ multicorrelator_cpu.init(2*d_correlation_length_samples,d_n_correlator_taps);
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
@@ -155,7 +136,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
// define residual code phase (in chips)
d_rem_code_phase_samples = 0.0;
// define residual carrier phase
- d_rem_carr_phase_rad = 0.0;
+ d_rem_carrier_phase_rad = 0.0;
// sample synchronization
d_sample_counter = 0;
@@ -166,8 +147,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
d_pull_in = false;
d_last_seg = 0;
- d_current_prn_length_samples = static_cast<int>(d_vector_length);
-
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES];
@@ -179,6 +158,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
systemName["G"] = std::string("GPS");
systemName["S"] = std::string("SBAS");
+
set_relative_rate(1.0/((double)d_vector_length*2));
d_channel_internal_queue = 0;
@@ -187,14 +167,15 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
d_acq_code_phase_samples = 0.0;
d_acq_carrier_doppler_hz = 0.0;
d_carrier_doppler_hz = 0.0;
- d_acc_carrier_phase_rad = 0.0;
+ d_acc_carrier_phase_cycles = 0.0;
d_code_phase_samples = 0.0;
- d_acc_code_phase_secs = 0.0;
+
+ d_pll_to_dll_assist_secs_Ti=0.0;
//set_min_output_buffer((long int)300);
}
-void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
+void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
{
/*
* correct the code phase according to the delay between acq and trk
@@ -204,30 +185,31 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
long int acq_trk_diff_samples;
- float acq_trk_diff_seconds;
+ double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
- acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
+ acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
//doppler effect
// Fd=(C/(C+Vr))*F
- float radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
+ double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
- float T_chip_mod_seconds;
- float T_prn_mod_seconds;
- float T_prn_mod_samples;
+ double T_chip_mod_seconds;
+ double T_prn_mod_seconds;
+ double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
+ d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
- T_prn_mod_samples = T_prn_mod_seconds * static_cast<float>(d_fs_in);
+ T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
- d_current_prn_length_samples = round(T_prn_mod_samples);
+ d_correlation_length_samples = round(T_prn_mod_samples);
- float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
- float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
- float T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds;
- float N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
- float corrected_acq_phase_samples, delay_correction_samples;
- corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
+ double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
+ double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
+ double T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds;
+ double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
+ double corrected_acq_phase_samples, delay_correction_samples;
+ corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
@@ -237,25 +219,28 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
d_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
+ d_carrier_phase_step_rad=GPS_TWO_PI*d_carrier_doppler_hz/static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
- d_carrier_loop_filter.initialize(); // initialize the carrier filter
+ d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); //The carrier loop filter implements the Doppler accumulator
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);
- d_local_code_shift_chips[0]=-d_early_late_spc_chips;
- d_local_code_shift_chips[1]=0.0;
- d_local_code_shift_chips[2]=d_early_late_spc_chips;
- multicorrelator_gpu->set_local_code_and_taps(GPS_L1_CA_CODE_LENGTH_CHIPS,d_ca_code, d_local_code_shift_chips,3);
+ 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 (int n=0;n<d_n_correlator_taps;n++)
+ {
+ d_correlator_outs[n] = gr_complex(0,0);
+ }
d_carrier_lock_fail_counter = 0;
- d_rem_code_phase_samples = 0;
- d_rem_carr_phase_rad = 0;
- d_acc_carrier_phase_rad = 0;
- d_acc_code_phase_secs = 0;
+ d_rem_code_phase_samples = 0.0;
+ d_rem_carrier_phase_rad = 0.0;
+ d_rem_code_phase_chips =0.0;
+ d_acc_carrier_phase_cycles = 0.0;
+ d_pll_to_dll_assist_secs_Ti=0.0;
d_code_phase_samples = d_acq_code_phase_samples;
@@ -277,51 +262,51 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
}
-Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc()
+gps_l1_ca_dll_pll_c_aid_tracking_cc::~gps_l1_ca_dll_pll_c_aid_tracking_cc()
{
d_dump_file.close();
- cudaFreeHost(in_gpu);
- cudaFreeHost(d_carr_sign_gpu);
- cudaFreeHost(d_corr_outs_gpu);
- cudaFreeHost(d_local_code_shift_chips);
-
- multicorrelator_gpu->free_cuda();
- delete(multicorrelator_gpu);
-
+ volk_free(d_local_code_shift_chips);
+ volk_free(d_correlator_outs);
volk_free(d_ca_code);
delete[] d_Prompt_buffer;
+ multicorrelator_cpu.free();
}
-int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
+int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
- // process vars
- float carr_error_hz = 0.0;
- float carr_error_filt_hz = 0.0;
- float code_error_chips = 0.0;
- float code_error_filt_chips = 0.0;
-
// Block input data and block output stream pointers
- const gr_complex* in = (gr_complex*) input_items[0];
+ const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data = Gnss_Synchro();
+ // process vars
+ 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 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)
{
// Receiver signal alignment
if (d_pull_in == true)
{
int samples_offset;
+ double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
- samples_offset = round(d_acq_code_phase_samples)+d_current_prn_length_samples - acq_to_trk_delay_samples%d_current_prn_length_samples;
- d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
+ acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_correlation_length_samples));
+ samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
+ d_sample_counter += samples_offset; //count for the processed samples
d_pull_in = false;
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
@@ -333,49 +318,36 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
- // UPDATE NCO COMMAND
- float phase_step_rad = static_cast<float>(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
-
- //code resampler on GPU (new)
- float code_phase_step_chips = static_cast<float>(d_code_freq_chips) / static_cast<float>(d_fs_in);
- float rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
-
- cudaProfilerStart();
- multicorrelator_gpu->Carrier_wipeoff_multicorrelator_resampler_cuda(
- d_corr_outs_gpu,
- in,
- d_rem_carr_phase_rad,
- phase_step_rad,
- code_phase_step_chips,
- rem_code_phase_chips,
- d_current_prn_length_samples,
- 3);
- cudaProfilerStop();
+ // ################# CARRIER WIPEOFF AND CORRELATORS ##############################
+ // perform carrier wipe-off and compute Early, Prompt and Late correlation
+ multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in);
+ multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad,d_carrier_phase_step_rad,d_rem_code_phase_chips,d_code_phase_step_chips,d_correlation_length_samples);
+
+ // UPDATE INTEGRATION TIME
+ CURRENT_INTEGRATION_TIME_S=(static_cast<double>(d_correlation_length_samples)/static_cast<double>(d_fs_in));
// ################## PLL ##########################################################
- // PLL discriminator
- carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
+ // Update PLL discriminator [rads/Ti -> Secs/Ti]
+ carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1])/GPS_TWO_PI; //prompt output
// Carrier discriminator filter
- carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
- // New carrier Doppler frequency estimation
- d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz;
- // New code Doppler frequency estimation
+ // NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
+ //d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_phase_error_filt_secs_ti/INTEGRATION_TIME;
+ // Input [s/Ti] -> output [Hz]
+ d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
+ // PLL to DLL assistance [Secs/Ti]
+ d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz*CURRENT_INTEGRATION_TIME_S)/GPS_L1_FREQ_HZ;
+ // code Doppler frequency update
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
- //carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
- //remanent carrier phase to prevent overflow in the code NCO
- d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
- d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ##########################################################
// DLL discriminator
- code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late); //[chips/Ti]
+ code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] //early and late
// Code discriminator filter
- code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
- //Code phase accumulator
- float code_error_filt_secs;
- code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds]
- d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
+ code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips_Ti); //input [chips/Ti] -> output [chips/second]
+ code_error_filt_secs_Ti = code_error_filt_chips*CURRENT_INTEGRATION_TIME_S/d_code_freq_chips; // [s/Ti]
+ // DLL code error estimation [s/Ti]
+ // TODO: PLL carrier aid to DLL is disabled. Re-enable it and measure performance
+ dll_code_error_secs_Ti=-code_error_filt_secs_Ti+d_pll_to_dll_assist_secs_Ti;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer
@@ -384,17 +356,41 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
double T_prn_samples;
double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
- T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
+ T_chip_seconds = 1 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
- K_blk_samples = T_prn_samples + d_rem_code_phase_samples + static_cast<double>(code_error_filt_secs) * static_cast<double>(d_fs_in);
- //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
+ 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_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_correlation_length_samples); //rounding error < 1 sample
+
+
+ // UPDATE REMNANT CARRIER PHASE
+ CORRECTED_INTEGRATION_TIME_S=(static_cast<double>(d_correlation_length_samples)/static_cast<double>(d_fs_in));
+ //remnant carrier phase [rad]
+ d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S,GPS_TWO_PI);
+ // UPDATE CARRIER PHASE ACCUULATOR
+ //carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
+ d_acc_carrier_phase_cycles -= d_carrier_doppler_hz*CORRECTED_INTEGRATION_TIME_S;
+
- // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
+ //################### PLL COMMANDS #################################################
+ //carrier phase step (NCO phase increment per sample) [rads/sample]
+ d_carrier_phase_step_rad=GPS_TWO_PI*d_carrier_doppler_hz/static_cast<double>(d_fs_in);
+
+ //################### DLL COMMANDS #################################################
+ //code phase step (Code resampler phase increment per sample) [chips/sample]
+ d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
+ //remnant code phase [chips]
+ d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
+
+
+ // ####### CN0 ESTIMATION AND LOCK DETECTORS #######################################
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
- d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
+ d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt
d_cn0_estimation_counter++;
}
else
@@ -426,26 +422,17 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
}
- // ########### Output the tracking data to navigation and PVT ##########
- current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
- current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
-
- // Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!)
- //compute remnant code phase samples BEFORE the Tracking timestamp
- //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
- //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples)/static_cast<double>(d_fs_in);
- // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
- current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
- //compute remnant code phase samples AFTER the Tracking timestamp
- d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
-
- //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast<double>(d_fs_in);
+ // ########### Output the tracking data to navigation and PVT ##########
+ current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
+ current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
+ // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
+ current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
- current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
- current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
- current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
+ current_synchro_data.Carrier_phase_rads = GPS_TWO_PI*d_acc_carrier_phase_cycles;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_pseudorange = false;
*out[0] = current_synchro_data;
@@ -494,9 +481,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
std::cout << tmp_str_stream.rdbuf() << std::flush;
}
}
- *d_Early = gr_complex(0,0);
- *d_Prompt = gr_complex(0,0);
- *d_Late = gr_complex(0,0);
+ for (int n=0;n<d_n_correlator_taps;n++)
+ {
+ d_correlator_outs[n] = gr_complex(0,0);
+ }
current_synchro_data.System = {'G'};
current_synchro_data.Flag_valid_pseudorange = false;
@@ -509,13 +497,12 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
float prompt_I;
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
- float tmp_float;
double tmp_double;
- prompt_I = (*d_Prompt).real();
- prompt_Q = (*d_Prompt).imag();
- tmp_E = std::abs<float>(*d_Early);
- tmp_P = std::abs<float>(*d_Prompt);
- tmp_L = std::abs<float>(*d_Late);
+ prompt_I = d_correlator_outs[1].real();
+ prompt_Q = d_correlator_outs[1].imag();
+ tmp_E = std::abs<float>(d_correlator_outs[0]);
+ tmp_P = std::abs<float>(d_correlator_outs[1]);
+ tmp_L = std::abs<float>(d_correlator_outs[2]);
try
{
// EPR
@@ -529,46 +516,43 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
//tmp_float=(float)d_sample_counter;
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_cycles), sizeof(double));
// carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- tmp_float=d_code_freq_chips;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
//PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&carr_phase_error_secs_Ti), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
//DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_chips_Ti), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
// CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
// AUX vars (for debug purposes)
- tmp_float = d_rem_code_phase_samples;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
+ tmp_double = d_rem_code_phase_samples;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+ tmp_double = static_cast<double>(d_sample_counter + d_correlation_length_samples);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
- catch (std::ifstream::failure e)
+ catch (const std::ifstream::failure* e)
{
- LOG(WARNING) << "Exception writing trk dump file " << e.what();
+ LOG(WARNING) << "Exception writing trk dump file " << e->what();
}
}
- consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
- d_sample_counter += d_current_prn_length_samples; //count for the processed samples
- //LOG(INFO)<<"GPS tracking output end on CH="<<this->d_channel << " SAMPLE STAMP="<<d_sample_counter<<std::endl;
+ consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates
+ d_sample_counter += d_correlation_length_samples; //count for the processed samples
+
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}
-
-
-void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::set_channel(unsigned int channel)
+void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel(unsigned int channel)
{
d_channel = channel;
LOG(INFO) << "Tracking Channel set to " << d_channel;
@@ -585,23 +569,20 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::set_channel(unsigned int channel)
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
}
- catch (std::ifstream::failure e)
+ catch (const std::ifstream::failure* e)
{
- LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl;
+ LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e->what() << std::endl;
}
}
}
}
-
-
-void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
+void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}
-
-void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
+void gps_l1_ca_dll_pll_c_aid_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_acquisition_gnss_synchro = p_gnss_synchro;
}
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.h
similarity index 74%
copy from src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
copy to src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.h
index 216e639..212b26a 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.h
@@ -1,5 +1,5 @@
/*!
- * \file gps_l1_ca_dll_pll_tracking_cc.h
+ * \file gps_l1_ca_dll_pll_c_aid_tracking_cc.h
* \brief Interface of a code DLL + carrier PLL tracking block
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
* Javier Arribas, 2011. jarribas(at)cttc.es
@@ -34,8 +34,8 @@
* -------------------------------------------------------------------------
*/
-#ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H
-#define GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H
+#ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_CC_H
+#define GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_CC_H
#include <fstream>
#include <queue>
@@ -49,16 +49,16 @@
#include "gps_sdr_signal_processing.h"
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
-#include "tracking_2nd_PLL_filter.h"
-#include "correlator.h"
+#include "tracking_FLL_PLL_filter.h"
+#include "cpu_multicorrelator.h"
-class Gps_L1_Ca_Dll_Pll_Tracking_cc;
+class gps_l1_ca_dll_pll_c_aid_tracking_cc;
-typedef boost::shared_ptr<Gps_L1_Ca_Dll_Pll_Tracking_cc>
- gps_l1_ca_dll_pll_tracking_cc_sptr;
+typedef boost::shared_ptr<gps_l1_ca_dll_pll_c_aid_tracking_cc>
+ gps_l1_ca_dll_pll_c_aid_tracking_cc_sptr;
-gps_l1_ca_dll_pll_tracking_cc_sptr
-gps_l1_ca_dll_pll_make_tracking_cc(long if_freq,
+gps_l1_ca_dll_pll_c_aid_tracking_cc_sptr
+gps_l1_ca_dll_pll_c_aid_make_tracking_cc(long if_freq,
long fs_in, unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
@@ -73,10 +73,10 @@ gps_l1_ca_dll_pll_make_tracking_cc(long if_freq,
/*!
* \brief This class implements a DLL + PLL tracking loop block
*/
-class Gps_L1_Ca_Dll_Pll_Tracking_cc: public gr::block
+class gps_l1_ca_dll_pll_c_aid_tracking_cc: public gr::block
{
public:
- ~Gps_L1_Ca_Dll_Pll_Tracking_cc();
+ ~gps_l1_ca_dll_pll_c_aid_tracking_cc();
void set_channel(unsigned int channel);
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
@@ -89,8 +89,8 @@ public:
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private:
- friend gps_l1_ca_dll_pll_tracking_cc_sptr
- gps_l1_ca_dll_pll_make_tracking_cc(long if_freq,
+ friend gps_l1_ca_dll_pll_c_aid_tracking_cc_sptr
+ gps_l1_ca_dll_pll_c_aid_make_tracking_cc(long if_freq,
long fs_in, unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
@@ -100,7 +100,7 @@ private:
float dll_bw_hz,
float early_late_space_chips);
- Gps_L1_Ca_Dll_Pll_Tracking_cc(long if_freq,
+ gps_l1_ca_dll_pll_c_aid_tracking_cc(long if_freq,
long fs_in, unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
@@ -109,8 +109,6 @@ private:
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
- void update_local_code();
- void update_local_carrier();
// tracking configuration vars
boost::shared_ptr<gr::msg_queue> d_queue;
@@ -125,41 +123,37 @@ private:
long d_fs_in;
double d_early_late_spc_chips;
+ int d_n_correlator_taps;
gr_complex* d_ca_code;
-
- gr_complex* d_early_code;
- gr_complex* d_late_code;
- gr_complex* d_prompt_code;
- gr_complex* d_carr_sign;
-
- gr_complex *d_Early;
- gr_complex *d_Prompt;
- gr_complex *d_Late;
+ float* d_local_code_shift_chips;
+ gr_complex* d_correlator_outs;
+ cpu_multicorrelator multicorrelator_cpu;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ double d_rem_code_phase_chips;
+ double d_rem_carrier_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
- Tracking_2nd_PLL_filter d_carrier_loop_filter;
+ Tracking_FLL_PLL_filter d_carrier_loop_filter;
// acquisition
- float d_acq_code_phase_samples;
- float d_acq_carrier_doppler_hz;
- // correlator
- Correlator d_correlator;
+ double d_acq_code_phase_samples;
+ double d_acq_carrier_doppler_hz;
// tracking vars
double d_code_freq_chips;
- float d_carrier_doppler_hz;
- float d_acc_carrier_phase_rad;
- float d_code_phase_samples;
- float d_acc_code_phase_secs;
+ double d_code_phase_step_chips;
+ double d_carrier_doppler_hz;
+ double d_carrier_phase_step_rad;
+ double d_acc_carrier_phase_cycles;
+ double d_code_phase_samples;
+ double d_pll_to_dll_assist_secs_Ti;
- //PRN period in samples
- int d_current_prn_length_samples;
+ //Integration period in samples
+ int d_correlation_length_samples;
//processing samples counters
unsigned long int d_sample_counter;
@@ -168,9 +162,9 @@ private:
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
- float d_carrier_lock_test;
- float d_CN0_SNV_dB_Hz;
- float d_carrier_lock_threshold;
+ double d_carrier_lock_test;
+ double d_CN0_SNV_dB_Hz;
+ double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
@@ -185,4 +179,4 @@ private:
std::string sys;
};
-#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H
+#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_CC_H
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
index 86886dc..019723f 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
@@ -183,29 +183,29 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
long int acq_trk_diff_samples;
- float acq_trk_diff_seconds;
+ double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp); //-d_vector_length;
LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect
// Fd=(C/(C+Vr))*F
- float radial_velocity;
+ double radial_velocity;
radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
- float T_chip_mod_seconds;
- float T_prn_mod_seconds;
- float T_prn_mod_samples;
+ double T_chip_mod_seconds;
+ double T_prn_mod_seconds;
+ double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<float>(d_fs_in);
d_current_prn_length_samples = round(T_prn_mod_samples);
- float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
- float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
- float T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
- float N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
- float corrected_acq_phase_samples, delay_correction_samples;
+ double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
+ double T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
+ double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
+ double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
+ double corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
@@ -338,10 +338,10 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
{
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
- float carr_error_hz;
- float carr_error_filt_hz;
- float code_error_chips;
- float code_error_filt_chips;
+ double carr_error_hz;
+ double carr_error_filt_hz;
+ double code_error_chips;
+ double code_error_filt_chips;
if (d_enable_tracking == true)
{
@@ -398,7 +398,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
#endif
// ################## PLL ##########################################################
// PLL discriminator
- carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
+ carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_TWO_PI;
// Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation
@@ -406,7 +406,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
// New code Doppler frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
+ d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
//remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
@@ -417,7 +417,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator
- float code_error_filt_secs;
+ double code_error_filt_secs;
code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
@@ -428,7 +428,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
double T_prn_samples;
double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
- T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
+ T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
@@ -563,23 +563,32 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
//tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// accumulated carrier phase
- d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
+ tmp_float=d_acc_carrier_phase_rad;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
// carrier and code frequency
- d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
- d_dump_file.write((char*)&d_code_freq_chips, sizeof(float));
+ tmp_float=d_carrier_doppler_hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
+ tmp_float=d_code_freq_chips;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
//PLL commands
- d_dump_file.write((char*)&carr_error_hz, sizeof(float));
- d_dump_file.write((char*)&carr_error_filt_hz, sizeof(float));
+ tmp_float=carr_error_hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
+ tmp_float=carr_error_filt_hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
//DLL commands
- d_dump_file.write((char*)&code_error_chips, sizeof(float));
- d_dump_file.write((char*)&code_error_filt_chips, sizeof(float));
+ tmp_float=code_error_chips;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
+ tmp_float=code_error_filt_chips;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
// CN0 and carrier lock test
- d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
- d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
+ tmp_float=d_CN0_SNV_dB_Hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
+ tmp_float=d_carrier_lock_test;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples;
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h
index 0aee4ce..52ce88e 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h
@@ -135,24 +135,24 @@ private:
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ double d_rem_carr_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
- float d_acq_code_phase_samples;
- float d_acq_carrier_doppler_hz;
+ double d_acq_code_phase_samples;
+ double d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
- float d_carrier_doppler_hz;
- float d_acc_carrier_phase_rad;
- float d_code_phase_samples;
- float d_acc_code_phase_secs;
+ double d_carrier_doppler_hz;
+ double d_acc_carrier_phase_rad;
+ double d_code_phase_samples;
+ double d_acc_code_phase_secs;
//PRN period in samples
int d_current_prn_length_samples;
@@ -164,9 +164,9 @@ private:
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
- float d_carrier_lock_test;
- float d_CN0_SNV_dB_Hz;
- float d_carrier_lock_threshold;
+ double d_carrier_lock_test;
+ double d_CN0_SNV_dB_Hz;
+ double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
index 3193d33..07d64b9 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
@@ -190,17 +190,17 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
long int acq_trk_diff_samples;
- float acq_trk_diff_seconds;
+ double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect
// Fd=(C/(C+Vr))*F
- float radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
+ double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
- float T_chip_mod_seconds;
- float T_prn_mod_seconds;
- float T_prn_mod_samples;
+ double T_chip_mod_seconds;
+ double T_prn_mod_seconds;
+ double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
@@ -208,11 +208,11 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_current_prn_length_samples = round(T_prn_mod_samples);
- float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
- float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
- float T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds;
- float N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
- float corrected_acq_phase_samples, delay_correction_samples;
+ double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
+ double T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
+ double T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds;
+ double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
+ double corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
@@ -297,7 +297,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier()
{
float sin_f, cos_f;
- float phase_step_rad = static_cast<float>(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
+ float phase_step_rad = static_cast<float>(GPS_TWO_PI) * static_cast<float>(d_carrier_doppler_hz) / static_cast<float>(d_fs_in);
int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
@@ -336,10 +336,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
- float carr_error_hz;
- float carr_error_filt_hz;
- float code_error_chips;
- float code_error_filt_chips;
+ double carr_error_hz;
+ double carr_error_filt_hz;
+ double code_error_chips;
+ double code_error_filt_chips;
// Block input data and block output stream pointers
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment
@@ -355,7 +355,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
if (d_pull_in == true)
{
int samples_offset;
- float acq_trk_shif_correction_samples;
+ double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
@@ -414,7 +414,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// ################## PLL ##########################################################
// PLL discriminator
- carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
+ carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_TWO_PI;
// Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation
@@ -422,7 +422,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// New code Doppler frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
+ d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
@@ -433,7 +433,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator
- float code_error_filt_secs;
+ double code_error_filt_secs;
code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
@@ -504,9 +504,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
- current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
- current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
- current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
+ current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_pseudorange = false;
*out[0] = current_synchro_data;
@@ -579,41 +579,41 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
tmp_L = std::abs<float>(*d_Late);
try
{
- // EPR
- d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
- // PROMPT I and Q (to analyze navigation symbols)
- d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
- // PRN start sample stamp
- //tmp_float=(float)d_sample_counter;
- d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
- // accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
-
- // carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- tmp_float=d_code_freq_chips;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
-
- //PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
-
- //DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
-
- // CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
-
- // AUX vars (for debug purposes)
- tmp_float = d_rem_code_phase_samples;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
- d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+
+ // EPR
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
+ // PROMPT I and Q (to analyze navigation symbols)
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
+ // PRN start sample stamp
+ //tmp_float=(float)d_sample_counter;
+ d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
+ // accumulated carrier phase
+ d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
+
+ // carrier and code frequency
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
+
+ //PLL commands
+ d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+
+ //DLL commands
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
+
+ // CN0 and carrier lock test
+ d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
+
+ // AUX vars (for debug purposes)
+ tmp_double = d_rem_code_phase_samples;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+ tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
catch (std::ifstream::failure e)
{
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
index 216e639..5cf55fa 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
@@ -139,24 +139,24 @@ private:
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ double d_rem_carr_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
- float d_acq_code_phase_samples;
- float d_acq_carrier_doppler_hz;
+ double d_acq_code_phase_samples;
+ double d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
- float d_carrier_doppler_hz;
- float d_acc_carrier_phase_rad;
- float d_code_phase_samples;
- float d_acc_code_phase_secs;
+ double d_carrier_doppler_hz;
+ double d_acc_carrier_phase_rad;
+ double d_code_phase_samples;
+ double d_acc_code_phase_secs;
//PRN period in samples
int d_current_prn_length_samples;
@@ -168,9 +168,9 @@ private:
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
- float d_carrier_lock_test;
- float d_CN0_SNV_dB_Hz;
- float d_carrier_lock_threshold;
+ double d_carrier_lock_test;
+ double d_CN0_SNV_dB_Hz;
+ double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
index fea305f..3927e03 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
@@ -47,7 +47,8 @@
#include "lock_detectors.h"
#include "GPS_L1_CA.h"
#include "control_message_factory.h"
-#include <volk/volk.h> // volk_alignment
+#include <volk/volk.h> //volk_alignement
+// includes
#include <cuda_profiler_api.h>
@@ -115,41 +116,32 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
//--- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
- // Initialization of local code replica
- // Get space for a vector with the C/A code replica sampled 1x/chip
- //d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
- d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment()));
-
- multicorrelator_gpu = new cuda_multicorrelator();
- int N_CORRELATORS = 3;
- //local code resampler on CPU (old)
- //multicorrelator_gpu->init_cuda(0, NULL, 2 * d_vector_length , 2 * d_vector_length , N_CORRELATORS);
-
- //local code resampler on GPU (new)
- multicorrelator_gpu->init_cuda_integrated_resampler(0, NULL, 2 * d_vector_length , GPS_L1_CA_CODE_LENGTH_CHIPS , N_CORRELATORS);
-
- // Get space for the resampled early / prompt / late local replicas
- cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped );
-
+ // Set GPU flags
+ cudaSetDeviceFlags(cudaDeviceMapHost);
//allocate host memory
//pinned memory mode - use special function to get OS-pinned memory
- cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped );
-
- //old local codes vector
- // (cudaHostAlloc((void**)&d_local_codes_gpu, (V_LEN * sizeof(gr_complex))*N_CORRELATORS, cudaHostAllocWriteCombined ));
-
- //new integrated shifts
- // (cudaHostAlloc((void**)&d_local_codes_gpu, (2 * d_vector_length * sizeof(gr_complex)), cudaHostAllocWriteCombined ));
-
- // correlator outputs (scalar)
- cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocWriteCombined );
+ int N_CORRELATORS=3;
+ // Get space for a vector with the C/A code replica sampled 1x/chip
+ cudaHostAlloc((void**)&d_ca_code, (GPS_L1_CA_CODE_LENGTH_CHIPS* sizeof(gr_complex)), cudaHostAllocMapped || cudaHostAllocWriteCombined);
+ // Get space for the resampled early / prompt / late local replicas
+ cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped || cudaHostAllocWriteCombined);
+ cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped || cudaHostAllocWriteCombined);
+ // correlator outputs (scalar)
+ cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocMapped || cudaHostAllocWriteCombined );
- //map to EPL pointers
+ //map to EPL pointers
d_Early = &d_corr_outs_gpu[0];
- d_Prompt = &d_corr_outs_gpu[1];
+ d_Prompt = &d_corr_outs_gpu[1];
d_Late = &d_corr_outs_gpu[2];
//--- Perform initializations ------------------------------
+ multicorrelator_gpu = new cuda_multicorrelator();
+ //local code resampler on GPU
+ multicorrelator_gpu->init_cuda_integrated_resampler(2 * d_vector_length,GPS_L1_CA_CODE_LENGTH_CHIPS,3);
+ multicorrelator_gpu->set_input_output_vectors(
+ d_corr_outs_gpu,
+ in_gpu
+ );
// define initial code frequency basis of NCO
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
// define residual code phase (in chips)
@@ -179,6 +171,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc(
systemName["G"] = std::string("GPS");
systemName["S"] = std::string("SBAS");
+
set_relative_rate(1.0/((double)d_vector_length*2));
d_channel_internal_queue = 0;
@@ -249,7 +242,12 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
d_local_code_shift_chips[1]=0.0;
d_local_code_shift_chips[2]=d_early_late_spc_chips;
- multicorrelator_gpu->set_local_code_and_taps(GPS_L1_CA_CODE_LENGTH_CHIPS,d_ca_code, d_local_code_shift_chips,3);
+ multicorrelator_gpu->set_local_code_and_taps(
+ GPS_L1_CA_CODE_LENGTH_CHIPS,
+ d_ca_code,
+ d_local_code_shift_chips,
+ 3
+ );
d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0;
@@ -281,16 +279,13 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc()
{
d_dump_file.close();
- cudaFreeHost(in_gpu);
- cudaFreeHost(d_carr_sign_gpu);
- cudaFreeHost(d_corr_outs_gpu);
- cudaFreeHost(d_local_code_shift_chips);
-
- multicorrelator_gpu->free_cuda();
- delete(multicorrelator_gpu);
-
- volk_free(d_ca_code);
+ cudaFreeHost(in_gpu);
+ cudaFreeHost(d_corr_outs_gpu);
+ cudaFreeHost(d_local_code_shift_chips);
+ cudaFreeHost(d_ca_code);
+ multicorrelator_gpu->free_cuda();
+ delete(multicorrelator_gpu);
delete[] d_Prompt_buffer;
}
@@ -300,10 +295,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
- float carr_error_hz = 0.0;
- float carr_error_filt_hz = 0.0;
- float code_error_chips = 0.0;
- float code_error_filt_chips = 0.0;
+ float carr_error_hz=0.0;
+ float carr_error_filt_hz=0.0;
+ float code_error_chips=0.0;
+ float code_error_filt_chips=0.0;
// Block input data and block output stream pointers
const gr_complex* in = (gr_complex*) input_items[0];
@@ -340,17 +335,16 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
float code_phase_step_chips = static_cast<float>(d_code_freq_chips) / static_cast<float>(d_fs_in);
float rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
+ memcpy(in_gpu,in,sizeof(gr_complex)*d_current_prn_length_samples);
cudaProfilerStart();
multicorrelator_gpu->Carrier_wipeoff_multicorrelator_resampler_cuda(
- d_corr_outs_gpu,
- in,
- d_rem_carr_phase_rad,
- phase_step_rad,
- code_phase_step_chips,
- rem_code_phase_chips,
- d_current_prn_length_samples,
- 3);
- cudaProfilerStop();
+ d_rem_carr_phase_rad,
+ phase_step_rad,
+ code_phase_step_chips,
+ rem_code_phase_chips,
+ d_current_prn_length_samples,
+ 3);
+ cudaProfilerStop();
// ################## PLL ##########################################################
// PLL discriminator
@@ -362,7 +356,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto
// New code Doppler frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
+ d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h
index a3108f8..e632c48 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h
@@ -128,13 +128,9 @@ private:
//GPU HOST PINNED MEMORY IN/OUT VECTORS
gr_complex* in_gpu;
- gr_complex* d_carr_sign_gpu;
- gr_complex* d_local_codes_gpu;
float* d_local_code_shift_chips;
gr_complex* d_corr_outs_gpu;
cuda_multicorrelator *multicorrelator_gpu;
-
-
gr_complex* d_ca_code;
gr_complex *d_Early;
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
index 018fd64..7090977 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
@@ -199,11 +199,11 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect
// Fd=(C/(C+Vr))*F
- float radial_velocity = (GPS_L2_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L2_FREQ_HZ;
+ double radial_velocity = (GPS_L2_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L2_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
- float T_chip_mod_seconds;
- float T_prn_mod_seconds;
- float T_prn_mod_samples;
+ double T_chip_mod_seconds;
+ double T_prn_mod_seconds;
+ double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L2_M_CODE_RATE_HZ;
T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L2_M_CODE_LENGTH_CHIPS;
@@ -211,11 +211,11 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
d_current_prn_length_samples = round(T_prn_mod_samples);
- float T_prn_true_seconds = GPS_L2_M_CODE_LENGTH_CHIPS / GPS_L2_M_CODE_RATE_HZ;
- float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
- float T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds;
- float N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
- float corrected_acq_phase_samples, delay_correction_samples;
+ double T_prn_true_seconds = GPS_L2_M_CODE_LENGTH_CHIPS / GPS_L2_M_CODE_RATE_HZ;
+ double T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
+ double T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds;
+ double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
+ double corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
@@ -276,7 +276,7 @@ void gps_l2_m_dll_pll_tracking_cc::update_local_code()
int epl_loop_length_samples;
// unified loop for E, P, L code vectors
- code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
+ code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
tcode_chips = -rem_code_phase_chips;
@@ -301,7 +301,7 @@ void gps_l2_m_dll_pll_tracking_cc::update_local_carrier()
{
float phase_rad, phase_step_rad;
- phase_step_rad = static_cast<float>(GPS_L2_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
+ phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
phase_rad = d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++)
{
@@ -337,10 +337,10 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
- float carr_error_hz=0;
- float carr_error_filt_hz=0;
- float code_error_chips=0;
- float code_error_filt_chips=0;
+ double carr_error_hz=0;
+ double carr_error_filt_hz=0;
+ double code_error_chips=0;
+ double code_error_filt_chips=0;
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data = Gnss_Synchro();
@@ -355,7 +355,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
if (d_pull_in == true)
{
int samples_offset;
- float acq_trk_shif_correction_samples;
+ double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = (d_sample_counter - (d_acq_sample_stamp-d_current_prn_length_samples));
acq_trk_shif_correction_samples = -fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
@@ -419,7 +419,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
// ################## PLL ##########################################################
// PLL discriminator
- carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_L2_TWO_PI);
+ carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_L2_TWO_PI;
// Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation
@@ -427,7 +427,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
// New code Doppler frequency estimation
d_code_freq_chips = GPS_L2_M_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L2_M_CODE_RATE_HZ) / GPS_L2_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
- d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD;
+ d_acc_carrier_phase_rad -= GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L2_TWO_PI);
@@ -438,7 +438,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator
- float code_error_filt_secs;
+ double code_error_filt_secs;
code_error_filt_secs = (GPS_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_CODE_RATE_HZ; //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
@@ -449,7 +449,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
double T_prn_samples;
double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
- T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
+ T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * GPS_L2_M_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
@@ -502,16 +502,16 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples)/static_cast<double>(d_fs_in);
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
- current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
+ current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
//compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
- current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
- current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
- current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
+ current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
+ current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
+ current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_tracking = true;
*out[0] = current_synchro_data;
@@ -585,40 +585,40 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
tmp_L = std::abs<float>(*d_Late);
try
{
- // EPR
- d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
- // PROMPT I and Q (to analyze navigation symbols)
- d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
- // PRN start sample stamp
- //tmp_float=(float)d_sample_counter;
- d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
- // accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
-
- // carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
-
- //PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
-
- //DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
-
- // CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
-
- // AUX vars (for debug purposes)
- tmp_float = d_rem_code_phase_samples;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
- d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+ // EPR
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
+ // PROMPT I and Q (to analyze navigation symbols)
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
+ // PRN start sample stamp
+ //tmp_float=(float)d_sample_counter;
+ d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
+ // accumulated carrier phase
+ d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
+
+ // carrier and code frequency
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
+
+ //PLL commands
+ d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+
+ //DLL commands
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
+
+ // CN0 and carrier lock test
+ d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
+ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
+
+ // AUX vars (for debug purposes)
+ tmp_double = d_rem_code_phase_samples;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+ tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
catch (std::ifstream::failure& e)
{
diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h
index 615df8c..4fc35f4 100644
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h
@@ -137,24 +137,24 @@ private:
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
- float d_rem_carr_phase_rad;
+ double d_rem_carr_phase_rad;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
- float d_acq_code_phase_samples;
- float d_acq_carrier_doppler_hz;
+ double d_acq_code_phase_samples;
+ double d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
- float d_carrier_doppler_hz;
- float d_acc_carrier_phase_rad;
- float d_code_phase_samples;
- float d_acc_code_phase_secs;
+ double d_carrier_doppler_hz;
+ double d_acc_carrier_phase_rad;
+ double d_code_phase_samples;
+ double d_acc_code_phase_secs;
//PRN period in samples
int d_current_prn_length_samples;
@@ -166,9 +166,9 @@ private:
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
- float d_carrier_lock_test;
- float d_CN0_SNV_dB_Hz;
- float d_carrier_lock_threshold;
+ double d_carrier_lock_test;
+ double d_CN0_SNV_dB_Hz;
+ double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt
index a6a51cd..665ebea 100644
--- a/src/algorithms/tracking/libs/CMakeLists.txt
+++ b/src/algorithms/tracking/libs/CMakeLists.txt
@@ -32,6 +32,7 @@ endif(ENABLE_CUDA)
set(TRACKING_LIB_SOURCES
correlator.cc
+ cpu_multicorrelator.cc
lock_detectors.cc
tcp_communication.cc
tcp_packet_data.cc
diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.cc b/src/algorithms/tracking/libs/cpu_multicorrelator.cc
new file mode 100644
index 0000000..0ed2ac9
--- /dev/null
+++ b/src/algorithms/tracking/libs/cpu_multicorrelator.cc
@@ -0,0 +1,167 @@
+/*!
+ * \file cpu_multicorrelator.cc
+ * \brief High optimized CPU vector multiTAP correlator class
+ * \authors <ul>
+ * <li> Javier Arribas, 2015. jarribas(at)cttc.es
+ * </ul>
+ *
+ * Class that implements a high optimized vector multiTAP correlator class for CPUs
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "cpu_multicorrelator.h"
+
+#include <iostream>
+#include <volk/volk.h>
+#include <gnuradio/fxpt.h> // fixed point sine and cosine
+#include <cmath>
+
+bool cpu_multicorrelator::init(
+ int max_signal_length_samples,
+ int n_correlators
+ )
+{
+
+ // ALLOCATE MEMORY FOR INTERNAL vectors
+ size_t size = max_signal_length_samples * sizeof(std::complex<float>);
+
+ // NCO signal
+ d_nco_in=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+
+ // Doppler-free signal
+ d_sig_doppler_wiped=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+
+ d_local_codes_resampled=new std::complex<float>*[n_correlators];
+ for (int n=0;n<n_correlators;n++)
+ {
+ d_local_codes_resampled[n]=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+ }
+ d_n_correlators=n_correlators;
+ return true;
+}
+
+bool cpu_multicorrelator::set_local_code_and_taps(
+ int code_length_chips,
+ const std::complex<float>* local_code_in,
+ float *shifts_chips
+ )
+{
+
+ d_local_code_in=local_code_in;
+ d_shifts_chips=shifts_chips;
+ d_code_length_chips=code_length_chips;
+ return true;
+}
+
+bool cpu_multicorrelator::set_input_output_vectors(
+ std::complex<float>* corr_out,
+ const std::complex<float>* sig_in
+ )
+{
+ // Save CPU pointers
+ d_sig_in =sig_in;
+ d_corr_out = corr_out;
+ return true;
+
+}
+
+void cpu_multicorrelator::update_local_code(int correlator_length_samples,float rem_code_phase_chips, float code_phase_step_chips)
+{
+ float local_code_chip_index;
+ for (int current_correlator_tap=0; current_correlator_tap<d_n_correlators;current_correlator_tap++)
+ {
+ for (int n = 0; n < correlator_length_samples; n++)
+ {
+ // resample code for current tap
+ local_code_chip_index= fmod(code_phase_step_chips*static_cast<float>(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips);
+ //Take into account that in multitap correlators, the shifts can be negative!
+ if (local_code_chip_index<0.0) local_code_chip_index+=d_code_length_chips;
+ d_local_codes_resampled[current_correlator_tap][n]=d_local_code_in[static_cast<int>(round(local_code_chip_index))];
+
+ }
+ }
+}
+
+
+void cpu_multicorrelator::update_local_carrier(int correlator_length_samples, float rem_carr_phase_rad, float phase_step_rad)
+{
+ float sin_f, cos_f;
+ int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
+ int phase_rad_i = gr::fxpt::float_to_fixed(rem_carr_phase_rad);
+
+ for(int i = 0; i < correlator_length_samples; i++)
+ {
+ gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f);
+ d_nco_in[i] = std::complex<float>(cos_f, -sin_f);
+ phase_rad_i += phase_step_rad_i;
+ }
+}
+
+bool cpu_multicorrelator::Carrier_wipeoff_multicorrelator_resampler(
+ float rem_carrier_phase_in_rad,
+ float phase_step_rad,
+ float rem_code_phase_chips,
+ float code_phase_step_chips,
+ int signal_length_samples)
+ {
+
+ update_local_carrier(signal_length_samples, rem_carrier_phase_in_rad, phase_step_rad);
+ update_local_code(signal_length_samples,rem_code_phase_chips, code_phase_step_chips);
+
+ volk_32fc_x2_multiply_32fc(d_sig_doppler_wiped, d_sig_in, d_nco_in, signal_length_samples);
+ for (int current_correlator_tap=0; current_correlator_tap<d_n_correlators;current_correlator_tap++)
+ {
+ volk_32fc_x2_dot_prod_32fc(&d_corr_out[current_correlator_tap], d_sig_doppler_wiped, d_local_codes_resampled[current_correlator_tap], signal_length_samples);
+ }
+ return true;
+}
+
+
+cpu_multicorrelator::cpu_multicorrelator()
+{
+ d_sig_in=NULL;
+ d_nco_in=NULL;
+ d_sig_doppler_wiped=NULL;
+ d_local_code_in=NULL;
+ d_shifts_chips=NULL;
+ d_corr_out=NULL;
+ d_code_length_chips=0;
+ d_n_correlators=0;
+}
+
+bool cpu_multicorrelator::free()
+{
+ // Free memory
+ if (d_sig_doppler_wiped!=NULL) volk_free(d_sig_doppler_wiped);
+ if (d_nco_in!=NULL) volk_free(d_nco_in);
+ for (int n=0;n<d_n_correlators;n++)
+ {
+ volk_free(d_local_codes_resampled[n]);
+ }
+ delete d_local_codes_resampled;
+ return true;
+}
+
diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.h b/src/algorithms/tracking/libs/cpu_multicorrelator.h
new file mode 100644
index 0000000..ca0f068
--- /dev/null
+++ b/src/algorithms/tracking/libs/cpu_multicorrelator.h
@@ -0,0 +1,98 @@
+/*!
+ * \file cpu_multicorrelator.h
+ * \brief High optimized CPU vector multiTAP correlator class
+ * \authors <ul>
+ * <li> Javier Arribas, 2015. jarribas(at)cttc.es
+ * </ul>
+ *
+ * Class that implements a high optimized vector multiTAP correlator class for CPUs
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (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 <http://www.gnu.org/licenses/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef CPU_MULTICORRELATOR_H_
+#define CPU_MULTICORRELATOR_H_
+
+#include <complex>
+
+/*!
+ * \brief Class that implements carrier wipe-off and correlators.
+ */
+class cpu_multicorrelator
+{
+public:
+ cpu_multicorrelator();
+ bool init(
+ int max_signal_length_samples,
+ int n_correlators
+ );
+ bool set_local_code_and_taps(
+ int code_length_chips,
+ const std::complex<float>* local_code_in,
+ float *shifts_chips
+ );
+ bool set_input_output_vectors(
+ std::complex<float>* corr_out,
+ const std::complex<float>* sig_in
+ );
+ void update_local_code(
+ int correlator_length_samples,
+ float rem_code_phase_chips,
+ float code_phase_step_chips
+ );
+
+ void update_local_carrier(
+ int correlator_length_samples,
+ float rem_carr_phase_rad,
+ float phase_step_rad
+ );
+ bool Carrier_wipeoff_multicorrelator_resampler(
+ float rem_carrier_phase_in_rad,
+ float phase_step_rad,
+ float rem_code_phase_chips,
+ float code_phase_step_chips,
+ int signal_length_samples);
+ bool free();
+
+private:
+ // Allocate the device input vectors
+ const std::complex<float> *d_sig_in;
+ std::complex<float> *d_nco_in;
+ std::complex<float> **d_local_codes_resampled;
+ std::complex<float> *d_sig_doppler_wiped;
+ const std::complex<float> *d_local_code_in;
+ std::complex<float> *d_corr_out;
+
+ float *d_shifts_chips;
+ int d_code_length_chips;
+ int d_n_correlators;
+
+ bool update_local_code();
+ bool update_local_carrier();
+
+};
+
+
+#endif /* CPU_MULTICORRELATOR_H_ */
diff --git a/src/algorithms/tracking/libs/cuda_multicorrelator.cu b/src/algorithms/tracking/libs/cuda_multicorrelator.cu
index 43ffeed..5017d14 100644
--- a/src/algorithms/tracking/libs/cuda_multicorrelator.cu
+++ b/src/algorithms/tracking/libs/cuda_multicorrelator.cu
@@ -32,26 +32,14 @@
* -------------------------------------------------------------------------
*/
-///////////////////////////////////////////////////////////////////////////////
-// On G80-class hardware 24-bit multiplication takes 4 clocks per warp
-// (the same as for floating point multiplication and addition),
-// whereas full 32-bit multiplication takes 16 clocks per warp.
-// So if integer multiplication operands are guaranteed to fit into 24 bits
-// (always lie withtin [-8M, 8M - 1] range in signed case),
-// explicit 24-bit multiplication is preferred for performance.
-///////////////////////////////////////////////////////////////////////////////
-#define IMUL(a, b) __mul24(a, b)
-
#include "cuda_multicorrelator.h"
#include <stdio.h>
-
+#include <iostream>
// For the CUDA runtime routines (prefixed with "cuda_")
#include <cuda_runtime.h>
-
-#define ACCUM_N 256
-
+#define ACCUM_N 128
__global__ void scalarProdGPUCPXxN_shifts_chips(
GPU_Complex *d_corr_out,
@@ -90,15 +78,17 @@ __global__ void scalarProdGPUCPXxN_shifts_chips(
for (int pos = iAccum; pos < elementN; pos += ACCUM_N)
{
+ //original sample code
//sum = sum + d_sig_in[pos-vectorBase] * d_nco_in[pos-vectorBase] * d_local_codes_in[pos];
//sum = sum + d_sig_in[pos-vectorBase] * d_local_codes_in[pos];
//sum.multiply_acc(d_sig_in[pos],d_local_codes_in[pos+d_shifts_samples[vec]]);
+ //custom code for multitap correlator
// 1.resample local code for the current shift
float local_code_chip_index= fmod(code_phase_step_chips*(float)pos + d_shifts_chips[vec] - rem_code_phase_chips, code_length_chips);
- //TODO: Take into account that in multitap correlators, the shifts can be negative!
+ //Take into account that in multitap correlators, the shifts can be negative!
if (local_code_chip_index<0.0) local_code_chip_index+=code_length_chips;
-
+ //printf("vec= %i, pos %i, chip_idx=%i chip_shift=%f \r\n",vec, pos,__float2int_rd(local_code_chip_index),local_code_chip_index);
// 2.correlate
sum.multiply_acc(d_sig_in[pos],d_local_code_in[__float2int_rd(local_code_chip_index)]);
@@ -127,90 +117,57 @@ __global__ void scalarProdGPUCPXxN_shifts_chips(
}
}
-
-///////////////////////////////////////////////////////////////////////////////
-// Calculate scalar products of VectorN vectors of ElementN elements on GPU
-// Parameters restrictions:
-// 1) ElementN is strongly preferred to be a multiple of warp size to
-// meet alignment constraints of memory coalescing.
-// 2) ACCUM_N must be a power of two.
-///////////////////////////////////////////////////////////////////////////////
-
-
-__global__ void scalarProdGPUCPXxN_shifts(
- GPU_Complex *d_corr_out,
- GPU_Complex *d_sig_in,
- GPU_Complex *d_local_codes_in,
- int *d_shifts_samples,
- int vectorN,
- int elementN
-)
+/**
+ * CUDA Kernel Device code
+ *
+ * Computes the carrier Doppler wipe-off by integrating the NCO in the CUDA kernel
+ */
+__global__ void
+CUDA_32fc_Doppler_wipeoff( GPU_Complex *sig_out, GPU_Complex *sig_in, float rem_carrier_phase_in_rad, float phase_step_rad, int numElements)
{
- //Accumulators cache
- __shared__ GPU_Complex accumResult[ACCUM_N];
-
- ////////////////////////////////////////////////////////////////////////////
- // Cycle through every pair of vectors,
- // taking into account that vector counts can be different
- // from total number of thread blocks
- ////////////////////////////////////////////////////////////////////////////
- for (int vec = blockIdx.x; vec < vectorN; vec += gridDim.x)
+ // CUDA version of floating point NCO and vector dot product integrated
+ float sin;
+ float cos;
+ for (int i = blockIdx.x * blockDim.x + threadIdx.x;
+ i < numElements;
+ i += blockDim.x * gridDim.x)
{
- int vectorBase = IMUL(elementN, vec);
- int vectorEnd = vectorBase + elementN;
-
- ////////////////////////////////////////////////////////////////////////
- // Each accumulator cycles through vectors with
- // stride equal to number of total number of accumulators ACCUM_N
- // At this stage ACCUM_N is only preferred be a multiple of warp size
- // to meet memory coalescing alignment constraints.
- ////////////////////////////////////////////////////////////////////////
- for (int iAccum = threadIdx.x; iAccum < ACCUM_N; iAccum += blockDim.x)
- {
- GPU_Complex sum = GPU_Complex(0,0);
-
- for (int pos = vectorBase + iAccum; pos < vectorEnd; pos += ACCUM_N)
- {
- //sum = sum + d_sig_in[pos-vectorBase] * d_nco_in[pos-vectorBase] * d_local_codes_in[pos];
- //sum = sum + d_sig_in[pos-vectorBase] * d_local_codes_in[pos];
- sum.multiply_acc(d_sig_in[pos-vectorBase],d_local_codes_in[pos-vectorBase+d_shifts_samples[vec]]);
- }
- accumResult[iAccum] = sum;
- }
-
- ////////////////////////////////////////////////////////////////////////
- // Perform tree-like reduction of accumulators' results.
- // ACCUM_N has to be power of two at this stage
- ////////////////////////////////////////////////////////////////////////
- for (int stride = ACCUM_N / 2; stride > 0; stride >>= 1)
- {
- __syncthreads();
-
- for (int iAccum = threadIdx.x; iAccum < stride; iAccum += blockDim.x)
- {
- accumResult[iAccum] += accumResult[stride + iAccum];
- }
- }
-
- if (threadIdx.x == 0)
- {
- d_corr_out[vec] = accumResult[0];
- }
+ __sincosf(rem_carrier_phase_in_rad + i*phase_step_rad, &sin, &cos);
+ sig_out[i] = sig_in[i] * GPU_Complex(cos,-sin);
}
}
-__global__ void scalarProdGPUCPXxN(
+__global__ void Doppler_wippe_scalarProdGPUCPXxN_shifts_chips(
GPU_Complex *d_corr_out,
GPU_Complex *d_sig_in,
- GPU_Complex *d_local_codes_in,
+ GPU_Complex *d_sig_wiped,
+ GPU_Complex *d_local_code_in,
+ float *d_shifts_chips,
+ float code_length_chips,
+ float code_phase_step_chips,
+ float rem_code_phase_chips,
int vectorN,
- int elementN
+ int elementN,
+ float rem_carrier_phase_in_rad,
+ float phase_step_rad
)
{
//Accumulators cache
__shared__ GPU_Complex accumResult[ACCUM_N];
+ // CUDA version of floating point NCO and vector dot product integrated
+ float sin;
+ float cos;
+ for (int i = blockIdx.x * blockDim.x + threadIdx.x;
+ i < elementN;
+ i += blockDim.x * gridDim.x)
+ {
+ __sincosf(rem_carrier_phase_in_rad + i*phase_step_rad, &sin, &cos);
+ d_sig_wiped[i] = d_sig_in[i] * GPU_Complex(cos,-sin);
+ }
+
+ __syncthreads();
////////////////////////////////////////////////////////////////////////////
// Cycle through every pair of vectors,
// taking into account that vector counts can be different
@@ -219,7 +176,7 @@ __global__ void scalarProdGPUCPXxN(
for (int vec = blockIdx.x; vec < vectorN; vec += gridDim.x)
{
//int vectorBase = IMUL(elementN, vec);
- //int vectorEnd = vectorBase + elementN;
+ //int vectorEnd = elementN;
////////////////////////////////////////////////////////////////////////
// Each accumulator cycles through vectors with
@@ -230,14 +187,26 @@ __global__ void scalarProdGPUCPXxN(
for (int iAccum = threadIdx.x; iAccum < ACCUM_N; iAccum += blockDim.x)
{
GPU_Complex sum = GPU_Complex(0,0);
-
- //for (int pos = vectorBase + iAccum; pos < vectorEnd; pos += ACCUM_N)
- for (int pos = iAccum; pos < elementN; pos += ACCUM_N)
+ float local_code_chip_index;
+ //float code_phase;
+ for (int pos = iAccum; pos < elementN; pos += ACCUM_N)
{
+ //original sample code
//sum = sum + d_sig_in[pos-vectorBase] * d_nco_in[pos-vectorBase] * d_local_codes_in[pos];
//sum = sum + d_sig_in[pos-vectorBase] * d_local_codes_in[pos];
- //sum.multiply_acc(d_sig_in[pos-vectorBase],d_local_codes_in[pos]);
- sum.multiply_acc(d_sig_in[pos],d_local_codes_in[pos]);
+ //sum.multiply_acc(d_sig_in[pos],d_local_codes_in[pos+d_shifts_samples[vec]]);
+
+ //custom code for multitap correlator
+ // 1.resample local code for the current shift
+
+ local_code_chip_index= fmodf(code_phase_step_chips*__int2float_rd(pos)+ d_shifts_chips[vec] - rem_code_phase_chips, code_length_chips);
+
+ //Take into account that in multitap correlators, the shifts can be negative!
+ if (local_code_chip_index<0.0) local_code_chip_index+=code_length_chips;
+ //printf("vec= %i, pos %i, chip_idx=%i chip_shift=%f \r\n",vec, pos,__float2int_rd(local_code_chip_index),local_code_chip_index);
+ // 2.correlate
+ sum.multiply_acc(d_sig_wiped[pos],d_local_code_in[__float2int_rd(local_code_chip_index)]);
+
}
accumResult[iAccum] = sum;
}
@@ -263,166 +232,7 @@ __global__ void scalarProdGPUCPXxN(
}
}
-
-//*********** CUDA processing **************
-// Treads: a minimal parallel execution code on GPU
-// Blocks: a set of N threads
-/**
- * CUDA Kernel Device code
- *
- * Computes the vectorial product of A and B into C. The 3 vectors have the same
- * number of elements numElements.
- */
-__global__ void CUDA_32fc_x2_multiply_32fc( GPU_Complex *A, GPU_Complex *B, GPU_Complex *C, int numElements)
-{
- for (int i = blockIdx.x * blockDim.x + threadIdx.x;
- i < numElements;
- i += blockDim.x * gridDim.x)
- {
- C[i] = A[i] * B[i];
- }
-}
-
-
-/**
- * CUDA Kernel Device code
- *
- * Computes the carrier Doppler wipe-off by integrating the NCO in the CUDA kernel
- */
-__global__ void
-CUDA_32fc_Doppler_wipeoff( GPU_Complex *sig_out, GPU_Complex *sig_in, float rem_carrier_phase_in_rad, float phase_step_rad, int numElements)
-{
- //*** NCO CPU code (GNURadio FXP NCO)
- //float sin_f, cos_f;
- //float phase_step_rad = static_cast<float>(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
- //int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
- //int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad);
- //
- //for(int i = 0; i < d_current_prn_length_samples; i++)
- // {
- // gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f);
- // d_carr_sign[i] = std::complex<float>(cos_f, -sin_f);
- // phase_rad_i += phase_step_rad_i;
- // }
-
- // CUDA version of floating point NCO and vector dot product integrated
-
- float sin;
- float cos;
- for (int i = blockIdx.x * blockDim.x + threadIdx.x;
- i < numElements;
- i += blockDim.x * gridDim.x)
- {
- __sincosf(rem_carrier_phase_in_rad + i*phase_step_rad, &sin, &cos);
- sig_out[i] = sig_in[i] * GPU_Complex(cos,-sin);
- }
-}
-
-
-/**
- * CUDA Kernel Device code
- *
- * Computes the vectorial product of A and B into C. The 3 vectors have the same
- * number of elements numElements.
- */
-__global__ void
-CUDA_32fc_x2_add_32fc( GPU_Complex *A, GPU_Complex *B, GPU_Complex *C, int numElements)
-{
- for (int i = blockIdx.x * blockDim.x + threadIdx.x;
- i < numElements;
- i += blockDim.x * gridDim.x)
- {
- C[i] = A[i] + B[i];
- }
-}
-
-
-bool cuda_multicorrelator::init_cuda(const int argc, const char **argv, int signal_length_samples, int local_codes_length_samples, int n_correlators)
-{
- // use command-line specified CUDA device, otherwise use device with highest Gflops/s
-// findCudaDevice(argc, (const char **)argv);
-// cudaDeviceProp prop;
-// int num_devices, device;
-// cudaGetDeviceCount(&num_devices);
-// if (num_devices > 1) {
-// int max_multiprocessors = 0, max_device = 0;
-// for (device = 0; device < num_devices; device++) {
-// cudaDeviceProp properties;
-// cudaGetDeviceProperties(&properties, device);
-// if (max_multiprocessors < properties.multiProcessorCount) {
-// max_multiprocessors = properties.multiProcessorCount;
-// max_device = device;
-// }
-// printf("Found GPU device # %i\n",device);
-// }
-// //cudaSetDevice(max_device);
-//
-// //set random device!
-// cudaSetDevice(rand() % num_devices); //generates a random number between 0 and num_devices to split the threads between GPUs
-//
-// cudaGetDeviceProperties( &prop, max_device );
-// //debug code
-// if (prop.canMapHostMemory != 1) {
-// printf( "Device can not map memory.\n" );
-// }
-// printf("L2 Cache size= %u \n",prop.l2CacheSize);
-// printf("maxThreadsPerBlock= %u \n",prop.maxThreadsPerBlock);
-// printf("maxGridSize= %i \n",prop.maxGridSize[0]);
-// printf("sharedMemPerBlock= %lu \n",prop.sharedMemPerBlock);
-// printf("deviceOverlap= %i \n",prop.deviceOverlap);
-// printf("multiProcessorCount= %i \n",prop.multiProcessorCount);
-// }else{
-// int whichDevice;
-// cudaGetDevice( &whichDevice );
-// cudaGetDeviceProperties( &prop, whichDevice );
-// //debug code
-// if (prop.canMapHostMemory != 1) {
-// printf( "Device can not map memory.\n" );
-// }
-//
-// printf("L2 Cache size= %u \n",prop.l2CacheSize);
-// printf("maxThreadsPerBlock= %u \n",prop.maxThreadsPerBlock);
-// printf("maxGridSize= %i \n",prop.maxGridSize[0]);
-// printf("sharedMemPerBlock= %lu \n",prop.sharedMemPerBlock);
-// printf("deviceOverlap= %i \n",prop.deviceOverlap);
-// printf("multiProcessorCount= %i \n",prop.multiProcessorCount);
-// }
-
- // (cudaFuncSetCacheConfig(CUDA_32fc_x2_multiply_x2_dot_prod_32fc_, cudaFuncCachePreferShared));
-
-
- // ALLOCATE GPU MEMORY FOR INPUT/OUTPUT and INTERNAL vectors
-
- size_t size = signal_length_samples * sizeof(GPU_Complex);
-
- cudaMalloc((void **)&d_sig_in, size);
- // (cudaMalloc((void **)&d_nco_in, size));
- cudaMalloc((void **)&d_sig_doppler_wiped, size);
-
- // old version: all local codes are independent vectors
- // (cudaMalloc((void **)&d_local_codes_in, size*n_correlators));
-
- // new version: only one vector with extra samples to shift the local code for the correlator set
- // Required: The last correlator tap in d_shifts_samples has the largest sample shift
- size_t size_local_code_bytes = local_codes_length_samples * sizeof(GPU_Complex);
- cudaMalloc((void **)&d_local_codes_in, size_local_code_bytes);
- cudaMalloc((void **)&d_shifts_samples, sizeof(int)*n_correlators);
-
- //scalars
- cudaMalloc((void **)&d_corr_out, sizeof(std::complex<float>)*n_correlators);
-
- // Launch the Vector Add CUDA Kernel
- threadsPerBlock = 256;
- blocksPerGrid =(int)(signal_length_samples+threadsPerBlock-1)/threadsPerBlock;
-
- cudaStreamCreate (&stream1) ;
- cudaStreamCreate (&stream2) ;
- return true;
-}
-
-
bool cuda_multicorrelator::init_cuda_integrated_resampler(
- const int argc, const char **argv,
int signal_length_samples,
int code_length_chips,
int n_correlators
@@ -480,34 +290,45 @@ bool cuda_multicorrelator::init_cuda_integrated_resampler(
// (cudaFuncSetCacheConfig(CUDA_32fc_x2_multiply_x2_dot_prod_32fc_, cudaFuncCachePreferShared));
// ALLOCATE GPU MEMORY FOR INPUT/OUTPUT and INTERNAL vectors
-
size_t size = signal_length_samples * sizeof(GPU_Complex);
- cudaMalloc((void **)&d_sig_in, size);
- cudaMemset(d_sig_in,0,size);
+ //********* ZERO COPY VERSION ************
+ // Set flag to enable zero copy access
+ // Optimal in shared memory devices (like Jetson K1)
+ cudaSetDeviceFlags(cudaDeviceMapHost);
+
+ //******** CudaMalloc version ***********
+
+ // input signal GPU memory (can be mapped to CPU memory in shared memory devices!)
+ // cudaMalloc((void **)&d_sig_in, size);
+ // cudaMemset(d_sig_in,0,size);
- // (cudaMalloc((void **)&d_nco_in, size));
+ // Doppler-free signal (internal GPU memory)
cudaMalloc((void **)&d_sig_doppler_wiped, size);
cudaMemset(d_sig_doppler_wiped,0,size);
+ // Local code GPU memory (can be mapped to CPU memory in shared memory devices!)
cudaMalloc((void **)&d_local_codes_in, sizeof(std::complex<float>)*code_length_chips);
cudaMemset(d_local_codes_in,0,sizeof(std::complex<float>)*code_length_chips);
d_code_length_chips=code_length_chips;
+ // Vector with the chip shifts for each correlator tap
+ //GPU memory (can be mapped to CPU memory in shared memory devices!)
cudaMalloc((void **)&d_shifts_chips, sizeof(float)*n_correlators);
cudaMemset(d_shifts_chips,0,sizeof(float)*n_correlators);
//scalars
- cudaMalloc((void **)&d_corr_out, sizeof(std::complex<float>)*n_correlators);
- cudaMemset(d_corr_out,0,sizeof(std::complex<float>)*n_correlators);
+ //cudaMalloc((void **)&d_corr_out, sizeof(std::complex<float>)*n_correlators);
+ //cudaMemset(d_corr_out,0,sizeof(std::complex<float>)*n_correlators);
// Launch the Vector Add CUDA Kernel
- threadsPerBlock = 256;
+ // TODO: write a smart load balance using device info!
+ threadsPerBlock = 64;
blocksPerGrid =(int)(signal_length_samples+threadsPerBlock-1)/threadsPerBlock;
cudaStreamCreate (&stream1) ;
- cudaStreamCreate (&stream2) ;
+ //cudaStreamCreate (&stream2) ;
return true;
}
@@ -518,103 +339,57 @@ bool cuda_multicorrelator::set_local_code_and_taps(
int n_correlators
)
{
- // local code CPU -> GPU copy memory
+ //********* ZERO COPY VERSION ************
+// // Get device pointer from host memory. No allocation or memcpy
+// cudaError_t code;
+// // local code CPU -> GPU copy memory
+// code=cudaHostGetDevicePointer((void **)&d_local_codes_in, (void *) local_codes_in, 0);
+// if (code!=cudaSuccess)
+// {
+// printf("cuda cudaHostGetDevicePointer error in set_local_code_and_taps \r\n");
+// }
+// // Correlator shifts vector CPU -> GPU copy memory (fractional chip shifts are allowed!)
+// code=cudaHostGetDevicePointer((void **)&d_shifts_chips, (void *) shifts_chips, 0);
+// if (code!=cudaSuccess)
+// {
+// printf("cuda cudaHostGetDevicePointer error in set_local_code_and_taps \r\n");
+// }
+
+ //******** CudaMalloc version ***********
+ //local code CPU -> GPU copy memory
cudaMemcpyAsync(d_local_codes_in, local_codes_in, sizeof(GPU_Complex)*code_length_chips, cudaMemcpyHostToDevice,stream1);
d_code_length_chips=(float)code_length_chips;
- // Correlator shifts vector CPU -> GPU copy memory (fractional chip shifts are allowed!)
+ //Correlator shifts vector CPU -> GPU copy memory (fractional chip shifts are allowed!)
cudaMemcpyAsync(d_shifts_chips, shifts_chips, sizeof(float)*n_correlators,
cudaMemcpyHostToDevice,stream1);
return true;
}
-
-
-bool cuda_multicorrelator::Carrier_wipeoff_multicorrelator_cuda(
+bool cuda_multicorrelator::set_input_output_vectors(
std::complex<float>* corr_out,
- const std::complex<float>* sig_in,
- const std::complex<float>* local_codes_in,
- float rem_carrier_phase_in_rad,
- float phase_step_rad,
- const int *shifts_samples,
- int signal_length_samples,
- int n_correlators)
- {
-
- size_t memSize = signal_length_samples * sizeof(std::complex<float>);
-
- // input signal CPU -> GPU copy memory
-
- cudaMemcpyAsync(d_sig_in, sig_in, memSize,
- cudaMemcpyHostToDevice, stream1);
-
- //***** NOTICE: NCO is computed on-the-fly, not need to copy NCO into GPU! ****
- // (cudaMemcpyAsync(d_nco_in, nco_in, memSize,
- // cudaMemcpyHostToDevice, stream1));
-
-
- // old version: all local codes are independent vectors
- // (cudaMemcpyAsync(d_local_codes_in, local_codes_in, memSize*n_correlators,
- // cudaMemcpyHostToDevice, stream2));
-
- // new version: only one vector with extra samples to shift the local code for the correlator set
- // Required: The last correlator tap in d_shifts_samples has the largest sample shift
-
- // local code CPU -> GPU copy memory
- cudaMemcpyAsync(d_local_codes_in, local_codes_in, memSize+sizeof(std::complex<float>)*shifts_samples[n_correlators-1],
- cudaMemcpyHostToDevice, stream2);
- // Correlator shifts vector CPU -> GPU copy memory
- cudaMemcpyAsync(d_shifts_samples, shifts_samples, sizeof(int)*n_correlators,
- cudaMemcpyHostToDevice, stream2);
-
-
- //Launch carrier wipe-off kernel here, while local codes are being copied to GPU!
- cudaStreamSynchronize(stream1);
- CUDA_32fc_Doppler_wipeoff<<<blocksPerGrid, threadsPerBlock,0, stream1>>>(d_sig_doppler_wiped, d_sig_in,rem_carrier_phase_in_rad,phase_step_rad, signal_length_samples);
-
+ std::complex<float>* sig_in
+ )
+{
- //printf("CUDA kernel launch with %d blocks of %d threads\n", blocksPerGrid, threadsPerBlock);
+ // Save CPU pointers
+ d_sig_in_cpu =sig_in;
+ d_corr_out_cpu = corr_out;
- //wait for Doppler wipeoff end...
- cudaStreamSynchronize(stream1);
- cudaStreamSynchronize(stream2);
- // (cudaDeviceSynchronize());
-
- //old
-// scalarProdGPUCPXxN<<<blocksPerGrid, threadsPerBlock,0 ,stream2>>>(
-// d_corr_out,
-// d_sig_doppler_wiped,
-// d_local_codes_in,
-// 3,
-// signal_length_samples
-// );
-
- //new
- //launch the multitap correlator
- scalarProdGPUCPXxN_shifts<<<blocksPerGrid, threadsPerBlock,0 ,stream2>>>(
- d_corr_out,
- d_sig_doppler_wiped,
- d_local_codes_in,
- d_shifts_samples,
- n_correlators,
- signal_length_samples
- );
- cudaGetLastError();
- //wait for correlators end...
- cudaStreamSynchronize(stream2);
- // Copy the device result vector in device memory to the host result vector
- // in host memory.
+ // Zero Copy version
+ // Get device pointer from host memory. No allocation or memcpy
+ cudaError_t code;
+ code=cudaHostGetDevicePointer((void **)&d_sig_in, (void *) sig_in, 0);
+ code=cudaHostGetDevicePointer((void **)&d_corr_out, (void *) corr_out, 0);
+ if (code!=cudaSuccess)
+ {
+ printf("cuda cudaHostGetDevicePointer error \r\n");
+ }
+ return true;
- //scalar products (correlators outputs)
- cudaMemcpy(corr_out, d_corr_out, sizeof(std::complex<float>)*n_correlators,
- cudaMemcpyDeviceToHost);
- return true;
}
-
bool cuda_multicorrelator::Carrier_wipeoff_multicorrelator_resampler_cuda(
- std::complex<float>* corr_out,
- const std::complex<float>* sig_in,
float rem_carrier_phase_in_rad,
float phase_step_rad,
float code_phase_step_chips,
@@ -623,26 +398,40 @@ bool cuda_multicorrelator::Carrier_wipeoff_multicorrelator_resampler_cuda(
int n_correlators)
{
- size_t memSize = signal_length_samples * sizeof(std::complex<float>);
+
+ // cudaMemCpy version
+ //size_t memSize = signal_length_samples * sizeof(std::complex<float>);
// input signal CPU -> GPU copy memory
- cudaMemcpyAsync(d_sig_in, sig_in, memSize,
- cudaMemcpyHostToDevice, stream2);
+ //cudaMemcpyAsync(d_sig_in, d_sig_in_cpu, memSize,
+ // cudaMemcpyHostToDevice, stream2);
//***** NOTICE: NCO is computed on-the-fly, not need to copy NCO into GPU! ****
-
//Launch carrier wipe-off kernel here, while local codes are being copied to GPU!
- cudaStreamSynchronize(stream2);
+ //cudaStreamSynchronize(stream2);
- CUDA_32fc_Doppler_wipeoff<<<blocksPerGrid, threadsPerBlock,0, stream2>>>(d_sig_doppler_wiped, d_sig_in,rem_carrier_phase_in_rad,phase_step_rad, signal_length_samples);
+ //CUDA_32fc_Doppler_wipeoff<<<blocksPerGrid, threadsPerBlock,0, stream1>>>(d_sig_doppler_wiped, d_sig_in,rem_carrier_phase_in_rad,phase_step_rad, signal_length_samples);
//wait for Doppler wipeoff end...
- cudaStreamSynchronize(stream1);
- cudaStreamSynchronize(stream2);
+ //cudaStreamSynchronize(stream1);
+ //cudaStreamSynchronize(stream2);
//launch the multitap correlator with integrated local code resampler!
- scalarProdGPUCPXxN_shifts_chips<<<blocksPerGrid, threadsPerBlock,0 ,stream1>>>(
+// scalarProdGPUCPXxN_shifts_chips<<<blocksPerGrid, threadsPerBlock,0 ,stream1>>>(
+// d_corr_out,
+// d_sig_doppler_wiped,
+// d_local_codes_in,
+// d_shifts_chips,
+// d_code_length_chips,
+// code_phase_step_chips,
+// rem_code_phase_chips,
+// n_correlators,
+// signal_length_samples
+// );
+
+ Doppler_wippe_scalarProdGPUCPXxN_shifts_chips<<<blocksPerGrid, threadsPerBlock,0 ,stream1>>>(
d_corr_out,
+ d_sig_in,
d_sig_doppler_wiped,
d_local_codes_in,
d_shifts_chips,
@@ -650,23 +439,33 @@ bool cuda_multicorrelator::Carrier_wipeoff_multicorrelator_resampler_cuda(
code_phase_step_chips,
rem_code_phase_chips,
n_correlators,
- signal_length_samples
- );
-
- cudaGetLastError();
+ signal_length_samples,
+ rem_carrier_phase_in_rad,
+ phase_step_rad
+ );
+
+ //debug
+// std::complex<float>* debug_signal;
+// debug_signal=static_cast<std::complex<float>*>(malloc(memSize));
+// cudaMemcpyAsync(debug_signal, d_sig_doppler_wiped, memSize,
+// cudaMemcpyDeviceToHost,stream1);
+// cudaStreamSynchronize(stream1);
+// std::cout<<"d_sig_doppler_wiped GPU="<<debug_signal[456]<<","<<debug_signal[1]<<","<<debug_signal[2]<<","<<debug_signal[3]<<std::endl;
+
+ //cudaGetLastError();
//wait for correlators end...
- cudaStreamSynchronize(stream1);
+ //cudaStreamSynchronize(stream1);
// Copy the device result vector in device memory to the host result vector
// in host memory.
//scalar products (correlators outputs)
- cudaMemcpyAsync(corr_out, d_corr_out, sizeof(std::complex<float>)*n_correlators,
- cudaMemcpyDeviceToHost,stream1);
+ //cudaMemcpyAsync(corr_out, d_corr_out, sizeof(std::complex<float>)*n_correlators,
+ // cudaMemcpyDeviceToHost,stream1);
+
cudaStreamSynchronize(stream1);
return true;
}
-
cuda_multicorrelator::cuda_multicorrelator()
{
d_sig_in=NULL;
@@ -689,22 +488,16 @@ bool cuda_multicorrelator::free_cuda()
if (d_sig_doppler_wiped!=NULL) cudaFree(d_sig_doppler_wiped);
if (d_local_codes_in!=NULL) cudaFree(d_local_codes_in);
if (d_corr_out!=NULL) cudaFree(d_corr_out);
-
-
if (d_shifts_samples!=NULL) cudaFree(d_shifts_samples);
if (d_shifts_chips!=NULL) cudaFree(d_shifts_chips);
-
- cudaStreamDestroy(stream1) ;
- cudaStreamDestroy(stream2) ;
-
// Reset the device and exit
// cudaDeviceReset causes the driver to clean up all state. While
// not mandatory in normal operation, it is good practice. It is also
// needed to ensure correct operation when the application is being
// profiled. Calling cudaDeviceReset causes all profile data to be
// flushed before the application exits
- // (cudaDeviceReset());
+ cudaDeviceReset();
return true;
}
diff --git a/src/algorithms/tracking/libs/cuda_multicorrelator.h b/src/algorithms/tracking/libs/cuda_multicorrelator.h
index df640f5..fb2c9a9 100644
--- a/src/algorithms/tracking/libs/cuda_multicorrelator.h
+++ b/src/algorithms/tracking/libs/cuda_multicorrelator.h
@@ -114,9 +114,7 @@ class cuda_multicorrelator
{
public:
cuda_multicorrelator();
- bool init_cuda(const int argc, const char **argv, int signal_length_samples, int local_codes_length_samples, int n_correlators);
bool init_cuda_integrated_resampler(
- const int argc, const char **argv,
int signal_length_samples,
int code_length_chips,
int n_correlators
@@ -127,19 +125,12 @@ public:
float *shifts_chips,
int n_correlators
);
+ bool set_input_output_vectors(
+ std::complex<float>* corr_out,
+ std::complex<float>* sig_in
+ );
bool free_cuda();
- bool Carrier_wipeoff_multicorrelator_cuda(
- std::complex<float>* corr_out,
- const std::complex<float>* sig_in,
- const std::complex<float>* local_codes_in,
- float rem_carrier_phase_in_rad,
- float phase_step_rad,
- const int *shifts_samples,
- int signal_length_samples,
- int n_correlators);
bool Carrier_wipeoff_multicorrelator_resampler_cuda(
- std::complex<float>* corr_out,
- const std::complex<float>* sig_in,
float rem_carrier_phase_in_rad,
float phase_step_rad,
float code_phase_step_chips,
@@ -154,6 +145,11 @@ private:
GPU_Complex *d_sig_doppler_wiped;
GPU_Complex *d_local_codes_in;
GPU_Complex *d_corr_out;
+
+ //
+ std::complex<float> *d_sig_in_cpu;
+ std::complex<float> *d_corr_out_cpu;
+
int *d_shifts_samples;
float *d_shifts_chips;
float d_code_length_chips;
@@ -162,7 +158,7 @@ private:
int blocksPerGrid;
cudaStream_t stream1;
- cudaStream_t stream2;
+ //cudaStream_t stream2;
int num_gpu_devices;
int selected_device;
};
diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc
index 119f40a..1aaa100 100644
--- a/src/algorithms/tracking/libs/tracking_discriminators.cc
+++ b/src/algorithms/tracking/libs/tracking_discriminators.cc
@@ -46,9 +46,9 @@
* \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second].
*/
-float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t1, float t2)
+double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2)
{
- float cross, dot;
+ double cross, dot;
dot = prompt_s1.real()*prompt_s2.real() + prompt_s1.imag()*prompt_s2.imag();
cross = prompt_s1.real()*prompt_s2.imag() - prompt_s2.real()*prompt_s1.imag();
return atan2(cross, dot) / (t2-t1);
@@ -62,7 +62,7 @@ float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t
* \f}
* where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
*/
-float pll_four_quadrant_atan(gr_complex prompt_s1)
+double pll_four_quadrant_atan(gr_complex prompt_s1)
{
return atan2(prompt_s1.imag(), prompt_s1.real());
}
@@ -75,7 +75,7 @@ float pll_four_quadrant_atan(gr_complex prompt_s1)
* \f}
* where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
*/
-float pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
+double pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
{
if (prompt_s1.real() != 0.0)
{
@@ -96,12 +96,12 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
* where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and
* \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips].
*/
-float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
+double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
{
- float P_early, P_late;
+ double P_early, P_late;
P_early = std::abs(early_s1);
P_late = std::abs(late_s1);
- return (P_early - P_late) / ((P_early + P_late));
+ return 0.5*(P_early - P_late) / ((P_early + P_late));
}
/*
@@ -113,9 +113,9 @@ float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
* where \f$E=\sqrt{I_{VE}^2+Q_{VE}^2+I_{E}^2+Q_{E}^2}\f$ and
* \f$L=\sqrt{I_{VL}^2+Q_{VL}^2+I_{L}^2+Q_{L}^2}\f$ . The output is in [chips].
*/
-float dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1)
+double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1)
{
- float P_early, P_late;
+ double P_early, P_late;
P_early = std::sqrt(std::norm(very_early_s1) + std::norm(early_s1));
P_late = std::sqrt(std::norm(very_late_s1) + std::norm(late_s1));
return (P_early - P_late) / ((P_early + P_late));
diff --git a/src/algorithms/tracking/libs/tracking_discriminators.h b/src/algorithms/tracking/libs/tracking_discriminators.h
index 8a78f9c..87fe3ab 100644
--- a/src/algorithms/tracking/libs/tracking_discriminators.h
+++ b/src/algorithms/tracking/libs/tracking_discriminators.h
@@ -50,7 +50,7 @@
* \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_1\f$, and
* \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second].
*/
-float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t1, float t2);
+double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2);
/*! \brief PLL four quadrant arctan discriminator
@@ -61,7 +61,7 @@ float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t
* \f}
* where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
*/
-float pll_four_quadrant_atan(gr_complex prompt_s1);
+double pll_four_quadrant_atan(gr_complex prompt_s1);
/*! \brief PLL Costas loop two quadrant arctan discriminator
@@ -72,7 +72,7 @@ float pll_four_quadrant_atan(gr_complex prompt_s1);
* \f}
* where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
*/
-float pll_cloop_two_quadrant_atan(gr_complex prompt_s1);
+double pll_cloop_two_quadrant_atan(gr_complex prompt_s1);
/*! \brief DLL Noncoherent Early minus Late envelope normalized discriminator
@@ -84,7 +84,7 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1);
* where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and
* \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips].
*/
-float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1);
+double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1);
/*! \brief DLL Noncoherent Very Early Minus Late Power (VEMLP) normalized discriminator
@@ -97,7 +97,7 @@ float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1);
* where \f$E=\sqrt{I_{VE}^2+Q_{VE}^2+I_{E}^2+Q_{E}^2}\f$ and
* \f$L=\sqrt{I_{VL}^2+Q_{VL}^2+I_{L}^2+Q_{L}^2}\f$ . The output is in [chips].
*/
-float dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1);
+double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1);
#endif
diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc
index 5010bc1..c5c74f3 100644
--- a/src/core/receiver/gnss_block_factory.cc
+++ b/src/core/receiver/gnss_block_factory.cc
@@ -78,6 +78,7 @@
#include "galileo_e1_pcps_quicksync_ambiguous_acquisition.h"
#include "galileo_e5a_noncoherent_iq_acquisition_caf.h"
#include "gps_l1_ca_dll_pll_tracking.h"
+#include "gps_l1_ca_dll_pll_c_aid_tracking.h"
#include "gps_l1_ca_dll_pll_optim_tracking.h"
#include "gps_l1_ca_dll_fll_pll_tracking.h"
#include "gps_l1_ca_tcp_connector_tracking.h"
@@ -1311,6 +1312,12 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
out_streams, queue));
block = std::move(block_);
}
+ else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0)
+ {
+ std::unique_ptr<TrackingInterface> block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams,
+ out_streams, queue));
+ block = std::move(block_);
+ }
else if (implementation.compare("GPS_L1_CA_DLL_PLL_Optim_Tracking") == 0)
{
std::unique_ptr<GNSSBlockInterface> block_(new GpsL1CaDllPllOptimTracking(configuration.get(), role, in_streams,
@@ -1576,6 +1583,12 @@ std::unique_ptr<TrackingInterface> GNSSBlockFactory::GetTrkBlock(
out_streams, queue));
block = std::move(block_);
}
+ else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0)
+ {
+ std::unique_ptr<TrackingInterface> block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams,
+ out_streams, queue));
+ block = std::move(block_);
+ }
else if (implementation.compare("GPS_L1_CA_DLL_PLL_Optim_Tracking") == 0)
{
std::unique_ptr<TrackingInterface> block_(new GpsL1CaDllPllOptimTracking(configuration.get(), role, in_streams,
diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h
index dea0cbb..cb63bb3 100644
--- a/src/core/system_parameters/GPS_L1_CA.h
+++ b/src/core/system_parameters/GPS_L1_CA.h
@@ -53,6 +53,7 @@ const double GPS_L1_FREQ_HZ = 1.57542e9; //!< L1 [Hz]
const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s]
const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips]
const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds]
+const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period [seconds]
/*!
* \brief Maximum Time-Of-Arrival (TOA) difference between satellites for a receiver operated on Earth surface is 20 ms
@@ -67,6 +68,9 @@ const double MAX_TOA_DELAY_MS = 20;
//#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here
const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
+
+// OBSERVABLE HISTORY DEEP FOR INTERPOLATION
+const int GPS_L1_CA_HISTORY_DEEP=100;
// NAVIGATION MESSAGE DEMODULATION AND DECODING
#define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1}
diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h
index 8d27405..1e18be6 100644
--- a/src/core/system_parameters/Galileo_E1.h
+++ b/src/core/system_parameters/Galileo_E1.h
@@ -42,6 +42,7 @@
// Physical constants
const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD
+const double GALILEO_TWO_PI = 6.283185307179600 ; //!< 2*Pi as defined in GALILEO ICD
const double GALILEO_GM = 3.986004418e14; //!< Geocentric gravitational constant[m^3/s^2]
const double GALILEO_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Mean angular velocity of the Earth [rad/s]
const double GALILEO_C_m_s = 299792458.0; //!< The speed of light, [m/s]
@@ -61,6 +62,10 @@ const int Galileo_E1_NUMBER_OF_CODES = 50;
const double GALILEO_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
+
+// OBSERVABLE HISTORY DEEP FOR INTERPOLATION
+const int GALILEO_E1_HISTORY_DEEP=100;
+
// Galileo INAV Telemetry structure
#define GALILEO_INAV_PREAMBLE {0, 1, 0, 1, 1, 0, 0, 0, 0, 0}
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list