First commit of the acquisition method based on the QuickSync algorithm.

Code developed by Damian Miralles.
This commit is contained in:
Carles Fernandez 2014-07-24 01:38:58 +02:00
parent 6967fc833e
commit 42ae5cbd88
16 changed files with 3690 additions and 1 deletions

View File

@ -43,6 +43,7 @@ Mara Branzanti mara.branzanti@gmail.com Developer
Marc Molina marc.molina.pena@gmail.com Developer
Daniel Fehr daniel.co@bluewin.ch Developer
Marc Sales marcsales92@gmail.com Developer
Damian Miralles dmiralles2009@gmail.com Developer
Leonardo Tonetto tonetto.dev@gmail.com Contributor
Ignacio Paniego ignacio.paniego@gmail.com Web design
Eva Puchol eva.puchol@gmail.com Web developer

View File

@ -0,0 +1,477 @@
; 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] or [Rtlsdr_Signal_Source]
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=/home/dmiralles2009/Downloads/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN.dat
;SignalSource.filename=/home/dmiralles2009/Downloads/GSoC_CTTC_capture_2012_07_26_4Msps_4ms.dat
;#item_type: Type and resolution for each of the signal samples.
;#Use gr_complex for 32 bits float I/Q or short for I/Q interleaved short integer.
;#If short is selected you should have to instantiate the Ishort_To_Complex data_type_adapter.
SignalSource.item_type=short
;#sampling_frequency: Original Signal sampling frequency in [Hz]
SignalSource.sampling_frequency=4000000
;#freq: RF front-end center frequency in [Hz]
SignalSource.freq=1575420000
;#gain: Front-end Gain in [dB]
SignalSource.gain=60
;#AGC_enabled: RTLSDR AGC enabled [true or false]
SignalSource.AGC_enabled=true
;#subdevice: UHD subdevice specification (for USRP1 use A:0 or B:0)
SignalSource.subdevice=B:0
;#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: Use [Ishort_To_Complex] or [Pass_Through]
DataTypeAdapter.implementation=Ishort_To_Complex
;#dump: Dump the filtered data to a file.
DataTypeAdapter.dump=false
;#dump_filename: Log path and filename.
DataTypeAdapter.dump_filename=../data/data_type_adapter.dat
;######### 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
#used for gps
InputFilter.band1_begin=0.0
;InputFilter.band1_end=0.8
InputFilter.band1_end=0.85
InputFilter.band2_begin=0.90
InputFilter.band2_end=1.0
#used for galileo
InputFilter.band1_begin=0.0
;InputFilter.band1_end=0.8
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 satellite channels.
Channels.count=4
;#in_acquisition: Number of channels simultaneously acquiring
Channels.in_acquisition=1
;######### CHANNEL 0 CONFIG ############
;#system: GPS, GLONASS, GALILEO, SBAS or COMPASS
;#if the option is disabled by default is assigned GPS
Channel.system = Galileo
;Channel.system = GPS
;#signal:
;# "1C" GPS L1 C/A
;# "1P" GPS L1 P
;# "1W" GPS L1 Z-tracking and similar (AS on)
;# "1Y" GPS L1 Y
;# "1M" GPS L1 M
;# "1N" GPS L1 codeless
;# "2C" GPS L2 C/A
;# "2D" GPS L2 L1(C/A)(P2-P1) semi-codeless
;# "2S" GPS L2 L2C (M)
;# "2L" GPS L2 L2C (L)
;# "2X" GPS L2 L2C (ML)
;# "2P" GPS L2 P
;# "2W" GPS L2 Z-tracking and similar (AS on)
;# "2Y" GPS L2 Y
;# "2M" GPS GPS L2 M
;# "2N" GPS L2 codeless
;# "5I" GPS L5 I
;# "5Q" GPS L5 Q
;# "5X" GPS L5 IQ
;# "1C" GLONASS G1 C/A
;# "1P" GLONASS G1 P
;# "2C" GLONASS G2 C/A (Glonass M)
;# "2P" GLONASS G2 P
;# "1A" GALILEO E1 A (PRS)
;# "1B" GALILEO E1 B (I/NAV OS/CS/SoL)
;# "1C" GALILEO E1 C (no data)
;# "1X" GALILEO E1 BC
;# "1Z" GALILEO E1 ABC
;# "5I" GALILEO E5a I (F/NAV OS)
;# "5Q" GALILEO E5a Q (no data)
;# "5X" GALILEO E5a IQ
;# "7I" GALILEO E5b I
;# "7Q" GALILEO E5b Q
;# "7X" GALILEO E5b IQ
;# "8I" GALILEO E5 I
;# "8Q" GALILEO E5 Q
;# "8X" GALILEO E5 IQ
;# "6A" GALILEO E6 A
;# "6B" GALILEO E6 B
;# "6C" GALILEO E6 C
;# "6X" GALILEO E6 BC
;# "6Z" GALILEO E6 ABC
;# "1C" SBAS L1 C/A
;# "5I" SBAS L5 I
;# "5Q" SBAS L5 Q
;# "5X" SBAS L5 IQ
;# "2I" COMPASS E2 I
;# "2Q" COMPASS E2 Q
;# "2X" COMPASS E2 IQ
;# "7I" COMPASS E5b I
;# "7Q" COMPASS E5b Q
;# "7X" COMPASS E5b IQ
;# "6I" COMPASS E6 I
;# "6Q" COMPASS E6 Q
;# "6X" COMPASS E6 IQ
;#if the option is disabled by default is assigned "1C" GPS L1 C/A
;#satellite: Satellite PRN ID for this channel. Disable this option to random search
;######### CHANNEL 0 CONFIG ############
;### Uncoment these lines for GPS Systems
;Channel0.system=GPS
;Channel0.signal=1C
;Channel0.satellite=11
;Channel0.repeat_satellite=true
;### Uncoment these lines for Galileo Systems
Channel0.system=Galileo
Channel0.signal=1B
Channel0.satellite=20
Channel0.repeat_satellite=true
;######### CHANNEL 1 CONFIG ############
;### Uncoment these lines for GPS Systems
;Channel1.system=GPS
;Channel1.signal=1C
;Channel1.satellite=1
;Channel1.repeat_satellite=true
;### Uncoment these lines for Galileo Systems
Channel1.system=Galileo
Channel1.signal=1B
Channel1.satellite=12
Channel1.repeat_satellite=true
;######### CHANNEL 2 CONFIG ############
;### Uncoment these lines for GPS Systems
;Channel2.system=GPS
;Channel2.signal=1C
;Channel2.satellite=17
;Channel2.repeat_satellite=true
;### Uncoment these lines for Galileo Systems
Channel2.system=Galileo
Channel2.signal=1B
Channel2.satellite=11
Channel2.repeat_satellite=true
;######### CHANNEL 3 CONFIG ############
;### Uncoment these lines for GPS Systems
;Channel3.system=GPS
;Channel3.signal=1C
;Channel3.satellite=20
;Channel3.repeat_satellite=false
;### Uncoment these lines for Galileo Systems
Channel3.system=Galileo
Channel3.signal=1B
Channel3.satellite=19
Channel3.repeat_satellite=true
;######### ACQUISITION GLOBAL CONFIG ############
;#dump: Enable or disable the acquisition internal data file logging [true] or [false]
Acquisition.dump=false
;#filename: Log path and filename
Acquisition.dump_filename=./acq_dump.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
Acquisition.item_type=gr_complex
;#if: Signal intermediate frequency in [Hz]
Acquisition.if=0
;#sampled_ms: Signal block duration for the acquisition signal detection [ms]
;Acquisition.coherent_integration_time_ms=4
Acquisition.repeat_satellite=true
;######### ACQUISITION CHANNELS CONFIG ######
;######### ACQUISITION CONFIG PARAMETERS ############
;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition]
;Acquisition0.implementation=GPS_L1_CA_PCPS_QuickSync_Acquisition
;Acquisition0.implementation=Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition
;#threshold: Acquisition threshold
;Acquisition0.threshold=0.010
;#doppler_max: Maximum expected Doppler shift [Hz]
;Acquisition0.doppler_max=10000
;#doppler_max: Doppler step in the grid search [Hz]
;Acquisition0.doppler_step=250
;#repeat_satellite: Use only jointly with the satellte PRN ID option.
;######### ACQUISITION CH 0 CONFIG ############
;### Uncoment these lines for GPS Systems
;Acquisition0.implementation=GPS_L1_CA_PCPS_QuickSync_Acquisition
;Acquisition0.threshold=0.010
;Acquisition0.doppler_step=250
;Acquisition0.doppler_max=10000
;Acquisition0.coherent_integration_time_ms=4
;### Uncoment these lines for Galileo Systems
Acquisition0.implementation=Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition
Acquisition0.doppler_step=62
Acquisition0.threshold=0.002
Acquisition0.doppler_max=10000
Acquisition0.coherent_integration_time_ms=16
;######### ACQUISITION CH 1 CONFIG ############
;### Uncoment these lines for GPS Systems
;Acquisition1.implementation=GPS_L1_CA_PCPS_QuickSync_Acquisition
;Acquisition1.threshold=0.010
;Acquisition1.doppler_step=250
;Acquisition1.doppler_max=10000
;Acquisition1.coherent_integration_time_ms=4
;### Uncoment these lines for Galileo Systems
Acquisition1.implementation=Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition
Acquisition1.doppler_step=62
Acquisition1.threshold=0.002
Acquisition1.doppler_max=10000
Acquisition1.coherent_integration_time_ms=16
;######### ACQUISITION CH 2 CONFIG ############
;### Uncoment these lines for GPS Systems
;Acquisition2.implementation=GPS_L1_CA_PCPS_QuickSync_Acquisition
;Acquisition2.threshold=0.002
;Acquisition2.doppler_max=10000
;Acquisition2.doppler_step=250
;Acquisition2.coherent_integration_time_ms=4
;### Uncoment these lines for Galileo Systems
Acquisition2.implementation=Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition
Acquisition2.threshold=0.6
Acquisition2.doppler_max=10000
Acquisition2.doppler_step=62
Acquisition2.coherent_integration_time_ms=16
;######### ACQUISITION CH 3 CONFIG ############
;### Uncoment these lines for GPS Systems
;Acquisition3.implementation=GPS_L1_CA_PCPS_QuickSync_Acquisition
;Acquisition3.threshold=0.002
;Acquisition3.doppler_max=10000
;Acquisition3.doppler_step=250
;Acquisition3.coherent_integration_time_ms=4
;### Uncoment these lines for Galileo Systems
Acquisition3.implementation=Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition
Acquisition3.threshold=0.8
Acquisition3.doppler_max=10000
Acquisition3.doppler_step=62
Acquisition3.coherent_integration_time_ms=16
;######### TRACKING GLOBAL CONFIG ############
;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking]
Tracking.implementation=GPS_L1_CA_DLL_PLL_Optim_Tracking
;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version.
Tracking.item_type=gr_complex
;#sampling_frequency: Signal Intermediate Frequency in [Hz]
Tracking.if=0
;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false]
Tracking.dump=false
;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number.
Tracking.dump_filename=./tracking_ch_
;#pll_bw_hz: PLL loop filter bandwidth [Hz]
Tracking.pll_bw_hz=50.0;
;#dll_bw_hz: DLL loop filter bandwidth [Hz]
Tracking.dll_bw_hz=4.0;
;#fll_bw_hz: FLL loop filter bandwidth [Hz]
Tracking.fll_bw_hz=10.0;
;#order: PLL/DLL loop filter order [2] or [3]
Tracking.order=3;
;#early_late_space_chips: correlator early-late space [chips]. Use [0.5]
Tracking.early_late_space_chips=0.5;
;######### TELEMETRY DECODER CONFIG ############
;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A.
TelemetryDecoder.implementation=GPS_L1_CA_Telemetry_Decoder
TelemetryDecoder.dump=false
;######### 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=false
;#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=true
;#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

View File

@ -23,9 +23,11 @@ if(OPENCL_FOUND)
gps_l1_ca_pcps_assisted_acquisition.cc
gps_l1_ca_pcps_acquisition_fine_doppler.cc
gps_l1_ca_pcps_tong_acquisition.cc
gps_l1_ca_pcps_quicksync_acquisition.cc
gps_l1_ca_pcps_opencl_acquisition.cc
galileo_e1_pcps_ambiguous_acquisition.cc
galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc
galileo_e1_pcps_quicksync_ambiguous_acquisition.cc
galileo_e1_pcps_tong_ambiguous_acquisition.cc
galileo_e1_pcps_8ms_ambiguous_acquisition.cc
)
@ -36,8 +38,10 @@ else(OPENCL_FOUND)
gps_l1_ca_pcps_assisted_acquisition.cc
gps_l1_ca_pcps_acquisition_fine_doppler.cc
gps_l1_ca_pcps_tong_acquisition.cc
gps_l1_ca_pcps_quicksync_acquisition.cc
galileo_e1_pcps_ambiguous_acquisition.cc
galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc
galileo_e1_pcps_quicksync_ambiguous_acquisition.cc
galileo_e1_pcps_tong_ambiguous_acquisition.cc
galileo_e1_pcps_8ms_ambiguous_acquisition.cc
)

View File

@ -0,0 +1,347 @@
/*!
* \file galileo_e1_pcps_quicksync_ambiguous_acquisition.cc
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
* Galileo E1 Signals using the QuickSync Algorithm
* \author Damian Miralles, 2014. dmiralles2009@gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "galileo_e1_pcps_quicksync_ambiguous_acquisition.h"
#include <iostream>
#include <boost/lexical_cast.hpp>
#include <boost/math/distributions/exponential.hpp>
#include <glog/logging.h>
#include <cmath>
#include "galileo_e1_signal_processing.h"
#include "Galileo_E1.h"
#include "configuration_interface.h"
using google::LogMessage;
GalileoE1PcpsQuickSyncAmbiguousAcquisition::GalileoE1PcpsQuickSyncAmbiguousAcquisition(
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams,
boost::shared_ptr<gr::msg_queue> queue) :
role_(role), in_streams_(in_streams), out_streams_(out_streams), queue_(queue)
{
configuration_ = configuration;
std::string default_item_type = "gr_complex";
std::string default_dump_filename = "../data/acquisition.dat";
DLOG(INFO) << "role " << role;
item_type_ = configuration_->property(role + ".item_type",
default_item_type);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000);
if_ = configuration_->property(role + ".ifreq", 0);
dump_ = configuration_->property(role + ".dump", false);
shift_resolution_ = configuration_->property(role + ".doppler_max", 15);
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 16);
/*--- Find number of samples per spreading code (4 ms) -----------------*/
code_length_ = round(
fs_in_
/ (Galileo_E1_CODE_CHIP_RATE_HZ
/ Galileo_E1_B_CODE_LENGTH_CHIPS));
int samples_per_ms = round(code_length_ / 4.0);
vector_length_ = sampled_ms_ * samples_per_ms;
/*Calculate the folding factor value based on the calculations*/
folding_factor_ = (unsigned int)ceil(sqrt(log2(code_length_)));
if (sampled_ms_ % (folding_factor_*4) != 0)
{
LOG(WARNING) << "QuickSync Algorithm requires a coherent_integration_time"
<< " multiple of "<<(folding_factor_*4)<<"ms, Value entered "
<<sampled_ms_<<" ms";
if(sampled_ms_ < (folding_factor_*4))
{
sampled_ms_ = (int) (folding_factor_*4);
}
else
{
sampled_ms_ = (int)(sampled_ms_/(folding_factor_*4)) * (folding_factor_*4);
}
LOG(WARNING) << "coherent_integration_time should be multiple of "
<< "Galileo code length (4 ms). coherent_integration_time = "
<< sampled_ms_ << " ms will be used.";
}
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
if (!bit_transition_flag_)
{
max_dwells_ = configuration_->property(role + ".max_dwells", 1);
}
else
{
max_dwells_ = 2;
}
dump_filename_ = configuration_->property(role + ".dump_filename",
default_dump_filename);
code_ = new gr_complex[code_length_];
LOG(INFO) <<"Vector Length: "<<code_length_
<<", Samples per ms: "<<samples_per_ms
<<", Folding factor: "<<folding_factor_
<<", Sampled ms: "<<sampled_ms_
<<", Code Length: "<<code_length_;
if (item_type_.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_quicksync_make_acquisition_cc(folding_factor_,
sampled_ms_, max_dwells_, shift_resolution_, if_, fs_in_,
samples_per_ms, code_length_, bit_transition_flag_, queue_,
dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_,
code_length_ * folding_factor_);
DLOG(INFO) << "stream_to_vector_quicksync("
<< stream_to_vector_->unique_id() << ")";
DLOG(INFO) << "acquisition_quicksync(" << acquisition_cc_->unique_id()
<< ")";
}
else
{
LOG(WARNING) << item_type_
<< " unknown acquisition item type";
}
}
GalileoE1PcpsQuickSyncAmbiguousAcquisition::~GalileoE1PcpsQuickSyncAmbiguousAcquisition()
{
delete[] code_;
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_channel(unsigned int channel)
{
channel_ = channel;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_channel(channel_);
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_threshold(float threshold)
{
float pfa = configuration_->property(role_+ boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa==0.0) pfa = configuration_->property(role_+".pfa", 0.0);
if(pfa==0.0)
{
threshold_ = threshold;
}
else
{
threshold_ = calculate_threshold(pfa);
}
DLOG(INFO) <<"Channel "<<channel_<<" Threshold = " << threshold_;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_threshold(threshold_);
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_max(unsigned int doppler_max)
{
doppler_max_ = doppler_max;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_doppler_max(doppler_max_);
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_doppler_step(unsigned int doppler_step)
{
doppler_step_ = doppler_step;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_doppler_step(doppler_step_);
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_channel_queue(
concurrent_queue<int> *channel_internal_queue)
{
channel_internal_queue_ = channel_internal_queue;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_channel_queue(channel_internal_queue_);
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_gnss_synchro(
Gnss_Synchro* gnss_synchro)
{
gnss_synchro_ = gnss_synchro;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_gnss_synchro(gnss_synchro_);
}
}
signed int
GalileoE1PcpsQuickSyncAmbiguousAcquisition::mag()
{
if (item_type_.compare("gr_complex") == 0)
{
return acquisition_cc_->mag();
}
else
{
return 0;
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::init()
{
acquisition_cc_->init();
set_local_code();
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::set_local_code()
{
if (item_type_.compare("gr_complex") == 0)
{
bool cboc = configuration_->property(
"Acquisition" + boost::lexical_cast<std::string>(channel_)
+ ".cboc", false);
std::complex<float> * code = new std::complex<float>[code_length_];
galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal,
cboc, gnss_synchro_->PRN, fs_in_, 0, false);
/*
for (unsigned int i = 0; i < sampled_ms_/4; i++)
{
memcpy(&(code_[i*code_length_]), code,
sizeof(gr_complex)*code_length_);
}
*/
memcpy(code_, code,sizeof(gr_complex)*code_length_);
acquisition_cc_->set_local_code(code_);
delete[] code;
code = NULL;
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::reset()
{
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_active(true);
}
}
float GalileoE1PcpsQuickSyncAmbiguousAcquisition::calculate_threshold(float pfa)
{
unsigned int frequency_bins = 0;
for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_)
{
frequency_bins++;
}
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
unsigned int ncells = code_length_ * frequency_bins;
double exponent = 1 / (double)ncells;
double val = pow(1.0 - pfa, exponent);
double lambda = double(code_length_);
boost::math::exponential_distribution<double> mydist (lambda);
float threshold = (float)quantile(mydist,val);
return threshold;
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::connect(gr::top_block_sptr top_block)
{
if (item_type_.compare("gr_complex") == 0)
{
top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
}
}
void
GalileoE1PcpsQuickSyncAmbiguousAcquisition::disconnect(gr::top_block_sptr top_block)
{
if (item_type_.compare("gr_complex") == 0)
{
top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0);
}
}
gr::basic_block_sptr GalileoE1PcpsQuickSyncAmbiguousAcquisition::get_left_block()
{
return stream_to_vector_;
}
gr::basic_block_sptr GalileoE1PcpsQuickSyncAmbiguousAcquisition::get_right_block()
{
return acquisition_cc_;
}
unsigned int GalileoE1PcpsQuickSyncAmbiguousAcquisition::get_folding_factor()
{
return folding_factor_;
}

View File

@ -0,0 +1,167 @@
/*!
* \file galileo_e1_pcps_quicksync_ambiguous_acquisition.h
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for Galileo E1 Signals
* \date June, 2014
* \author Damian Miralles Sanchez. dmiralles2009@gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_E1_PCPS_QUICKSYNC_AMBIGUOUS_ACQUISITION_H_
#define GNSS_SDR_GALILEO_E1_PCPS_QUICKSYNC_AMBIGUOUS_ACQUISITION_H_
#include <string>
#include <gnuradio/msg_queue.h>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_quicksync_acquisition_cc.h"
class ConfigurationInterface;
/*!
* \brief This class adapts a PCPS acquisition block to an
* AcquisitionInterface for Galileo E1 Signals
*/
class GalileoE1PcpsQuickSyncAmbiguousAcquisition: public AcquisitionInterface
{
public:
GalileoE1PcpsQuickSyncAmbiguousAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams,
unsigned int out_streams, boost::shared_ptr<gr::msg_queue> queue);
virtual ~GalileoE1PcpsQuickSyncAmbiguousAcquisition();
std::string role()
{
return role_;
}
/*!
* \brief Returns "Galileo_E1_PCPS_Ambiguous_Acquisition"
*/
std::string implementation()
{
return "Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition";
}
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 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 acquisition channel unique ID
*/
void set_channel(unsigned int channel);
/*!
* \brief Set statistics threshold of PCPS algorithm
*/
void set_threshold(float threshold);
/*!
* \brief Set maximum Doppler off grid search
*/
void set_doppler_max(unsigned int doppler_max);
/*!
* \brief Set Doppler steps for the grid search
*/
void set_doppler_step(unsigned int doppler_step);
/*!
* \brief Set tracking channel internal queue
*/
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Sets local code for Galileo E1 PCPS acquisition algorithm.
*/
void set_local_code();
/*!
* \brief Returns the maximum peak of grid search
*/
signed int mag();
/*!
* \brief Restart acquisition algorithm
*/
void reset();
/*!
* \brief Get the folding factor value
*/
unsigned int get_folding_factor();
private:
ConfigurationInterface* configuration_;
pcps_quicksync_acquisition_cc_sptr acquisition_cc_;
gr::blocks::stream_to_vector::sptr stream_to_vector_;
size_t item_size_;
std::string item_type_;
unsigned int vector_length_;
unsigned int code_length_;
bool bit_transition_flag_;
unsigned int channel_;
float threshold_;
unsigned int doppler_max_;
unsigned int doppler_step_;
unsigned int shift_resolution_;
unsigned int sampled_ms_;
unsigned int max_dwells_;
unsigned int folding_factor_;
long fs_in_;
long if_;
bool dump_;
std::string dump_filename_;
std::complex<float> * code_;
Gnss_Synchro * gnss_synchro_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
boost::shared_ptr<gr::msg_queue> queue_;
concurrent_queue<int> *channel_internal_queue_;
float calculate_threshold(float pfa);
};
#endif /* GNSS_SDR_GALILEO_E1_PCPS_QUICKSYNC_AMBIGUOUS_ACQUISITION_H_ */

View File

@ -0,0 +1,333 @@
/*!
* \file gps_l1_ca_pcps_quicksync_acquisition.cc
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
* GPS L1 C/A signals using the QuickSync Algorithm
* \author Damian Miralles, 2014. dmiralles2009@gmail.com
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_pcps_quicksync_acquisition.h"
#include <iostream>
#include <cmath>
#include <stdexcept>
#include <boost/math/distributions/exponential.hpp>
#include <glog/logging.h>
#include <gnuradio/msg_queue.h>
#include "gps_sdr_signal_processing.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
using google::LogMessage;
GpsL1CaPcpsQuickSyncAcquisition::GpsL1CaPcpsQuickSyncAcquisition(
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams,
gr::msg_queue::sptr queue) :
role_(role), in_streams_(in_streams), out_streams_(out_streams), queue_(queue)
{
configuration_ = configuration;
std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat";
DLOG(INFO) << "role " << role;
item_type_ = configuration_->property(role + ".item_type", default_item_type);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000);
if_ = configuration_->property(role + ".ifreq", 0);
dump_ = configuration_->property(role + ".dump", false);
shift_resolution_ = configuration_->property(role + ".doppler_max", 15);
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 4);
//--- Find number of samples per spreading code -------------------------
code_length_ = round(fs_in_
/ (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
vector_length_ = code_length_ * sampled_ms_;
/*Calculate the folding factor value based on the calculations*/
folding_factor_ = (unsigned int)ceil(sqrt(log2(code_length_)));
if ( sampled_ms_ % folding_factor_ != 0)
{
LOG(WARNING) << "QuickSync Algorithm requires a coherent_integration_time"
<< " multiple of " << folding_factor_ << "ms, Value entered "
<< sampled_ms_ << " ms";
if(sampled_ms_ < folding_factor_)
{
sampled_ms_ = (int) folding_factor_;
}
else
{
sampled_ms_ = (int)(sampled_ms_/folding_factor_) * folding_factor_;
}
LOG(WARNING) <<" Coherent_integration_time of "
<< sampled_ms_ << " ms will be used instead.";
}
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
if (!bit_transition_flag_)
{
max_dwells_ = configuration_->property(role + ".max_dwells", 1);
}
else
{
max_dwells_ = 2;
}
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
int samples_per_ms = round(code_length_);
code_= new gr_complex[code_length_];
/*Object relevant information for debugging*/
LOG(INFO) <<"Implementation: "<<this->implementation()
<<", Vector Length: "<<code_length_*sampled_ms_
<<", Samples per ms: "<<samples_per_ms
<<", Folding factor: "<<folding_factor_
<<", Sampled ms: "<<sampled_ms_
<<", Code Length: "<<code_length_;
if (item_type_.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_quicksync_make_acquisition_cc(folding_factor_,
sampled_ms_, max_dwells_,shift_resolution_, if_, fs_in_,
samples_per_ms, code_length_,bit_transition_flag_, queue_,
dump_, dump_filename_);
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_,
code_length_ * folding_factor_);
DLOG(INFO) << "stream_to_vector_quicksync(" << stream_to_vector_->unique_id() << ")";
DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() << ")";
}
else
{
LOG(WARNING) << item_type_ << " unknown acquisition item type";
}
}
GpsL1CaPcpsQuickSyncAcquisition::~GpsL1CaPcpsQuickSyncAcquisition()
{
delete[] code_;
}
void GpsL1CaPcpsQuickSyncAcquisition::set_channel(unsigned int channel)
{
channel_ = channel;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_channel(channel_);
}
}
void GpsL1CaPcpsQuickSyncAcquisition::set_threshold(float threshold)
{
float pfa = configuration_->property(role_ +
boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
if(pfa == 0.0)
{
pfa = configuration_->property(role_+".pfa", 0.0);
}
if(pfa == 0.0)
{
threshold_ = threshold;
}
else
{
threshold_ = calculate_threshold(pfa);
}
DLOG(INFO) <<"Channel "<<channel_<<" Threshold = " << threshold_;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_threshold(threshold_);
}
}
void GpsL1CaPcpsQuickSyncAcquisition::set_doppler_max(unsigned int doppler_max)
{
doppler_max_ = doppler_max;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_doppler_max(doppler_max_);
}
}
void GpsL1CaPcpsQuickSyncAcquisition::set_doppler_step(unsigned int doppler_step)
{
doppler_step_ = doppler_step;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_doppler_step(doppler_step_);
}
}
void GpsL1CaPcpsQuickSyncAcquisition::set_channel_queue(
concurrent_queue<int> *channel_internal_queue)
{
channel_internal_queue_ = channel_internal_queue;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_channel_queue(channel_internal_queue_);
}
}
void GpsL1CaPcpsQuickSyncAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
{
gnss_synchro_ = gnss_synchro;
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_gnss_synchro(gnss_synchro_);
}
}
signed int GpsL1CaPcpsQuickSyncAcquisition::mag()
{
if (item_type_.compare("gr_complex") == 0)
{
return acquisition_cc_->mag();
}
else
{
return 0;
}
}
void GpsL1CaPcpsQuickSyncAcquisition::init()
{
acquisition_cc_->init();
set_local_code();
}
void GpsL1CaPcpsQuickSyncAcquisition::set_local_code()
{
if (item_type_.compare("gr_complex") == 0)
{
std::complex<float>* code = new std::complex<float>[code_length_];
gps_l1_ca_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0);
/*
for (unsigned int i = 0; i < sampled_ms_; i++)
{
memcpy(&(code_[i*code_length_]), code,
sizeof(gr_complex)*code_length_);
}
*/
memcpy(code_, code,sizeof(gr_complex)*code_length_);
acquisition_cc_->set_local_code(code_);
delete[] code;
}
}
void GpsL1CaPcpsQuickSyncAcquisition::reset()
{
if (item_type_.compare("gr_complex") == 0)
{
acquisition_cc_->set_active(true);
}
}
float GpsL1CaPcpsQuickSyncAcquisition::calculate_threshold(float pfa)
{
//Calculate the threshold
unsigned int frequency_bins = 0;
for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_)
{
frequency_bins++;
}
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa;
unsigned int ncells = code_length_*frequency_bins;
double exponent = 1/(double)ncells;
double val = pow(1.0 - pfa, exponent);
double lambda = double(code_length_);
boost::math::exponential_distribution<double> mydist (lambda);
float threshold = (float)quantile(mydist,val);
return threshold;
}
void GpsL1CaPcpsQuickSyncAcquisition::connect(gr::top_block_sptr top_block)
{
if (item_type_.compare("gr_complex") == 0)
{
top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
}
}
void GpsL1CaPcpsQuickSyncAcquisition::disconnect(gr::top_block_sptr top_block)
{
if (item_type_.compare("gr_complex") == 0)
{
top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0);
}
}
gr::basic_block_sptr GpsL1CaPcpsQuickSyncAcquisition::get_left_block()
{
return stream_to_vector_;
}
gr::basic_block_sptr GpsL1CaPcpsQuickSyncAcquisition::get_right_block()
{
return acquisition_cc_;
}
unsigned int GpsL1CaPcpsQuickSyncAcquisition::get_folding_factor()
{
return folding_factor_;
}

View File

@ -0,0 +1,173 @@
/*!
* \file gps_l1_ca_pcps_quicksync_acquisition.h
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for GPS L1 C/A signals implementing the QuickSync Algorithm.
* \date June, 2014
* \author Damian Miralles Sanchez. dmiralles2009@gmail.com
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GPS_L1_CA_PCPS_QUICKSYNC_ACQUISITION_H_
#define GNSS_SDR_GPS_L1_CA_PCPS_QUICKSYNC_ACQUISITION_H_
#include <string>
#include <gnuradio/msg_queue.h>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_quicksync_acquisition_cc.h"
#include "configuration_interface.h"
class ConfigurationInterface;
/*!
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
* for GPS L1 C/A signals
*/
class GpsL1CaPcpsQuickSyncAcquisition: public AcquisitionInterface
{
public:
GpsL1CaPcpsQuickSyncAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams,
unsigned int out_streams, boost::shared_ptr<gr::msg_queue> queue);
virtual ~GpsL1CaPcpsQuickSyncAcquisition();
std::string role()
{
return role_;
}
/*!
* \brief Returns "GPS_L1_CA_PCPS_QuickSync_Acquisition"
*/
std::string implementation()
{
return "GPS_L1_CA_PCPS_QuickSync_Acquisition";
}
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 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 acquisition channel unique ID
*/
void set_channel(unsigned int channel);
/*!
* \brief Set statistics threshold of PCPS algorithm
*/
void set_threshold(float threshold);
/*!
* \brief Set maximum Doppler off grid search
*/
void set_doppler_max(unsigned int doppler_max);
/*!
* \brief Set Doppler steps for the grid search
*/
void set_doppler_step(unsigned int doppler_step);
/*!
* \brief Set tracking channel internal queue
*/
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Sets local code for GPS L1/CA PCPS acquisition algorithm.
*/
void set_local_code();
/*!
* \brief Returns the maximum peak of grid search
*/
signed int mag();
/*!
* \brief Restart acquisition algorithm
*/
void reset();
/*!
* \brief Get the folding factor value
*/
unsigned int get_folding_factor();
private:
ConfigurationInterface* configuration_;
pcps_quicksync_acquisition_cc_sptr acquisition_cc_;
gr::blocks::stream_to_vector::sptr stream_to_vector_;
size_t item_size_;
std::string item_type_;
unsigned int vector_length_;
unsigned int code_length_;
bool bit_transition_flag_;
unsigned int channel_;
float threshold_;
unsigned int doppler_max_;
unsigned int doppler_step_;
unsigned int shift_resolution_;
unsigned int sampled_ms_;
unsigned int max_dwells_;
unsigned int folding_factor_;
long fs_in_;
long if_;
bool dump_;
std::string dump_filename_;
std::complex<float> * code_;
Gnss_Synchro * gnss_synchro_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
boost::shared_ptr<gr::msg_queue> queue_;
concurrent_queue<int> *channel_internal_queue_;
float calculate_threshold(float pfa);
};
#endif /* GNSS_SDR_GPS_L1_CA_PCPS_QUICKSYNC_ACQUISITION_H_ */

View File

@ -24,6 +24,7 @@ if(OPENCL_FOUND)
pcps_acquisition_fine_doppler_cc.cc
pcps_tong_acquisition_cc.cc
pcps_cccwsr_acquisition_cc.cc
pcps_quicksync_acquisition_cc.cc
galileo_pcps_8ms_acquisition_cc.cc
pcps_opencl_acquisition_cc.cc # Needs OpenCL
)
@ -35,6 +36,7 @@ else(OPENCL_FOUND)
pcps_acquisition_fine_doppler_cc.cc
pcps_tong_acquisition_cc.cc
pcps_cccwsr_acquisition_cc.cc
pcps_quicksync_acquisition_cc.cc
galileo_pcps_8ms_acquisition_cc.cc
)
endif(OPENCL_FOUND)

View File

@ -0,0 +1,606 @@
/*!
* \file pcps_quicksync_acquisition_cc.cc
* \brief This class implements a Parallel Code Phase Search Acquisition
* \author Damian Miralles Sanchez, 2014. dmiralles2009(at)gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "pcps_quicksync_acquisition_cc.h"
#include <ctime>
#include <cmath>
#include <sstream>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <volk/volk.h>
#include "control_message_factory.h"
#include "gnss_signal_processing.h"
using google::LogMessage;
pcps_quicksync_acquisition_cc_sptr pcps_quicksync_make_acquisition_cc(
unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename)
{
return pcps_quicksync_acquisition_cc_sptr(
new pcps_quicksync_acquisition_cc(
folding_factor,
sampled_ms, max_dwells, doppler_max,
freq, fs_in, samples_per_ms,
samples_per_code,
bit_transition_flag,
queue, dump, dump_filename));
}
pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename):
gr::block("pcps_quicksync_acquisition_cc",
gr::io_signature::make(1, 1, (sizeof(gr_complex)*sampled_ms * samples_per_ms )),
gr::io_signature::make(0, 0, (sizeof(gr_complex)*sampled_ms * samples_per_ms )))
{
//DLOG(INFO) << "START CONSTRUCTOR";
d_sample_counter = 0; // SAMPLE COUNTER
d_active = false;
d_state = 0;
d_queue = queue;
d_freq = freq;
d_fs_in = fs_in;
d_samples_per_ms = samples_per_ms;
d_samples_per_code = samples_per_code;
d_sampled_ms = sampled_ms;
d_max_dwells = max_dwells;
d_well_count = 0;
d_doppler_max = doppler_max;
d_mag = 0;
d_input_power = 0.0;
d_num_doppler_bins = 0;
d_bit_transition_flag = bit_transition_flag;
d_folding_factor = folding_factor;
//fft size is reduced.
d_fft_size = (d_samples_per_code) / d_folding_factor;
//todo: do something if posix_memalign fails
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_magnitude, 16, d_samples_per_code * d_folding_factor * sizeof(float)) == 0){};
if (posix_memalign((void**)&d_magnitude_folded, 16, d_fft_size * sizeof(float)) == 0){};
d_possible_delay = new unsigned int[d_folding_factor];
/*Create the d_code signal , which would store the values of the code in its
original form to perform later correlation in time domain*/
d_code = new gr_complex[d_samples_per_code]();
// Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
// Inverse FFT
d_ifft = new gr::fft::fft_complex(d_fft_size, false);
// For dumping samples into a file
d_dump = dump;
d_dump_filename = dump_filename;
// DLOG(INFO) << "END CONSTRUCTOR";
}
pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
{
//DLOG(INFO) << "START DESTROYER";
if (d_num_doppler_bins > 0)
{
for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{
free(d_grid_doppler_wipeoffs[i]);
}
delete[] d_grid_doppler_wipeoffs;
}
free(d_fft_codes);
free(d_magnitude);
free(d_magnitude_folded);
delete d_ifft;
d_ifft = NULL;
delete d_fft_if;
d_fft_if = NULL;
delete d_code;
d_code = NULL;
delete d_possible_delay;
d_possible_delay = NULL;
if (d_dump)
{
d_dump_file.close();
}
// DLOG(INFO) << "END DESTROYER";
}
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float> * code)
{
// DLOG(INFO) << "START LOCAL CODE";
/*save a local copy of the code without the folding process to perform corre-
lation in time in the final steps of the acquisition stage*/
memcpy(d_code, code, sizeof(gr_complex)*d_samples_per_code);
d_code_folded = new gr_complex[d_fft_size]();
memcpy(d_fft_if->get_inbuf(), d_code_folded, sizeof(gr_complex)*(d_fft_size));
/*perform folding of the code by the factorial factor parameter. Notice that
folding of the code in the time stage would result in a downsampled spectrum
in the frequency domain after applying the fftw operation*/
for (unsigned int i = 0; i < d_folding_factor; i++)
{
std::transform ((code + i*d_fft_size), (code + ((i+1)*d_fft_size)) ,
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
std::plus<gr_complex>());
}
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
if (is_unaligned())
{
volk_32fc_conjugate_32fc_u(d_fft_codes,d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(), d_fft_size);
}
// DLOG(INFO) << "END LOCAL CODE";
}
void pcps_quicksync_acquisition_cc::init()
{
//DLOG(INFO) << "START init";
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_mag = 0.0;
d_input_power = 0.0;
// Count the number of bins
d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max);
doppler <= (int)(d_doppler_max);
doppler += d_doppler_step)
{
d_num_doppler_bins++;
}
// Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16,
d_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){};
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in,
d_samples_per_code * d_folding_factor);
}
// DLOG(INFO) << "end init";
}
int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
/*
* By J.Arribas, L.Esteve and M.Molina
* Acquisition strategy (Kay Borre book + CFAR threshold):
* 1. Compute the input signal power estimation
* 2. Doppler serial search loop
* 3. Perform the FFT-based circular convolution (parallel time search)
* 4. Record the maximum peak and the associated synchronization parameters
* 5. Compute the test statistics and compare to the threshold
* 6. Declare positive or negative acquisition using a message queue
*/
//DLOG(INFO) << "START GENERAL WORK";
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
//std::cout<<"general_work in quicksync gnuradio block"<<std::endl;
switch (d_state)
{
case 0:
{
//DLOG(INFO) << "START CASE 0";
if (d_active)
{
//restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
d_state = 1;
}
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
//DLOG(INFO) << "END CASE 0";
break;
}
case 1:
{
/* initialize acquisition implementing the QuickSync algorithm*/
//DLOG(INFO) << "START CASE 1";
int doppler;
unsigned int indext = 0;
float magt = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
gr_complex *in_temp;
if (posix_memalign((void**)&(in_temp), 16,d_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){};
gr_complex *in_temp_folded;
if (posix_memalign((void**)&(in_temp_folded), 16,d_fft_size * sizeof(gr_complex)) == 0){};
/*Create a signal to store a signal of size 1ms, to perform correlation
in time. No folding on this data is required*/
gr_complex *in_1code;
if (posix_memalign((void**)&(in_1code), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
/*Stores the values of the correlation output between the local code
and the signal with doppler shift corrected */
gr_complex *corr_output;
if (posix_memalign((void**)&(corr_output), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
/*Stores a copy of the folded version of the signal.This is used for
the FFT operations in future steps of excecution*/
// gr_complex in_folded[d_fft_size];
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size;
d_input_power = 0.0;
d_mag = 0.0;
d_test_statistics = 0.0;
d_noise_floor_power = 0.0;
d_sample_counter += d_sampled_ms * d_samples_per_ms; // sample counter
d_well_count++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: "
<< d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
<< " ,algorithm: pcps_quicksync_acquisition"
<< " ,folding factor: " << d_folding_factor
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_doppler_max
<< ", doppler_step: " << d_doppler_step << ", Signal Size: "
<< d_samples_per_code * d_folding_factor;
/* 1- Compute the input signal power estimation. This operation is
being performed in a signal of size nxp */
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_samples_per_code * d_folding_factor);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
d_input_power /= (float)(d_samples_per_code * d_folding_factor);
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++)
{
/*Ensure that the signal is going to start with all samples
at zero. This is done to avoid over acumulation when performing
the folding process to be stored in d_fft_if->get_inbuf()*/
d_signal_folded = new gr_complex[d_fft_size]();
memcpy( d_fft_if->get_inbuf(),d_signal_folded,
sizeof(gr_complex)*(d_fft_size));
/*Doppler search steps and then multiplication of the incoming
signal with the doppler wipeoffs to eliminate frequency offset
*/
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index;
/*Perform multiplication of the incoming signal with the
complex exponential vector. This removes the frequency doppler
shift offset*/
volk_32fc_x2_multiply_32fc_a(in_temp, in,
d_grid_doppler_wipeoffs[doppler_index],
d_samples_per_code * d_folding_factor);
/*Perform folding of the carrier wiped-off incoming signal. Since
superlinear method is being used the folding factor in the
incoming raw data signal is of d_folding_factor^2*/
for ( int i = 0; i < (int)(d_folding_factor*d_folding_factor); i++)
{
std::transform ((in_temp+i*d_fft_size),
(in_temp+((i+1)*d_fft_size)) ,
d_fft_if->get_inbuf(),
d_fft_if->get_inbuf(),
std::plus<gr_complex>());
}
/* 3- Perform the FFT-based convolution (parallel time search)
Compute the FFT of the carrier wiped--off incoming signal*/
d_fft_if->execute();
/*Multiply carrier wiped--off, Fourier transformed incoming
signal with the local FFT'd code reference using SIMD
operations with VOLK library*/
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
/* compute the inverse FFT of the aliased signal*/
d_ifft->execute();
/* Compute the magnitude and get the maximum value with its
index position*/
volk_32fc_magnitude_squared_32f_a(d_magnitude_folded,
d_ifft->get_outbuf(), d_fft_size);
/* Normalize the maximum value to correct the scale factor
introduced by FFTW*/
//volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded,
// (1 / (fft_normalization_factor * fft_normalization_factor)), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude_folded, d_fft_size);
magt = d_magnitude_folded[indext]/ (fft_normalization_factor * fft_normalization_factor);
delete d_signal_folded;
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
{
d_mag = magt;
/* In case that d_bit_transition_flag = true, we compare the potentially
new maximum test statistics (d_mag/d_input_power) with the value in
d_test_statistics. When the second dwell is being processed, the value
of d_mag/d_input_power could be lower than d_test_statistics (i.e,
the maximum test statistics in the previous dwell is greater than
current d_mag/d_input_power). Note that d_test_statistics is not
restarted between consecutive dwells in multidwell operation.*/
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{
unsigned int detected_delay_samples_folded = 0;
detected_delay_samples_folded = (indext % d_samples_per_code);
float corr_output_f[d_folding_factor];
gr_complex complex_acumulator[100];
//gr_complex complex_acumulator[d_folding_factor];
//const int ff = d_folding_factor;
//gr_complex complex_acumulator[ff];
//gr_complex complex_acumulator[];
//complex_acumulator = new gr_complex[d_folding_factor]();
for (int i = 0; i < (int)d_folding_factor; i++)
{
d_possible_delay[i]= detected_delay_samples_folded+
(i)*d_fft_size;
}
for ( int i = 0; i < (int)d_folding_factor; i++)
{
/*Copy a signal of 1 code length into suggested buffer.
The copied signal must have doppler effect corrected*/
memcpy(in_1code,&in_temp[d_possible_delay[i]],
sizeof(gr_complex)*(d_samples_per_code));
/*Perform multiplication of the unmodified local
generated code with the incoming signal with doppler
effect corrected and accumulates its value. This
is indeed correlation in time for an specific value
of a shift*/
volk_32fc_x2_multiply_32fc_a(corr_output, in_1code,
d_code, d_samples_per_code);
for(int j=0; j < (d_samples_per_code); j++)
{
complex_acumulator[i] += (corr_output[j]);
}
}
/*Obtain maximun value of correlation given the
possible delay selected */
volk_32fc_magnitude_squared_32f_a(corr_output_f,
complex_acumulator, d_folding_factor);
volk_32f_index_max_16u_a(&indext, corr_output_f,
d_folding_factor);
/*Display correlation results for galileo satellites*/
/*Display correlation results for gps satellites*/
LOG_IF(INFO, (d_possible_delay[0] == 351) || (d_possible_delay[0] == 2351))
<< " Doppler: " << doppler
<< ", Mag: " << d_mag
<< ", Corr_value: "
<< corr_output_f[0] << " "
<< corr_output_f[1] << " "
<< corr_output_f[2] << " "
<< corr_output_f[3] << "\n";
/*Now save the real code phase in the gnss_syncro
block for use in other stages*/
d_gnss_synchro->Acq_delay_samples = (double)
(d_possible_delay[indext]);
d_gnss_synchro->Acq_doppler_hz = (double)doppler;
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
/* 5- Compute the test statistics and compare to the threshold
d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
d_test_statistics = d_mag / d_input_power;
//delete complex_acumulator;
}
}
// Record results to file if required
if (d_dump)
{
/*
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_ifft->get_outbuf(), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
*/
/*Since QuickSYnc performs a folded correlation in frequency by means
of the FFT, it is esential to also keep the values obtained from the
possible delay to show how it is maximize*/
std::stringstream filename;
std::streamsize n = sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_magnitude_folded, n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
}
if (!d_bit_transition_flag)
{
if (d_test_statistics > d_threshold)
{
d_state = 2; // Positive acquisition
}
else if (d_well_count == d_max_dwells)
{
d_state = 3; // Negative acquisition
}
}
else
{
if (d_well_count == d_max_dwells) // d_max_dwells = 2
{
if (d_test_statistics > d_threshold)
{
d_state = 2; // Positive acquisition
}
else
{
d_state = 3; // Negative acquisition
}
}
}
consume_each(1);
delete d_code_folded;
d_code_folded = NULL;
free(in_temp);
free(in_1code);
free(corr_output);
break;
}
case 2:
{
//DLOG(INFO) << "START CASE 2";
// 6.1- Declare positive acquisition using a message queue
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor " << d_folding_factor;
DLOG(INFO) << "possible delay";
for (int i = 0; i < (int)d_folding_factor; i++) DLOG(INFO) << d_possible_delay[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 1;
d_channel_internal_queue->push(acquisition_message);
//DLOG(INFO) << "END CASE 2";
break;
}
case 3:
{
//DLOG(INFO) << "START CASE 3";
// 6.2- Declare negative acquisition using a message queue
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor "<<d_folding_factor;
DLOG(INFO) << "possible delay ";
for (int i = 0; i < (int)d_folding_factor; i++) DLOG(INFO) << d_possible_delay[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
d_state = 0;
d_sample_counter += d_sampled_ms * d_samples_per_ms * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
acquisition_message = 2;
d_channel_internal_queue->push(acquisition_message);
//DLOG(INFO) << "END CASE 3";
break;
}
}
//DLOG(INFO) << "END GENERAL WORK";
return 0;
}

View File

@ -0,0 +1,259 @@
/*!
* \file pcps_quicksync_acquisition_cc.h
* \brief This class implements a Parallel Code Phase Search Acquisition with the
* QuickSync Algorithm
*
* Acquisition strategy (Kay Borre book CFAR + threshold).
* <ol>
* <li> Compute the input signal power estimation
* <li> Doppler serial search loop
* <li> Perform folding of the incoming signal and local generated code
* <li> Perform the FFT-based circular convolution (parallel time search)
* <li> Record the maximum peak and the associated synchronization parameters
* <li> Compute the test statistics and compare to the threshold
* <li> Declare positive or negative acquisition using a message queue
* <li> Obtain the adequate acquisition parameters by correlating the incoming
* signal shifted by the possible folded delays
* </ol>
*
* Kay Borre book: 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. pp 81-84
*
* \date Jun2 2014
* \author Damian Miralles Sanchez, dmiralles2009@gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_PCPS_QUICKSYNC_ACQUISITION_CC_H_
#define GNSS_SDR_PCPS_QUICKSYNC_ACQUISITION_CC_H_
#include <fstream>
#include <queue>
#include <string>
#include <algorithm>
#include <functional>
#include <assert.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
#include <gnuradio/gr_complex.h>
#include <gnuradio/fft/fft.h>
#include "concurrent_queue.h"
#include "gnss_synchro.h"
class pcps_quicksync_acquisition_cc;
typedef boost::shared_ptr<pcps_quicksync_acquisition_cc>
pcps_quicksync_acquisition_cc_sptr;
pcps_quicksync_acquisition_cc_sptr
pcps_quicksync_make_acquisition_cc(unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
/*!
* \brief This class implements a Parallel Code Phase Search Acquisition with
* the implementation of the Sparse QuickSync Algorithm.
*
* Check \ref Navitec2012 "Faster GPS via the Sparse Fourier Transform",
* for details of its implementation and functionality.
*/
class pcps_quicksync_acquisition_cc: public gr::block
{
private:
friend pcps_quicksync_acquisition_cc_sptr
pcps_quicksync_make_acquisition_cc(unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
pcps_quicksync_acquisition_cc(unsigned int folding_factor,
unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset);
gr_complex* d_code;
unsigned int d_folding_factor; // also referred in the paper as 'p'
float * d_corr_acumulator;
unsigned int *d_possible_delay;
float * d_magnitude_folded;
gr_complex *d_signal_folded;
gr_complex *d_code_folded;
float d_noise_floor_power;
long d_fs_in;
long d_freq;
int d_samples_per_ms;
int d_samples_per_code;
unsigned int d_doppler_resolution;
float d_threshold;
std::string d_satellite_str;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_max_dwells;
unsigned int d_well_count;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins;
gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_fft_if2;
gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
float d_input_power;
float d_test_statistics;
bool d_bit_transition_flag;
gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
int d_state;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
public:
/*!
* \brief Default destructor.
*/
~pcps_quicksync_acquisition_cc();
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code.
*/
void set_local_code(std::complex<float> * code);
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int doppler_step)
{
d_doppler_step = doppler_step;
}
/*!
* \brief Set tracking channel internal queue.
* \param channel_internal_queue - Channel's internal blocks information queue.
*/
void set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}
/*!
* \brief Parallel Code Phase Search Acquisition signal processing.
*/
int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
};
#endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/

View File

@ -61,10 +61,12 @@
#include "gps_l1_ca_pcps_tong_acquisition.h"
#include "gps_l1_ca_pcps_assisted_acquisition.h"
#include "gps_l1_ca_pcps_acquisition_fine_doppler.h"
#include "gps_l1_ca_pcps_quicksync_acquisition.h"
#include "galileo_e1_pcps_ambiguous_acquisition.h"
#include "galileo_e1_pcps_8ms_ambiguous_acquisition.h"
#include "galileo_e1_pcps_tong_ambiguous_acquisition.h"
#include "galileo_e1_pcps_cccwsr_ambiguous_acquisition.h"
#include "galileo_e1_pcps_quicksync_ambiguous_acquisition.h"
#include "gps_l1_ca_dll_pll_tracking.h"
#include "gps_l1_ca_dll_pll_optim_tracking.h"
#include "gps_l1_ca_dll_fll_pll_tracking.h"
@ -429,6 +431,12 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
out_streams, queue));
block = std::move(block_);
}
else if (implementation.compare("GPS_L1_CA_PCPS_QuickSync_Acquisition") == 0)
{
std::unique_ptr<GNSSBlockInterface> block_( new GpsL1CaPcpsQuickSyncAcquisition(configuration.get(), role, in_streams,
out_streams, queue));
block = std::move(block_);
}
else if (implementation.compare("Galileo_E1_PCPS_Ambiguous_Acquisition") == 0)
{
std::unique_ptr<GNSSBlockInterface> block_(new GalileoE1PcpsAmbiguousAcquisition(configuration.get(), role, in_streams,
@ -453,6 +461,12 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
out_streams, queue));
block = std::move(block_);
}
else if (implementation.compare("Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition") == 0)
{
std::unique_ptr<GNSSBlockInterface> block_( new GalileoE1PcpsQuickSyncAmbiguousAcquisition(configuration.get(), role, in_streams,
out_streams, queue));
block = std::move(block_);
}
// TRACKING BLOCKS -------------------------------------------------------------
else if (implementation.compare("GPS_L1_CA_DLL_PLL_Tracking") == 0)
@ -618,6 +632,12 @@ std::unique_ptr<AcquisitionInterface> GNSSBlockFactory::GetAcqBlock(
out_streams, queue));
block = std::move(block_);
}
else if (implementation.compare("GPS_L1_CA_PCPS_QuickSync_Acquisition") == 0)
{
std::unique_ptr<AcquisitionInterface> block_( new GpsL1CaPcpsQuickSyncAcquisition(configuration.get(), role, in_streams,
out_streams, queue));
block = std::move(block_);
}
else if (implementation.compare("Galileo_E1_PCPS_Ambiguous_Acquisition") == 0)
{
std::unique_ptr<AcquisitionInterface> block_(new GalileoE1PcpsAmbiguousAcquisition(configuration.get(), role, in_streams,
@ -642,6 +662,12 @@ std::unique_ptr<AcquisitionInterface> GNSSBlockFactory::GetAcqBlock(
out_streams, queue));
block = std::move(block_);
}
else if (implementation.compare("Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition") == 0)
{
std::unique_ptr<AcquisitionInterface> block_( new GalileoE1PcpsQuickSyncAmbiguousAcquisition(configuration.get(), role, in_streams,
out_streams, queue));
block = std::move(block_);
}
else
{
// Log fatal. This causes execution to stop.

View File

@ -193,11 +193,13 @@ add_executable(gnss_block_test EXCLUDE_FROM_ALL
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/gps_l1_ca_pcps_multithread_acquisition_gsoc2013_test.cc
# ${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_pcps_ambiguous_acquisition_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_pcps_ambiguous_acquisition_gsoc_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_pcps_ambiguous_acquisition_gsoc2013_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_pcps_tong_ambiguous_acquisition_gsoc2013_test.cc
#${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/galileo_e1_dll_pll_veml_tracking_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/gnss_block/file_output_filter_test.cc

View File

@ -0,0 +1,640 @@
/*!
* \file galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc
* \brief This class implements an acquisition test for
* GalileoE1PcpsAmbiguousAcquisition class.
* \author Damian Miralles, 2014. dmiralles2009@gmail.com
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <ctime>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <glog/logging.h>
#include <gnuradio/top_block.h>
#include <gnuradio/blocks/file_source.h>
#include <gnuradio/analog/sig_source_waveform.h>
#include <gnuradio/analog/sig_source_c.h>
#include <gnuradio/msg_queue.h>
#include <gnuradio/blocks/null_sink.h>
#include "gnss_block_interface.h"
#include "in_memory_configuration.h"
#include "gnss_synchro.h"
#include "signal_generator.h"
#include "signal_generator_c.h"
#include "fir_filter.h"
#include "gen_signal_source.h"
#include "gnss_sdr_valve.h"
#include "galileo_e1_pcps_quicksync_ambiguous_acquisition.h"
using google::LogMessage;
class GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test: public ::testing::Test
{
protected:
GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test()
{
queue = gr::msg_queue::make(0);
top_block = gr::make_top_block("Acquisition test");
factory = std::make_shared<GNSSBlockFactory>();
item_size = sizeof(gr_complex);
stop = false;
message = 0;
}
~GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test()
{
}
void init();
void config_1();
void config_2();
void start_queue();
void wait_message();
void process_message();
void stop_queue();
gr::msg_queue::sptr queue;
gr::top_block_sptr top_block;
//GalileoE1PcpsQuickSyncAmbiguousAcquisition *acquisition;
std::shared_ptr<GalileoE1PcpsQuickSyncAmbiguousAcquisition> acquisition;
//InMemoryConfiguration* config;
std::shared_ptr<GNSSBlockFactory> factory;
std::shared_ptr<InMemoryConfiguration> config;
Gnss_Synchro gnss_synchro;
size_t item_size;
concurrent_queue<int> channel_internal_queue;
bool stop;
int message;
boost::thread ch_thread;
unsigned int integration_time_ms;
unsigned int fs_in;
unsigned int folding_factor;
double expected_delay_chips;
double expected_doppler_hz;
float max_doppler_error_hz;
float max_delay_error_chips;
unsigned int num_of_realizations;
unsigned int realization_counter;
unsigned int detection_counter;
unsigned int correct_estimation_counter;
unsigned int acquired_samples;
unsigned int mean_acq_time_us;
double mse_doppler;
double mse_delay;
double Pd; // Probability of detection
double Pfa_p; // Probability of false alarm on present satellite
double Pfa_a; // Probability of false alarm on absent satellite
double Pmd; // Probability of miss detection
std::ofstream pdpfafile;
double threshold_config2;
unsigned int miss_detection_counter;
unsigned int CN0_dB_0;
unsigned int CN0_dB_1;
unsigned int CN0_dB_2;
unsigned int CN0_dB_3;
};
void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::init()
{
message = 0;
realization_counter = 0;
detection_counter = 0;
correct_estimation_counter = 0;
acquired_samples = 0;
mse_doppler = 0;
mse_delay = 0;
mean_acq_time_us = 0;
Pd = 0;
Pfa_p = 0;
Pfa_a = 0;
miss_detection_counter = 0;
Pmd = 0;
CN0_dB_1 = 0;
CN0_dB_2 = 0;
CN0_dB_3 = 0;
}
void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::config_1()
{
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'E';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal, 2, 0);
integration_time_ms = 16;
fs_in = 4e6;
expected_delay_chips = 600;
expected_doppler_hz = 750;
max_doppler_error_hz = 2/(3*integration_time_ms*1e-3);
max_delay_error_chips = 0.50;
num_of_realizations = 1;
config = std::make_shared<InMemoryConfiguration>();
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.item_type", "gr_complex");
config->set_property("SignalSource.num_satellites", "1");
config->set_property("SignalSource.system_0", "E");
config->set_property("SignalSource.PRN_0", "10");
config->set_property("SignalSource.CN0_dB_0", "44");
config->set_property("SignalSource.doppler_Hz_0",
std::to_string(expected_doppler_hz));
config->set_property("SignalSource.delay_chips_0",
std::to_string(expected_delay_chips));
config->set_property("SignalSource.noise_flag", "false");
config->set_property("SignalSource.data_flag", "false");
config->set_property("SignalSource.BW_BB", "0.97");
config->set_property("InputFilter.implementation", "Fir_Filter");
config->set_property("InputFilter.input_item_type", "gr_complex");
config->set_property("InputFilter.output_item_type", "gr_complex");
config->set_property("InputFilter.taps_item_type", "float");
config->set_property("InputFilter.number_of_taps", "11");
config->set_property("InputFilter.number_of_bands", "2");
config->set_property("InputFilter.band1_begin", "0.0");
config->set_property("InputFilter.band1_end", "0.97");
config->set_property("InputFilter.band2_begin", "0.98");
config->set_property("InputFilter.band2_end", "1.0");
config->set_property("InputFilter.ampl1_begin", "1.0");
config->set_property("InputFilter.ampl1_end", "1.0");
config->set_property("InputFilter.ampl2_begin", "0.0");
config->set_property("InputFilter.ampl2_end", "0.0");
config->set_property("InputFilter.band1_error", "1.0");
config->set_property("InputFilter.band2_error", "1.0");
config->set_property("InputFilter.filter_type", "bandpass");
config->set_property("InputFilter.grid_density", "16");
config->set_property("Acquisition.item_type", "gr_complex");
config->set_property("Acquisition.if", "0");
config->set_property("Acquisition.coherent_integration_time_ms",
std::to_string(integration_time_ms));
config->set_property("Acquisition.max_dwells", "1");
config->set_property("Acquisition.bit_transition_flag","false");
config->set_property("Acquisition.implementation", "Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition");
config->set_property("Acquisition.threshold", "10");
config->set_property("Acquisition.doppler_max", "10000");
config->set_property("Acquisition.doppler_step", "250");
config->set_property("Acquisition.dump", "true");
}
void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::config_2()
{
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'E';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal, 2, 0);
integration_time_ms = 16;
fs_in = 4e6;
expected_delay_chips = 600;
expected_doppler_hz = 750;
max_doppler_error_hz = 2 / (3 * integration_time_ms * 1e-3);
max_delay_error_chips = 0.50;
threshold_config2 = 3.0000;
CN0_dB_0 = 50;
num_of_realizations = 100;
config = std::make_shared<InMemoryConfiguration>();
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.item_type", "gr_complex");
config->set_property("SignalSource.num_satellites", "4");
config->set_property("SignalSource.system_0", "E");
config->set_property("SignalSource.PRN_0", "10");
config->set_property("SignalSource.CN0_dB_0",
std::to_string(CN0_dB_0));
config->set_property("SignalSource.doppler_Hz_0",
std::to_string(expected_doppler_hz));
config->set_property("SignalSource.delay_chips_0",
std::to_string(expected_delay_chips));
config->set_property("SignalSource.system_1", "E");
config->set_property("SignalSource.PRN_1", "15");
config->set_property("SignalSource.CN0_dB_1", "44");
config->set_property("SignalSource.doppler_Hz_1", "1000");
config->set_property("SignalSource.delay_chips_1", "100");
config->set_property("SignalSource.system_2", "E");
config->set_property("SignalSource.PRN_2", "21");
config->set_property("SignalSource.CN0_dB_2", "44");
config->set_property("SignalSource.doppler_Hz_2", "2000");
config->set_property("SignalSource.delay_chips_2", "200");
config->set_property("SignalSource.system_3", "E");
config->set_property("SignalSource.PRN_3", "22");
config->set_property("SignalSource.CN0_dB_3", "44");
config->set_property("SignalSource.doppler_Hz_3", "3000");
config->set_property("SignalSource.delay_chips_3", "300");
config->set_property("SignalSource.noise_flag", "true");
config->set_property("SignalSource.data_flag", "true");
config->set_property("SignalSource.BW_BB", "0.97");
config->set_property("InputFilter.implementation", "Fir_Filter");
config->set_property("InputFilter.input_item_type", "gr_complex");
config->set_property("InputFilter.output_item_type", "gr_complex");
config->set_property("InputFilter.taps_item_type", "float");
config->set_property("InputFilter.number_of_taps", "11");
config->set_property("InputFilter.number_of_bands", "2");
config->set_property("InputFilter.band1_begin", "0.0");
config->set_property("InputFilter.band1_end", "0.97");
config->set_property("InputFilter.band2_begin", "0.98");
config->set_property("InputFilter.band2_end", "1.0");
config->set_property("InputFilter.ampl1_begin", "1.0");
config->set_property("InputFilter.ampl1_end", "1.0");
config->set_property("InputFilter.ampl2_begin", "0.0");
config->set_property("InputFilter.ampl2_end", "0.0");
config->set_property("InputFilter.band1_error", "1.0");
config->set_property("InputFilter.band2_error", "1.0");
config->set_property("InputFilter.filter_type", "bandpass");
config->set_property("InputFilter.grid_density", "16");
config->set_property("Acquisition.item_type", "gr_complex");
config->set_property("Acquisition.if", "0");
config->set_property("Acquisition.coherent_integration_time_ms",
std::to_string(integration_time_ms));
config->set_property("Acquisition.max_dwells", "1");
config->set_property("Acquisition.bit_transition_flag","false");
config->set_property("Acquisition.implementation", "Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition");
config->set_property("Acquisition.threshold", std::to_string(threshold_config2));
config->set_property("Acquisition.doppler_max", "10000");
config->set_property("Acquisition.doppler_step", "250");
config->set_property("Acquisition.dump", "false");
}
void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::start_queue()
{
stop = false;
ch_thread = boost::thread(&GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::wait_message, this);
}
void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::wait_message()
{
struct timeval tv;
long long int begin = 0;
long long int end = 0;
while (!stop)
{
acquisition->reset();
gettimeofday(&tv, NULL);
begin = tv.tv_sec*1e6 + tv.tv_usec;
channel_internal_queue.wait_and_pop(message);
gettimeofday(&tv, NULL);
end = tv.tv_sec*1e6 + tv.tv_usec;
mean_acq_time_us += (end - begin);
process_message();
}
}
void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::process_message()
{
if (message == 1)
{
detection_counter++;
// The term -5 is here to correct the additional delay introduced by the FIR filter
double delay_error_chips = abs((double)expected_delay_chips - (double)(gnss_synchro.Acq_delay_samples - 5) * 1023.0 / ((double)fs_in * 1e-3));
double doppler_error_hz = abs(expected_doppler_hz - gnss_synchro.Acq_doppler_hz);
mse_delay += std::pow(delay_error_chips, 2);
mse_doppler += std::pow(doppler_error_hz, 2);
if ((delay_error_chips < max_delay_error_chips) && (doppler_error_hz < max_doppler_error_hz))
{
correct_estimation_counter++;
}
}
else if(message == 2 && gnss_synchro.PRN == 10)
{
/*
if ((delay_error_chips < max_delay_error_chips) && (doppler_error_hz < max_doppler_error_hz))
{
miss_detection_counter++;
}
*/
miss_detection_counter++;
}
realization_counter++;
std::cout << "Progress: " << round((float)realization_counter / num_of_realizations * 100) << "% \r" << std::flush;
if (realization_counter == num_of_realizations)
{
mse_delay /= (double)num_of_realizations;
mse_doppler /= (double)num_of_realizations;
Pd = (double)correct_estimation_counter / (double)num_of_realizations;
Pfa_a = (double)detection_counter / (double)num_of_realizations;
Pfa_p = (double)(detection_counter-correct_estimation_counter) / (double)num_of_realizations;
Pmd = (double)miss_detection_counter / (double)num_of_realizations;
mean_acq_time_us /= (double)num_of_realizations;
stop_queue();
top_block->stop();
}
}
void GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test::stop_queue()
{
stop = true;
}
TEST_F(GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test, Instantiate)
{
config_1();
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(config, "Acquisition", "Galileo_E1_PCPS_Quicksync_Ambiguous_Acquisition", 1, 1, queue);
acquisition = std::dynamic_pointer_cast<GalileoE1PcpsQuickSyncAmbiguousAcquisition>(acq_);
}
TEST_F(GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test, ConnectAndRun)
{
LOG(INFO)<<"**Start connect and run test";
int nsamples = floor(fs_in*integration_time_ms*1e-3);
struct timeval tv;
long long int begin = 0;
long long int end = 0;
config_1();
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(config, "Acquisition", "Galileo_E1_PCPS_Quicksync_Ambiguous_Acquisition", 1, 1, queue);
acquisition = std::dynamic_pointer_cast<GalileoE1PcpsQuickSyncAmbiguousAcquisition>(acq_);
ASSERT_NO_THROW( {
acquisition->connect(top_block);
boost::shared_ptr<gr::analog::sig_source_c> source =
gr::analog::sig_source_c::make(fs_in, gr::analog::GR_SIN_WAVE, 1000, 1, gr_complex(0));
boost::shared_ptr<gr::block> valve =
gnss_sdr_make_valve(sizeof(gr_complex), nsamples, queue);
top_block->connect(source, 0, valve, 0);
top_block->connect(valve, 0, acquisition->get_left_block(), 0);
}) << "Failure connecting the blocks of acquisition test."<< std::endl;
EXPECT_NO_THROW( {
gettimeofday(&tv, NULL);
begin = tv.tv_sec * 1e6 + tv.tv_usec;
top_block->run(); // Start threads and wait
gettimeofday(&tv, NULL);
end = tv.tv_sec * 1e6 + tv.tv_usec;
}) << "Failure running the top_block."<< std::endl;
std::cout << "Processed " << nsamples << " samples in " << (end - begin) << " microseconds" << std::endl;
LOG(INFO) <<"----end connect and run test-----";
LOG(INFO) <<"**End connect and run test";
}
TEST_F(GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test, ValidationOfResults)
{
LOG(INFO)<<"Start validation of results test";
config_1();
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(config, "Acquisition", "Galileo_E1_PCPS_Quicksync_Ambiguous_Acquisition", 1, 1, queue);
acquisition = std::dynamic_pointer_cast<GalileoE1PcpsQuickSyncAmbiguousAcquisition>(acq_);
ASSERT_NO_THROW( {
acquisition->set_channel(1);
}) << "Failure setting channel."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_gnss_synchro(&gnss_synchro);
}) << "Failure setting gnss_synchro."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_channel_queue(&channel_internal_queue);
}) << "Failure setting channel_internal_queue."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 10000));
}) << "Failure setting doppler_max."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 125));
}) << "Failure setting doppler_step."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_threshold(config->property("Acquisition.threshold", 0.0));
}) << "Failure setting threshold."<< std::endl;
ASSERT_NO_THROW( {
acquisition->connect(top_block);
}) << "Failure connecting acquisition to the top_block." << std::endl;
acquisition->init();
ASSERT_NO_THROW( {
boost::shared_ptr<GenSignalSource> signal_source;
SignalGenerator* signal_generator = new SignalGenerator(config.get(), "SignalSource", 0, 1, queue);
FirFilter* filter = new FirFilter(config.get(), "InputFilter", 1, 1, queue);
signal_source.reset(new GenSignalSource(config.get(), signal_generator, filter, "SignalSource", queue));
signal_source->connect(top_block);
top_block->connect(signal_source->get_right_block(), 0, acquisition->get_left_block(), 0);
}) << "Failure connecting the blocks of acquisition test." << std::endl;
// i = 0 --> satellite in acquisition is visible
// i = 1 --> satellite in acquisition is not visible
for (unsigned int i = 0; i < 2; i++)
{
init();
if (i == 0)
{
gnss_synchro.PRN = 10; // This satellite is visible
}
else if (i == 1)
{
gnss_synchro.PRN = 20; // This satellite is not visible
}
acquisition->set_local_code();
folding_factor = acquisition->get_folding_factor();
start_queue();
EXPECT_NO_THROW( {
top_block->run(); // Start threads and wait
}) << "Failure running the top_block."<< std::endl;
if (i == 0)
{
EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS.";
if (message == 1)
{
EXPECT_EQ((unsigned int)1, correct_estimation_counter) << "Acquisition failure. Incorrect parameters estimation.";
}
}
else if (i == 1)
{
EXPECT_EQ(2, message) << "Acquisition failure. Expected message: 2=ACQ FAIL.";
}
}
LOG(INFO) << "End validation of results test";
}
TEST_F(GalileoE1PcpsQuickSyncAmbiguousAcquisitionGSoC2014Test, ValidationOfResultsProbabilities)
{
config_2();
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(config, "Acquisition", "Galileo_E1_PCPS_Quicksync_Ambiguous_Acquisition", 1, 1, queue);
acquisition = std::dynamic_pointer_cast<GalileoE1PcpsQuickSyncAmbiguousAcquisition>(acq_);
ASSERT_NO_THROW( {
acquisition->set_channel(1);
}) << "Failure setting channel."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_gnss_synchro(&gnss_synchro);
}) << "Failure setting gnss_synchro."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_channel_queue(&channel_internal_queue);
}) << "Failure setting channel_internal_queue."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 10000));
}) << "Failure setting doppler_max."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 500));
}) << "Failure setting doppler_step."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_threshold(config->property("Acquisition.threshold", 0.0));
}) << "Failure setting threshold."<< std::endl;
ASSERT_NO_THROW( {
acquisition->connect(top_block);
}) << "Failure connecting acquisition to the top_block." << std::endl;
acquisition->init();
ASSERT_NO_THROW( {
boost::shared_ptr<GenSignalSource> signal_source;
SignalGenerator* signal_generator = new SignalGenerator(config.get(), "SignalSource", 0, 1, queue);
FirFilter* filter = new FirFilter(config.get(), "InputFilter", 1, 1, queue);
signal_source.reset(new GenSignalSource(config.get(), signal_generator, filter, "SignalSource", queue));
signal_source->connect(top_block);
top_block->connect(signal_source->get_right_block(), 0, acquisition->get_left_block(), 0);
}) << "Failure connecting the blocks of acquisition test." << std::endl;
std::cout << "Probability of false alarm (target) = " << 0.1 << std::endl;
// i = 0 --> satellite in acquisition is visible (prob of detection and prob of detection with wrong estimation)
// i = 1 --> satellite in acquisition is not visible (prob of false detection)
for (unsigned int i = 0; i < 2; i++)
{
init();
if (i == 0)
{
gnss_synchro.PRN = 10; // This satellite is visible
}
else if (i == 1)
{
gnss_synchro.PRN = 20; // This satellite is not visible
}
acquisition->set_local_code();
start_queue();
EXPECT_NO_THROW( {
top_block->run(); // Start threads and wait
}) << "Failure running the top_block." << std::endl;
if (i == 0)
{
std::cout << "Estimated probability of detection = " << Pd << std::endl;
std::cout << "Estimated probability of false alarm (satellite present) = " << Pfa_p << std::endl;
std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl;
std::stringstream filenamepd;
filenamepd.str("");
filenamepd << "../data/test_statistics_" << gnss_synchro.System
<<"_" << gnss_synchro.Signal << "_sat_"
<< gnss_synchro.PRN << "CN0_dB_0_" << CN0_dB_0 << "_dBHz.csv";
pdpfafile.open(filenamepd.str().c_str(), std::ios::app | std::ios::out);
pdpfafile << threshold_config2 << "," << Pd << "," << Pfa_p << "," << Pmd << std::endl;
pdpfafile.close();
}
else if (i == 1)
{
std::cout << "Estimated probability of false alarm (satellite absent) = " << Pfa_a << std::endl;
std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl;
std::stringstream filenamepf;
filenamepf.str("");
filenamepf << "../data/test_statistics_" << gnss_synchro.System
<< "_" << gnss_synchro.Signal << "_sat_"
<< gnss_synchro.PRN << "CN0_dB_0_" << CN0_dB_0 << "_dBHz.csv";
pdpfafile.open(filenamepf.str().c_str(), std::ios::app | std::ios::out);
pdpfafile << threshold_config2 << "," << Pfa_a << std::endl;
pdpfafile.close();
}
}
}

View File

@ -20,7 +20,7 @@
* 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.
* (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
@ -194,6 +194,31 @@ TEST(GNSS_Block_Factory_Test, InstantiateGpsL1CaPcpsAcquisition)
}
TEST(GNSS_Block_Factory_Test, InstantiateGpsL1CaPcpsQuickSyncAcquisition)
{
std::shared_ptr<InMemoryConfiguration> configuration = std::make_shared<InMemoryConfiguration>();
configuration->set_property("Acquisition.implementation", "GPS_L1_CA_PCPS_QuickSync_Acquisition");
gr::msg_queue::sptr queue = gr::msg_queue::make(0);
std::shared_ptr<GNSSBlockFactory> factory = std::make_shared<GNSSBlockFactory>();
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(configuration, "Acquisition", "GPS_L1_CA_PCPS_QuickSync_Acquisition", 1, 1, queue);
std::shared_ptr<AcquisitionInterface> acquisition = std::dynamic_pointer_cast<AcquisitionInterface>(acq_);
EXPECT_STREQ("Acquisition", acquisition->role().c_str());
EXPECT_STREQ("GPS_L1_CA_PCPS_QuickSync_Acquisition", acquisition->implementation().c_str());
}
TEST(GNSS_Block_Factory_Test, InstantiateGalileoE1PcpsQuickSyncAmbiguousAcquisition)
{
std::shared_ptr<InMemoryConfiguration> configuration = std::make_shared<InMemoryConfiguration>();
configuration->set_property("Acquisition.implementation", "Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition");
gr::msg_queue::sptr queue = gr::msg_queue::make(0);
std::shared_ptr<GNSSBlockFactory> factory = std::make_shared<GNSSBlockFactory>();
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(configuration, "Acquisition", "Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition", 1, 1, queue);
std::shared_ptr<AcquisitionInterface> acquisition = std::dynamic_pointer_cast<AcquisitionInterface>(acq_);
EXPECT_STREQ("Acquisition", acquisition->role().c_str());
EXPECT_STREQ("Galileo_E1_PCPS_QuickSync_Ambiguous_Acquisition", acquisition->implementation().c_str());
}
TEST(GNSS_Block_Factory_Test, InstantiateGalileoE1PcpsAmbiguousAcquisition)
{
std::shared_ptr<InMemoryConfiguration> configuration = std::make_shared<InMemoryConfiguration>();

View File

@ -0,0 +1,625 @@
/*!
* \file gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc
* \brief This class implements an acquisition test for
* GpsL1CaPcpsQuickSyncAcquisition class based on some input parameters.
* \author Damian Miralles Sanchez, 2014. dmiralles2009(at)gmail.com
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <ctime>
#include <iostream>
#include <stdexcept>
#include <glog/logging.h>
#include <gnuradio/top_block.h>
#include <gnuradio/blocks/file_source.h>
#include <gnuradio/analog/sig_source_waveform.h>
#include <gnuradio/analog/sig_source_c.h>
#include <gnuradio/msg_queue.h>
#include <gnuradio/blocks/null_sink.h>
#include "gnss_block_factory.h"
#include "gnss_block_interface.h"
#include "in_memory_configuration.h"
#include "gnss_sdr_valve.h"
#include "gnss_synchro.h"
#include "gps_l1_ca_pcps_quicksync_acquisition.h"
using google::LogMessage;
class GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test: public ::testing::Test
{
protected:
GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test()
{
queue = gr::msg_queue::make(0);
top_block = gr::make_top_block("Acquisition test");
factory = std::make_shared<GNSSBlockFactory>();
item_size = sizeof(gr_complex);
stop = false;
message = 0;
}
~GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test()
{}
void init();
void config_1();
void config_2();
void start_queue();
void wait_message();
void process_message();
void stop_queue();
gr::msg_queue::sptr queue;
gr::top_block_sptr top_block;
std::shared_ptr<GNSSBlockFactory> factory;
std::shared_ptr<GpsL1CaPcpsQuickSyncAcquisition> acquisition;
std::shared_ptr<InMemoryConfiguration> config;
Gnss_Synchro gnss_synchro;
size_t item_size;
concurrent_queue<int> channel_internal_queue;
bool stop;
int message;
boost::thread ch_thread;
unsigned int integration_time_ms;
unsigned int fs_in;
unsigned int folding_factor;
double expected_delay_chips;
double expected_doppler_hz;
float max_doppler_error_hz;
float max_delay_error_chips;
unsigned int num_of_realizations;
unsigned int realization_counter;
unsigned int detection_counter;
unsigned int correct_estimation_counter;
unsigned int acquired_samples;
unsigned int mean_acq_time_us;
double mse_doppler;
double mse_delay;
double Pd;
double Pfa_p;
double Pfa_a;
double Pmd;
std::ofstream pdpfafile;
double threshold_config2;
unsigned int miss_detection_counter;
unsigned int CN0_dB_0;
};
void GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::init()
{
message = 0;
realization_counter = 0;
detection_counter = 0;
correct_estimation_counter = 0;
acquired_samples = 0;
mse_doppler = 0;
mse_delay = 0;
mean_acq_time_us = 0;
Pd = 0;
Pfa_p = 0;
Pfa_a = 0;
miss_detection_counter = 0;
Pmd = 0;
}
void GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::config_1()
{
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'G';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal,2,0);
integration_time_ms = 4;
fs_in = 4e6;
expected_delay_chips = 600;
expected_doppler_hz = 750;
max_doppler_error_hz = 2/(3*integration_time_ms*1e-3);
max_delay_error_chips = 0.50;
num_of_realizations = 1;
config = std::make_shared<InMemoryConfiguration>();
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.item_type", "gr_complex");
config->set_property("SignalSource.num_satellites", "1");
config->set_property("SignalSource.repeat", "true");
config->set_property("SignalSource.system_0", "G");
config->set_property("SignalSource.PRN_0", "10");
config->set_property("SignalSource.CN0_dB_0", "44");
config->set_property("SignalSource.doppler_Hz_0", std::to_string(expected_doppler_hz));
config->set_property("SignalSource.delay_chips_0", std::to_string(expected_delay_chips));
config->set_property("SignalSource.noise_flag", "false");
config->set_property("SignalSource.data_flag", "false");
config->set_property("SignalSource.BW_BB", "0.97");
config->set_property("InputFilter.implementation", "Fir_Filter");
config->set_property("InputFilter.input_item_type", "gr_complex");
config->set_property("InputFilter.output_item_type", "gr_complex");
config->set_property("InputFilter.taps_item_type", "float");
config->set_property("InputFilter.number_of_taps", "11");
config->set_property("InputFilter.number_of_bands", "2");
config->set_property("InputFilter.band1_begin", "0.0");
config->set_property("InputFilter.band1_end", "0.97");
config->set_property("InputFilter.band2_begin", "0.98");
config->set_property("InputFilter.band2_end", "1.0");
config->set_property("InputFilter.ampl1_begin", "1.0");
config->set_property("InputFilter.ampl1_end", "1.0");
config->set_property("InputFilter.ampl2_begin", "0.0");
config->set_property("InputFilter.ampl2_end", "0.0");
config->set_property("InputFilter.band1_error", "1.0");
config->set_property("InputFilter.band2_error", "1.0");
config->set_property("InputFilter.filter_type", "bandpass");
config->set_property("InputFilter.grid_density", "16");
config->set_property("Acquisition.item_type", "gr_complex");
config->set_property("Acquisition.if", "0");
config->set_property("Acquisition.coherent_integration_time_ms",
std::to_string(integration_time_ms));
config->set_property("Acquisition.max_dwells", "1");
config->set_property("Acquisition.implementation", "GPS_L1_CA_PCPS_QuickSync_Acquisition");
config->set_property("Acquisition.threshold", "100");
config->set_property("Acquisition.doppler_max", "10000");
config->set_property("Acquisition.doppler_step", "250");
config->set_property("Acquisition.bit_transition_flag", "false");
config->set_property("Acquisition.dump", "true");
}
void GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::config_2()
{
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'G';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal,2,0);
integration_time_ms = 4;
fs_in = 4e6;
expected_delay_chips = 600;
expected_doppler_hz = 750;
max_doppler_error_hz = 2/(3*integration_time_ms*1e-3);
max_delay_error_chips = 0.50;
threshold_config2 = 8.0000;
CN0_dB_0 = 50;
num_of_realizations = 100;
config = std::make_shared<InMemoryConfiguration>();
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.item_type", "gr_complex");
config->set_property("SignalSource.num_satellites", "4");
config->set_property("SignalSource.system_0", "G");
config->set_property("SignalSource.PRN_0", "10");
config->set_property("SignalSource.CN0_dB_0", std::to_string(CN0_dB_0));
config->set_property("SignalSource.doppler_Hz_0", std::to_string(expected_doppler_hz));
config->set_property("SignalSource.delay_chips_0", std::to_string(expected_delay_chips));
config->set_property("SignalSource.system_1", "G");
config->set_property("SignalSource.PRN_1", "15");
config->set_property("SignalSource.CN0_dB_1", "44");
config->set_property("SignalSource.doppler_Hz_1", "1000");
config->set_property("SignalSource.delay_chips_1", "100");
config->set_property("SignalSource.system_2", "G");
config->set_property("SignalSource.PRN_2", "21");
config->set_property("SignalSource.CN0_dB_2", "44");
config->set_property("SignalSource.doppler_Hz_2", "2000");
config->set_property("SignalSource.delay_chips_2", "200");
config->set_property("SignalSource.system_3", "G");
config->set_property("SignalSource.PRN_3", "22");
config->set_property("SignalSource.CN0_dB_3", "44");
config->set_property("SignalSource.doppler_Hz_3", "3000");
config->set_property("SignalSource.delay_chips_3", "300");
config->set_property("SignalSource.noise_flag", "true");
config->set_property("SignalSource.data_flag", "true");
config->set_property("SignalSource.BW_BB", "0.97");
config->set_property("InputFilter.implementation", "Fir_Filter");
config->set_property("InputFilter.input_item_type", "gr_complex");
config->set_property("InputFilter.output_item_type", "gr_complex");
config->set_property("InputFilter.taps_item_type", "float");
config->set_property("InputFilter.number_of_taps", "11");
config->set_property("InputFilter.number_of_bands", "2");
config->set_property("InputFilter.band1_begin", "0.0");
config->set_property("InputFilter.band1_end", "0.97");
config->set_property("InputFilter.band2_begin", "0.98");
config->set_property("InputFilter.band2_end", "1.0");
config->set_property("InputFilter.ampl1_begin", "1.0");
config->set_property("InputFilter.ampl1_end", "1.0");
config->set_property("InputFilter.ampl2_begin", "0.0");
config->set_property("InputFilter.ampl2_end", "0.0");
config->set_property("InputFilter.band1_error", "1.0");
config->set_property("InputFilter.band2_error", "1.0");
config->set_property("InputFilter.filter_type", "bandpass");
config->set_property("InputFilter.grid_density", "16");
config->set_property("Acquisition.item_type", "gr_complex");
config->set_property("Acquisition.if", "0");
config->set_property("Acquisition.coherent_integration_time_ms",
std::to_string(integration_time_ms));
config->set_property("Acquisition.max_dwells", "1");
config->set_property("Acquisition.implementation", "GPS_L1_CA_PCPS_QuickSync_Acquisition");
config->set_property("Acquisition.threshold", std::to_string(threshold_config2));
config->set_property("Acquisition.doppler_max", "10000");
config->set_property("Acquisition.doppler_step", "250");
config->set_property("Acquisition.bit_transition_flag", "false");
config->set_property("Acquisition.dump", "false");
}
void GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::start_queue()
{
stop = false;
ch_thread = boost::thread(&GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::wait_message, this);
}
void GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::wait_message()
{
struct timeval tv;
long long int begin = 0;
long long int end = 0;
while (!stop)
{
acquisition->reset();
gettimeofday(&tv, NULL);
begin = tv.tv_sec * 1e6 + tv.tv_usec;
channel_internal_queue.wait_and_pop(message);
gettimeofday(&tv, NULL);
end = tv.tv_sec * 1e6 + tv.tv_usec;
mean_acq_time_us += (end - begin);
process_message();
}
}
void GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::process_message()
{
if (message == 1)
{
detection_counter++;
// The term -5 is here to correct the additional delay introduced by the FIR filter
double delay_error_chips = abs((double)expected_delay_chips - (double)(gnss_synchro.Acq_delay_samples - 5) * 1023.0/ ((double)fs_in * 1e-3));
double doppler_error_hz = abs(expected_doppler_hz - gnss_synchro.Acq_doppler_hz);
mse_delay += std::pow(delay_error_chips, 2);
mse_doppler += std::pow(doppler_error_hz, 2);
if ((delay_error_chips < max_delay_error_chips) && (doppler_error_hz < max_doppler_error_hz))
{
correct_estimation_counter++;
}
}
else if(message == 2 && gnss_synchro.PRN == 10)
{
miss_detection_counter++;
}
realization_counter++;
std::cout << "Progress: " << round((float)realization_counter/num_of_realizations*100) << "% \r" << std::flush;
if (realization_counter == num_of_realizations)
{
mse_delay /= num_of_realizations;
mse_doppler /= num_of_realizations;
Pd = (double)correct_estimation_counter / (double)num_of_realizations;
Pfa_a = (double)detection_counter / (double)num_of_realizations;
Pfa_p = (double)(detection_counter-correct_estimation_counter) / (double)num_of_realizations;
Pmd = (double)miss_detection_counter / (double)num_of_realizations;
mean_acq_time_us /= num_of_realizations;
stop_queue();
top_block->stop();
}
}
void GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test::stop_queue()
{
stop = true;
}
TEST_F(GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test, Instantiate)
{
config_1();
acquisition = std::make_shared<GpsL1CaPcpsQuickSyncAcquisition>(config.get(), "Acquisition", 1, 1, queue);
}
TEST_F(GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test, ConnectAndRun)
{
int nsamples = floor(fs_in*integration_time_ms*1e-3);
struct timeval tv;
long long int begin = 0;
long long int end = 0;
config_1();
acquisition = std::make_shared<GpsL1CaPcpsQuickSyncAcquisition>(config.get(), "Acquisition", 1, 1, queue);
ASSERT_NO_THROW( {
acquisition->connect(top_block);
boost::shared_ptr<gr::analog::sig_source_c> source = gr::analog::sig_source_c::make(fs_in, gr::analog::GR_SIN_WAVE, 1000, 1, gr_complex(0));
boost::shared_ptr<gr::block> valve = gnss_sdr_make_valve(sizeof(gr_complex), nsamples, queue);
top_block->connect(source, 0, valve, 0);
top_block->connect(valve, 0, acquisition->get_left_block(), 0);
}) << "Failure connecting the blocks of acquisition test."<< std::endl;
EXPECT_NO_THROW( {
gettimeofday(&tv, NULL);
begin = tv.tv_sec *1e6 + tv.tv_usec;
top_block->run(); // Start threads and wait
gettimeofday(&tv, NULL);
end = tv.tv_sec *1e6 + tv.tv_usec;
}) << "Failure running the top_block."<< std::endl;
std::cout << "Processed " << nsamples << " samples in " << (end-begin) << " microseconds" << std::endl;
}
TEST_F(GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test, ValidationOfResults)
{
config_1();
acquisition = std::make_shared<GpsL1CaPcpsQuickSyncAcquisition>(config.get(), "Acquisition", 1, 1, queue);
ASSERT_NO_THROW( {
acquisition->set_channel(1);
}) << "Failure setting channel."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_gnss_synchro(&gnss_synchro);
}) << "Failure setting gnss_synchro."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_channel_queue(&channel_internal_queue);
}) << "Failure setting channel_internal_queue."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 10000));
}) << "Failure setting doppler_max."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 250));
}) << "Failure setting doppler_step."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_threshold(config->property("Acquisition.threshold", 0.0));
}) << "Failure setting threshold."<< std::endl;
ASSERT_NO_THROW( {
acquisition->connect(top_block);
}) << "Failure connecting acquisition to the top_block."<< std::endl;
acquisition->init();
ASSERT_NO_THROW( {
boost::shared_ptr<GenSignalSource> signal_source;
SignalGenerator* signal_generator = new SignalGenerator(config.get(), "SignalSource", 0, 1, queue);
FirFilter* filter = new FirFilter(config.get(), "InputFilter", 1, 1, queue);
signal_source.reset(new GenSignalSource(config.get(), signal_generator, filter, "SignalSource", queue));
signal_source->connect(top_block);
top_block->connect(signal_source->get_right_block(), 0, acquisition->get_left_block(), 0);
}) << "Failure connecting the blocks of acquisition test." << std::endl;
// i = 0 --> satellite in acquisition is visible
// i = 1 --> satellite in acquisition is not visible
for (unsigned int i = 0; i < 2; i++)
{
init();
if (i == 0)
{
gnss_synchro.PRN = 10; // This satellite is visible
}
else if (i == 1)
{
gnss_synchro.PRN = 20; // This satellite is not visible
}
acquisition->set_local_code();
folding_factor = acquisition->get_folding_factor();
start_queue();
EXPECT_NO_THROW( {
top_block->run(); // Start threads and wait
}) << "Failure running the top_block." << std::endl;
if (i == 0)
{
EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS.";
if (message == 1)
{
EXPECT_EQ((unsigned int)1, correct_estimation_counter)
<< "Acquisition failure. Incorrect parameters estimation.";
}
}
else if (i == 1)
{
EXPECT_EQ(2, message)
<< "Acquisition failure. Expected message: 2=ACQ FAIL.";
}
}
unsigned long int nsamples = gnss_synchro.Acq_samplestamp_samples;
std::cout << "----Acquired: " << nsamples << " samples"<< std::endl;
}
TEST_F(GpsL1CaPcpsQuickSyncAcquisitionGSoC2014Test, ValidationOfResultsProbabilities)
{
config_2();
acquisition = std::make_shared<GpsL1CaPcpsQuickSyncAcquisition>(config.get(), "Acquisition", 1, 1, queue);
ASSERT_NO_THROW( {
acquisition->set_channel(1);
}) << "Failure setting channel."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_gnss_synchro(&gnss_synchro);
}) << "Failure setting gnss_synchro."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_channel_queue(&channel_internal_queue);
}) << "Failure setting channel_internal_queue."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 10000));
}) << "Failure setting doppler_max."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 500));
}) << "Failure setting doppler_step."<< std::endl;
ASSERT_NO_THROW( {
acquisition->set_threshold(config->property("Acquisition.threshold", 0.0));
}) << "Failure setting threshold."<< std::endl;
ASSERT_NO_THROW( {
acquisition->connect(top_block);
}) << "Failure connecting acquisition to the top_block."<< std::endl;
acquisition->init();
ASSERT_NO_THROW( {
boost::shared_ptr<GenSignalSource> signal_source;
SignalGenerator* signal_generator = new SignalGenerator(config.get(), "SignalSource", 0, 1, queue);
FirFilter* filter = new FirFilter(config.get(), "InputFilter", 1, 1, queue);
signal_source.reset(new GenSignalSource(config.get(), signal_generator, filter, "SignalSource", queue));
signal_source->connect(top_block);
top_block->connect(signal_source->get_right_block(), 0, acquisition->get_left_block(), 0);
}) << "Failure connecting the blocks of acquisition test." << std::endl;
std::cout << "Probability of false alarm (target) = " << 0.1 << std::endl;
// i = 0 --> satellite in acquisition is visible (prob of detection and prob of detection with wrong estimation)
// i = 1 --> satellite in acquisition is not visible (prob of false detection)
for (unsigned int i = 0; i < 2; i++)
{
init();
if (i == 0)
{
gnss_synchro.PRN = 10; // This satellite is visible
}
else if (i == 1)
{
gnss_synchro.PRN = 20; // This satellite is not visible
}
acquisition->set_local_code();
start_queue();
EXPECT_NO_THROW( {
top_block->run(); // Start threads and wait
}) << "Failure running the top_block." << std::endl;
if (i == 0)
{
std::cout << "Estimated probability of detection = " << Pd << std::endl;
std::cout << "Estimated probability of false alarm (satellite present) = " << Pfa_p << std::endl;
std::cout << "Estimated probability of miss detection (satellite present) = " << Pmd << std::endl;
std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl;
std::stringstream filenamepd;
filenamepd.str("");
filenamepd << "../data/test_statistics_" << gnss_synchro.System
<< "_" << gnss_synchro.Signal << "_sat_"
<< gnss_synchro.PRN << "CN0_dB_0_" << CN0_dB_0 << "_dBHz.csv";
pdpfafile.open(filenamepd.str().c_str(), std::ios::app | std::ios::out);
pdpfafile << threshold_config2 << "," << Pd << "," << Pfa_p << "," << Pmd << std::endl;
pdpfafile.close();
}
else if (i == 1)
{
std::cout << "Estimated probability of false alarm (satellite absent) = " << Pfa_a << std::endl;
std::cout << "Mean acq time = " << mean_acq_time_us << " microseconds." << std::endl;
std::stringstream filenamepf;
filenamepf.str("");
filenamepf << "../data/test_statistics_" << gnss_synchro.System
<< "_" << gnss_synchro.Signal << "_sat_"
<< gnss_synchro.PRN << "CN0_dB_0_" << CN0_dB_0 << "_dBHz.csv";
std::cout << filenamepf.str().c_str() << std::endl;
pdpfafile.open(filenamepf.str().c_str(), std::ios::app | std::ios::out);
if (pdpfafile.is_open())
{
std::cout << "File successfully open" << std::endl;
pdpfafile << threshold_config2 << "," << Pfa_a << std::endl;
pdpfafile.close();
}
else
{
std::cout << "Error opening file" << std::endl;
}
}
}
}

View File

@ -90,6 +90,7 @@ DECLARE_string(log_dir);
#if OPENCL_BLOCKS_TEST
#include "gnss_block/gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc"
#endif
#include "gnss_block/gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc"
#include "gnss_block/gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc"
#include "gnss_block/galileo_e1_pcps_ambiguous_acquisition_test.cc"
#include "gnss_block/galileo_e1_pcps_ambiguous_acquisition_gsoc_test.cc"
@ -97,6 +98,7 @@ DECLARE_string(log_dir);
#include "gnss_block/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc"
#include "gnss_block/galileo_e1_pcps_tong_ambiguous_acquisition_gsoc2013_test.cc"
#include "gnss_block/galileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc"
#include "gnss_block/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc"
#include "gnss_block/galileo_e1_dll_pll_veml_tracking_test.cc"
#include "gnuradio_block/gnss_sdr_valve_test.cc"
#include "gnuradio_block/direct_resampler_conditioner_cc_test.cc"