mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-12 11:10:33 +00:00
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
This commit is contained in:
Javier Arribas
parent
6336556163
commit
8c22e5846d
@ -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
|
||||
|
||||
346
conf/gnss-sdr_Hybrid_byte_sim.conf
Normal file
346
conf/gnss-sdr_Hybrid_byte_sim.conf
Normal file
@ -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
|
||||
@ -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, ' ');
|
||||
|
||||
@ -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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
@ -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_
|
||||
@ -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}
|
||||
)
|
||||
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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));
|
||||
// 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));
|
||||
|
||||
//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));
|
||||
// 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));
|
||||
|
||||
//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));
|
||||
//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));
|
||||
|
||||
// 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));
|
||||
//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));
|
||||
|
||||
// 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));
|
||||
}
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -0,0 +1,588 @@
|
||||
/*!
|
||||
* \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
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* 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_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"
|
||||
#include "tracking_discriminators.h"
|
||||
#include "lock_detectors.h"
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "control_message_factory.h"
|
||||
|
||||
|
||||
/*!
|
||||
* \todo Include in definition header file
|
||||
*/
|
||||
#define CN0_ESTIMATION_SAMPLES 20
|
||||
#define MINIMUM_VALID_CN0 25
|
||||
#define MAXIMUM_LOCK_FAIL_COUNTER 50
|
||||
#define CARRIER_LOCK_THRESHOLD 0.85
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
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,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips)
|
||||
{
|
||||
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_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
|
||||
}
|
||||
|
||||
|
||||
|
||||
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,
|
||||
boost::shared_ptr<gr::msg_queue> queue,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips) :
|
||||
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
|
||||
d_queue = queue;
|
||||
d_dump = dump;
|
||||
d_if_freq = if_freq;
|
||||
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_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(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
// correlator outputs (scalar)
|
||||
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;
|
||||
|
||||
multicorrelator_cpu.init(2*d_correlation_length_samples,d_n_correlator_taps);
|
||||
|
||||
//--- Perform initializations ------------------------------
|
||||
// define initial code frequency basis of NCO
|
||||
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
|
||||
// define residual code phase (in chips)
|
||||
d_rem_code_phase_samples = 0.0;
|
||||
// define residual carrier phase
|
||||
d_rem_carrier_phase_rad = 0.0;
|
||||
|
||||
// sample synchronization
|
||||
d_sample_counter = 0;
|
||||
//d_sample_counter_seconds = 0;
|
||||
d_acq_sample_stamp = 0;
|
||||
|
||||
d_enable_tracking = false;
|
||||
d_pull_in = false;
|
||||
d_last_seg = 0;
|
||||
|
||||
// CN0 estimation and lock detector buffers
|
||||
d_cn0_estimation_counter = 0;
|
||||
d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES];
|
||||
d_carrier_lock_test = 1;
|
||||
d_CN0_SNV_dB_Hz = 0;
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD;
|
||||
|
||||
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;
|
||||
d_acquisition_gnss_synchro = 0;
|
||||
d_channel = 0;
|
||||
d_acq_code_phase_samples = 0.0;
|
||||
d_acq_carrier_doppler_hz = 0.0;
|
||||
d_carrier_doppler_hz = 0.0;
|
||||
d_acc_carrier_phase_cycles = 0.0;
|
||||
d_code_phase_samples = 0.0;
|
||||
|
||||
d_pll_to_dll_assist_secs_Ti=0.0;
|
||||
//set_min_output_buffer((long int)300);
|
||||
}
|
||||
|
||||
|
||||
void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
|
||||
{
|
||||
/*
|
||||
* correct the code phase according to the delay between acq and trk
|
||||
*/
|
||||
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
|
||||
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
|
||||
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
|
||||
|
||||
long int acq_trk_diff_samples;
|
||||
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<double>(acq_trk_diff_samples) / static_cast<double>(d_fs_in);
|
||||
//doppler effect
|
||||
// Fd=(C/(C+Vr))*F
|
||||
double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
|
||||
// new chip and prn sequence periods based on acq Doppler
|
||||
double T_chip_mod_seconds;
|
||||
double T_prn_mod_seconds;
|
||||
double T_prn_mod_samples;
|
||||
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_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<double>(d_fs_in);
|
||||
|
||||
d_correlation_length_samples = round(T_prn_mod_samples);
|
||||
|
||||
double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_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;
|
||||
}
|
||||
delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
|
||||
|
||||
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(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);
|
||||
|
||||
|
||||
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.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;
|
||||
|
||||
std::string sys_ = &d_acquisition_gnss_synchro->System;
|
||||
sys = sys_.substr(0,1);
|
||||
|
||||
// DEBUG OUTPUT
|
||||
std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
|
||||
|
||||
|
||||
// enable tracking
|
||||
d_pull_in = true;
|
||||
d_enable_tracking = true;
|
||||
|
||||
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
|
||||
<< " Code Phase correction [samples]=" << delay_correction_samples
|
||||
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
|
||||
}
|
||||
|
||||
|
||||
gps_l1_ca_dll_pll_c_aid_tracking_cc::~gps_l1_ca_dll_pll_c_aid_tracking_cc()
|
||||
{
|
||||
d_dump_file.close();
|
||||
|
||||
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_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)
|
||||
{
|
||||
// Block input data and block output stream pointers
|
||||
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;
|
||||
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;
|
||||
*out[0] = current_synchro_data;
|
||||
consume_each(samples_offset); //shift input to perform alignment with local replica
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Fill the acquisition data
|
||||
current_synchro_data = *d_acquisition_gnss_synchro;
|
||||
|
||||
// ################# 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 ##########################################################
|
||||
// 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
|
||||
// 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);
|
||||
|
||||
// ################## DLL ##########################################################
|
||||
// DLL discriminator
|
||||
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_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
|
||||
double T_chip_seconds;
|
||||
double T_prn_seconds;
|
||||
double T_prn_samples;
|
||||
double K_blk_samples;
|
||||
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
|
||||
T_chip_seconds = 1 / d_code_freq_chips;
|
||||
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
|
||||
T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
|
||||
|
||||
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;
|
||||
|
||||
|
||||
//################### 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_correlator_outs[1]; //prompt
|
||||
d_cn0_estimation_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_cn0_estimation_counter = 0;
|
||||
// Code lock indicator
|
||||
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
|
||||
// Carrier lock indicator
|
||||
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
|
||||
// Loss of lock detection
|
||||
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
|
||||
{
|
||||
d_carrier_lock_fail_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
|
||||
}
|
||||
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
|
||||
{
|
||||
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
|
||||
LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
|
||||
std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
|
||||
if (d_queue != gr::msg_queue::sptr())
|
||||
{
|
||||
d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
|
||||
}
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
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_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 = 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;
|
||||
|
||||
// ########## DEBUG OUTPUT
|
||||
/*!
|
||||
* \todo The stop timer has to be moved to the signal source!
|
||||
*/
|
||||
// debug: Second counter in channel 0
|
||||
if (d_channel == 0)
|
||||
{
|
||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter / d_fs_in);
|
||||
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
|
||||
DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
|
||||
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
|
||||
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter / d_fs_in);
|
||||
DLOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
|
||||
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled)
|
||||
/*!
|
||||
* \todo The stop timer has to be moved to the signal source!
|
||||
*/
|
||||
// stream to collect cout calls to improve thread safety
|
||||
std::stringstream tmp_str_stream;
|
||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter / d_fs_in);
|
||||
|
||||
if (d_channel == 0)
|
||||
{
|
||||
// debug: Second counter in channel 0
|
||||
tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
|
||||
std::cout << tmp_str_stream.rdbuf() << std::flush;
|
||||
}
|
||||
}
|
||||
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;
|
||||
*out[0] = current_synchro_data;
|
||||
}
|
||||
|
||||
if(d_dump)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
float prompt_I;
|
||||
float prompt_Q;
|
||||
float tmp_E, tmp_P, tmp_L;
|
||||
double tmp_double;
|
||||
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
|
||||
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_cycles), 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_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_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(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_correlation_length_samples);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure* e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing trk dump file " << e->what();
|
||||
}
|
||||
}
|
||||
|
||||
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_c_aid_tracking_cc::set_channel(unsigned int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
LOG(INFO) << "Tracking Channel set to " << d_channel;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
||||
d_dump_filename.append(".dat");
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
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 (const std::ifstream::failure* e)
|
||||
{
|
||||
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e->what() << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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_c_aid_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
|
||||
{
|
||||
d_acquisition_gnss_synchro = p_gnss_synchro;
|
||||
}
|
||||
@ -0,0 +1,182 @@
|
||||
/*!
|
||||
* \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
|
||||
*
|
||||
* 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/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#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>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/thread.hpp>
|
||||
#include <gnuradio/block.h>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include "concurrent_queue.h"
|
||||
#include "gps_sdr_signal_processing.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "tracking_2nd_DLL_filter.h"
|
||||
#include "tracking_FLL_PLL_filter.h"
|
||||
#include "cpu_multicorrelator.h"
|
||||
|
||||
class gps_l1_ca_dll_pll_c_aid_tracking_cc;
|
||||
|
||||
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_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,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class implements a DLL + PLL tracking loop block
|
||||
*/
|
||||
class gps_l1_ca_dll_pll_c_aid_tracking_cc: public gr::block
|
||||
{
|
||||
public:
|
||||
~gps_l1_ca_dll_pll_c_aid_tracking_cc();
|
||||
|
||||
void set_channel(unsigned int channel);
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
|
||||
void start_tracking();
|
||||
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
|
||||
|
||||
int general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
|
||||
|
||||
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
|
||||
|
||||
private:
|
||||
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,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
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,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
// tracking configuration vars
|
||||
boost::shared_ptr<gr::msg_queue> d_queue;
|
||||
concurrent_queue<int> *d_channel_internal_queue;
|
||||
unsigned int d_vector_length;
|
||||
bool d_dump;
|
||||
|
||||
Gnss_Synchro* d_acquisition_gnss_synchro;
|
||||
unsigned int d_channel;
|
||||
int d_last_seg;
|
||||
long d_if_freq;
|
||||
long d_fs_in;
|
||||
|
||||
double d_early_late_spc_chips;
|
||||
int d_n_correlator_taps;
|
||||
|
||||
gr_complex* d_ca_code;
|
||||
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;
|
||||
double d_rem_code_phase_chips;
|
||||
double d_rem_carrier_phase_rad;
|
||||
|
||||
// PLL and DLL filter library
|
||||
Tracking_2nd_DLL_filter d_code_loop_filter;
|
||||
Tracking_FLL_PLL_filter d_carrier_loop_filter;
|
||||
|
||||
// acquisition
|
||||
double d_acq_code_phase_samples;
|
||||
double d_acq_carrier_doppler_hz;
|
||||
|
||||
// tracking vars
|
||||
double d_code_freq_chips;
|
||||
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;
|
||||
|
||||
//Integration period in samples
|
||||
int d_correlation_length_samples;
|
||||
|
||||
//processing samples counters
|
||||
unsigned long int d_sample_counter;
|
||||
unsigned long int d_acq_sample_stamp;
|
||||
|
||||
// CN0 estimation and lock detector
|
||||
int d_cn0_estimation_counter;
|
||||
gr_complex* d_Prompt_buffer;
|
||||
double d_carrier_lock_test;
|
||||
double d_CN0_SNV_dB_Hz;
|
||||
double d_carrier_lock_threshold;
|
||||
int d_carrier_lock_fail_counter;
|
||||
|
||||
// control vars
|
||||
bool d_enable_tracking;
|
||||
bool d_pull_in;
|
||||
|
||||
// file dump
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
|
||||
std::map<std::string, std::string> systemName;
|
||||
std::string sys;
|
||||
};
|
||||
|
||||
#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_C_AID_TRACKING_CC_H
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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));
|
||||
// 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));
|
||||
|
||||
//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));
|
||||
// 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));
|
||||
|
||||
//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));
|
||||
//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));
|
||||
|
||||
// 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));
|
||||
//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));
|
||||
|
||||
// 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));
|
||||
// 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)
|
||||
{
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 );
|
||||
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 );
|
||||
|
||||
//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 );
|
||||
|
||||
//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);
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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));
|
||||
// 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(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
|
||||
// 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(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
|
||||
//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(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
|
||||
//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(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
|
||||
// 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_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));
|
||||
// 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)
|
||||
{
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
167
src/algorithms/tracking/libs/cpu_multicorrelator.cc
Normal file
167
src/algorithms/tracking/libs/cpu_multicorrelator.cc
Normal file
@ -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;
|
||||
}
|
||||
|
||||
98
src/algorithms/tracking/libs/cpu_multicorrelator.h
Normal file
98
src/algorithms/tracking/libs/cpu_multicorrelator.h
Normal file
@ -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_ */
|
||||
@ -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,163 +117,6 @@ __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
|
||||
)
|
||||
{
|
||||
//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)
|
||||
{
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
__global__ void scalarProdGPUCPXxN(
|
||||
GPU_Complex *d_corr_out,
|
||||
GPU_Complex *d_sig_in,
|
||||
GPU_Complex *d_local_codes_in,
|
||||
int vectorN,
|
||||
int elementN
|
||||
)
|
||||
{
|
||||
//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)
|
||||
{
|
||||
//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)
|
||||
for (int pos = iAccum; pos < elementN; 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]);
|
||||
sum.multiply_acc(d_sig_in[pos],d_local_codes_in[pos]);
|
||||
}
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//*********** 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
|
||||
*
|
||||
@ -292,21 +125,7 @@ __global__ void CUDA_32fc_x2_multiply_32fc( GPU_Complex *A, GPU_Complex *B,
|
||||
__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;
|
||||
@ -319,110 +138,101 @@ CUDA_32fc_Doppler_wipeoff( GPU_Complex *sig_out, GPU_Complex *sig_in, float rem
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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)
|
||||
__global__ void Doppler_wippe_scalarProdGPUCPXxN_shifts_chips(
|
||||
GPU_Complex *d_corr_out,
|
||||
GPU_Complex *d_sig_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,
|
||||
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 < numElements;
|
||||
i < elementN;
|
||||
i += blockDim.x * gridDim.x)
|
||||
{
|
||||
C[i] = A[i] + B[i];
|
||||
__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
|
||||
// from total number of thread blocks
|
||||
////////////////////////////////////////////////////////////////////////////
|
||||
for (int vec = blockIdx.x; vec < vectorN; vec += gridDim.x)
|
||||
{
|
||||
//int vectorBase = IMUL(elementN, vec);
|
||||
//int vectorEnd = 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);
|
||||
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],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;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
// 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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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((void **)&d_nco_in, size));
|
||||
//******** 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);
|
||||
|
||||
// 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)
|
||||
std::complex<float>* sig_in
|
||||
)
|
||||
{
|
||||
|
||||
// Save CPU pointers
|
||||
d_sig_in_cpu =sig_in;
|
||||
d_corr_out_cpu = corr_out;
|
||||
|
||||
// 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;
|
||||
|
||||
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);
|
||||
|
||||
|
||||
//printf("CUDA kernel launch with %d blocks of %d threads\n", blocksPerGrid, threadsPerBlock);
|
||||
|
||||
//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.
|
||||
|
||||
//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
|
||||
);
|
||||
signal_length_samples,
|
||||
rem_carrier_phase_in_rad,
|
||||
phase_step_rad
|
||||
);
|
||||
|
||||
cudaGetLastError();
|
||||
//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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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}
|
||||
|
||||
@ -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}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user