mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 04:30:33 +00:00
Fix GPS L1 CA fine Doppler acquisition implementation and migrate the configuration interface
This commit is contained in:
Javier Arribas
parent
16d9e25137
commit
adcc04c3b6
@ -38,6 +38,7 @@
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "configuration_interface.h"
|
||||
#include "gnss_sdr_flags.h"
|
||||
#include "acq_conf.h"
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
@ -49,29 +50,35 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler(
|
||||
std::string default_dump_filename = "./data/acquisition.dat";
|
||||
|
||||
DLOG(INFO) << "role " << role;
|
||||
Acq_Conf acq_parameters = Acq_Conf();
|
||||
|
||||
item_type_ = configuration->property(role + ".item_type", default_item_type);
|
||||
long fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
|
||||
fs_in_ = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
|
||||
acq_parameters.fs_in = fs_in_;
|
||||
dump_ = configuration->property(role + ".dump", false);
|
||||
acq_parameters.dump = dump_;
|
||||
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
|
||||
acq_parameters.dump_filename = dump_filename_;
|
||||
doppler_max_ = configuration->property(role + ".doppler_max", 5000);
|
||||
if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
|
||||
doppler_min_ = configuration->property(role + ".doppler_min", -doppler_max_);
|
||||
acq_parameters.doppler_max = doppler_max_;
|
||||
sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1);
|
||||
acq_parameters.sampled_ms = sampled_ms_;
|
||||
max_dwells_ = configuration->property(role + ".max_dwells", 1);
|
||||
acq_parameters.max_dwells = max_dwells_;
|
||||
|
||||
acq_parameters.blocking_on_standby = configuration->property(role + ".blocking_on_standby", false);
|
||||
|
||||
//--- Find number of samples per spreading code -------------------------
|
||||
vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
|
||||
|
||||
acq_parameters.samples_per_ms = vector_length_;
|
||||
code_ = new gr_complex[vector_length_];
|
||||
|
||||
if (item_type_.compare("gr_complex") == 0)
|
||||
{
|
||||
item_size_ = sizeof(gr_complex);
|
||||
acquisition_cc_ = pcps_make_acquisition_fine_doppler_cc(max_dwells_, sampled_ms_,
|
||||
doppler_max_, doppler_min_, fs_in_, vector_length_,
|
||||
dump_, dump_filename_);
|
||||
acquisition_cc_ = pcps_make_acquisition_fine_doppler_cc(acq_parameters);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@ -131,7 +131,6 @@ private:
|
||||
float threshold_;
|
||||
int doppler_max_;
|
||||
unsigned int doppler_step_;
|
||||
int doppler_min_;
|
||||
unsigned int sampled_ms_;
|
||||
int max_dwells_;
|
||||
long fs_in_;
|
||||
|
||||
@ -40,46 +40,43 @@
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <algorithm> // std::rotate, std::fill_n
|
||||
#include <sstream>
|
||||
#include <matio.h>
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(
|
||||
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min,
|
||||
long fs_in, int samples_per_ms, bool dump,
|
||||
std::string dump_filename)
|
||||
pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(const Acq_Conf &conf_)
|
||||
{
|
||||
return pcps_acquisition_fine_doppler_cc_sptr(
|
||||
new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min,
|
||||
fs_in, samples_per_ms, dump, dump_filename));
|
||||
new pcps_acquisition_fine_doppler_cc(conf_));
|
||||
}
|
||||
|
||||
|
||||
pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
|
||||
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min,
|
||||
long fs_in, int samples_per_ms, bool dump,
|
||||
std::string dump_filename) : gr::block("pcps_acquisition_fine_doppler_cc",
|
||||
pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Conf &conf_)
|
||||
: gr::block("pcps_acquisition_fine_doppler_cc",
|
||||
gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(0, 0, sizeof(gr_complex)))
|
||||
{
|
||||
this->message_port_register_out(pmt::mp("events"));
|
||||
acq_parameters = conf_;
|
||||
d_sample_counter = 0; // SAMPLE COUNTER
|
||||
d_active = false;
|
||||
d_fs_in = fs_in;
|
||||
d_samples_per_ms = samples_per_ms;
|
||||
d_sampled_ms = sampled_ms;
|
||||
d_config_doppler_max = doppler_max;
|
||||
d_config_doppler_min = doppler_min;
|
||||
d_fs_in = conf_.fs_in;
|
||||
d_samples_per_ms = conf_.samples_per_ms;
|
||||
d_sampled_ms = conf_.sampled_ms;
|
||||
d_config_doppler_max = conf_.doppler_max;
|
||||
d_config_doppler_min = -conf_.doppler_max;
|
||||
d_fft_size = d_sampled_ms * d_samples_per_ms;
|
||||
// HS Acquisition
|
||||
d_max_dwells = max_dwells;
|
||||
d_max_dwells = conf_.max_dwells;
|
||||
d_gnuradio_forecast_samples = d_fft_size;
|
||||
d_input_power = 0.0;
|
||||
d_state = 0;
|
||||
d_carrier = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
||||
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_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
||||
|
||||
d_10_ms_buffer = static_cast<gr_complex *>(volk_gnsssdr_malloc(10 * d_samples_per_ms * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
|
||||
|
||||
@ -87,10 +84,10 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
|
||||
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_dump = conf_.dump;
|
||||
d_dump_filename = conf_.dump_filename;
|
||||
|
||||
d_doppler_resolution = 0;
|
||||
d_n_samples_in_buffer = 0;
|
||||
d_threshold = 0;
|
||||
d_num_doppler_points = 0;
|
||||
d_doppler_step = 0;
|
||||
@ -102,9 +99,28 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
|
||||
d_test_statistics = 0;
|
||||
d_well_count = 0;
|
||||
d_channel = 0;
|
||||
d_positive_acq = 0;
|
||||
d_dump_number = 0;
|
||||
d_dump_channel = 0; // this implementation can only produce dumps in channel 0
|
||||
//todo: migrate config parameters to the unified acquisition config class
|
||||
}
|
||||
|
||||
|
||||
// Finds next power of two
|
||||
// for n. If n itself is a
|
||||
// power of two then returns n
|
||||
unsigned int pcps_acquisition_fine_doppler_cc::nextPowerOf2(unsigned int n)
|
||||
{
|
||||
n--;
|
||||
n |= n >> 1;
|
||||
n |= n >> 2;
|
||||
n |= n >> 4;
|
||||
n |= n >> 8;
|
||||
n |= n >> 16;
|
||||
n++;
|
||||
return n;
|
||||
}
|
||||
|
||||
void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step)
|
||||
{
|
||||
d_doppler_step = doppler_step;
|
||||
@ -138,12 +154,9 @@ pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
|
||||
volk_gnsssdr_free(d_carrier);
|
||||
volk_gnsssdr_free(d_fft_codes);
|
||||
volk_gnsssdr_free(d_magnitude);
|
||||
volk_gnsssdr_free(d_10_ms_buffer);
|
||||
delete d_ifft;
|
||||
delete d_fft_if;
|
||||
if (d_dump)
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
free_grid_memory();
|
||||
}
|
||||
|
||||
@ -167,8 +180,12 @@ void pcps_acquisition_fine_doppler_cc::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_input_power = 0.0;
|
||||
d_state = 0;
|
||||
|
||||
if (d_dump)
|
||||
{
|
||||
grid_ = arma::fmat(d_fft_size, d_num_doppler_points, arma::fill::zeros);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -187,6 +204,7 @@ void pcps_acquisition_fine_doppler_cc::reset_grid()
|
||||
d_well_count = 0;
|
||||
for (int i = 0; i < d_num_doppler_points; i++)
|
||||
{
|
||||
//todo: use memset here
|
||||
for (unsigned int j = 0; j < d_fft_size; j++)
|
||||
{
|
||||
d_grid_data[i][j] = 0.0;
|
||||
@ -215,52 +233,71 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
|
||||
}
|
||||
|
||||
|
||||
double pcps_acquisition_fine_doppler_cc::search_maximum()
|
||||
double pcps_acquisition_fine_doppler_cc::compute_CAF()
|
||||
{
|
||||
float magt = 0.0;
|
||||
float fft_normalization_factor;
|
||||
float firstPeak = 0.0;
|
||||
int index_doppler = 0;
|
||||
uint32_t tmp_intex_t = 0;
|
||||
uint32_t index_time = 0;
|
||||
|
||||
// Look for correlation peaks in the results ==============================
|
||||
// Find the highest peak and compare it to the second highest peak
|
||||
// The second peak is chosen not closer than 1 chip to the highest peak
|
||||
//--- Find the correlation peak and the carrier frequency --------------
|
||||
for (int i = 0; i < d_num_doppler_points; i++)
|
||||
{
|
||||
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i], d_fft_size);
|
||||
if (d_grid_data[i][tmp_intex_t] > magt)
|
||||
if (d_grid_data[i][tmp_intex_t] > firstPeak)
|
||||
{
|
||||
magt = d_grid_data[i][tmp_intex_t];
|
||||
//std::cout<<magt<<std::endl;
|
||||
firstPeak = d_grid_data[i][tmp_intex_t];
|
||||
index_doppler = i;
|
||||
index_time = tmp_intex_t;
|
||||
}
|
||||
|
||||
// Record results to file if required
|
||||
if (d_dump and d_channel == d_dump_channel)
|
||||
{
|
||||
memcpy(grid_.colptr(i), d_grid_data[i], sizeof(float) * d_fft_size);
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
||||
fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
||||
magt = magt / (fft_normalization_factor * fft_normalization_factor);
|
||||
|
||||
// -- - Find 1 chip wide code phase exclude range around the peak
|
||||
uint32_t samplesPerChip = ceil(GPS_L1_CA_CHIP_PERIOD * static_cast<float>(this->d_fs_in));
|
||||
int32_t excludeRangeIndex1 = index_time - samplesPerChip;
|
||||
int32_t excludeRangeIndex2 = index_time + samplesPerChip;
|
||||
|
||||
// -- - Correct code phase exclude range if the range includes array boundaries
|
||||
if (excludeRangeIndex1 < 0)
|
||||
{
|
||||
excludeRangeIndex1 = d_fft_size + excludeRangeIndex1;
|
||||
}
|
||||
else if (excludeRangeIndex2 >= static_cast<int>(d_fft_size))
|
||||
{
|
||||
excludeRangeIndex2 = excludeRangeIndex2 - d_fft_size;
|
||||
}
|
||||
|
||||
int32_t idx = excludeRangeIndex1;
|
||||
do
|
||||
{
|
||||
d_grid_data[index_doppler][idx] = 0.0;
|
||||
idx++;
|
||||
if (idx == static_cast<int>(d_fft_size)) idx = 0;
|
||||
}
|
||||
while (idx != excludeRangeIndex2);
|
||||
|
||||
//--- Find the second highest correlation peak in the same freq. bin ---
|
||||
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[index_doppler], d_fft_size);
|
||||
float secondPeak = d_grid_data[index_doppler][tmp_intex_t];
|
||||
|
||||
// 5- Compute the test statistics and compare to the threshold
|
||||
d_test_statistics = magt / (d_input_power * std::sqrt(d_well_count));
|
||||
d_test_statistics = firstPeak / secondPeak;
|
||||
|
||||
// 4- record the maximum peak and the associated synchronization parameters
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step + d_config_doppler_min);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
|
||||
|
||||
// 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_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
|
||||
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
|
||||
d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler]), n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
d_dump_file.close();
|
||||
}
|
||||
|
||||
return d_test_statistics;
|
||||
}
|
||||
|
||||
@ -308,10 +345,9 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons
|
||||
d_ifft->execute();
|
||||
|
||||
// save the grid matrix delay file
|
||||
|
||||
volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
|
||||
const float *old_vector = d_grid_data[doppler_index];
|
||||
volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
|
||||
//accumulate grid values
|
||||
volk_32f_x2_add_32f(d_grid_data[doppler_index], d_grid_data[doppler_index], p_tmp_vector, d_fft_size);
|
||||
}
|
||||
|
||||
volk_gnsssdr_free(p_tmp_vector);
|
||||
@ -319,18 +355,21 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons
|
||||
}
|
||||
|
||||
|
||||
int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star &input_items)
|
||||
int pcps_acquisition_fine_doppler_cc::estimate_Doppler()
|
||||
{
|
||||
// Direct FFT
|
||||
int zero_padding_factor = 2;
|
||||
int fft_size_extended = d_fft_size * zero_padding_factor;
|
||||
int zero_padding_factor = 8;
|
||||
int prn_replicas = 10;
|
||||
int signal_samples = prn_replicas * d_fft_size;
|
||||
//int fft_size_extended = nextPowerOf2(signal_samples * zero_padding_factor);
|
||||
int fft_size_extended = signal_samples * zero_padding_factor;
|
||||
gr::fft::fft_complex *fft_operator = new gr::fft::fft_complex(fft_size_extended, true);
|
||||
|
||||
//zero padding the entire vector
|
||||
std::fill_n(fft_operator->get_inbuf(), fft_size_extended, gr_complex(0.0, 0.0));
|
||||
|
||||
//1. generate local code aligned with the acquisition code phase estimation
|
||||
gr_complex *code_replica = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
||||
gr_complex *code_replica = static_cast<gr_complex *>(volk_gnsssdr_malloc(signal_samples * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
||||
|
||||
gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
|
||||
|
||||
@ -342,10 +381,13 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
|
||||
std::rotate(code_replica, code_replica + (d_fft_size - shift_index), code_replica + d_fft_size - 1);
|
||||
}
|
||||
|
||||
for (int n = 0; n < prn_replicas - 1; n++)
|
||||
{
|
||||
memcpy(&code_replica[(n + 1) * d_fft_size], code_replica, d_fft_size * sizeof(gr_complex));
|
||||
}
|
||||
//2. Perform code wipe-off
|
||||
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
|
||||
|
||||
volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
|
||||
volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), d_10_ms_buffer, code_replica, signal_samples);
|
||||
|
||||
// 3. Perform the FFT (zero padded!)
|
||||
fft_operator->execute();
|
||||
@ -380,40 +422,12 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
|
||||
if (std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000)
|
||||
{
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(fftFreqBins[tmp_index_freq]);
|
||||
//std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
|
||||
//std::cout << "FFT maximum present at " << fftFreqBins[tmp_index_freq] << " [Hz]" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz);
|
||||
DLOG(INFO) << "Error estimating fine frequency Doppler";
|
||||
//debug log
|
||||
//
|
||||
// std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
|
||||
// std::stringstream filename;
|
||||
// std::streamsize n = sizeof(gr_complex) * (d_fft_size);
|
||||
//
|
||||
// filename.str("");
|
||||
// filename << "../data/code_prn_" << d_gnss_synchro->PRN << ".dat";
|
||||
// d_dump_file.open(filename.str().c_str(), std::ios::out
|
||||
// | std::ios::binary);
|
||||
// d_dump_file.write(reinterpret_cast<char*>(code_replica), n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
// d_dump_file.close();
|
||||
//
|
||||
// filename.str("");
|
||||
// filename << "../data/signal_prn_" << d_gnss_synchro->PRN << ".dat";
|
||||
// d_dump_file.open(filename.str().c_str(), std::ios::out
|
||||
// | std::ios::binary);
|
||||
// d_dump_file.write(reinterpret_cast<char*>(in), n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
// d_dump_file.close();
|
||||
//
|
||||
//
|
||||
// n = sizeof(float) * (fft_size_extended);
|
||||
// filename.str("");
|
||||
// filename << "../data/fft_prn_" << d_gnss_synchro->PRN << ".dat";
|
||||
// d_dump_file.open(filename.str().c_str(), std::ios::out
|
||||
// | std::ios::binary);
|
||||
// d_dump_file.write(reinterpret_cast<char*>(p_tmp_vector), n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
// d_dump_file.close();
|
||||
}
|
||||
|
||||
// free memory!!
|
||||
@ -423,6 +437,12 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
|
||||
return d_fft_size;
|
||||
}
|
||||
|
||||
// Called by gnuradio to enable drivers, etc for i/o devices.
|
||||
bool pcps_acquisition_fine_doppler_cc::start()
|
||||
{
|
||||
d_sample_counter = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
|
||||
gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items,
|
||||
@ -442,29 +462,33 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
|
||||
* S5. Negative_Acq: Send message and stop acq -> S0
|
||||
*/
|
||||
|
||||
int samples_remaining;
|
||||
switch (d_state)
|
||||
{
|
||||
case 0: // S0. StandBy
|
||||
//DLOG(INFO) <<"S0"<<std::endl;
|
||||
if (d_active == true)
|
||||
{
|
||||
reset_grid();
|
||||
d_state = 1;
|
||||
}
|
||||
if (!acq_parameters.blocking_on_standby)
|
||||
{
|
||||
d_sample_counter += d_fft_size; // sample counter
|
||||
consume_each(d_fft_size);
|
||||
}
|
||||
break;
|
||||
case 1: // S1. ComputeGrid
|
||||
//DLOG(INFO) <<"S1"<<std::endl;
|
||||
compute_and_accumulate_grid(input_items);
|
||||
d_well_count++;
|
||||
if (d_well_count >= d_max_dwells)
|
||||
{
|
||||
d_state = 2;
|
||||
}
|
||||
d_sample_counter += d_fft_size; // sample counter
|
||||
consume_each(d_fft_size);
|
||||
break;
|
||||
case 2: // Compute test statistics and decide
|
||||
//DLOG(INFO) <<"S2"<<std::endl;
|
||||
d_input_power = estimate_input_power(input_items);
|
||||
d_test_statistics = search_maximum();
|
||||
d_test_statistics = compute_CAF();
|
||||
if (d_test_statistics > d_threshold)
|
||||
{
|
||||
d_state = 3; //perform fine doppler estimation
|
||||
@ -473,15 +497,35 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
|
||||
{
|
||||
d_state = 5; //negative acquisition
|
||||
}
|
||||
d_n_samples_in_buffer = 0;
|
||||
// Record results to file if required
|
||||
if (d_dump and d_channel == d_dump_channel)
|
||||
{
|
||||
dump_results(d_fft_size);
|
||||
}
|
||||
d_sample_counter += d_fft_size; // sample counter
|
||||
consume_each(d_fft_size);
|
||||
break;
|
||||
case 3: // Fine doppler estimation
|
||||
//DLOG(INFO) <<"S3"<<std::endl;
|
||||
DLOG(INFO) << "Performing fine Doppler estimation";
|
||||
estimate_Doppler(input_items); //disabled in repo
|
||||
samples_remaining = 10 * d_samples_per_ms - d_n_samples_in_buffer;
|
||||
|
||||
if (samples_remaining > noutput_items)
|
||||
{
|
||||
memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast<const gr_complex *>(input_items[0]), noutput_items * sizeof(gr_complex));
|
||||
d_n_samples_in_buffer += noutput_items;
|
||||
d_sample_counter += noutput_items; // sample counter
|
||||
consume_each(noutput_items);
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast<const gr_complex *>(input_items[0]), samples_remaining * sizeof(gr_complex));
|
||||
estimate_Doppler(); //disabled in repo
|
||||
d_sample_counter += samples_remaining; // sample counter
|
||||
consume_each(samples_remaining);
|
||||
d_state = 4;
|
||||
}
|
||||
break;
|
||||
case 4: // Positive_Acq
|
||||
//DLOG(INFO) <<"S4"<<std::endl;
|
||||
DLOG(INFO) << "positive acquisition";
|
||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||
DLOG(INFO) << "sample_stamp " << d_sample_counter;
|
||||
@ -489,15 +533,18 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
|
||||
DLOG(INFO) << "test statistics threshold " << d_threshold;
|
||||
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
|
||||
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
DLOG(INFO) << "input signal power " << d_input_power;
|
||||
|
||||
d_positive_acq = 1;
|
||||
d_active = false;
|
||||
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||
this->message_port_pub(pmt::mp("events"), pmt::from_long(1));
|
||||
d_state = 0;
|
||||
if (!acq_parameters.blocking_on_standby)
|
||||
{
|
||||
d_sample_counter += noutput_items; // sample counter
|
||||
consume_each(noutput_items);
|
||||
}
|
||||
break;
|
||||
case 5: // Negative_Acq
|
||||
//DLOG(INFO) <<"S5"<<std::endl;
|
||||
DLOG(INFO) << "negative acquisition";
|
||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||
DLOG(INFO) << "sample_stamp " << d_sample_counter;
|
||||
@ -505,20 +552,103 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
|
||||
DLOG(INFO) << "test statistics threshold " << d_threshold;
|
||||
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
|
||||
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
DLOG(INFO) << "input signal power " << d_input_power;
|
||||
|
||||
d_positive_acq = 0;
|
||||
d_active = false;
|
||||
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||
this->message_port_pub(pmt::mp("events"), pmt::from_long(2));
|
||||
d_state = 0;
|
||||
if (!acq_parameters.blocking_on_standby)
|
||||
{
|
||||
d_sample_counter += noutput_items; // sample counter
|
||||
consume_each(noutput_items);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
d_state = 0;
|
||||
if (!acq_parameters.blocking_on_standby)
|
||||
{
|
||||
d_sample_counter += noutput_items; // sample counter
|
||||
consume_each(noutput_items);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
//DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl;
|
||||
d_sample_counter += d_fft_size; // sample counter
|
||||
consume_each(d_fft_size);
|
||||
return noutput_items;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void pcps_acquisition_fine_doppler_cc::dump_results(int effective_fft_size)
|
||||
{
|
||||
d_dump_number++;
|
||||
std::string filename = d_dump_filename;
|
||||
filename.append("_");
|
||||
filename.append(1, d_gnss_synchro->System);
|
||||
filename.append("_");
|
||||
filename.append(1, d_gnss_synchro->Signal[0]);
|
||||
filename.append(1, d_gnss_synchro->Signal[1]);
|
||||
filename.append("_ch_");
|
||||
filename.append(std::to_string(d_channel));
|
||||
filename.append("_");
|
||||
filename.append(std::to_string(d_dump_number));
|
||||
filename.append("_sat_");
|
||||
filename.append(std::to_string(d_gnss_synchro->PRN));
|
||||
filename.append(".mat");
|
||||
|
||||
mat_t *matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
|
||||
if (matfp == NULL)
|
||||
{
|
||||
std::cout << "Unable to create or open Acquisition dump file" << std::endl;
|
||||
d_dump = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
size_t dims[2] = {static_cast<size_t>(effective_fft_size), static_cast<size_t>(d_num_doppler_points)};
|
||||
matvar_t *matvar = Mat_VarCreate("acq_grid", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, grid_.memptr(), 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
dims[0] = static_cast<size_t>(1);
|
||||
dims[1] = static_cast<size_t>(1);
|
||||
matvar = Mat_VarCreate("doppler_max", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_config_doppler_max, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("doppler_step", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_doppler_step, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("d_positive_acq", MAT_C_INT32, MAT_T_INT32, 1, dims, &d_positive_acq, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
float aux = static_cast<float>(d_gnss_synchro->Acq_doppler_hz);
|
||||
matvar = Mat_VarCreate("acq_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &aux, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
aux = static_cast<float>(d_gnss_synchro->Acq_delay_samples);
|
||||
matvar = Mat_VarCreate("acq_delay_samples", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &aux, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("test_statistic", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &d_test_statistics, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("threshold", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &d_threshold, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
aux = 0.0;
|
||||
matvar = Mat_VarCreate("input_power", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &aux, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("sample_counter", MAT_C_UINT64, MAT_T_UINT64, 1, dims, &d_sample_counter, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_gnss_synchro->PRN, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
Mat_Close(matfp);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,8 +1,9 @@
|
||||
/*!
|
||||
* \file pcps_acquisition_fine_doppler_acquisition_cc.h
|
||||
* \brief This class implements a Parallel Code Phase Search Acquisition with multi-dwells and fine Doppler estimation
|
||||
* for GPS L1 C/A signal
|
||||
*
|
||||
* Acquisition strategy (Kay Borre book + CFAR threshold).
|
||||
* Acquisition strategy (Kay Borre book).
|
||||
* <ol>
|
||||
* <li> Compute the input signal power estimation
|
||||
* <li> Doppler serial search loop
|
||||
@ -49,11 +50,13 @@
|
||||
#define GNSS_SDR_PCPS_ACQUISITION_FINE_DOPPLER_CC_H_
|
||||
|
||||
#include "gnss_synchro.h"
|
||||
#include "acq_conf.h"
|
||||
#include <gnuradio/block.h>
|
||||
#include <gnuradio/gr_complex.h>
|
||||
#include <gnuradio/fft/fft.h>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <armadillo>
|
||||
|
||||
class pcps_acquisition_fine_doppler_cc;
|
||||
|
||||
@ -61,46 +64,33 @@ typedef boost::shared_ptr<pcps_acquisition_fine_doppler_cc>
|
||||
pcps_acquisition_fine_doppler_cc_sptr;
|
||||
|
||||
pcps_acquisition_fine_doppler_cc_sptr
|
||||
pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
|
||||
int doppler_max, int doppler_min, long fs_in, int samples_per_ms,
|
||||
bool dump, std::string dump_filename);
|
||||
pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_);
|
||||
|
||||
/*!
|
||||
* \brief This class implements a Parallel Code Phase Search Acquisition.
|
||||
*
|
||||
* Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver",
|
||||
* Algorithm 1, for a pseudocode description of this implementation.
|
||||
*/
|
||||
|
||||
class pcps_acquisition_fine_doppler_cc : public gr::block
|
||||
{
|
||||
private:
|
||||
friend pcps_acquisition_fine_doppler_cc_sptr
|
||||
pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
|
||||
int doppler_max, int doppler_min, long fs_in,
|
||||
int samples_per_ms, bool dump,
|
||||
std::string dump_filename);
|
||||
|
||||
pcps_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
|
||||
int doppler_max, int doppler_min, long fs_in,
|
||||
int samples_per_ms, bool dump,
|
||||
std::string dump_filename);
|
||||
|
||||
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
|
||||
int doppler_offset);
|
||||
pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_);
|
||||
pcps_acquisition_fine_doppler_cc(const Acq_Conf& conf_);
|
||||
|
||||
int compute_and_accumulate_grid(gr_vector_const_void_star& input_items);
|
||||
int estimate_Doppler(gr_vector_const_void_star& input_items);
|
||||
int estimate_Doppler();
|
||||
float estimate_input_power(gr_vector_const_void_star& input_items);
|
||||
double search_maximum();
|
||||
double compute_CAF();
|
||||
void reset_grid();
|
||||
void update_carrier_wipeoff();
|
||||
void free_grid_memory();
|
||||
bool start();
|
||||
|
||||
Acq_Conf acq_parameters;
|
||||
long d_fs_in;
|
||||
int d_samples_per_ms;
|
||||
int d_max_dwells;
|
||||
unsigned int d_doppler_resolution;
|
||||
int d_gnuradio_forecast_samples;
|
||||
float d_threshold;
|
||||
std::string d_satellite_str;
|
||||
@ -114,6 +104,7 @@ private:
|
||||
unsigned long int d_sample_counter;
|
||||
gr_complex* d_carrier;
|
||||
gr_complex* d_fft_codes;
|
||||
gr_complex* d_10_ms_buffer;
|
||||
float* d_magnitude;
|
||||
|
||||
float** d_grid_data;
|
||||
@ -124,17 +115,23 @@ private:
|
||||
Gnss_Synchro* d_gnss_synchro;
|
||||
unsigned int d_code_phase;
|
||||
float d_doppler_freq;
|
||||
float d_input_power;
|
||||
float d_test_statistics;
|
||||
std::ofstream d_dump_file;
|
||||
int d_positive_acq;
|
||||
|
||||
int d_state;
|
||||
bool d_active;
|
||||
int d_well_count;
|
||||
int d_n_samples_in_buffer;
|
||||
bool d_dump;
|
||||
unsigned int d_channel;
|
||||
|
||||
std::string d_dump_filename;
|
||||
|
||||
arma::fmat grid_;
|
||||
long int d_dump_number;
|
||||
unsigned int d_dump_channel;
|
||||
|
||||
|
||||
public:
|
||||
/*!
|
||||
* \brief Default destructor.
|
||||
@ -187,6 +184,7 @@ public:
|
||||
inline void set_channel(unsigned int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
d_dump_channel = d_channel;
|
||||
}
|
||||
|
||||
/*!
|
||||
@ -222,6 +220,14 @@ public:
|
||||
gr_vector_void_star& output_items);
|
||||
|
||||
void forecast(int noutput_items, gr_vector_int& ninput_items_required);
|
||||
|
||||
/*!
|
||||
* \brief Obtains the next power of 2 greater or equal to the input parameter
|
||||
* \param n - Integer value to obtain the next power of 2.
|
||||
*/
|
||||
unsigned int nextPowerOf2(unsigned int n);
|
||||
|
||||
void dump_results(int effective_fft_size);
|
||||
};
|
||||
|
||||
#endif /* pcps_acquisition_fine_doppler_cc*/
|
||||
|
||||
@ -36,6 +36,7 @@
|
||||
|
||||
// Input signal configuration
|
||||
DEFINE_bool(enable_external_signal_file, false, "Use an external signal file capture instead of the software-defined signal generator");
|
||||
DEFINE_int32(external_signal_acquisition_threshold, 2.0, "Threshold for satellite acquisition when external file is used");
|
||||
DEFINE_string(signal_file, std::string("signal_out.bin"), "Path of the external signal capture file");
|
||||
DEFINE_double(CN0_dBHz_start, std::numeric_limits<double>::infinity(), "Enable noise generator and set the CN0 start sweep value [dB-Hz]");
|
||||
DEFINE_double(CN0_dBHz_stop, std::numeric_limits<double>::infinity(), "Enable noise generator and set the CN0 stop sweep value [dB-Hz]");
|
||||
|
||||
@ -334,15 +334,17 @@ bool GpsL1CADllPllTrackingPullInTest::acquire_GPS_L1CA_signal(int SV_ID)
|
||||
config = std::make_shared<InMemoryConfiguration>();
|
||||
config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(baseband_sampling_freq));
|
||||
|
||||
config->set_property("Acquisition.max_dwells", "10");
|
||||
config->set_property("Acquisition.dump", "true");
|
||||
GNSSBlockFactory block_factory;
|
||||
GpsL1CaPcpsAcquisitionFineDoppler* acquisition;
|
||||
acquisition = new GpsL1CaPcpsAcquisitionFineDoppler(config.get(), "Acquisition", 1, 1);
|
||||
|
||||
acquisition->set_channel(1);
|
||||
acquisition->set_gnss_synchro(&tmp_gnss_synchro);
|
||||
acquisition->set_threshold(config->property("Acquisition.threshold", 0.005));
|
||||
acquisition->set_threshold(config->property("Acquisition.threshold", FLAGS_external_signal_acquisition_threshold));
|
||||
acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 10000));
|
||||
acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 250));
|
||||
acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 500));
|
||||
|
||||
boost::shared_ptr<Acquisition_msg_rx> msg_rx;
|
||||
try
|
||||
@ -522,7 +524,7 @@ TEST_F(GpsL1CADllPllTrackingPullInTest, ValidationOfResults)
|
||||
<< "Maybe sat PRN #" + std::to_string(FLAGS_test_satellite_PRN) +
|
||||
" is not available?";
|
||||
std::cout << "Testing satellite PRN=" << test_satellite_PRN << std::endl;
|
||||
std::cout << "True Initial Doppler " << true_obs_data.doppler_l1_hz << "[Hz], true Initial code delay [Chips]=" << true_obs_data.prn_delay_chips << "[Chips]" << std::endl;
|
||||
std::cout << "True Initial Doppler " << true_obs_data.doppler_l1_hz << " [Hz], true Initial code delay [Chips]=" << true_obs_data.prn_delay_chips << "[Chips]" << std::endl;
|
||||
true_acq_doppler_hz = true_obs_data.doppler_l1_hz;
|
||||
true_acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * static_cast<double>(baseband_sampling_freq) * GPS_L1_CA_CODE_PERIOD;
|
||||
acq_samplestamp_samples = 0;
|
||||
@ -531,8 +533,10 @@ TEST_F(GpsL1CADllPllTrackingPullInTest, ValidationOfResults)
|
||||
{
|
||||
true_acq_doppler_hz = doppler_measurements_map.find(FLAGS_test_satellite_PRN)->second;
|
||||
true_acq_delay_samples = code_delay_measurements_map.find(FLAGS_test_satellite_PRN)->second;
|
||||
acq_samplestamp_samples = 0; //acq_samplestamp_map.find(FLAGS_test_satellite_PRN)->second;
|
||||
std::cout << "Estimated Initial Doppler " << true_acq_doppler_hz << "[Hz], estimated Initial code delay " << true_acq_delay_samples << " [Samples]" << std::endl;
|
||||
acq_samplestamp_samples = 0;
|
||||
std::cout << "Estimated Initial Doppler " << true_acq_doppler_hz
|
||||
<< " [Hz], estimated Initial code delay " << true_acq_delay_samples << " [Samples]"
|
||||
<< " Acquisition SampleStamp is " << acq_samplestamp_map.find(FLAGS_test_satellite_PRN)->second << std::endl;
|
||||
}
|
||||
//CN0 LOOP
|
||||
std::vector<std::vector<double>> pull_in_results_v_v;
|
||||
|
||||
@ -33,6 +33,7 @@ include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-supl
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/adapters
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/gnuradio_blocks
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/libs
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/libs
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
|
||||
@ -144,8 +144,6 @@ FrontEndCal_msg_rx::FrontEndCal_msg_rx() : gr::block("FrontEndCal_msg_rx", gr::i
|
||||
|
||||
|
||||
FrontEndCal_msg_rx::~FrontEndCal_msg_rx() {}
|
||||
|
||||
|
||||
void wait_message()
|
||||
{
|
||||
while (!stop)
|
||||
@ -353,13 +351,14 @@ int main(int argc, char** argv)
|
||||
gnss_synchro->PRN = 1;
|
||||
|
||||
long fs_in_ = configuration->property("GNSS-SDR.internal_fs_sps", 2048000);
|
||||
configuration->set_property("Acquisition.max_dwells", "10");
|
||||
|
||||
GNSSBlockFactory block_factory;
|
||||
acquisition = new GpsL1CaPcpsAcquisitionFineDoppler(configuration.get(), "Acquisition", 1, 1);
|
||||
|
||||
acquisition->set_channel(1);
|
||||
acquisition->set_gnss_synchro(gnss_synchro);
|
||||
acquisition->set_threshold(configuration->property("Acquisition.threshold", 0.0));
|
||||
acquisition->set_threshold(configuration->property("Acquisition.threshold", 2.0));
|
||||
acquisition->set_doppler_max(configuration->property("Acquisition.doppler_max", 10000));
|
||||
acquisition->set_doppler_step(configuration->property("Acquisition.doppler_step", 250));
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user