diff --git a/README.md b/README.md index 1dc8c864c..00afe01a5 100644 --- a/README.md +++ b/README.md @@ -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 diff --git a/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf b/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf index 6bfc9bb6c..7d3c3719a 100644 --- a/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf +++ b/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf @@ -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 diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf new file mode 100644 index 000000000..2f8cb0654 --- /dev/null +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -0,0 +1,346 @@ +; Default configuration file +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. +GNSS-SDR.internal_fs_hz=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 diff --git a/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt b/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt index 27c266fb9..6f4b25ffb 100644 --- a/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt @@ -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}) diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 8df2646f5..4e867738d 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #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(d_nchannels)); + d_carrier_doppler_queue_hz.push_back(std::deque(d_nchannels)); + d_symbol_TOW_queue_s.push_back(std::deque(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(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(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(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(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]="< 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 queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging); + + //Tracking observable history + std::vector> d_acc_carrier_phase_queue_rads; + std::vector> d_carrier_doppler_queue_hz; + std::vector> d_symbol_TOW_queue_s; + // class private vars boost::shared_ptr d_queue; bool d_dump; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 260f6ded8..0df1c0a0f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -135,6 +135,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_decimation_output_factor = 1; d_channel = 0; Prn_timestamp_at_preamble_ms = 0.0; + flag_PLL_180_deg_phase_locked = false; //set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble } @@ -224,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 diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index f9c595711..10f305ca3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -35,6 +35,7 @@ #include #include #include +#include #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 d_symbol_TOW_queue_s; + // Doppler and Phase accumulator queue for interpolation in Observables + std::deque d_carrier_doppler_queue_hz; + std::deque d_acc_carrier_phase_queue_rads; + double Prn_timestamp_at_preamble_ms; bool flag_TOW_set; + bool flag_PLL_180_deg_phase_locked; std::string d_dump_filename; std::ofstream d_dump_file; diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index 55029b019..7a611f46d 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -28,6 +28,7 @@ set(TRACKING_ADAPTER_SOURCES gps_l1_ca_dll_fll_pll_tracking.cc gps_l1_ca_dll_pll_optim_tracking.cc gps_l1_ca_dll_pll_tracking.cc + gps_l1_ca_dll_pll_artemisa_tracking.cc gps_l1_ca_tcp_connector_tracking.cc galileo_e5a_dll_pll_tracking.cc gps_l2_m_dll_pll_tracking.cc diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.cc new file mode 100644 index 000000000..69d39630f --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "gps_l1_ca_dll_pll_artemisa_tracking.h" +#include +#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 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 *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_; +} + diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.h new file mode 100644 index 000000000..06b91e84a --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_artemisa_tracking.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_ARTEMISA_TRACKING_H_ +#define GNSS_SDR_GPS_L1_CA_DLL_PLL_ARTEMISA_TRACKING_H_ + +#include +#include +#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 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 *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 queue_; + concurrent_queue *channel_internal_queue_; +}; + +#endif // GNSS_SDR_GPS_L1_CA_DLL_PLL_ARTEMISA_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 0ea92589c..934e00abf 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -236,7 +236,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() void galileo_e1_dll_pll_veml_tracking_cc::update_local_code() { double tcode_half_chips; - float rem_code_phase_half_chips; + double rem_code_phase_half_chips; int associated_chip_index; int code_length_half_chips = static_cast(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(d_code_freq_chips)) / (static_cast(d_fs_in)); - code_phase_step_half_chips = (2.0 * static_cast(d_code_freq_chips)) / (static_cast(d_fs_in)); + code_phase_step_chips = d_code_freq_chips / (static_cast(d_fs_in)); + code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(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(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(acq_to_trk_delay_samples), static_cast(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(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(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(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(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(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(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(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + tmp_float = carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + tmp_float = code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h index 10b073802..c42eb048b 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h @@ -126,8 +126,8 @@ private: long d_if_freq; long d_fs_in; - float d_early_late_spc_chips; - float d_very_early_late_spc_chips; + double d_early_late_spc_chips; + double d_very_early_late_spc_chips; gr_complex* d_ca_code; @@ -146,22 +146,22 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; + double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_acc_code_phase_secs; @@ -175,9 +175,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index 74a399d18..ec5119e48 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -387,7 +387,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve // New code Doppler frequency estimation d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; + d_acc_carrier_phase_rad -= GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; //remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc index 0a988a1af..90d7f1bd3 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc @@ -217,18 +217,18 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking() d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; long int acq_trk_diff_samples; - float acq_trk_diff_seconds; + double acq_trk_diff_seconds; acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(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(acq_trk_diff_samples) / static_cast(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(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(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(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(d_code_freq_chips) / static_cast(d_fs_in); + code_phase_step_chips = d_code_freq_chips / static_cast(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(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); + float phase_step_rad = static_cast(2.0 * GALILEO_PI * d_carrier_doppler_hz / static_cast(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(acq_to_trk_delay_samples), static_cast(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(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(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(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(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + d_code_error_filt_secs * static_cast(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(d_sample_counter) + static_cast(d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(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(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(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(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } catch (std::ifstream::failure e) { diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h index 5b40a7c73..7257f80d8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h @@ -137,10 +137,10 @@ private: long d_fs_in; double d_early_late_spc_chips; - float d_dll_bw_hz; - float d_pll_bw_hz; - float d_dll_bw_init_hz; - float d_pll_bw_init_hz; + double d_dll_bw_hz; + double d_pll_bw_hz; + double d_dll_bw_init_hz; + double d_pll_bw_init_hz; gr_complex* d_codeQ; gr_complex* d_codeI; @@ -160,26 +160,26 @@ private: float tmp_P; float tmp_L; // remaining code phase and carrier phase between tracking loops - float d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_code_phase_samples; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars - float d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; - float d_code_error_filt_secs; + double d_code_freq_chips; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; + double d_code_error_filt_secs; //PRN period in samples int d_current_prn_length_samples; @@ -191,9 +191,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc index d9ae8bd9c..c3c659686 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc @@ -253,7 +253,7 @@ void galileo_volk_e1_dll_pll_veml_tracking_cc::start_tracking() void galileo_volk_e1_dll_pll_veml_tracking_cc::update_local_code() { double tcode_half_chips; - float rem_code_phase_half_chips; + double rem_code_phase_half_chips; int code_length_half_chips = static_cast(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(d_code_freq_chips)) / (static_cast(d_fs_in)); - code_phase_step_half_chips = (2.0 * static_cast(d_code_freq_chips)) / (static_cast(d_fs_in)); + code_phase_step_chips = (d_code_freq_chips) / (static_cast(d_fs_in)); + code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(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(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(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); + phase_step_rad = static_cast (GPS_TWO_PI * d_carrier_doppler_hz / static_cast(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 (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(acq_to_trk_delay_samples), static_cast(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(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(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(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(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(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(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(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + tmp_float = carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + tmp_float = code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h index 7f118abc5..ead8d0502 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h @@ -126,8 +126,8 @@ private: long d_if_freq; long d_fs_in; - float d_early_late_spc_chips; - float d_very_early_late_spc_chips; + double d_early_late_spc_chips; + double d_very_early_late_spc_chips; gr_complex* d_ca_code; @@ -162,22 +162,22 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; + double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_acc_code_phase_secs; @@ -191,9 +191,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc index fec174e40..6db945272 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc @@ -315,7 +315,7 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier() phase += phase_step; } d_rem_carr_phase = fmod(phase, GPS_TWO_PI); - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + phase; + d_acc_carrier_phase_rad -= d_acc_carrier_phase_rad + phase; } @@ -439,6 +439,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto if (d_FLL_wait == 1) { d_Prompt_prev = *d_Prompt; + d_FLL_discriminator_hz=0.0; d_FLL_wait = 0; } else @@ -532,7 +533,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto T_prn_samples = T_prn_seconds * d_fs_in; float code_error_filt_samples; - code_error_filt_samples = T_prn_seconds * code_error_filt_chips * T_chip_seconds * static_cast(d_fs_in); //[seconds] + code_error_filt_samples = GPS_L1_CA_CODE_PERIOD * code_error_filt_chips * GPS_L1_CA_CHIP_PERIOD * static_cast(d_fs_in); //[seconds] d_acc_code_phase_samples = d_acc_code_phase_samples + code_error_filt_samples; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc new file mode 100644 index 000000000..17a4e9ac4 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "gps_l1_ca_dll_pll_artemisa_tracking_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#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 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(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 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(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(volk_malloc(static_cast(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(volk_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_get_alignment())); + for (int n=0;n(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(d_sample_counter) - static_cast(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(acq_trk_diff_samples) / static_cast(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(d_code_freq_chips) / static_cast(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(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(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(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(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(GPS_L1_CA_CODE_LENGTH_CHIPS),d_ca_code,d_local_code_shift_chips); + for (int n=0;nSystem; + 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(acq_to_trk_delay_samples), static_cast(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(d_correlation_length_samples)/static_cast(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(d_fs_in); + K_blk_samples = T_prn_samples + d_rem_code_phase_samples - dll_code_error_secs_Ti * static_cast(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(d_correlation_length_samples); //rounding error < 1 sample + + + // UPDATE REMNANT CARRIER PHASE + CORRECTED_INTEGRATION_TIME_S=(static_cast(d_correlation_length_samples)/static_cast(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(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(d_fs_in); + //remnant code phase [chips] + d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast(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 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((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((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(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast(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_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_cycles), sizeof(double)); + + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_phase_error_secs_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips_Ti), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); + d_dump_file.write(reinterpret_cast(&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(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 *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; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h new file mode 100644 index 000000000..6961dd2a5 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 +#include +#include +#include +#include +#include +#include +#include +#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_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 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 *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 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 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 d_queue; + concurrent_queue *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 systemName; + std::string sys; +}; + +#endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_ARTEMISA_TRACKING_CC_H diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc index 86886dcee..bd5dcce0b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc @@ -183,29 +183,29 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking() d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; long int acq_trk_diff_samples; - float acq_trk_diff_seconds; + double acq_trk_diff_seconds; acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(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(acq_trk_diff_samples) / static_cast(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(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(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(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(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(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(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(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); @@ -563,23 +563,32 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec //tmp_float=(float)d_sample_counter; d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float)); + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write((char*)&tmp_float, sizeof(float)); // carrier and code frequency - d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float)); - d_dump_file.write((char*)&d_code_freq_chips, sizeof(float)); + tmp_float = d_carrier_doppler_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); //PLL commands - d_dump_file.write((char*)&carr_error_hz, sizeof(float)); - d_dump_file.write((char*)&carr_error_filt_hz, sizeof(float)); + tmp_float = carr_error_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = carr_error_filt_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); //DLL commands - d_dump_file.write((char*)&code_error_chips, sizeof(float)); - d_dump_file.write((char*)&code_error_filt_chips, sizeof(float)); + tmp_float = code_error_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); // CN0 and carrier lock test - d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float)); - d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float)); + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write((char*)&tmp_float, sizeof(float)); // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h index 0aee4ce8b..52ce88efc 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h @@ -135,24 +135,24 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; //PRN period in samples int d_current_prn_length_samples; @@ -164,9 +164,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index 3193d335e..07d64b945 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -190,17 +190,17 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; long int acq_trk_diff_samples; - float acq_trk_diff_seconds; + double acq_trk_diff_seconds; acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(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(acq_trk_diff_samples) / static_cast(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(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(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(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(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); + float phase_step_rad = static_cast(GPS_TWO_PI) * static_cast(d_carrier_doppler_hz) / static_cast(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(acq_to_trk_delay_samples), static_cast(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(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(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(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(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(*d_Late); try { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - //tmp_float=(float)d_sample_counter; - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - tmp_float=d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } catch (std::ifstream::failure e) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h index 216e63955..5cf55fa56 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h @@ -139,24 +139,24 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; //PRN period in samples int d_current_prn_length_samples; @@ -168,9 +168,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index fea305fa3..592d35161 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -47,7 +47,8 @@ #include "lock_detectors.h" #include "GPS_L1_CA.h" #include "control_message_factory.h" -#include // volk_alignment +#include //volk_alignement +// includes #include @@ -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(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment())); - d_ca_code = static_cast(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(d_code_freq_chips) / static_cast(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); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h index a3108f8c7..e632c48a4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.h @@ -128,13 +128,9 @@ private: //GPU HOST PINNED MEMORY IN/OUT VECTORS gr_complex* in_gpu; - gr_complex* d_carr_sign_gpu; - gr_complex* d_local_codes_gpu; float* d_local_code_shift_chips; gr_complex* d_corr_outs_gpu; cuda_multicorrelator *multicorrelator_gpu; - - gr_complex* d_ca_code; gr_complex *d_Early; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc index 018fd6486..e34204bde 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc @@ -199,11 +199,11 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(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(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(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(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(d_code_freq_chips) / static_cast(d_fs_in); + code_phase_step_chips = d_code_freq_chips / static_cast(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(GPS_L2_TWO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); + phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast(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(acq_to_trk_delay_samples), static_cast(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(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(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(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(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(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(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(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(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(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(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(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h index 615df8cfd..4fc35f44b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h @@ -137,24 +137,24 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; //PRN period in samples int d_current_prn_length_samples; @@ -166,9 +166,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index a6a51cd01..665ebeacf 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -32,6 +32,7 @@ endif(ENABLE_CUDA) set(TRACKING_LIB_SOURCES correlator.cc + cpu_multicorrelator.cc lock_detectors.cc tcp_communication.cc tcp_packet_data.cc diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.cc b/src/algorithms/tracking/libs/cpu_multicorrelator.cc new file mode 100644 index 000000000..befac3170 --- /dev/null +++ b/src/algorithms/tracking/libs/cpu_multicorrelator.cc @@ -0,0 +1,163 @@ +/*! + * \file cpu_multicorrelator.cc + * \brief High optimized CPU vector multiTAP correlator class + * \authors
    + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + *
+ * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#include "cpu_multicorrelator.h" +#include +#include +#include // fixed point sine and cosine +#include + + +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); + + // NCO signal + d_nco_in = static_cast*>(volk_malloc(size, volk_get_alignment())); + + // Doppler-free signal + d_sig_doppler_wiped = static_cast*>(volk_malloc(size, volk_get_alignment())); + + d_local_codes_resampled = new std::complex*[n_correlators]; + for (int n = 0; n < n_correlators; n++) + { + d_local_codes_resampled[n] = static_cast*>(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* 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* corr_out, const std::complex* 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(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(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(cos_f, -sin_f); + phase_rad_i += phase_step_rad_i; + } +} + +bool cpu_multicorrelator::Carrier_wipeoff_multicorrelator_resampler( + float rem_carrier_phase_in_rad, + float phase_step_rad, + float rem_code_phase_chips, + float code_phase_step_chips, + int signal_length_samples) +{ + + update_local_carrier(signal_length_samples, rem_carrier_phase_in_rad, phase_step_rad); + update_local_code(signal_length_samples,rem_code_phase_chips, code_phase_step_chips); + + volk_32fc_x2_multiply_32fc(d_sig_doppler_wiped, d_sig_in, d_nco_in, signal_length_samples); + for (int current_correlator_tap = 0; current_correlator_tap < d_n_correlators; current_correlator_tap++) + { + volk_32fc_x2_dot_prod_32fc(&d_corr_out[current_correlator_tap], d_sig_doppler_wiped, d_local_codes_resampled[current_correlator_tap], signal_length_samples); + } + return true; +} + + +cpu_multicorrelator::cpu_multicorrelator() +{ + d_sig_in = NULL; + d_nco_in = NULL; + d_sig_doppler_wiped = NULL; + d_local_code_in = NULL; + d_shifts_chips = NULL; + d_corr_out = NULL; + d_code_length_chips = 0; + d_n_correlators = 0; +} + +bool cpu_multicorrelator::free() +{ + // Free memory + if (d_sig_doppler_wiped != NULL) volk_free(d_sig_doppler_wiped); + if (d_nco_in != NULL) volk_free(d_nco_in); + for (int n = 0; n < d_n_correlators; n++) + { + volk_free(d_local_codes_resampled[n]); + } + delete d_local_codes_resampled; + return true; +} + diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.h b/src/algorithms/tracking/libs/cpu_multicorrelator.h new file mode 100644 index 000000000..1d7b7c6e9 --- /dev/null +++ b/src/algorithms/tracking/libs/cpu_multicorrelator.h @@ -0,0 +1,98 @@ +/*! + * \file cpu_multicorrelator.h + * \brief High optimized CPU vector multiTAP correlator class + * \authors
    + *
  • Javier Arribas, 2015. jarribas(at)cttc.es + *
+ * + * 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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_CPU_MULTICORRELATOR_H_ +#define GNSS_SDR_CPU_MULTICORRELATOR_H_ + +#include + +/*! + * \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* local_code_in, + float *shifts_chips + ); + bool set_input_output_vectors( + std::complex* corr_out, + const std::complex* 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 *d_sig_in; + std::complex *d_nco_in; + std::complex **d_local_codes_resampled; + std::complex *d_sig_doppler_wiped; + const std::complex *d_local_code_in; + std::complex *d_corr_out; + + float *d_shifts_chips; + int d_code_length_chips; + int d_n_correlators; + + bool update_local_code(); + bool update_local_carrier(); + +}; + + +#endif /* CPU_MULTICORRELATOR_H_ */ diff --git a/src/algorithms/tracking/libs/cuda_multicorrelator.cu b/src/algorithms/tracking/libs/cuda_multicorrelator.cu index 43ffeed00..5017d1493 100644 --- a/src/algorithms/tracking/libs/cuda_multicorrelator.cu +++ b/src/algorithms/tracking/libs/cuda_multicorrelator.cu @@ -32,26 +32,14 @@ * ------------------------------------------------------------------------- */ -/////////////////////////////////////////////////////////////////////////////// -// On G80-class hardware 24-bit multiplication takes 4 clocks per warp -// (the same as for floating point multiplication and addition), -// whereas full 32-bit multiplication takes 16 clocks per warp. -// So if integer multiplication operands are guaranteed to fit into 24 bits -// (always lie withtin [-8M, 8M - 1] range in signed case), -// explicit 24-bit multiplication is preferred for performance. -/////////////////////////////////////////////////////////////////////////////// -#define IMUL(a, b) __mul24(a, b) - #include "cuda_multicorrelator.h" #include - +#include // For the CUDA runtime routines (prefixed with "cuda_") #include - -#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(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast(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(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)*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)*code_length_chips); cudaMemset(d_local_codes_in,0,sizeof(std::complex)*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)*n_correlators); - cudaMemset(d_corr_out,0,sizeof(std::complex)*n_correlators); + //cudaMalloc((void **)&d_corr_out, sizeof(std::complex)*n_correlators); + //cudaMemset(d_corr_out,0,sizeof(std::complex)*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* corr_out, - const std::complex* sig_in, - const std::complex* 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* 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); - - // 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)*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<<>>(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<<>>( -// d_corr_out, -// d_sig_doppler_wiped, -// d_local_codes_in, -// 3, -// signal_length_samples -// ); - - //new - //launch the multitap correlator - scalarProdGPUCPXxN_shifts<<>>( - 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)*n_correlators, - cudaMemcpyDeviceToHost); - return true; } - bool cuda_multicorrelator::Carrier_wipeoff_multicorrelator_resampler_cuda( - std::complex* corr_out, - const std::complex* 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); + + // cudaMemCpy version + //size_t memSize = signal_length_samples * sizeof(std::complex); // 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<<>>(d_sig_doppler_wiped, d_sig_in,rem_carrier_phase_in_rad,phase_step_rad, signal_length_samples); + //CUDA_32fc_Doppler_wipeoff<<>>(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<<>>( +// scalarProdGPUCPXxN_shifts_chips<<>>( +// 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<<>>( 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* debug_signal; +// debug_signal=static_cast*>(malloc(memSize)); +// cudaMemcpyAsync(debug_signal, d_sig_doppler_wiped, memSize, +// cudaMemcpyDeviceToHost,stream1); +// cudaStreamSynchronize(stream1); +// std::cout<<"d_sig_doppler_wiped GPU="<)*n_correlators, - cudaMemcpyDeviceToHost,stream1); + //cudaMemcpyAsync(corr_out, d_corr_out, sizeof(std::complex)*n_correlators, + // cudaMemcpyDeviceToHost,stream1); + cudaStreamSynchronize(stream1); return true; } - cuda_multicorrelator::cuda_multicorrelator() { d_sig_in=NULL; @@ -689,22 +488,16 @@ bool cuda_multicorrelator::free_cuda() if (d_sig_doppler_wiped!=NULL) cudaFree(d_sig_doppler_wiped); if (d_local_codes_in!=NULL) cudaFree(d_local_codes_in); if (d_corr_out!=NULL) cudaFree(d_corr_out); - - if (d_shifts_samples!=NULL) cudaFree(d_shifts_samples); if (d_shifts_chips!=NULL) cudaFree(d_shifts_chips); - - cudaStreamDestroy(stream1) ; - cudaStreamDestroy(stream2) ; - // Reset the device and exit // cudaDeviceReset causes the driver to clean up all state. While // not mandatory in normal operation, it is good practice. It is also // needed to ensure correct operation when the application is being // profiled. Calling cudaDeviceReset causes all profile data to be // flushed before the application exits - // (cudaDeviceReset()); + cudaDeviceReset(); return true; } diff --git a/src/algorithms/tracking/libs/cuda_multicorrelator.h b/src/algorithms/tracking/libs/cuda_multicorrelator.h index df640f534..e55b17dd5 100644 --- a/src/algorithms/tracking/libs/cuda_multicorrelator.h +++ b/src/algorithms/tracking/libs/cuda_multicorrelator.h @@ -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* corr_out, + std::complex* sig_in + ); bool free_cuda(); - bool Carrier_wipeoff_multicorrelator_cuda( - std::complex* corr_out, - const std::complex* sig_in, - const std::complex* 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* corr_out, - const std::complex* 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 *d_sig_in_cpu; + std::complex *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_ */ diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc index 119f40a2b..1aaa10004 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.cc +++ b/src/algorithms/tracking/libs/tracking_discriminators.cc @@ -46,9 +46,9 @@ * \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second]. */ -float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t1, float t2) +double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2) { - float cross, dot; + double cross, dot; dot = prompt_s1.real()*prompt_s2.real() + prompt_s1.imag()*prompt_s2.imag(); cross = prompt_s1.real()*prompt_s2.imag() - prompt_s2.real()*prompt_s1.imag(); return atan2(cross, dot) / (t2-t1); @@ -62,7 +62,7 @@ float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_four_quadrant_atan(gr_complex prompt_s1) +double pll_four_quadrant_atan(gr_complex prompt_s1) { return atan2(prompt_s1.imag(), prompt_s1.real()); } @@ -75,7 +75,7 @@ float pll_four_quadrant_atan(gr_complex prompt_s1) * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_cloop_two_quadrant_atan(gr_complex prompt_s1) +double pll_cloop_two_quadrant_atan(gr_complex prompt_s1) { if (prompt_s1.real() != 0.0) { @@ -96,12 +96,12 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1) * where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and * \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips]. */ -float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) +double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) { - float P_early, P_late; + double P_early, P_late; P_early = std::abs(early_s1); P_late = std::abs(late_s1); - return (P_early - P_late) / ((P_early + P_late)); + return 0.5*(P_early - P_late) / ((P_early + P_late)); } /* @@ -113,9 +113,9 @@ float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) * where \f$E=\sqrt{I_{VE}^2+Q_{VE}^2+I_{E}^2+Q_{E}^2}\f$ and * \f$L=\sqrt{I_{VL}^2+Q_{VL}^2+I_{L}^2+Q_{L}^2}\f$ . The output is in [chips]. */ -float dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1) +double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1) { - float P_early, P_late; + double P_early, P_late; P_early = std::sqrt(std::norm(very_early_s1) + std::norm(early_s1)); P_late = std::sqrt(std::norm(very_late_s1) + std::norm(late_s1)); return (P_early - P_late) / ((P_early + P_late)); diff --git a/src/algorithms/tracking/libs/tracking_discriminators.h b/src/algorithms/tracking/libs/tracking_discriminators.h index 8a78f9c1c..87fe3abab 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.h +++ b/src/algorithms/tracking/libs/tracking_discriminators.h @@ -50,7 +50,7 @@ * \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_1\f$, and * \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second]. */ -float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t1, float t2); +double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2); /*! \brief PLL four quadrant arctan discriminator @@ -61,7 +61,7 @@ float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_four_quadrant_atan(gr_complex prompt_s1); +double pll_four_quadrant_atan(gr_complex prompt_s1); /*! \brief PLL Costas loop two quadrant arctan discriminator @@ -72,7 +72,7 @@ float pll_four_quadrant_atan(gr_complex prompt_s1); * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_cloop_two_quadrant_atan(gr_complex prompt_s1); +double pll_cloop_two_quadrant_atan(gr_complex prompt_s1); /*! \brief DLL Noncoherent Early minus Late envelope normalized discriminator @@ -84,7 +84,7 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1); * where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and * \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips]. */ -float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); +double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); /*! \brief DLL Noncoherent Very Early Minus Late Power (VEMLP) normalized discriminator @@ -97,7 +97,7 @@ float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); * where \f$E=\sqrt{I_{VE}^2+Q_{VE}^2+I_{E}^2+Q_{E}^2}\f$ and * \f$L=\sqrt{I_{VL}^2+Q_{VL}^2+I_{L}^2+Q_{L}^2}\f$ . The output is in [chips]. */ -float dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1); +double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1); #endif diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index dea0cbbb8..16b6b2ac4 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -53,6 +53,7 @@ const double GPS_L1_FREQ_HZ = 1.57542e9; //!< L1 [Hz] const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] +const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period [seconds] /*! * \brief Maximum Time-Of-Arrival (TOA) difference between satellites for a receiver operated on Earth surface is 20 ms @@ -67,6 +68,9 @@ const double MAX_TOA_DELAY_MS = 20; //#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) + +// OBSERVABLE HISTORY DEEP FOR INTERPOLATION +const int GPS_L1_CA_HISTORY_DEEP = 100; // NAVIGATION MESSAGE DEMODULATION AND DECODING #define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 9b2f7b61a..1776c26b2 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -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 diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index df688e178..9c603f90e 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -32,6 +32,7 @@ #define GNSS_SDR_GNSS_SYNCHRO_H_ #include "gnss_signal.h" +#include /*! * \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 diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 6e8c888db..021bdd56b 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -39,6 +39,8 @@ #include #include #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 & 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 & 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 & pseudoranges) +{ + std::string sat_data; + sat_data.clear(); + + Rtcm::set_DF394(pseudoranges); + unsigned int num_satellites = DF394.count(); + + std::map pseudoranges_ordered; + + std::map::const_iterator gnss_synchro_iter; + std::vector pos; + std::vector::iterator it; + + for(gnss_synchro_iter = pseudoranges.begin(); + gnss_synchro_iter != pseudoranges.end(); + gnss_synchro_iter++) + { + pseudoranges_ordered.insert(std::pair(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 & pseudoranges) +{ + std::string s("Not implemented"); + return s; +} + +std::string Rtcm::get_MSM_4_content_sat_data(const std::map & pseudoranges) +{ + //std::map::const_iterator gnss_synchro_iter; + std::string data("Not implemented"); + return data; +} + +//std::string Rtcm::get_MSM_1_content(const std::map & 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(std::round((obs_time + 604800 * static_cast(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 & gnss_synchro) return 0; } +std::string Rtcm::set_DF396(const std::map & pseudoranges) +{ + std::string DF396; + std::map::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 > matrix( num_signals, std::vector(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(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(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(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(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(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; +} + diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index b17f3cdf8..178459497 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -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 & 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); // 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 & 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 & pseudoranges); + std::string get_MSM_1_content_signal_data(const std::map & pseudoranges); + + std::string get_MSM_4_content_sat_data(const std::map & 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 & gnss_synchro); + int set_DF394(const std::map & pseudoranges); std::bitset<32> DF395; - int set_DF395(const std::map & gnss_synchro); + int set_DF395(const std::map & pseudoranges); + + std::string set_DF396(const std::map & 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 > DF397; // 8*NSAT - std::vector > DF398; // 10*NSAT - - // Content of Satellite data for MSM5 and MSM7 - std::vector > DF399; // 14*NSAT + int set_DF418(int carrier_smoothing_interval_s); }; #endif