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

579 lines
27 KiB
C++
Raw Blame History

/*!
* \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-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "pcps_quicksync_acquisition_cc.h"
#include "control_message_factory.h"
#include "GPS_L1_CA.h"
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <volk/volk.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <cmath>
#include <sstream>
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,
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,
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,
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)))
{
this->message_port_register_out(pmt::mp("events"));
d_sample_counter = 0; // SAMPLE COUNTER
d_active = false;
d_state = 0;
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;
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(float), volk_gnsssdr_get_alignment()));
d_magnitude_folded = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_possible_delay = new unsigned int[d_folding_factor];
d_corr_output_f = new float[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;
d_corr_acumulator = 0;
d_signal_folded = 0;
d_code_folded = new gr_complex[d_fft_size]();
d_noise_floor_power = 0;
d_doppler_resolution = 0;
d_threshold = 0;
d_doppler_step = 0;
d_grid_doppler_wipeoffs = 0;
d_fft_if2 = 0;
d_gnss_synchro = 0;
d_code_phase = 0;
d_doppler_freq = 0;
d_test_statistics = 0;
d_channel = 0;
//d_code_folded = 0;
// 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++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
}
delete[] d_grid_doppler_wipeoffs;
}
volk_gnsssdr_free(d_fft_codes);
volk_gnsssdr_free(d_magnitude);
volk_gnsssdr_free(d_magnitude_folded);
delete d_ifft;
delete d_fft_if;
delete d_code;
delete d_possible_delay;
delete d_corr_output_f;
delete[] d_code_folded;
if (d_dump)
{
d_dump_file.close();
}
// DLOG(INFO) << "END DESTROYER";
}
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* 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
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
void pcps_quicksync_acquisition_cc::init()
{
d_gnss_synchro->Flag_valid_acquisition = false;
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
//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;
if (d_doppler_step == 0) d_doppler_step = 250;
// Count the number of bins
d_num_doppler_bins = 0;
for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= static_cast<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++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * (d_freq + doppler) / static_cast<float>(d_fs_in);
float _phase[1];
_phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index], -phase_step_rad, _phase, d_samples_per_code * d_folding_factor);
}
// DLOG(INFO) << "end init";
}
void pcps_quicksync_acquisition_cc::set_state(int state)
{
d_state = state;
if (d_state == 1)
{
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_active = 1;
}
else if (d_state == 0)
{
}
else
{
LOG(ERROR) << "State can only be set to 0 or 1";
}
}
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 __attribute__((unused)))
{
/*
* 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;
uint32_t indext = 0;
float magt = 0.0;
const gr_complex* in = reinterpret_cast<const gr_complex*>(input_items[0]); //Get the input samples pointer
gr_complex* in_temp = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
gr_complex* in_temp_folded = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
/*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 = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
/*Stores the values of the correlation output between the local code
and the signal with doppler shift corrected */
gr_complex* corr_output = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_samples_per_code * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
/*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 = static_cast<float>(d_fft_size) * static_cast<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(d_magnitude, in, d_samples_per_code * d_folding_factor);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
d_input_power /= static_cast<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 = -static_cast<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(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 < static_cast<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(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(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_gnsssdr_32f_index_max_32u(&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);
gr_complex complex_acumulator[100];
//gr_complex complex_acumulator[d_folding_factor];
for (int i = 0; i < static_cast<int>(d_folding_factor); i++)
{
d_possible_delay[i] = detected_delay_samples_folded + (i)*d_fft_size;
}
for (int i = 0; i < static_cast<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(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(d_corr_output_f, complex_acumulator, d_folding_factor);
volk_gnsssdr_32f_index_max_32u(&indext, d_corr_output_f, d_folding_factor);
/*Now save the real code phase in the gnss_syncro block for use in other stages*/
d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
d_gnss_synchro->Acq_doppler_hz = static_cast<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)
{
/*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(reinterpret_cast<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
}
}
}
volk_gnsssdr_free(in_temp);
volk_gnsssdr_free(in_temp_folded);
volk_gnsssdr_free(in_1code);
volk_gnsssdr_free(corr_output);
consume_each(1);
break;
}
case 2:
{
//DLOG(INFO) << "START CASE 2";
// 6.1- Declare positive acquisition using a message port
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 correlation output";
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[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;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 2";
break;
}
case 3:
{
//DLOG(INFO) << "START CASE 3";
// 6.2- Declare negative acquisition using a message port
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 corr output";
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[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;
this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message));
//DLOG(INFO) << "END CASE 3";
break;
}
}
return noutput_items;
}
Reference in New Issue View Git Blame Copy Permalink