mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Merging branch 'next' of git://github.com/Arribas/gnss-sdr

This commit is contained in:
Carles Fernandez 2015-11-27 13:32:21 +01:00
parent 513e92da23 c8f7e08127
commit 2a6381c6fa
41 changed files with 2802 additions and 799 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
README.md
View File

@ -761,6 +761,30 @@ SignalSource.dump=false
SignalSource.dump_filename=../data/signal_source.dat
~~~~~~
Example for a dual-frequency receiver:
~~~~~~
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=UHD_Signal_Source
SignalSource.device_address=192.168.40.2 ; Put your USRP IP address here
SignalSource.item_type=gr_complex
SignalSource.RF_channels=2
SignalSource.sampling_frequency=4000000
SignalSource.subdevice=A:0 B:0
;######### RF Channels specific settings ######
SignalSource.freq0=1575420000
SignalSource.gain0=50
SignalSource.samples0=0
SignalSource.dump0=false
SignalSource.freq1=1227600000
SignalSource.gain1=50
SignalSource.samples1=0
SignalSource.dump1=false
~~~~~~
Other examples are available at [gnss-sdr/conf/](./conf/).
### Signal Conditioner

4
conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf
View File

@ -17,7 +17,7 @@ ControlThread.wait_for_flowgraph=false
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=/home/javier/signals/4msps.dat
SignalSource.filename=/media/javier/SISTEMA/signals/New York/4msps.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=gr_complex
@ -165,7 +165,7 @@ Resampler.sample_freq_out=4000000
;######### CHANNELS GLOBAL CONFIG ############
;#count: Number of available GPS satellite channels.
Channels_GPS.count=1
Channels_GPS.count=8
;#count: Number of available Galileo satellite channels.
Channels_Galileo.count=0
;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver

346
conf/gnss-sdr_Hybrid_byte_sim.conf Normal file
View 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=4000000
;######### 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=/home/javier/ClionProjects/gnss-sim/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=4000000
;#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=4000000
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=4000000
;#sample_freq_out: the desired sample frequency of the output signal
Resampler.sample_freq_out=4000000
;######### CHANNELS GLOBAL CONFIG ############
;#count: Number of available GPS satellite channels.
Channels_1C.count=12
;#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_Artemisa_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=15.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

5
src/algorithms/observables/gnuradio_blocks/CMakeLists.txt
View File

@ -31,6 +31,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/algorithms/libs
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs
${GNURADIO_RUNTIME_INCLUDE_DIRS}
${ARMADILLO_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
)
@ -38,5 +39,5 @@ include_directories(
file(GLOB OBS_GR_BLOCKS_HEADERS "*.h")
add_library(obs_gr_blocks ${OBS_GR_BLOCKS_SOURCES} ${OBS_GR_BLOCKS_HEADERS})
source_group(Headers FILES ${OBS_GR_BLOCKS_HEADERS})
add_dependencies(obs_gr_blocks glog-${glog_RELEASE})
target_link_libraries(obs_gr_blocks ${GNURADIO_RUNTIME_LIBRARIES})
add_dependencies(obs_gr_blocks glog-${glog_RELEASE} armadillo-${armadillo_RELEASE})
target_link_libraries(obs_gr_blocks ${GNURADIO_RUNTIME_LIBRARIES} ${ARMADILLO_LIBRARIES})

114
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@ -35,6 +35,7 @@
#include <map>
#include <sstream>
#include <vector>
#include <armadillo>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include "control_message_factory.h"
@ -63,6 +64,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 +136,35 @@ 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,19 +187,63 @@ 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
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 + GPS_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)/1000 + GPS_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 + GPS_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].debug_var1 = delta_rx_time_ms;
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 + 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];
}
}
}
if(d_dump == true)
@ -175,11 +256,16 @@ 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));
tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
//tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
//tmp_double = current_gnss_synchro[i].debug_var1;
tmp_double = current_gnss_synchro[i].debug_var2;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double));

9
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
View File

@ -33,10 +33,13 @@
#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 "concurrent_queue.h"
@ -68,6 +71,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;

20
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -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,7 +225,16 @@ 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;
LOG(INFO) <<" Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
if (corr_value < 0)
{
flag_PLL_180_deg_phase_locked = true; //PLL is locked to opposite phase!
LOG(INFO) << " PLL in opposite phase for Sat "<< this->d_satellite.get_PRN();
}
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 +323,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 +337,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

7
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View 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;

1
src/algorithms/tracking/adapters/CMakeLists.txt
View 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_artemisa_tracking.cc
gps_l1_ca_tcp_connector_tracking.cc
galileo_e5a_dll_pll_tracking.cc
gps_l2_m_dll_pll_tracking.cc

159
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.cc Normal file
View File

@ -0,0 +1,159 @@
/*!
* \file gps_l1_ca_dll_pll_artemisa_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_artemisa_tracking.h"
#include <glog/logging.h>
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
using google::LogMessage;
GpsL1CaDllPllArtemisaTracking::GpsL1CaDllPllArtemisaTracking(
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_artemisa_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() << ")";
}
GpsL1CaDllPllArtemisaTracking::~GpsL1CaDllPllArtemisaTracking()
{}
void GpsL1CaDllPllArtemisaTracking::start_tracking()
{
tracking_->start_tracking();
}
/*
* Set tracking channel unique ID
*/
void GpsL1CaDllPllArtemisaTracking::set_channel(unsigned int channel)
{
channel_ = channel;
tracking_->set_channel(channel);
}
/*
* Set tracking channel internal queue
*/
void GpsL1CaDllPllArtemisaTracking::set_channel_queue(
concurrent_queue<int> *channel_internal_queue)
{
channel_internal_queue_ = channel_internal_queue;
tracking_->set_channel_queue(channel_internal_queue_);
}
void GpsL1CaDllPllArtemisaTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
tracking_->set_gnss_synchro(p_gnss_synchro);
}
void GpsL1CaDllPllArtemisaTracking::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 GpsL1CaDllPllArtemisaTracking::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 GpsL1CaDllPllArtemisaTracking::get_left_block()
{
return tracking_;
}
gr::basic_block_sptr GpsL1CaDllPllArtemisaTracking::get_right_block()
{
return tracking_;
}

114
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.h Normal file
View File

@ -0,0 +1,114 @@
/*!
* \file gps_l1_ca_dll_pll_artemisa_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_ARTEMISA_TRACKING_H_
#define GNSS_SDR_GPS_L1_CA_DLL_PLL_ARTEMISA_TRACKING_H_
#include <string>
#include <gnuradio/msg_queue.h>
#include "tracking_interface.h"
#include "gps_l1_ca_dll_pll_artemisa_tracking_cc.h"
class ConfigurationInterface;
/*!
* \brief This class implements a code DLL + carrier PLL tracking loop
*/
class GpsL1CaDllPllArtemisaTracking : public TrackingInterface
{
public:
GpsL1CaDllPllArtemisaTracking(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
boost::shared_ptr<gr::msg_queue> queue);
virtual ~GpsL1CaDllPllArtemisaTracking();
std::string role()
{
return role_;
}
//! Returns "gps_l1_ca_dll_pll_artemisa_tracking"
std::string implementation()
{
return "gps_l1_ca_dll_pll_artemisa_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_artemisa_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_ARTEMISA_TRACKING_H_

58
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
View File

@ -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,11 @@ 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 = 0.0;
double carr_error_filt_hz = 0.0;
double code_error_chips = 0.0;
double code_error_filt_chips = 0.0;
if (d_enable_tracking == true)
{
@ -323,7 +324,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));
@ -372,7 +373,7 @@ 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 -= 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);
@ -383,7 +384,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 +396,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 +461,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 +548,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));

18
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h
View File

@ -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

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

@ -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);

111
src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
View File

@ -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)
{

34
src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h
View File

@ -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

61
src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc
View File

@ -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));

18
src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h
View File

@ -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

5
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
View File

@ -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;

588
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc Normal file
View File

@ -0,0 +1,588 @@
/*!
* \file gps_l1_ca_dll_pll_artemisa_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_artemisa_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_artemisa_tracking_cc_sptr
gps_l1_ca_dll_pll_artemisa_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_artemisa_tracking_cc_sptr(new gps_l1_ca_dll_pll_artemisa_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_artemisa_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_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_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_artemisa_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_artemisa_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_artemisa_tracking_cc::~gps_l1_ca_dll_pll_artemisa_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_artemisa_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_artemisa_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_artemisa_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}
void gps_l1_ca_dll_pll_artemisa_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_acquisition_gnss_synchro = p_gnss_synchro;
}

182
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h Normal file
View File

@ -0,0 +1,182 @@
/*!
* \file gps_l1_ca_dll_pll_artemisa_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_ARTEMISA_TRACKING_CC_H
#define GNSS_SDR_GPS_L1_CA_DLL_PLL_ARTEMISA_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_artemisa_tracking_cc;
typedef boost::shared_ptr<gps_l1_ca_dll_pll_artemisa_tracking_cc>
gps_l1_ca_dll_pll_artemisa_tracking_cc_sptr;
gps_l1_ca_dll_pll_artemisa_tracking_cc_sptr
gps_l1_ca_dll_pll_artemisa_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_artemisa_tracking_cc: public gr::block
{
public:
~gps_l1_ca_dll_pll_artemisa_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_artemisa_tracking_cc_sptr
gps_l1_ca_dll_pll_artemisa_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_artemisa_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_ARTEMISA_TRACKING_CC_H

63
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
View File

@ -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;

20
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h
View File

@ -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

104
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

@ -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)
{

20
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
View File

@ -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

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

@ -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,29 @@ 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_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)
@ -249,7 +238,7 @@ 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;
@ -280,17 +269,13 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc()
{
d_dump_file.close();
cudaFreeHost(in_gpu);
cudaFreeHost(d_carr_sign_gpu);
cudaFreeHost(d_corr_outs_gpu);
cudaFreeHost(d_local_code_shift_chips);
cudaFreeHost(d_ca_code);
multicorrelator_gpu->free_cuda();
delete(multicorrelator_gpu);
volk_free(d_ca_code);
delete[] d_Prompt_buffer;
}
@ -340,16 +325,9 @@ 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);
multicorrelator_gpu->Carrier_wipeoff_multicorrelator_resampler_cuda(d_rem_carr_phase_rad, phase_step_rad, code_phase_step_chips, rem_code_phase_chips, d_current_prn_length_samples, 3);
cudaProfilerStop();
// ################## PLL ##########################################################
@ -362,7 +340,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);

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

@ -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;

70
src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
View File

@ -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;
@ -596,27 +596,27 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
//tmp_float=(float)d_sample_counter;
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase
d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_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));
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
//PLL commands
d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
d_dump_file.write(reinterpret_cast<char*>(&carr_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));
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));
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 = 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));
}

20
src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h
View File

@ -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

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

@ -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

163
src/algorithms/tracking/libs/cpu_multicorrelator.cc Normal file
View File

@ -0,0 +1,163 @@
/*!
* \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 <cmath>
#include <iostream>
#include <gnuradio/fxpt.h> // fixed point sine and cosine
#include <volk/volk.h>
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
View 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 GNSS_SDR_CPU_MULTICORRELATOR_H_
#define GNSS_SDR_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_ */

587
src/algorithms/tracking/libs/cuda_multicorrelator.cu
View File

@ -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;
}

32
src/algorithms/tracking/libs/cuda_multicorrelator.h
View File

@ -32,8 +32,8 @@
* -------------------------------------------------------------------------
*/
#ifndef CUDA_MULTICORRELATOR_H_
#define CUDA_MULTICORRELATOR_H_
#ifndef GNSS_SDR_CUDA_MULTICORRELATOR_H_
#define GNSS_SDR_CUDA_MULTICORRELATOR_H_
#ifdef __CUDACC__
@ -107,6 +107,8 @@ struct GPU_Complex_Short
return GPU_Complex_Short(r+a.r, i+a.i);
}
};
/*!
* \brief Class that implements carrier wipe-off and correlators using NVIDIA CUDA GPU accelerators.
*/
@ -114,9 +116,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 +127,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 +147,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,10 +160,10 @@ private:
int blocksPerGrid;
cudaStream_t stream1;
cudaStream_t stream2;
//cudaStream_t stream2;
int num_gpu_devices;
int selected_device;
};
#endif /* CUDA_MULTICORRELATOR_H_ */
#endif /* GNSS_SDR_CUDA_MULTICORRELATOR_H_ */

18
src/algorithms/tracking/libs/tracking_discriminators.cc
View File

@ -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));

10
src/algorithms/tracking/libs/tracking_discriminators.h
View File

@ -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

4
src/core/system_parameters/GPS_L1_CA.h
View File

@ -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}

2
src/core/system_parameters/MATH_CONSTANTS.h
View File

@ -55,7 +55,7 @@ const double TWO_N2 = (0.25); //!< 2^-2
const double TWO_N5 = (0.03125); //!< 2^-5
const double TWO_N8 = (0.00390625); //!< 2^-8
const double TWO_N9 = (0.001953125); //!< 2^-9
const double TWO_N10 = (0.0009765625); //!< 2^-10
const double TWO_N11 = (4.882812500000000e-004); //!< 2^-11
const double TWO_N14 = (0.00006103515625); //!< 2^-14
const double TWO_N15 = (0.00003051757813); //!< 2^-15

5
src/core/system_parameters/gnss_synchro.h
View File

@ -32,6 +32,7 @@
#define GNSS_SDR_GNSS_SYNCHRO_H_
#include "gnss_signal.h"
#include <deque>
/*!
* \brief This is the class that contains the information that is shared
@ -73,6 +74,10 @@ public:
// Pseudorange
double Pseudorange_m;
bool Flag_valid_pseudorange;
//debug
double debug_var1;
double debug_var2;
};
#endif

373
src/core/system_parameters/rtcm.cc
View File

@ -39,6 +39,8 @@
#include <gflags/gflags.h>
#include <glog/logging.h>
#include "GPS_L1_CA.h"
#include "GPS_L2C.h"
#include "MATH_CONSTANTS.h"
@ -314,11 +316,7 @@ std::bitset<58> Rtcm::get_MT1001_sat_content(const Gnss_Synchro & gnss_synchro)
Rtcm::set_DF009(gnss_synchro_);
Rtcm::set_DF010(code_indicator); // code indicator 0: C/A code 1: P(Y) code direct
Rtcm::set_DF011(gnss_synchro_);
long int gps_L1_phaserange_minus_L1_pseudorange;
long int phaserange_m = (gnss_synchro.Carrier_phase_rads * GPS_C_m_s) / (GPS_TWO_PI * GPS_L1_FREQ_HZ);
gps_L1_phaserange_minus_L1_pseudorange = phaserange_m; // TODO
DF012 = std::bitset<20>(gps_L1_phaserange_minus_L1_pseudorange);
Rtcm::set_DF012(gnss_synchro_);
unsigned int lock_time_indicator = 0; // TODO
DF013 = std::bitset<7>(lock_time_indicator);
@ -949,6 +947,159 @@ int Rtcm::read_MT1045(const std::string & message, Galileo_Ephemeris & gal_eph)
}
// **********************************************
//
// MESSAGE TYPE MSM1 (COMPACT PSEUDORANGES)
//
// **********************************************
std::string Rtcm::print_MSM_1( const Gps_Ephemeris & gps_eph,
const Galileo_Ephemeris & gal_eph,
double obs_time,
const std::map<int, Gnss_Synchro> & pseudoranges,
unsigned int ref_id,
unsigned int clock_steering_indicator,
unsigned int external_clock_indicator,
int smooth_int,
bool sync_flag,
bool divergence_free,
bool more_messages)
{
unsigned int msg_number = 1071; /// check for Galileo, it's 1091
std::string header = Rtcm::get_MSM_header(msg_number, gps_eph,
gal_eph,
obs_time,
pseudoranges,
ref_id,
clock_steering_indicator,
external_clock_indicator,
smooth_int,
sync_flag,
divergence_free,
more_messages);
std::string sat_data = Rtcm::get_MSM_1_content_sat_data(pseudoranges);
std::string signal_data = Rtcm::get_MSM_1_content_signal_data(pseudoranges);
std::string message = build_message(header + sat_data + signal_data);
return message;
}
std::string Rtcm::get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & gps_eph,
const Galileo_Ephemeris & gal_eph,
double obs_time,
const std::map<int, Gnss_Synchro> & pseudoranges,
unsigned int ref_id,
unsigned int clock_steering_indicator,
unsigned int external_clock_indicator,
int smooth_int,
bool sync_flag,
bool divergence_free,
bool more_messages)
{
Rtcm::set_DF002(msg_number);
Rtcm::set_DF003(ref_id);
if(gps_eph.i_satellite_PRN != 0)
{
Rtcm::set_DF004(gps_eph, obs_time);
}
else
{
Rtcm::set_DF248(gal_eph, obs_time);
}
Rtcm::set_DF393(more_messages);
Rtcm::set_DF409(0); // Issue of Data Station. 0: not utilized
std::bitset<7> DF001_ = std::bitset<7>("0000000");
Rtcm::set_DF411(clock_steering_indicator);
Rtcm::set_DF412(external_clock_indicator);
Rtcm::set_DF417(divergence_free);
Rtcm::set_DF418(smooth_int);
Rtcm::set_DF394(pseudoranges);
Rtcm::set_DF395(pseudoranges);
std::string header = DF002.to_string() + DF003.to_string();
if(gps_eph.i_satellite_PRN != 0)
{
header += DF004.to_string();
}
else
{
header += DF248.to_string();
}
header = header + DF393.to_string() +
DF409.to_string() +
DF001_.to_string() +
DF411.to_string() +
DF412.to_string() +
DF418.to_string() +
DF394.to_string() +
DF395.to_string() +
Rtcm::set_DF396(pseudoranges);
return header;
}
std::string Rtcm::get_MSM_1_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges)
{
std::string sat_data;
sat_data.clear();
Rtcm::set_DF394(pseudoranges);
unsigned int num_satellites = DF394.count();
std::map<int, Gnss_Synchro> pseudoranges_ordered;
std::map<int, Gnss_Synchro>::const_iterator gnss_synchro_iter;
std::vector<unsigned int> pos;
std::vector<unsigned int>::iterator it;
for(gnss_synchro_iter = pseudoranges.begin();
gnss_synchro_iter != pseudoranges.end();
gnss_synchro_iter++)
{
pseudoranges_ordered.insert(std::pair<int, Gnss_Synchro>(65 - gnss_synchro_iter->second.PRN, gnss_synchro_iter->second));
it = std::find (pos.begin(), pos.end(), 65 - gnss_synchro_iter->second.PRN);
if (it == pos.end())
{
pos.push_back(65 - gnss_synchro_iter->second.PRN);
}
}
std::sort(pos.begin(), pos.end());
std::reverse(pos.begin(), pos.end());
for(unsigned int Nsat = 1; Nsat < num_satellites; Nsat++)
{
Rtcm::set_DF398(pseudoranges_ordered.at( pos.at(Nsat-1) ));
sat_data += DF398.to_string();
}
return sat_data;
}
std::string Rtcm::get_MSM_1_content_signal_data(const std::map<int, Gnss_Synchro> & pseudoranges)
{
std::string s("Not implemented");
return s;
}
std::string Rtcm::get_MSM_4_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges)
{
//std::map<int, Gnss_Synchro>::const_iterator gnss_synchro_iter;
std::string data("Not implemented");
return data;
}
//std::string Rtcm::get_MSM_1_content(const std::map<int, Gnss_Synchro> & pseudoranges)
//{
// std::string DF396 = set_DF396(pseudoranges);
//}
// *****************************************************************************************************
//
// DATA FIELDS AS DEFINED AT RTCM STANDARD 10403.2
@ -1005,6 +1156,8 @@ int Rtcm::reset_data_fields()
DF103.reset();
DF137.reset();
DF248.reset();
// Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045
DF252.reset();
DF289.reset();
@ -1040,6 +1193,10 @@ int Rtcm::reset_data_fields()
DF394.reset();
DF395.reset();
DF397.reset();
DF398.reset();
DF399.reset();
DF409.reset();
DF411.reset();
@ -1502,6 +1659,19 @@ int Rtcm::set_DF137(const Gps_Ephemeris & gps_eph)
}
int Rtcm::set_DF248(const Galileo_Ephemeris & gal_eph, double obs_time)
{
// TOW in milliseconds from the beginning of the Galileo week, measured in Galileo time
unsigned long int tow = static_cast<unsigned long int>(std::round((obs_time + 604800 * static_cast<double>(gal_eph.WN_5)) * 1000));
if(tow > 604799999)
{
LOG(WARNING) << "To large TOW! Set to the last millisecond of the week";
tow = 604799999;
}
DF248 = std::bitset<30>(tow);
return 0;
}
int Rtcm::set_DF252(const Galileo_Ephemeris & gal_eph)
{
@ -1819,6 +1989,145 @@ int Rtcm::set_DF395(const std::map<int, Gnss_Synchro> & gnss_synchro)
return 0;
}
std::string Rtcm::set_DF396(const std::map<int, Gnss_Synchro> & pseudoranges)
{
std::string DF396;
std::map<int, Gnss_Synchro>::const_iterator pseudoranges_iter;
Rtcm::set_DF394(pseudoranges);
Rtcm::set_DF395(pseudoranges);
unsigned int num_signals = DF395.count();
unsigned int num_satellites = DF394.count();
std::vector<std::vector<bool> > matrix( num_signals, std::vector<bool>(num_satellites) );
unsigned int element = 0;
std::string sig;
// fill matrix
for(unsigned int row = 0; row < num_signals - 1; row++)
{
for (unsigned int signal_id = 1; signal_id < 32; signal_id++)
{
bool we_have_signal_for_this_sat = false;
if(static_cast<bool>(DF395.test(signal_id)))
{
for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++)
{
std::string sig_(pseudoranges_iter->second.Signal);
sig = sig_.substr(0,2);
std::string sys(pseudoranges_iter->second.System, 1);
bool this_sat = static_cast<bool>(DF394.test(65 - pseudoranges_iter->second.PRN));
if (this_sat)
{
if( (signal_id == 2) && (sig.compare("1C") == 0) && (sys.compare("G") == 0 ) )
{
we_have_signal_for_this_sat = true;
}
if( (signal_id == 4) && (sig.compare("1B") == 0) && (sys.compare("E") == 0 ) )
{
we_have_signal_for_this_sat = true;
}
if( (signal_id == 15) && (sig.compare("2S") == 0) && (sys.compare("G") == 0 ) )
{
we_have_signal_for_this_sat = true;
}
if( (signal_id == 24) && (sig.compare("5X") == 0) && (sys.compare("E") == 0 ) )
{
we_have_signal_for_this_sat = true;
}
matrix[row].push_back(we_have_signal_for_this_sat);
}
}
}
}
}
// write matrix
DF396.clear();
std::stringstream ss;
for(unsigned int row = 0; row < num_signals - 1; row++)
{
for(unsigned int col = 0; col < num_satellites - 1; col++)
{
ss << std::boolalpha << matrix[row].at(col);
DF396 += ss.str();
}
}
return DF396;
}
int Rtcm::set_DF397(const Gnss_Synchro & gnss_synchro)
{
double meters_to_miliseconds = GPS_C_m_s * 0.001;
double rough_range_ms = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
unsigned int int_ms = 0;
if (rough_range_ms == 0.0)
{
int_ms = 255;
}
else if((rough_range_ms < 0.0) || (rough_range_ms > meters_to_miliseconds * 255.0))
{
int_ms = 255;
}
else
{
int_ms = static_cast<unsigned int>(std::floor(rough_range_ms / meters_to_miliseconds / TWO_N10) + 0.5) >> 10;
}
DF397 = std::bitset<8>(int_ms);
return 0;
}
int Rtcm::set_DF398(const Gnss_Synchro & gnss_synchro)
{
double meters_to_miliseconds = GPS_C_m_s * 0.001;
double rough_range_ms = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
DF398 = std::bitset<10>(static_cast<unsigned int>(rough_range_ms));
return 0;
}
int Rtcm::set_DF399(const Gnss_Synchro & gnss_synchro)
{
double lambda;
std::string sig_(gnss_synchro.Signal);
std::string sig = sig_.substr(0,2);
if (sig.compare("1C") == 0 )
{
lambda = GPS_C_m_s / GPS_L1_FREQ_HZ;
}
if (sig.compare("2S") == 0 )
{
lambda = GPS_C_m_s / GPS_L2_FREQ_HZ;
}
if (sig.compare("5X") == 0 )
{
lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ;
}
if (sig.compare("1B") == 0 )
{
lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ;
}
double rough_phase_range_ms = std::round(- gnss_synchro.Carrier_Doppler_hz / lambda);
DF399 = std::bitset<14>(static_cast<int>(rough_phase_range_ms));
return 0;
}
int Rtcm::set_DF409(unsigned int iods)
{
@ -1826,9 +2135,63 @@ int Rtcm::set_DF409(unsigned int iods)
return 0;
}
int Rtcm::set_DF411(unsigned int clock_steering_indicator)
{
DF411 = std::bitset<2>(clock_steering_indicator);
return 0;
}
int Rtcm::set_DF412(unsigned int external_clock_indicator)
{
DF412 = std::bitset<2>(external_clock_indicator);
return 0;
}
int Rtcm::set_DF417(bool using_divergence_free_smoothing)
{
DF417 = std::bitset<1>(using_divergence_free_smoothing);
return 0;
}
int Rtcm::set_DF418(int carrier_smoothing_interval_s)
{
unsigned int smoothing_int = abs(carrier_smoothing_interval_s);
if(carrier_smoothing_interval_s < 0)
{
DF418 = std::bitset<3>("111");
}
else
{
if(carrier_smoothing_interval_s == 0)
{
DF418 = std::bitset<3>("000");
}
else if(carrier_smoothing_interval_s < 30)
{
DF418 = std::bitset<3>("001");
}
else if(carrier_smoothing_interval_s < 60)
{
DF418 = std::bitset<3>("010");
}
else if(carrier_smoothing_interval_s < 120)
{
DF418 = std::bitset<3>("011");
}
else if(carrier_smoothing_interval_s < 240)
{
DF418 = std::bitset<3>("100");
}
else if(carrier_smoothing_interval_s < 480)
{
DF418 = std::bitset<3>("101");
}
else
{
DF418 = std::bitset<3>("110");
}
}
return 0;
}

61
src/core/system_parameters/rtcm.h
View File

@ -42,6 +42,7 @@
#include "galileo_fnav_message.h"
#include "gps_navigation_message.h"
/*!
* \brief This class implements the generation and reading of some Message Types
* defined in the RTCM 3.2 Standard.
@ -84,6 +85,18 @@ public:
*/
int read_MT1045(const std::string & message, Galileo_Ephemeris & gal_eph);
std::string print_MSM_1( const Gps_Ephemeris & gps_eph,
const Galileo_Ephemeris & gal_eph,
double obs_time,
const std::map<int, Gnss_Synchro> & pseudoranges,
unsigned int ref_id,
unsigned int clock_steering_indicator,
unsigned int external_clock_indicator,
int smooth_int,
bool sync_flag,
bool divergence_free,
bool more_messages);
std::string bin_to_hex(const std::string& s); //<! Returns a string of hexadecimal symbols from a string of binary symbols
std::string hex_to_bin(const std::string& s); //<! Returns a string of binary symbols from a string of hexadecimal symbols
@ -124,6 +137,23 @@ private:
std::bitset<152> get_MT1005_test();
std::string get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & gps_eph,
const Galileo_Ephemeris & gal_eph,
double obs_time,
const std::map<int, Gnss_Synchro> & pseudoranges,
unsigned int ref_id,
unsigned int clock_steering_indicator,
unsigned int external_clock_indicator,
int smooth_int,
bool sync_flag,
bool divergence_free,
bool more_messages);
std::string get_MSM_1_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges);
std::string get_MSM_1_content_signal_data(const std::map<int, Gnss_Synchro> & pseudoranges);
std::string get_MSM_4_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges);
//
// Transport Layer
//
@ -302,6 +332,9 @@ private:
std::bitset<1> DF142;
int set_DF142(const Gps_Ephemeris & gps_eph);
std::bitset<30> DF248;
int set_DF248(const Galileo_Ephemeris & gal_eph, double obs_time);
// Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045
std::bitset<6> DF252;
int set_DF252(const Galileo_Ephemeris & gal_eph);
@ -394,28 +427,36 @@ private:
int set_DF393(bool more_messages); //1 indicates that more MSMs follow for given physical time and reference station ID
std::bitset<64> DF394;
int set_DF394(const std::map<int, Gnss_Synchro> & gnss_synchro);
int set_DF394(const std::map<int, Gnss_Synchro> & pseudoranges);
std::bitset<32> DF395;
int set_DF395(const std::map<int, Gnss_Synchro> & gnss_synchro);
int set_DF395(const std::map<int, Gnss_Synchro> & pseudoranges);
std::string set_DF396(const std::map<int, Gnss_Synchro> & pseudoranges);
std::bitset<8> DF397;
int set_DF397(const Gnss_Synchro & gnss_synchro);
std::bitset<10> DF398;
int set_DF398(const Gnss_Synchro & gnss_synchro);
std::bitset<14> DF399;
int set_DF399(const Gnss_Synchro & gnss_synchro);
//std::bitset<1> DF396; //variable
std::bitset<3> DF409;
int set_DF409(unsigned int iods);
std::bitset<2> DF411;
int set_DF411(unsigned int clock_steering_indicator);
std::bitset<2> DF412;
int set_DF412(unsigned int external_clock_indicator);
std::bitset<1> DF417;
int set_DF417(bool using_divergence_free_smoothing);
std::bitset<3> DF418;
// Content of Satellite data for MSM4 and MSM6
std::vector<std::bitset<8> > DF397; // 8*NSAT
std::vector<std::bitset<10> > DF398; // 10*NSAT
// Content of Satellite data for MSM5 and MSM7
std::vector<std::bitset<14> > DF399; // 14*NSAT
int set_DF418(int carrier_smoothing_interval_s);
};
#endif