mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-21 09:34:53 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into fpga

Browse Source
This commit is contained in:
Marc Majoral
2018-08-08 14:25:12 +02:00
parent b1a7031e52 0b5c827eda
commit ee132f445f
33 changed files with 2576 additions and 816 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@@ -176,6 +176,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
band2 = true;
}
}
break;
default:
{
}

19
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc
View File

@@ -76,16 +76,13 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
acq_parameters.max_dwells = max_dwells_;
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
acq_parameters.dump_filename = dump_filename_;
acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1);
//--- Find number of samples per spreading code -------------------------
code_length_ = static_cast<unsigned int>(std::round(static_cast<double>(fs_in_) / (GPS_L5i_CODE_RATE_HZ / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS))));
vector_length_ = code_length_;
if (bit_transition_flag_)
{
vector_length_ *= 2;
}
code_length_ = static_cast<unsigned int>(std::floor(static_cast<double>(fs_in_) / (GPS_L5i_CODE_RATE_HZ / GPS_L5i_CODE_LENGTH_CHIPS)));
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GPS_L5i_PERIOD * 1000.0);
vector_length_ = std::floor(acq_parameters.sampled_ms * acq_parameters.samples_per_ms) * (acq_parameters.bit_transition_flag ? 2 : 1);
code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0)
@@ -96,11 +93,9 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
{
item_size_ = sizeof(gr_complex);
}
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GPS_L5i_PERIOD * 1000.0);
acq_parameters.ms_per_code = 1;
acq_parameters.it_size = item_size_;
acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1);
num_codes_ = acq_parameters.sampled_ms;
acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4);
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
@@ -108,7 +103,6 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
acq_parameters.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false);
acquisition_ = pcps_make_acquisition(acq_parameters);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")";
@@ -117,7 +111,6 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
cbyte_to_float_x2_ = make_complex_byte_to_float_x2();
float_to_complex_ = gr::blocks::float_to_complex::make();
}
channel_ = 0;
threshold_ = 0.0;
doppler_step_ = 0;

174
src/algorithms/input_filter/adapters/freq_xlating_fir_filter.cc
View File

@@ -43,38 +43,112 @@ using google::LogMessage;
FreqXlatingFirFilter::FreqXlatingFirFilter(ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : config_(configuration), role_(role), in_streams_(in_streams), out_streams_(out_streams)
{
size_t item_size;
(*this).init();
int decimation_factor;
std::string default_input_item_type = "gr_complex";
std::string default_output_item_type = "gr_complex";
std::string default_taps_item_type = "float";
std::string default_dump_filename = "../data/input_filter.dat";
double default_intermediate_freq = 0.0;
double default_sampling_freq = 4000000.0;
int default_number_of_taps = 6;
unsigned int default_number_of_bands = 2;
std::vector<double> default_bands = {0.0, 0.4, 0.6, 1.0};
std::vector<double> default_ampl = {1.0, 1.0, 0.0, 0.0};
std::vector<double> default_error_w = {1.0, 1.0};
std::string default_filter_type = "bandpass";
int default_grid_density = 16;
int default_decimation_factor = 1;
decimation_factor = config_->property(role_ + ".decimation_factor", default_decimation_factor);
DLOG(INFO) << "role " << role_;
input_item_type_ = config_->property(role_ + ".input_item_type", default_input_item_type);
output_item_type_ = config_->property(role_ + ".output_item_type", default_output_item_type);
taps_item_type_ = config_->property(role_ + ".taps_item_type", default_taps_item_type);
dump_ = config_->property(role_ + ".dump", false);
dump_filename_ = config_->property(role_ + ".dump_filename", default_dump_filename);
intermediate_freq_ = config_->property(role_ + ".IF", default_intermediate_freq);
sampling_freq_ = config_->property(role_ + ".sampling_frequency", default_sampling_freq);
int number_of_taps = config_->property(role_ + ".number_of_taps", default_number_of_taps);
unsigned int number_of_bands = config_->property(role_ + ".number_of_bands", default_number_of_bands);
std::string filter_type = config_->property(role_ + ".filter_type", default_filter_type);
decimation_factor_ = config_->property(role_ + ".decimation_factor", default_decimation_factor);
if (filter_type.compare("lowpass") != 0)
{
std::vector<double> taps_d;
std::vector<double> bands;
std::vector<double> ampl;
std::vector<double> error_w;
std::string option;
double option_value;
for (unsigned int i = 0; i < number_of_bands; i++)
{
option = ".band" + boost::lexical_cast<std::string>(i + 1) + "_begin";
option_value = config_->property(role_ + option, default_bands[i]);
bands.push_back(option_value);
option = ".band" + boost::lexical_cast<std::string>(i + 1) + "_end";
option_value = config_->property(role_ + option, default_bands[i]);
bands.push_back(option_value);
option = ".ampl" + boost::lexical_cast<std::string>(i + 1) + "_begin";
option_value = config_->property(role_ + option, default_bands[i]);
ampl.push_back(option_value);
option = ".ampl" + boost::lexical_cast<std::string>(i + 1) + "_end";
option_value = config_->property(role_ + option, default_bands[i]);
ampl.push_back(option_value);
option = ".band" + boost::lexical_cast<std::string>(i + 1) + "_error";
option_value = config_->property(role_ + option, default_bands[i]);
error_w.push_back(option_value);
}
int grid_density = config_->property(role_ + ".grid_density", default_grid_density);
taps_d = gr::filter::pm_remez(number_of_taps - 1, bands, ampl, error_w, filter_type, grid_density);
taps_.reserve(taps_d.size());
for (std::vector<double>::iterator it = taps_d.begin(); it != taps_d.end(); it++)
{
taps_.push_back(static_cast<float>(*it));
}
}
else
{
double default_bw = (sampling_freq_ / decimation_factor_) / 2;
double bw_ = config_->property(role_ + ".bw", default_bw);
double default_tw = bw_ / 10.0;
double tw_ = config_->property(role_ + ".tw", default_tw);
taps_ = gr::filter::firdes::low_pass(1.0, sampling_freq_, bw_, tw_);
}
size_t item_size;
if ((taps_item_type_.compare("float") == 0) && (input_item_type_.compare("gr_complex") == 0) && (output_item_type_.compare("gr_complex") == 0))
{
item_size = sizeof(gr_complex); //output
input_size_ = sizeof(gr_complex); //input
freq_xlating_fir_filter_ccf_ = gr::filter::freq_xlating_fir_filter_ccf::make(decimation_factor, taps_, intermediate_freq_, sampling_freq_);
freq_xlating_fir_filter_ccf_ = gr::filter::freq_xlating_fir_filter_ccf::make(decimation_factor_, taps_, intermediate_freq_, sampling_freq_);
DLOG(INFO) << "input_filter(" << freq_xlating_fir_filter_ccf_->unique_id() << ")";
}
else if ((taps_item_type_.compare("float") == 0) && (input_item_type_.compare("float") == 0) && (output_item_type_.compare("gr_complex") == 0))
{
item_size = sizeof(gr_complex);
input_size_ = sizeof(float); //input
freq_xlating_fir_filter_fcf_ = gr::filter::freq_xlating_fir_filter_fcf::make(decimation_factor, taps_, intermediate_freq_, sampling_freq_);
freq_xlating_fir_filter_fcf_ = gr::filter::freq_xlating_fir_filter_fcf::make(decimation_factor_, taps_, intermediate_freq_, sampling_freq_);
DLOG(INFO) << "input_filter(" << freq_xlating_fir_filter_fcf_->unique_id() << ")";
}
else if ((taps_item_type_.compare("float") == 0) && (input_item_type_.compare("short") == 0) && (output_item_type_.compare("gr_complex") == 0))
{
item_size = sizeof(gr_complex);
input_size_ = sizeof(int16_t); //input
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor, taps_, intermediate_freq_, sampling_freq_);
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor_, taps_, intermediate_freq_, sampling_freq_);
DLOG(INFO) << "input_filter(" << freq_xlating_fir_filter_scf_->unique_id() << ")";
}
else if ((taps_item_type_.compare("float") == 0) && (input_item_type_.compare("short") == 0) && (output_item_type_.compare("cshort") == 0))
{
item_size = sizeof(lv_16sc_t);
input_size_ = sizeof(int16_t); //input
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor, taps_, intermediate_freq_, sampling_freq_);
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor_, taps_, intermediate_freq_, sampling_freq_);
DLOG(INFO) << "input_filter(" << freq_xlating_fir_filter_scf_->unique_id() << ")";
complex_to_float_ = gr::blocks::complex_to_float::make();
float_to_short_1_ = gr::blocks::float_to_short::make();
@@ -86,7 +160,7 @@ FreqXlatingFirFilter::FreqXlatingFirFilter(ConfigurationInterface* configuration
item_size = sizeof(gr_complex);
input_size_ = sizeof(int8_t); //input
gr_char_to_short_ = gr::blocks::char_to_short::make();
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor, taps_, intermediate_freq_, sampling_freq_);
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor_, taps_, intermediate_freq_, sampling_freq_);
DLOG(INFO) << "input_filter(" << freq_xlating_fir_filter_scf_->unique_id() << ")";
}
else if ((taps_item_type_.compare("float") == 0) && (input_item_type_.compare("byte") == 0) && (output_item_type_.compare("cbyte") == 0))
@@ -94,7 +168,7 @@ FreqXlatingFirFilter::FreqXlatingFirFilter(ConfigurationInterface* configuration
item_size = sizeof(lv_8sc_t);
input_size_ = sizeof(int8_t); //input
gr_char_to_short_ = gr::blocks::char_to_short::make();
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor, taps_, intermediate_freq_, sampling_freq_);
freq_xlating_fir_filter_scf_ = gr::filter::freq_xlating_fir_filter_scf::make(decimation_factor_, taps_, intermediate_freq_, sampling_freq_);
DLOG(INFO) << "input_filter(" << freq_xlating_fir_filter_scf_->unique_id() << ")";
complex_to_complex_byte_ = make_complex_float_to_complex_byte();
}
@@ -311,83 +385,3 @@ gr::basic_block_sptr FreqXlatingFirFilter::get_right_block()
LOG(ERROR) << " Unknown input filter input/output item type conversion";
}
}
void FreqXlatingFirFilter::init()
{
std::string default_input_item_type = "gr_complex";
std::string default_output_item_type = "gr_complex";
std::string default_taps_item_type = "float";
std::string default_dump_filename = "../data/input_filter.dat";
double default_intermediate_freq = 0.0;
double default_sampling_freq = 4000000.0;
int default_number_of_taps = 6;
unsigned int default_number_of_bands = 2;
std::vector<double> default_bands = {0.0, 0.4, 0.6, 1.0};
std::vector<double> default_ampl = {1.0, 1.0, 0.0, 0.0};
std::vector<double> default_error_w = {1.0, 1.0};
std::string default_filter_type = "bandpass";
int default_grid_density = 16;
DLOG(INFO) << "role " << role_;
input_item_type_ = config_->property(role_ + ".input_item_type", default_input_item_type);
output_item_type_ = config_->property(role_ + ".output_item_type", default_output_item_type);
taps_item_type_ = config_->property(role_ + ".taps_item_type", default_taps_item_type);
dump_ = config_->property(role_ + ".dump", false);
dump_filename_ = config_->property(role_ + ".dump_filename", default_dump_filename);
intermediate_freq_ = config_->property(role_ + ".IF", default_intermediate_freq);
sampling_freq_ = config_->property(role_ + ".sampling_frequency", default_sampling_freq);
int number_of_taps = config_->property(role_ + ".number_of_taps", default_number_of_taps);
unsigned int number_of_bands = config_->property(role_ + ".number_of_bands", default_number_of_bands);
std::string filter_type = config_->property(role_ + ".filter_type", default_filter_type);
if (filter_type.compare("lowpass") != 0)
{
std::vector<double> taps_d;
std::vector<double> bands;
std::vector<double> ampl;
std::vector<double> error_w;
std::string option;
double option_value;
for (unsigned int i = 0; i < number_of_bands; i++)
{
option = ".band" + boost::lexical_cast<std::string>(i + 1) + "_begin";
option_value = config_->property(role_ + option, default_bands[i]);
bands.push_back(option_value);
option = ".band" + boost::lexical_cast<std::string>(i + 1) + "_end";
option_value = config_->property(role_ + option, default_bands[i]);
bands.push_back(option_value);
option = ".ampl" + boost::lexical_cast<std::string>(i + 1) + "_begin";
option_value = config_->property(role_ + option, default_bands[i]);
ampl.push_back(option_value);
option = ".ampl" + boost::lexical_cast<std::string>(i + 1) + "_end";
option_value = config_->property(role_ + option, default_bands[i]);
ampl.push_back(option_value);
option = ".band" + boost::lexical_cast<std::string>(i + 1) + "_error";
option_value = config_->property(role_ + option, default_bands[i]);
error_w.push_back(option_value);
}
int grid_density = config_->property(role_ + ".grid_density", default_grid_density);
taps_d = gr::filter::pm_remez(number_of_taps - 1, bands, ampl, error_w, filter_type, grid_density);
taps_.reserve(taps_d.size());
for (std::vector<double>::iterator it = taps_d.begin(); it != taps_d.end(); it++)
{
taps_.push_back(static_cast<float>(*it));
}
}
else
{
double default_bw = 2000000.0;
double bw_ = config_->property(role_ + ".bw", default_bw);
double default_tw = bw_ / 10.0;
double tw_ = config_->property(role_ + ".tw", default_tw);
taps_ = gr::filter::firdes::low_pass(1.0, sampling_freq_, bw_, tw_);
}
}

2
src/algorithms/input_filter/adapters/freq_xlating_fir_filter.h
View File

@@ -95,6 +95,7 @@ private:
gr::filter::freq_xlating_fir_filter_fcf::sptr freq_xlating_fir_filter_fcf_;
gr::filter::freq_xlating_fir_filter_scf::sptr freq_xlating_fir_filter_scf_;
ConfigurationInterface* config_;
int decimation_factor_;
bool dump_;
std::string dump_filename_;
std::string input_item_type_;
@@ -114,7 +115,6 @@ private:
gr::blocks::float_to_short::sptr float_to_short_2_;
short_x2_to_cshort_sptr short_x2_to_cshort_;
complex_float_to_complex_byte_sptr complex_to_complex_byte_;
void init();
};
#endif // GNSS_SDR_FREQ_XLATING_FIR_FILTER_H_

19
src/algorithms/libs/gnss_sdr_sample_counter.cc
View File

@@ -36,14 +36,16 @@
#include <iostream>
#include <string>
gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr::sync_decimator("sample_counter",
gr::io_signature::make(1, 1, _size),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
static_cast<unsigned int>(std::round(_fs * 0.001)))
gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, int _interval_ms, size_t _size) : gr::sync_decimator("sample_counter",
gr::io_signature::make(1, 1, _size),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
static_cast<unsigned int>(std::round(_fs * static_cast<double>(_interval_ms) / 1e3)))
{
message_port_register_out(pmt::mp("sample_counter"));
set_max_noutput_items(1);
samples_per_output = std::round(_fs * 0.001);
interval_ms = _interval_ms;
fs = _fs;
samples_per_output = std::round(fs * static_cast<double>(interval_ms) / 1e3);
sample_counter = 0;
current_T_rx_ms = 0;
current_s = 0;
@@ -58,9 +60,9 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr:
}
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size)
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size)
{
gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _size));
gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _interval_ms, _size));
return sample_counter_;
}
@@ -74,6 +76,7 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)),
out[0].Flag_valid_symbol_output = false;
out[0].Flag_valid_word = false;
out[0].Channel_ID = -1;
out[0].fs = fs;
if ((current_T_rx_ms % report_interval_ms) == 0)
{
current_s++;
@@ -134,6 +137,6 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)),
}
sample_counter += samples_per_output;
out[0].Tracking_sample_counter = sample_counter;
current_T_rx_ms++;
current_T_rx_ms += interval_ms;
return 1;
}

10
src/algorithms/libs/gnss_sdr_sample_counter.h
View File

@@ -39,14 +39,16 @@ class gnss_sdr_sample_counter;
typedef boost::shared_ptr<gnss_sdr_sample_counter> gnss_sdr_sample_counter_sptr;
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size);
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size);
class gnss_sdr_sample_counter : public gr::sync_decimator
{
private:
gnss_sdr_sample_counter(double _fs, size_t _size);
gnss_sdr_sample_counter(double _fs, int _interval_ms, size_t _size);
unsigned int samples_per_output;
unsigned long int sample_counter;
double fs;
unsigned long long int sample_counter;
int interval_ms;
long long int current_T_rx_ms; // Receiver time in ms since the beginning of the run
unsigned int current_s; // Receiver time in seconds, modulo 60
bool flag_m; // True if the receiver has been running for at least 1 minute
@@ -59,7 +61,7 @@ private:
bool flag_enable_send_msg;
public:
friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size);
friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size);
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);

281
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h Normal file
View File

@@ -0,0 +1,281 @@
/*!
* \file volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h
* \brief VOLK_GNSSSDR puppet for the multiple 32-bit float vector fast resampler kernel.
* \authors <ul>
* <li> Cillian O'Driscoll 2017 cillian.odriscoll at gmail dot com
* <li> Javier Arribas, 2018. javiarribas(at)gmail.com
* </ul>
*
* VOLK_GNSSSDR puppet for integrating the multiple resampler into the test system
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (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 <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef INCLUDED_volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_H
#define INCLUDED_volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_H
#include "volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h"
#include <volk_gnsssdr/volk_gnsssdr_malloc.h>
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <string.h>
#ifdef LV_HAVE_GENERIC
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_generic(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
int num_out_vectors = 3;
float rem_code_phase_chips = -0.8234;
float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0);
unsigned int n;
float shifts_chips[3] = {-0.1, 0.0, 0.1};
float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
for (n = 0; n < num_out_vectors; n++)
{
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_generic(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
for (n = 0; n < num_out_vectors; n++)
{
volk_gnsssdr_free(result_aux[n]);
}
volk_gnsssdr_free(result_aux);
}
#endif /* LV_HAVE_GENERIC */
#ifdef LV_HAVE_SSE3
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse3(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
int num_out_vectors = 3;
float rem_code_phase_chips = -0.8234;
float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0);
unsigned int n;
float shifts_chips[3] = {-0.1, 0.0, 0.1};
float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
for (n = 0; n < num_out_vectors; n++)
{
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse3(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
for (n = 0; n < num_out_vectors; n++)
{
volk_gnsssdr_free(result_aux[n]);
}
volk_gnsssdr_free(result_aux);
}
#endif
#ifdef LV_HAVE_SSE3
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
int num_out_vectors = 3;
float rem_code_phase_chips = -0.8234;
float code_phase_rate_step_chips = 1.0 / powf(2.0, 33.0);
unsigned int n;
float shifts_chips[3] = {-0.1, 0.0, 0.1};
float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
for (n = 0; n < num_out_vectors; n++)
{
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse3(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
for (n = 0; n < num_out_vectors; n++)
{
volk_gnsssdr_free(result_aux[n]);
}
volk_gnsssdr_free(result_aux);
}
#endif
//
//
//#ifdef LV_HAVE_SSE4_1
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_u_sse4_1(float* result, const float* local_code, unsigned int num_points)
//{
// int code_length_chips = 2046;
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
// int num_out_vectors = 3;
// float rem_code_phase_chips = -0.234;
// unsigned int n;
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
//
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
// for (n = 0; n < num_out_vectors; n++)
// {
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
// }
//
// volk_gnsssdr_32f_xn_resampler_32f_xn_u_sse4_1(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
//
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
//
// for (n = 0; n < num_out_vectors; n++)
// {
// volk_gnsssdr_free(result_aux[n]);
// }
// volk_gnsssdr_free(result_aux);
//}
//
//#endif
//
//#ifdef LV_HAVE_SSE4_1
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_a_sse4_1(float* result, const float* local_code, unsigned int num_points)
//{
// int code_length_chips = 2046;
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
// int num_out_vectors = 3;
// float rem_code_phase_chips = -0.234;
// unsigned int n;
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
//
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
// for (n = 0; n < num_out_vectors; n++)
// {
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
// }
//
// volk_gnsssdr_32f_xn_resampler_32f_xn_a_sse4_1(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
//
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
//
// for (n = 0; n < num_out_vectors; n++)
// {
// volk_gnsssdr_free(result_aux[n]);
// }
// volk_gnsssdr_free(result_aux);
//}
//
//#endif
//
//#ifdef LV_HAVE_AVX
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_a_avx(float* result, const float* local_code, unsigned int num_points)
//{
// int code_length_chips = 2046;
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
// int num_out_vectors = 3;
// float rem_code_phase_chips = -0.234;
// unsigned int n;
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
//
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
// for (n = 0; n < num_out_vectors; n++)
// {
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
// }
//
// volk_gnsssdr_32f_xn_resampler_32f_xn_a_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
//
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
//
// for (n = 0; n < num_out_vectors; n++)
// {
// volk_gnsssdr_free(result_aux[n]);
// }
// volk_gnsssdr_free(result_aux);
//}
//#endif
//
//
//#ifdef LV_HAVE_AVX
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_u_avx(float* result, const float* local_code, unsigned int num_points)
//{
// int code_length_chips = 2046;
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
// int num_out_vectors = 3;
// float rem_code_phase_chips = -0.234;
// unsigned int n;
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
//
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
// for (n = 0; n < num_out_vectors; n++)
// {
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
// }
//
// volk_gnsssdr_32f_xn_resampler_32f_xn_u_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
//
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
//
// for (n = 0; n < num_out_vectors; n++)
// {
// volk_gnsssdr_free(result_aux[n]);
// }
// volk_gnsssdr_free(result_aux);
//}
//#endif
//
//#ifdef LV_HAVE_NEONV7
//static inline void volk_gnsssdr_32f_resamplerxnpuppet_32f_neon(float* result, const float* local_code, unsigned int num_points)
//{
// int code_length_chips = 2046;
// float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
// int num_out_vectors = 3;
// float rem_code_phase_chips = -0.234;
// unsigned int n;
// float shifts_chips[3] = {-0.1, 0.0, 0.1};
//
// float** result_aux = (float**)volk_gnsssdr_malloc(sizeof(float*) * num_out_vectors, volk_gnsssdr_get_alignment());
// for (n = 0; n < num_out_vectors; n++)
// {
// result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
// }
//
// volk_gnsssdr_32f_xn_resampler_32f_xn_neon(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
//
// memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
//
// for (n = 0; n < num_out_vectors; n++)
// {
// volk_gnsssdr_free(result_aux[n]);
// }
// volk_gnsssdr_free(result_aux);
//}
//#endif
#endif // INCLUDED_volk_gnsssdr_32f_fast_resamplerpuppet_32f_H

657
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h Normal file
View File

@@ -0,0 +1,657 @@
/*!
* \file volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h
* \brief VOLK_GNSSSDR kernel: Resamples 1 complex 32-bit float vectors using zero hold resample algorithm
* and produces the delayed replicas by copying and rotating the resulting resampled signal.
* \authors <ul>
* <li> Cillian O'Driscoll, 2017. cillian.odirscoll(at)gmail.com
* <li> Javier Arribas, 2018. javiarribas(at)gmail.com
* </ul>
*
* VOLK_GNSSSDR kernel that resamples N 32-bit float vectors using zero hold resample algorithm.
* It is optimized to resample a single GNSS local code signal replica into 1 vector fractional-resampled and fractional-delayed
* and produces the delayed replicas by copying and rotating the resulting resampled signal.
* (i.e. it creates the Early, Prompt, and Late code replicas)
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (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 <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
/*!
* \page volk_gnsssdr_32f_xn_fast_resampler_32f_xn
*
* \b Overview
*
* Resamples a 32-bit floating point vector , providing \p num_out_vectors outputs.
*
* <b>Dispatcher Prototype</b>
* \code
* void volk_gnsssdr_32f_xn_fast_resampler_32f_xn(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
* \endcode
*
* \b Inputs
* \li local_code: Vector to be resampled.
* \li rem_code_phase_chips: Remnant code phase [chips].
* \li code_phase_step_chips: Phase increment per sample [chips/sample].
* \li code_phase_rate_step_chips: Phase rate increment per sample [chips/sample^2].
* \li shifts_chips: Vector of floats that defines the spacing (in chips) between the replicas of \p local_code
* \li code_length_chips: Code length in chips.
* \li num_out_vectors Number of output vectors.
* \li num_points: The number of data values to be in the resampled vector.
*
* \b Outputs
* \li result: Pointer to a vector of pointers where the results will be stored.
*
*/
#ifndef INCLUDED_volk_gnsssdr_32f_xn_fast_resampler_32f_xn_H
#define INCLUDED_volk_gnsssdr_32f_xn_fast_resampler_32f_xn_H
#include <assert.h>
#include <math.h>
#include <stdlib.h> /* abs */
#include <stdint.h> /* int64_t */
#include <stdio.h>
#include <volk_gnsssdr/volk_gnsssdr_common.h>
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
#ifdef LV_HAVE_GENERIC
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_generic(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
int local_code_chip_index;
int current_correlator_tap;
int n;
//first correlator
for (n = 0; n < num_points; n++)
{
// resample code for first tap
local_code_chip_index = (int)floor(code_phase_step_chips * (float)n + code_phase_rate_step_chips * (float)(n * n) + shifts_chips[0] - rem_code_phase_chips);
// Take into account that in multitap correlators, the shifts can be negative!
if (local_code_chip_index < 0) local_code_chip_index += (int)code_length_chips * (abs(local_code_chip_index) / code_length_chips + 1);
local_code_chip_index = local_code_chip_index % code_length_chips;
result[0][n] = local_code[local_code_chip_index];
}
//adjacent correlators
unsigned int shift_samples = 0;
for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++)
{
shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips);
memcpy(&result[current_correlator_tap][0], &result[0][shift_samples], (num_points - shift_samples) * sizeof(float));
memcpy(&result[current_correlator_tap][num_points - shift_samples], &result[0][0], shift_samples * sizeof(float));
}
}
#endif /*LV_HAVE_GENERIC*/
#ifdef LV_HAVE_SSE3
#include <pmmintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse3(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int quarterPoints = num_points / 4;
// int current_correlator_tap;
unsigned int n;
unsigned int k;
unsigned int current_correlator_tap;
const __m128 ones = _mm_set1_ps(1.0f);
const __m128 fours = _mm_set1_ps(4.0f);
const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips);
const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips);
const __m128 code_phase_rate_step_chips_reg = _mm_set_ps1(code_phase_rate_step_chips);
__VOLK_ATTR_ALIGNED(16)
int local_code_chip_index[4];
int local_code_chip_index_;
const __m128i zeros = _mm_setzero_si128();
const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips);
const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips);
__m128i local_code_chip_index_reg, aux_i, negatives;
__m128 aux, aux2, aux3, indexnn, shifts_chips_reg, i, fi, igx, j, c, cTrunc, base;
__m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
shifts_chips_reg = _mm_set_ps1((float)shifts_chips[0]);
aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
for (n = 0; n < quarterPoints; n++)
{
aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
indexnn = _mm_mul_ps(indexn, indexn);
aux3 = _mm_mul_ps(code_phase_rate_step_chips_reg, indexnn);
aux = _mm_add_ps(aux, aux3);
aux = _mm_add_ps(aux, aux2);
// floor
i = _mm_cvttps_epi32(aux);
fi = _mm_cvtepi32_ps(i);
igx = _mm_cmpgt_ps(fi, aux);
j = _mm_and_ps(igx, ones);
aux = _mm_sub_ps(fi, j);
// Correct negative shift
c = _mm_div_ps(aux, code_length_chips_reg_f);
aux3 = _mm_add_ps(c, ones);
i = _mm_cvttps_epi32(aux3);
cTrunc = _mm_cvtepi32_ps(i);
base = _mm_mul_ps(cTrunc, code_length_chips_reg_f);
local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base));
negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros);
aux_i = _mm_and_si128(code_length_chips_reg_i, negatives);
local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i);
_mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
for (k = 0; k < 4; ++k)
{
_result[0][n * 4 + k] = local_code[local_code_chip_index[k]];
}
indexn = _mm_add_ps(indexn, fours);
}
for (n = quarterPoints * 4; n < num_points; n++)
{
// resample code for first tap
local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + code_phase_rate_step_chips * (float)(n * n) + shifts_chips[0] - rem_code_phase_chips);
// Take into account that in multitap correlators, the shifts can be negative!
if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
_result[0][n] = local_code[local_code_chip_index_];
}
// adjacent correlators
unsigned int shift_samples = 0;
for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++)
{
shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips);
memcpy(&_result[current_correlator_tap][0], &_result[0][shift_samples], (num_points - shift_samples) * sizeof(float));
memcpy(&_result[current_correlator_tap][num_points - shift_samples], &_result[0][0], shift_samples * sizeof(float));
}
}
#endif
#ifdef LV_HAVE_SSE3
#include <pmmintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse3(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int quarterPoints = num_points / 4;
// int current_correlator_tap;
unsigned int n;
unsigned int k;
unsigned int current_correlator_tap;
const __m128 ones = _mm_set1_ps(1.0f);
const __m128 fours = _mm_set1_ps(4.0f);
const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips);
const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips);
const __m128 code_phase_rate_step_chips_reg = _mm_set_ps1(code_phase_rate_step_chips);
__VOLK_ATTR_ALIGNED(16)
int local_code_chip_index[4];
int local_code_chip_index_;
const __m128i zeros = _mm_setzero_si128();
const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips);
const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips);
__m128i local_code_chip_index_reg, aux_i, negatives;
__m128 aux, aux2, aux3, indexnn, shifts_chips_reg, i, fi, igx, j, c, cTrunc, base;
__m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
shifts_chips_reg = _mm_set_ps1((float)shifts_chips[0]);
aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
for (n = 0; n < quarterPoints; n++)
{
aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
indexnn = _mm_mul_ps(indexn, indexn);
aux3 = _mm_mul_ps(code_phase_rate_step_chips_reg, indexnn);
aux = _mm_add_ps(aux, aux3);
aux = _mm_add_ps(aux, aux2);
// floor
i = _mm_cvttps_epi32(aux);
fi = _mm_cvtepi32_ps(i);
igx = _mm_cmpgt_ps(fi, aux);
j = _mm_and_ps(igx, ones);
aux = _mm_sub_ps(fi, j);
// Correct negative shift
c = _mm_div_ps(aux, code_length_chips_reg_f);
aux3 = _mm_add_ps(c, ones);
i = _mm_cvttps_epi32(aux3);
cTrunc = _mm_cvtepi32_ps(i);
base = _mm_mul_ps(cTrunc, code_length_chips_reg_f);
local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base));
negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros);
aux_i = _mm_and_si128(code_length_chips_reg_i, negatives);
local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i);
_mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
for (k = 0; k < 4; ++k)
{
_result[0][n * 4 + k] = local_code[local_code_chip_index[k]];
}
indexn = _mm_add_ps(indexn, fours);
}
for (n = quarterPoints * 4; n < num_points; n++)
{
// resample code for first tap
local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + code_phase_rate_step_chips * (float)(n * n) + shifts_chips[0] - rem_code_phase_chips);
// Take into account that in multitap correlators, the shifts can be negative!
if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
_result[0][n] = local_code[local_code_chip_index_];
}
// adjacent correlators
unsigned int shift_samples = 0;
for (current_correlator_tap = 1; current_correlator_tap < num_out_vectors; current_correlator_tap++)
{
shift_samples += (int)round((shifts_chips[current_correlator_tap] - shifts_chips[current_correlator_tap - 1]) / code_phase_step_chips);
memcpy(&_result[current_correlator_tap][0], &_result[0][shift_samples], (num_points - shift_samples) * sizeof(float));
memcpy(&_result[current_correlator_tap][num_points - shift_samples], &_result[0][0], shift_samples * sizeof(float));
}
}
#endif
//
//
//#ifdef LV_HAVE_SSE4_1
//#include <smmintrin.h>
//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse4_1(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
//{
// float** _result = result;
// const unsigned int quarterPoints = num_points / 4;
// int current_correlator_tap;
// unsigned int n;
// unsigned int k;
// const __m128 fours = _mm_set1_ps(4.0f);
// const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips);
// const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips);
//
// __VOLK_ATTR_ALIGNED(16)
// int local_code_chip_index[4];
// int local_code_chip_index_;
//
// const __m128i zeros = _mm_setzero_si128();
// const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips);
// const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips);
// __m128i local_code_chip_index_reg, aux_i, negatives, i;
// __m128 aux, aux2, shifts_chips_reg, c, cTrunc, base;
//
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
// {
// shifts_chips_reg = _mm_set_ps1((float)shifts_chips[current_correlator_tap]);
// aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
// __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
// for (n = 0; n < quarterPoints; n++)
// {
// aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
// aux = _mm_add_ps(aux, aux2);
// // floor
// aux = _mm_floor_ps(aux);
//
// // fmod
// c = _mm_div_ps(aux, code_length_chips_reg_f);
// i = _mm_cvttps_epi32(c);
// cTrunc = _mm_cvtepi32_ps(i);
// base = _mm_mul_ps(cTrunc, code_length_chips_reg_f);
// local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base));
//
// negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros);
// aux_i = _mm_and_si128(code_length_chips_reg_i, negatives);
// local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i);
// _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
// for (k = 0; k < 4; ++k)
// {
// _result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]];
// }
// indexn = _mm_add_ps(indexn, fours);
// }
// for (n = quarterPoints * 4; n < num_points; n++)
// {
// // resample code for current tap
// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
// //Take into account that in multitap correlators, the shifts can be negative!
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
// }
// }
//}
//
//#endif
//
//
//#ifdef LV_HAVE_SSE4_1
//#include <smmintrin.h>
//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse4_1(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
//{
// float** _result = result;
// const unsigned int quarterPoints = num_points / 4;
// int current_correlator_tap;
// unsigned int n;
// unsigned int k;
// const __m128 fours = _mm_set1_ps(4.0f);
// const __m128 rem_code_phase_chips_reg = _mm_set_ps1(rem_code_phase_chips);
// const __m128 code_phase_step_chips_reg = _mm_set_ps1(code_phase_step_chips);
//
// __VOLK_ATTR_ALIGNED(16)
// int local_code_chip_index[4];
// int local_code_chip_index_;
//
// const __m128i zeros = _mm_setzero_si128();
// const __m128 code_length_chips_reg_f = _mm_set_ps1((float)code_length_chips);
// const __m128i code_length_chips_reg_i = _mm_set1_epi32((int)code_length_chips);
// __m128i local_code_chip_index_reg, aux_i, negatives, i;
// __m128 aux, aux2, shifts_chips_reg, c, cTrunc, base;
//
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
// {
// shifts_chips_reg = _mm_set_ps1((float)shifts_chips[current_correlator_tap]);
// aux2 = _mm_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
// __m128 indexn = _mm_set_ps(3.0f, 2.0f, 1.0f, 0.0f);
// for (n = 0; n < quarterPoints; n++)
// {
// aux = _mm_mul_ps(code_phase_step_chips_reg, indexn);
// aux = _mm_add_ps(aux, aux2);
// // floor
// aux = _mm_floor_ps(aux);
//
// // fmod
// c = _mm_div_ps(aux, code_length_chips_reg_f);
// i = _mm_cvttps_epi32(c);
// cTrunc = _mm_cvtepi32_ps(i);
// base = _mm_mul_ps(cTrunc, code_length_chips_reg_f);
// local_code_chip_index_reg = _mm_cvtps_epi32(_mm_sub_ps(aux, base));
//
// negatives = _mm_cmplt_epi32(local_code_chip_index_reg, zeros);
// aux_i = _mm_and_si128(code_length_chips_reg_i, negatives);
// local_code_chip_index_reg = _mm_add_epi32(local_code_chip_index_reg, aux_i);
// _mm_store_si128((__m128i*)local_code_chip_index, local_code_chip_index_reg);
// for (k = 0; k < 4; ++k)
// {
// _result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]];
// }
// indexn = _mm_add_ps(indexn, fours);
// }
// for (n = quarterPoints * 4; n < num_points; n++)
// {
// // resample code for current tap
// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
// //Take into account that in multitap correlators, the shifts can be negative!
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
// }
// }
//}
//
//#endif
//
//
//#ifdef LV_HAVE_AVX
//#include <immintrin.h>
//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
//{
// float** _result = result;
// const unsigned int avx_iters = num_points / 8;
// int current_correlator_tap;
// unsigned int n;
// unsigned int k;
// const __m256 eights = _mm256_set1_ps(8.0f);
// const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips);
// const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips);
//
// __VOLK_ATTR_ALIGNED(32)
// int local_code_chip_index[8];
// int local_code_chip_index_;
//
// const __m256 zeros = _mm256_setzero_ps();
// const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips);
// const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f);
//
// __m256i local_code_chip_index_reg, i;
// __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn;
//
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
// {
// shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[current_correlator_tap]);
// aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
// indexn = n0;
// for (n = 0; n < avx_iters; n++)
// {
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][8 * n + 7], 1, 0);
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3);
// aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn);
// aux = _mm256_add_ps(aux, aux2);
// // floor
// aux = _mm256_floor_ps(aux);
//
// // fmod
// c = _mm256_div_ps(aux, code_length_chips_reg_f);
// i = _mm256_cvttps_epi32(c);
// cTrunc = _mm256_cvtepi32_ps(i);
// base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f);
// local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base));
//
// // no negatives
// c = _mm256_cvtepi32_ps(local_code_chip_index_reg);
// negatives = _mm256_cmp_ps(c, zeros, 0x01);
// aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
// aux = _mm256_add_ps(c, aux3);
// local_code_chip_index_reg = _mm256_cvttps_epi32(aux);
//
// _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg);
// for (k = 0; k < 8; ++k)
// {
// _result[current_correlator_tap][n * 8 + k] = local_code[local_code_chip_index[k]];
// }
// indexn = _mm256_add_ps(indexn, eights);
// }
// }
// _mm256_zeroupper();
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
// {
// for (n = avx_iters * 8; n < num_points; n++)
// {
// // resample code for current tap
// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
// //Take into account that in multitap correlators, the shifts can be negative!
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
// }
// }
//}
//
//#endif
//
//
//#ifdef LV_HAVE_AVX
//#include <immintrin.h>
//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
//{
// float** _result = result;
// const unsigned int avx_iters = num_points / 8;
// int current_correlator_tap;
// unsigned int n;
// unsigned int k;
// const __m256 eights = _mm256_set1_ps(8.0f);
// const __m256 rem_code_phase_chips_reg = _mm256_set1_ps(rem_code_phase_chips);
// const __m256 code_phase_step_chips_reg = _mm256_set1_ps(code_phase_step_chips);
//
// __VOLK_ATTR_ALIGNED(32)
// int local_code_chip_index[8];
// int local_code_chip_index_;
//
// const __m256 zeros = _mm256_setzero_ps();
// const __m256 code_length_chips_reg_f = _mm256_set1_ps((float)code_length_chips);
// const __m256 n0 = _mm256_set_ps(7.0f, 6.0f, 5.0f, 4.0f, 3.0f, 2.0f, 1.0f, 0.0f);
//
// __m256i local_code_chip_index_reg, i;
// __m256 aux, aux2, aux3, shifts_chips_reg, c, cTrunc, base, negatives, indexn;
//
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
// {
// shifts_chips_reg = _mm256_set1_ps((float)shifts_chips[current_correlator_tap]);
// aux2 = _mm256_sub_ps(shifts_chips_reg, rem_code_phase_chips_reg);
// indexn = n0;
// for (n = 0; n < avx_iters; n++)
// {
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][8 * n + 7], 1, 0);
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&local_code_chip_index[8], 1, 3);
// aux = _mm256_mul_ps(code_phase_step_chips_reg, indexn);
// aux = _mm256_add_ps(aux, aux2);
// // floor
// aux = _mm256_floor_ps(aux);
//
// // fmod
// c = _mm256_div_ps(aux, code_length_chips_reg_f);
// i = _mm256_cvttps_epi32(c);
// cTrunc = _mm256_cvtepi32_ps(i);
// base = _mm256_mul_ps(cTrunc, code_length_chips_reg_f);
// local_code_chip_index_reg = _mm256_cvttps_epi32(_mm256_sub_ps(aux, base));
//
// // no negatives
// c = _mm256_cvtepi32_ps(local_code_chip_index_reg);
// negatives = _mm256_cmp_ps(c, zeros, 0x01);
// aux3 = _mm256_and_ps(code_length_chips_reg_f, negatives);
// aux = _mm256_add_ps(c, aux3);
// local_code_chip_index_reg = _mm256_cvttps_epi32(aux);
//
// _mm256_store_si256((__m256i*)local_code_chip_index, local_code_chip_index_reg);
// for (k = 0; k < 8; ++k)
// {
// _result[current_correlator_tap][n * 8 + k] = local_code[local_code_chip_index[k]];
// }
// indexn = _mm256_add_ps(indexn, eights);
// }
// }
// _mm256_zeroupper();
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
// {
// for (n = avx_iters * 8; n < num_points; n++)
// {
// // resample code for current tap
// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
// //Take into account that in multitap correlators, the shifts can be negative!
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
// }
// }
//}
//
//#endif
//
//
//#ifdef LV_HAVE_NEONV7
//#include <arm_neon.h>
//
//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_neon(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
//{
// float** _result = result;
// const unsigned int neon_iters = num_points / 4;
// int current_correlator_tap;
// unsigned int n;
// unsigned int k;
// const int32x4_t ones = vdupq_n_s32(1);
// const float32x4_t fours = vdupq_n_f32(4.0f);
// const float32x4_t rem_code_phase_chips_reg = vdupq_n_f32(rem_code_phase_chips);
// const float32x4_t code_phase_step_chips_reg = vdupq_n_f32(code_phase_step_chips);
//
// __VOLK_ATTR_ALIGNED(16)
// int32_t local_code_chip_index[4];
// int32_t local_code_chip_index_;
//
// const int32x4_t zeros = vdupq_n_s32(0);
// const float32x4_t code_length_chips_reg_f = vdupq_n_f32((float)code_length_chips);
// const int32x4_t code_length_chips_reg_i = vdupq_n_s32((int32_t)code_length_chips);
// int32x4_t local_code_chip_index_reg, aux_i, negatives, i;
// float32x4_t aux, aux2, shifts_chips_reg, fi, c, j, cTrunc, base, indexn, reciprocal;
// __VOLK_ATTR_ALIGNED(16)
// const float vec[4] = {0.0f, 1.0f, 2.0f, 3.0f};
// uint32x4_t igx;
// reciprocal = vrecpeq_f32(code_length_chips_reg_f);
// reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal);
// reciprocal = vmulq_f32(vrecpsq_f32(code_length_chips_reg_f, reciprocal), reciprocal); // this refinement is required!
// float32x4_t n0 = vld1q_f32((float*)vec);
//
// for (current_correlator_tap = 0; current_correlator_tap < num_out_vectors; current_correlator_tap++)
// {
// shifts_chips_reg = vdupq_n_f32((float)shifts_chips[current_correlator_tap]);
// aux2 = vsubq_f32(shifts_chips_reg, rem_code_phase_chips_reg);
// indexn = n0;
// for (n = 0; n < neon_iters; n++)
// {
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][4 * n + 3], 1, 0);
// __VOLK_GNSSSDR_PREFETCH(&local_code_chip_index[4]);
// aux = vmulq_f32(code_phase_step_chips_reg, indexn);
// aux = vaddq_f32(aux, aux2);
//
// //floor
// i = vcvtq_s32_f32(aux);
// fi = vcvtq_f32_s32(i);
// igx = vcgtq_f32(fi, aux);
// j = vcvtq_f32_s32(vandq_s32(vreinterpretq_s32_u32(igx), ones));
// aux = vsubq_f32(fi, j);
//
// // fmod
// c = vmulq_f32(aux, reciprocal);
// i = vcvtq_s32_f32(c);
// cTrunc = vcvtq_f32_s32(i);
// base = vmulq_f32(cTrunc, code_length_chips_reg_f);
// aux = vsubq_f32(aux, base);
// local_code_chip_index_reg = vcvtq_s32_f32(aux);
//
// negatives = vreinterpretq_s32_u32(vcltq_s32(local_code_chip_index_reg, zeros));
// aux_i = vandq_s32(code_length_chips_reg_i, negatives);
// local_code_chip_index_reg = vaddq_s32(local_code_chip_index_reg, aux_i);
//
// vst1q_s32((int32_t*)local_code_chip_index, local_code_chip_index_reg);
//
// for (k = 0; k < 4; ++k)
// {
// _result[current_correlator_tap][n * 4 + k] = local_code[local_code_chip_index[k]];
// }
// indexn = vaddq_f32(indexn, fours);
// }
// for (n = neon_iters * 4; n < num_points; n++)
// {
// __VOLK_GNSSSDR_PREFETCH_LOCALITY(&_result[current_correlator_tap][n], 1, 0);
// // resample code for current tap
// local_code_chip_index_ = (int)floor(code_phase_step_chips * (float)n + shifts_chips[current_correlator_tap] - rem_code_phase_chips);
// //Take into account that in multitap correlators, the shifts can be negative!
// if (local_code_chip_index_ < 0) local_code_chip_index_ += (int)code_length_chips * (abs(local_code_chip_index_) / code_length_chips + 1);
// local_code_chip_index_ = local_code_chip_index_ % code_length_chips;
// _result[current_correlator_tap][n] = local_code[local_code_chip_index_];
// }
// }
//}
//
//#endif
#endif /*INCLUDED_volk_gnsssdr_32f_xn_fast_resampler_32f_xn_H*/

1
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h
View File

@@ -93,6 +93,7 @@ std::vector<volk_gnsssdr_test_case_t> init_test_list(volk_gnsssdr_test_params_t
QA(VOLK_INIT_PUPP(volk_gnsssdr_16i_resamplerxnpuppet_16i, volk_gnsssdr_16i_xn_resampler_16i_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_resamplerxnpuppet_32fc, volk_gnsssdr_32fc_xn_resampler_32fc_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_32f_resamplerxnpuppet_32f, volk_gnsssdr_32f_xn_resampler_32f_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_32f_fast_resamplerxnpuppet_32f, volk_gnsssdr_32f_xn_fast_resampler_32f_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_x2_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_x2_dot_prod_16ic_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_x2_rotator_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn, test_params_int16))
QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_16i_rotator_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_16i_rotator_dot_prod_16ic_xn, test_params_int16))

539
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@@ -32,12 +32,12 @@
#include "hybrid_observables_cc.h"
#include "display.h"
#include "GPS_L1_CA.h"
#include <armadillo>
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
#include <matio.h>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <limits>
@@ -59,15 +59,11 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro)))
{
d_dump = dump;
d_nchannels = nchannels_out;
d_nchannels_out = nchannels_out;
d_nchannels_in = nchannels_in;
d_dump_filename = dump_filename;
T_rx_s = 0.0;
T_rx_step_ms = 1; // 1 ms
max_delta = 1.5; // 1.5 s
d_latency = 0.5; // 300 ms
valid_channels.resize(d_nchannels, false);
d_num_valid_channels = 0;
d_gnss_synchro_history = new Gnss_circular_deque<Gnss_Synchro>(static_cast<unsigned int>(max_delta * 1000.0), d_nchannels);
T_rx_clock_step_samples = 0;
d_gnss_synchro_history = new Gnss_circular_deque<Gnss_Synchro>(500, d_nchannels_out);
// ############# ENABLE DATA FILE LOG #################
if (d_dump)
@@ -88,7 +84,12 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
}
}
T_rx_TOW_ms = 0;
T_rx_TOW_offset_ms = 0;
T_rx_TOW_set = false;
//rework
d_Rx_clock_buffer.resize(10); //10*20ms= 200 ms of data in buffer
d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
}
@@ -120,7 +121,7 @@ int hybrid_observables_cc::save_matfile()
// READ DUMP FILE
std::ifstream::pos_type size;
int number_of_double_vars = 7;
int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels;
int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out;
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@@ -144,15 +145,15 @@ int hybrid_observables_cc::save_matfile()
{
return 1;
}
double **RX_time = new double *[d_nchannels];
double **TOW_at_current_symbol_s = new double *[d_nchannels];
double **Carrier_Doppler_hz = new double *[d_nchannels];
double **Carrier_phase_cycles = new double *[d_nchannels];
double **Pseudorange_m = new double *[d_nchannels];
double **PRN = new double *[d_nchannels];
double **Flag_valid_pseudorange = new double *[d_nchannels];
double **RX_time = new double *[d_nchannels_out];
double **TOW_at_current_symbol_s = new double *[d_nchannels_out];
double **Carrier_Doppler_hz = new double *[d_nchannels_out];
double **Carrier_phase_cycles = new double *[d_nchannels_out];
double **Pseudorange_m = new double *[d_nchannels_out];
double **PRN = new double *[d_nchannels_out];
double **Flag_valid_pseudorange = new double *[d_nchannels_out];
for (unsigned int i = 0; i < d_nchannels; i++)
for (unsigned int i = 0; i < d_nchannels_out; i++)
{
RX_time[i] = new double[num_epoch];
TOW_at_current_symbol_s[i] = new double[num_epoch];
@@ -169,7 +170,7 @@ int hybrid_observables_cc::save_matfile()
{
for (long int i = 0; i < num_epoch; i++)
{
for (unsigned int chan = 0; chan < d_nchannels; chan++)
for (unsigned int chan = 0; chan < d_nchannels_out; chan++)
{
dump_file.read(reinterpret_cast<char *>(&RX_time[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_s[chan][i]), sizeof(double));
@@ -186,7 +187,7 @@ int hybrid_observables_cc::save_matfile()
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
for (unsigned int i = 0; i < d_nchannels; i++)
for (unsigned int i = 0; i < d_nchannels_out; i++)
{
delete[] RX_time[i];
delete[] TOW_at_current_symbol_s[i];
@@ -207,17 +208,17 @@ int hybrid_observables_cc::save_matfile()
return 1;
}
double *RX_time_aux = new double[d_nchannels * num_epoch];
double *TOW_at_current_symbol_s_aux = new double[d_nchannels * num_epoch];
double *Carrier_Doppler_hz_aux = new double[d_nchannels * num_epoch];
double *Carrier_phase_cycles_aux = new double[d_nchannels * num_epoch];
double *Pseudorange_m_aux = new double[d_nchannels * num_epoch];
double *PRN_aux = new double[d_nchannels * num_epoch];
double *Flag_valid_pseudorange_aux = new double[d_nchannels * num_epoch];
double *RX_time_aux = new double[d_nchannels_out * num_epoch];
double *TOW_at_current_symbol_s_aux = new double[d_nchannels_out * num_epoch];
double *Carrier_Doppler_hz_aux = new double[d_nchannels_out * num_epoch];
double *Carrier_phase_cycles_aux = new double[d_nchannels_out * num_epoch];
double *Pseudorange_m_aux = new double[d_nchannels_out * num_epoch];
double *PRN_aux = new double[d_nchannels_out * num_epoch];
double *Flag_valid_pseudorange_aux = new double[d_nchannels_out * num_epoch];
unsigned int k = 0;
for (long int j = 0; j < num_epoch; j++)
{
for (unsigned int i = 0; i < d_nchannels; i++)
for (unsigned int i = 0; i < d_nchannels_out; i++)
{
RX_time_aux[k] = RX_time[i][j];
TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j];
@@ -242,7 +243,7 @@ int hybrid_observables_cc::save_matfile()
matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
if (reinterpret_cast<long *>(matfp) != NULL)
{
size_t dims[2] = {static_cast<size_t>(d_nchannels), static_cast<size_t>(num_epoch)};
size_t dims[2] = {static_cast<size_t>(d_nchannels_out), static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
@@ -273,7 +274,7 @@ int hybrid_observables_cc::save_matfile()
}
Mat_Close(matfp);
for (unsigned int i = 0; i < d_nchannels; i++)
for (unsigned int i = 0; i < d_nchannels_out; i++)
{
delete[] RX_time[i];
delete[] TOW_at_current_symbol_s[i];
@@ -302,174 +303,166 @@ int hybrid_observables_cc::save_matfile()
}
bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned int &ch, const double &ti)
double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro &a)
{
if ((ti < d_gnss_synchro_history->front(ch).RX_time) or (ti > d_gnss_synchro_history->back(ch).RX_time))
return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs));
}
bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const unsigned int &ch, const unsigned long int &rx_clock)
{
int nearest_element = -1;
long int abs_diff;
long int old_abs_diff = std::numeric_limits<long int>::max();
for (unsigned int i = 0; i < d_gnss_synchro_history->size(ch); i++)
{
abs_diff = labs(static_cast<long int>(rx_clock) - static_cast<long int>(d_gnss_synchro_history->at(ch, i).Tracking_sample_counter));
if (old_abs_diff > abs_diff)
{
old_abs_diff = abs_diff;
nearest_element = i;
}
}
if (nearest_element != -1 and nearest_element != static_cast<int>(d_gnss_synchro_history->size(ch)))
{
if ((static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs)) < 0.02)
{
int neighbor_element;
if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter)
{
neighbor_element = nearest_element + 1;
}
else
{
neighbor_element = nearest_element - 1;
}
if (neighbor_element < static_cast<int>(d_gnss_synchro_history->size(ch)) and neighbor_element >= 0)
{
int t1_idx;
int t2_idx;
if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter)
{
//std::cout << "S1= " << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter << std::endl;
t1_idx = nearest_element;
t2_idx = neighbor_element;
}
else
{
//std::cout << "inv S1= " << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter << std::endl;
t1_idx = neighbor_element;
t2_idx = nearest_element;
}
// 1st: copy the nearest gnss_synchro data for that channel
interpolated_obs = d_gnss_synchro_history->at(ch, nearest_element);
// 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
double T_rx_s = static_cast<double>(rx_clock) / static_cast<double>(interpolated_obs.fs);
double time_factor = (T_rx_s - d_gnss_synchro_history->at(ch, t1_idx).RX_time) /
(d_gnss_synchro_history->at(ch, t2_idx).RX_time -
d_gnss_synchro_history->at(ch, t1_idx).RX_time);
// CARRIER PHASE INTERPOLATION
interpolated_obs.Carrier_phase_rads = d_gnss_synchro_history->at(ch, t1_idx).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, t2_idx).Carrier_phase_rads - d_gnss_synchro_history->at(ch, t1_idx).Carrier_phase_rads) * time_factor;
// CARRIER DOPPLER INTERPOLATION
interpolated_obs.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, t1_idx).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, t2_idx).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, t1_idx).Carrier_Doppler_hz) * time_factor;
// TOW INTERPOLATION
interpolated_obs.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms) - static_cast<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor;
//
// std::cout << "Rx samplestamp: " << T_rx_s << " Channel " << ch << " interp buff idx " << nearest_element
// << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs) << " s)\n";
return true;
}
else
{
return false;
}
}
else
{
// std::cout << "ALERT: Channel " << ch << " interp buff idx " << nearest_element
// << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs) << " s)\n";
// usleep(1000);
return false;
}
}
else
{
return false;
}
find_interp_elements(ch, ti);
// 1st: copy the nearest gnss_synchro data for that channel
out = d_gnss_synchro_history->at(ch, 0);
// 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
// CARRIER PHASE INTERPOLATION
out.Carrier_phase_rads = d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, 1).Carrier_phase_rads - d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
// CARRIER DOPPLER INTERPOLATION
out.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, 1).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
// TOW INTERPOLATION
out.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_ms) - static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms)) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
return true;
}
void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required)
{
for (int n = 0; n < static_cast<int>(d_nchannels_in) - 1; n++)
{
ninput_items_required[n] = 0;
}
//last input channel is the sample counter, triggered each ms
ninput_items_required[d_nchannels_in - 1] = 1;
}
double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro &a)
{
if (a.Flag_valid_word)
{
return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs));
}
else
{
return 0.0;
}
}
void hybrid_observables_cc::find_interp_elements(const unsigned int &ch, const double &ti)
{
unsigned int closest = 0;
double dif = std::numeric_limits<double>::max();
double dt = 0.0;
for (unsigned int i = 0; i < d_gnss_synchro_history->size(ch); i++)
{
dt = std::fabs(ti - d_gnss_synchro_history->at(ch, i).RX_time);
if (dt < dif)
{
closest = i;
dif = dt;
}
else
{
break;
}
}
if (ti > d_gnss_synchro_history->at(ch, closest).RX_time)
{
while (closest > 0)
{
d_gnss_synchro_history->pop_front(ch);
closest--;
}
}
else
{
while (closest > 1)
{
d_gnss_synchro_history->pop_front(ch);
closest--;
}
}
}
void hybrid_observables_cc::forecast(int noutput_items, gr_vector_int &ninput_items_required)
{
for (unsigned int i = 0; i < d_nchannels; i++)
{
ninput_items_required[i] = 0;
}
ninput_items_required[d_nchannels] = noutput_items;
}
void hybrid_observables_cc::clean_history(unsigned int pos)
{
while (d_gnss_synchro_history->size(pos) > 0)
{
if ((T_rx_s - d_gnss_synchro_history->front(pos).RX_time) > max_delta)
{
d_gnss_synchro_history->pop_front(pos);
}
else
{
return;
}
}
}
void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Synchro> &data)
void hybrid_observables_cc::update_TOW(std::vector<Gnss_Synchro> &data)
{
//1. Set the TOW using the minimum TOW in the observables.
// this will be the receiver time.
//2. If the TOW is set, it must be incremented by the desired receiver time step.
// the time step must match the observables timer block (connected to the las input channel)
std::vector<Gnss_Synchro>::iterator it;
/////////////////////// DEBUG //////////////////////////
// Logs if there is a pseudorange difference between
// signals of the same satellite higher than a threshold
////////////////////////////////////////////////////////
#ifndef NDEBUG
std::vector<Gnss_Synchro>::iterator it2;
double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters
for (it = data.begin(); it != (data.end() - 1); it++)
// if (!T_rx_TOW_set)
// {
//unsigned int TOW_ref = std::numeric_limits<unsigned int>::max();
unsigned int TOW_ref = 0;
for (it = data.begin(); it != data.end(); it++)
{
for (it2 = it + 1; it2 != data.end(); it2++)
{
if (it->PRN == it2->PRN and it->System == it2->System)
{
double tow_dif_ = std::fabs(it->TOW_at_current_symbol_ms - it2->TOW_at_current_symbol_ms);
if (tow_dif_ > thr_ * 1000.0)
{
DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal
<< ". TOW difference in PRN " << it->PRN
<< " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT
<< " meters in pseudorange";
std::cout << TEXT_RED << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal
<< ". TOW difference in PRN " << it->PRN
<< " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT
<< " meters in pseudorange" << TEXT_RESET << std::endl;
}
}
}
}
#endif
if (!T_rx_TOW_set)
{
unsigned int TOW_ref = std::numeric_limits<unsigned int>::lowest();
for (it = data.begin(); it != data.end(); it++)
if (it->Flag_valid_word)
{
if (it->TOW_at_current_symbol_ms > TOW_ref)
{
TOW_ref = it->TOW_at_current_symbol_ms;
T_rx_TOW_set = true;
}
}
T_rx_TOW_ms = TOW_ref;
T_rx_TOW_set = true;
}
else
{
T_rx_TOW_ms += T_rx_step_ms;
//todo: check what happens during the week rollover
if (T_rx_TOW_ms >= 604800000)
{
T_rx_TOW_ms = T_rx_TOW_ms % 604800000;
}
}
T_rx_TOW_ms = TOW_ref;
//}
// else
// {
// T_rx_TOW_ms += T_rx_step_ms;
// //todo: check what happens during the week rollover
// if (T_rx_TOW_ms >= 604800000)
// {
// T_rx_TOW_ms = T_rx_TOW_ms % 604800000;
// }
// }
// std::cout << "T_rx_TOW_ms: " << T_rx_TOW_ms << std::endl;
}
void hybrid_observables_cc::compute_pranges(std::vector<Gnss_Synchro> &data)
{
std::vector<Gnss_Synchro>::iterator it;
for (it = data.begin(); it != data.end(); it++)
{
double traveltime_s = (static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
//std::cout.precision(17);
//std::cout << "Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
it->RX_time = (T_rx_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
if (it->Flag_valid_word)
{
double traveltime_s = (static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
//todo: check what happens during the week rollover (TOW rollover at 604800000s)
it->RX_time = (static_cast<double>(T_rx_TOW_ms) + GPS_STARTOFFSET_ms) / 1000.0;
it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
it->Flag_valid_pseudorange = true;
//debug code
// std::cout.precision(17);
// std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl;
// std::cout << "[" << it->Channel_ID << "] Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
// std::cout << "[" << it->Channel_ID << "] Pseudorange_m: " << it->Pseudorange_m << std::endl;
}
}
// usleep(1000);
}
@@ -480,156 +473,105 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]);
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
unsigned int i;
unsigned int returned_elements = 0;
int total_input_items = 0;
for (i = 0; i < d_nchannels; i++)
//push receiver clock into history buffer (connected to the last of the input channels)
//The clock buffer gives time to the channels to compute the tracking observables
if (ninput_items[d_nchannels_in - 1] > 0)
{
total_input_items += ninput_items[i];
d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter);
if (T_rx_clock_step_samples == 0)
{
T_rx_clock_step_samples = std::round(static_cast<double>(in[d_nchannels_in - 1][0].fs) * 1e-3); // 1 ms
std::cout << "Observables clock step samples set to " << T_rx_clock_step_samples << std::endl;
usleep(1000000);
}
//consume one item from the clock channel (last of the input channels)
consume(d_nchannels_in - 1, 1);
}
for (int epoch = 0; epoch < ninput_items[d_nchannels]; epoch++)
//push the tracking observables into buffers to allow the observable interpolation at the desired Rx clock
for (unsigned int n = 0; n < d_nchannels_out; n++)
{
T_rx_s += (static_cast<double>(T_rx_step_ms) / 1000.0);
//////////////////////////////////////////////////////////////////////////
if ((total_input_items == 0) and (d_num_valid_channels == 0))
// push the valid tracking Gnss_Synchros to their corresponding deque
for (int m = 0; m < ninput_items[n]; m++)
{
consume(d_nchannels, epoch + 1);
return returned_elements;
}
//////////////////////////////////////////////////////////////////////////
if (total_input_items > 0 and epoch == 0)
{
for (i = 0; i < d_nchannels; i++)
if (in[n][m].Flag_valid_word)
{
if (ninput_items[i] > 0)
if (d_gnss_synchro_history->size(n) > 0)
{
// Add the new Gnss_Synchros to their corresponding deque
for (int aux = 0; aux < ninput_items[i]; aux++)
// Check if the last Gnss_Synchro comes from the same satellite as the previous ones
if (d_gnss_synchro_history->front(n).PRN != in[n][m].PRN)
{
if (in[i][aux].Flag_valid_word)
{
d_gnss_synchro_history->push_back(i, in[i][aux]);
d_gnss_synchro_history->back(i).RX_time = compute_T_rx_s(in[i][aux]);
// Check if the last Gnss_Synchro comes from the same satellite as the previous ones
if (d_gnss_synchro_history->size(i) > 1)
{
if (d_gnss_synchro_history->front(i).PRN != d_gnss_synchro_history->back(i).PRN)
{
d_gnss_synchro_history->clear(i);
}
}
}
d_gnss_synchro_history->clear(n);
}
consume(i, ninput_items[i]);
}
d_gnss_synchro_history->push_back(n, in[n][m]);
d_gnss_synchro_history->back(n).RX_time = compute_T_rx_s(in[n][m]);
}
}
consume(n, ninput_items[n]);
}
for (i = 0; i < d_nchannels; i++)
{
if (d_gnss_synchro_history->size(i) > 2)
{
valid_channels[i] = true;
}
else
{
valid_channels[i] = false;
}
}
d_num_valid_channels = valid_channels.count();
// Check if there is any valid channel after reading the new incoming Gnss_Synchro data
if (d_num_valid_channels == 0)
{
consume(d_nchannels, epoch + 1);
return returned_elements;
}
for (i = 0; i < d_nchannels; i++) // Discard observables with T_rx higher than the threshold
{
if (valid_channels[i])
{
clean_history(i);
if (d_gnss_synchro_history->size(i) < 2)
{
valid_channels[i] = false;
}
}
}
// Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time
d_num_valid_channels = valid_channels.count();
double T_rx_s_out = T_rx_s - d_latency;
if ((d_num_valid_channels == 0) or (T_rx_s_out < 0.0))
{
consume(d_nchannels, epoch + 1);
return returned_elements;
}
if (d_Rx_clock_buffer.size() == d_Rx_clock_buffer.capacity())
{
std::vector<Gnss_Synchro> epoch_data;
for (i = 0; i < d_nchannels; i++)
int n_valid = 0;
for (unsigned int n = 0; n < d_nchannels_out; n++)
{
if (valid_channels[i])
Gnss_Synchro interpolated_gnss_synchro;
if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front() + T_rx_TOW_offset_ms * T_rx_clock_step_samples))
{
Gnss_Synchro interpolated_gnss_synchro; // empty set, it is required to COPY the nearest in the interpolation history = d_gnss_synchro_history->back(i);
if (interpolate_data(interpolated_gnss_synchro, i, T_rx_s_out))
{
epoch_data.push_back(interpolated_gnss_synchro);
}
else
{
valid_channels[i] = false;
}
}
}
d_num_valid_channels = valid_channels.count();
if (d_num_valid_channels == 0)
{
consume(d_nchannels, epoch + 1);
return returned_elements;
}
correct_TOW_and_compute_prange(epoch_data);
std::vector<Gnss_Synchro>::iterator it = epoch_data.begin();
for (i = 0; i < d_nchannels; i++)
{
if (valid_channels[i])
{
out[i][epoch] = (*it);
out[i][epoch].Flag_valid_pseudorange = true;
it++;
//produce an empty observation
interpolated_gnss_synchro = Gnss_Synchro();
interpolated_gnss_synchro.Flag_valid_pseudorange = false;
interpolated_gnss_synchro.Flag_valid_word = false;
interpolated_gnss_synchro.Flag_valid_acquisition = false;
interpolated_gnss_synchro.fs = 0;
interpolated_gnss_synchro.Channel_ID = n;
}
else
{
out[i][epoch] = Gnss_Synchro();
out[i][epoch].Flag_valid_pseudorange = false;
n_valid++;
}
epoch_data.push_back(interpolated_gnss_synchro);
}
if (n_valid > 0)
{
update_TOW(epoch_data);
if (T_rx_TOW_ms % 20 != 0)
{
T_rx_TOW_offset_ms = T_rx_TOW_ms % 20;
}
}
if (n_valid > 0) compute_pranges(epoch_data);
for (unsigned int n = 0; n < d_nchannels_out; n++)
{
out[n][0] = epoch_data.at(n);
}
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
for (i = 0; i < d_nchannels; i++)
for (unsigned int i = 0; i < d_nchannels_out; i++)
{
tmp_double = out[i][epoch].RX_time;
tmp_double = out[i][0].RX_time;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].interp_TOW_ms / 1000.0;
tmp_double = out[i][0].interp_TOW_ms / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Carrier_Doppler_hz;
tmp_double = out[i][0].Carrier_Doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Carrier_phase_rads / GPS_TWO_PI;
tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Pseudorange_m;
tmp_double = out[i][0].Pseudorange_m;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][epoch].PRN);
tmp_double = static_cast<double>(out[i][0].PRN);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][epoch].Flag_valid_pseudorange);
tmp_double = static_cast<double>(out[i][0].Flag_valid_pseudorange);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
}
@@ -639,9 +581,10 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
d_dump = false;
}
}
returned_elements++;
return 1;
}
else
{
return 0;
}
consume(d_nchannels, ninput_items[d_nchannels]);
return returned_elements;
}

21
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
View File

@@ -65,26 +65,25 @@ private:
friend hybrid_observables_cc_sptr
hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename);
hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename);
void clean_history(unsigned int pos);
double compute_T_rx_s(const Gnss_Synchro& a);
bool interpolate_data(Gnss_Synchro& out, const unsigned int& ch, const double& ti);
void find_interp_elements(const unsigned int& ch, const double& ti);
void correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data);
bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const unsigned int& ch, const unsigned long int& rx_clock);
double compute_T_rx_s(const Gnss_Synchro& a);
void compute_pranges(std::vector<Gnss_Synchro>& data);
void update_TOW(std::vector<Gnss_Synchro>& data);
int save_matfile();
//time history
boost::circular_buffer<unsigned long int> d_Rx_clock_buffer;
//Tracking observable history
Gnss_circular_deque<Gnss_Synchro>* d_gnss_synchro_history;
boost::dynamic_bitset<> valid_channels;
double T_rx_s;
unsigned int T_rx_step_ms;
unsigned int T_rx_clock_step_samples;
//rx time follow GPST
bool T_rx_TOW_set;
unsigned int T_rx_TOW_ms;
double max_delta;
double d_latency;
unsigned int T_rx_TOW_offset_ms;
bool d_dump;
unsigned int d_nchannels;
unsigned int d_num_valid_channels;
unsigned int d_nchannels_in;
unsigned int d_nchannels_out;
std::string d_dump_filename;
std::ofstream d_dump_file;
};

103
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
View File

@@ -117,7 +117,8 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc(
d_flag_frame_sync = false;
d_flag_parity = false;
d_TOW_at_current_symbol = 0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
delta_t = 0;
d_CRC_error_counter = 0;
flag_even_word_arrived = 0;
@@ -251,9 +252,9 @@ void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols, in
DLOG(INFO) << "T0G=" << tmp_obj->t_0G_10;
DLOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10;
DLOG(INFO) << "Current parameters:";
DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol;
DLOG(INFO) << "d_TOW_at_current_symbol_ms=" << d_TOW_at_current_symbol_ms;
DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0;
delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64)));
delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64)));
DLOG(INFO) << "delta_t=" << delta_t << "[s]";
}
}
@@ -406,6 +407,9 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
d_nav.flag_TOW_set = false;
}
}
}
@@ -419,73 +423,76 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
if (d_nav.flag_TOW_5 == true) // page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
// TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_5 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols + 1) * GALILEO_E1_CODE_PERIOD;
d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.TOW_5 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS;
d_nav.flag_TOW_5 = false;
}
else if (d_nav.flag_TOW_6 == true) // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
// TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_6 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols + 1) * GALILEO_E1_CODE_PERIOD;
d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.TOW_6 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS;
d_nav.flag_TOW_6 = false;
}
else
{
// this page has no timing information
d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD;
d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD;
}
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD;
}
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
{
delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0)));
}
if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true)
{
current_symbol.Flag_valid_word = true;
}
else
{
current_symbol.Flag_valid_word = false;
}
current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
// todo: Galileo to GPS time conversion should be moved to observable block.
// current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW
if (d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
if (d_nav.flag_TOW_set == true)
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = 0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS;
}
}
// remove used symbols from history
// todo: Use circular buffer here
if (d_symbol_history.size() > required_symbols)
{
d_symbol_history.pop_front();
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
if (d_nav.flag_TOW_set)
{
if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
{
delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0)));
}
current_symbol.Flag_valid_word = d_nav.flag_TOW_set;
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// todo: Galileo to GPS time conversion should be moved to observable block.
// current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW
if (d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}
else
{
return 0;
}
}

3
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h
View File

@@ -105,7 +105,8 @@ private:
Gnss_Satellite d_satellite;
int d_channel;
double d_TOW_at_current_symbol;
unsigned int d_TOW_at_Preamble_ms;
unsigned int d_TOW_at_current_symbol_ms;
bool flag_TOW_set;
double delta_t; //GPS-GALILEO time offset

86
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
View File

@@ -154,7 +154,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
// initialize internal vars
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "GALILEO E5A TELEMETRY PROCESSING: satellite " << d_satellite;
// set the preamble
for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
@@ -182,7 +181,8 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
d_flag_preamble = false;
d_preamble_index = 0;
d_flag_frame_sync = false;
d_TOW_at_current_symbol = 0.0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
flag_TOW_set = false;
d_CRC_error_counter = 0;
d_channel = 0;
@@ -345,7 +345,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
// ****************** Frame sync ******************
if ((d_stat == 0) && new_symbol) // no preamble information
{
if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
{
d_preamble_index = d_sample_counter; // record the preamble sample stamp
LOG(INFO) << "Preamble detection for Galileo E5a satellite " << d_satellite;
@@ -354,7 +354,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
}
else if ((d_stat == 1) && new_symbol) // possible preamble lock
{
if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
{
// check preamble separation
preamble_diff = d_sample_counter - d_preamble_index;
@@ -418,6 +418,9 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
d_flag_frame_sync = false;
d_stat = 0;
flag_bit_start = false;
d_nav.flag_TOW_set = false;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
}
}
}
@@ -432,73 +435,72 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
{
if (d_nav.flag_TOW_1 == true)
{
d_TOW_at_current_symbol = d_nav.FNAV_TOW_1 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_1 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_1 = false;
}
else if (d_nav.flag_TOW_2 == true)
{
d_TOW_at_current_symbol = d_nav.FNAV_TOW_2 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_2 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_2 = false;
}
else if (d_nav.flag_TOW_3 == true)
{
d_TOW_at_current_symbol = d_nav.FNAV_TOW_3 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_3 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_3 = false;
}
else if (d_nav.flag_TOW_4 == true)
{
d_TOW_at_current_symbol = d_nav.FNAV_TOW_4 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_4 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_4 = false;
}
else
{
d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD;
d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS;
}
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD;
}
//if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
if (d_flag_frame_sync and d_nav.flag_TOW_set)
{
current_sample.Flag_valid_word = true;
}
else
{
current_sample.Flag_valid_word = false;
}
current_sample.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
if (d_nav.flag_TOW_set == true)
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_sample.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = 0.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what();
d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS;
}
}
// remove used symbols from history
// todo: Use circular buffer here
while (d_symbol_history.size() > required_symbols)
{
d_symbol_history.pop_front();
}
// 3. Make the output
if (current_sample.Flag_valid_word)
if (d_nav.flag_TOW_set)
{
current_sample.Flag_valid_word = true;
current_sample.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_sample.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what();
}
}
// 3. Make the output
out[0] = current_sample;
return 1;
}

3
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h
View File

@@ -104,7 +104,8 @@ private:
bool new_symbol;
double d_prompt_acum;
double page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
double d_TOW_at_current_symbol;
unsigned int d_TOW_at_Preamble_ms;
unsigned int d_TOW_at_current_symbol_ms;
double delta_t; //GPS-GALILEO time offset
std::string d_dump_filename;
std::ofstream d_dump_file;

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

@@ -419,6 +419,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
flag_TOW_set = false;
d_current_subframe_symbol = 0;
d_crc_error_synchronization_counter = 0;
d_TOW_at_current_symbol_ms = 0;
}
}
}
@@ -426,47 +427,57 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
//2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_flag_new_tow_available == true)
{
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_nav.d_TOW) * 1000 + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms;
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_nav.d_TOW * 1000.0) + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.d_TOW * 1000.0);
flag_TOW_set = true;
d_flag_new_tow_available = false;
}
else
{
d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS;
}
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_symbol.Flag_valid_word = flag_TOW_set;
if (flag_PLL_180_deg_phase_locked == true)
{
//correct the accumulated phase for the Costas loop phase shift, if required
current_symbol.Carrier_phase_rads += GPS_PI;
}
if (d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
if (flag_TOW_set == true)
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) * 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS;
}
}
//3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
if (flag_TOW_set == true)
{
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_symbol.Flag_valid_word = flag_TOW_set;
return 1;
if (flag_PLL_180_deg_phase_locked == true)
{
//correct the accumulated phase for the Costas loop phase shift, if required
current_symbol.Carrier_phase_rads += GPS_PI;
}
if (d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
}
//3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}
else
{
return 0;
}
}

67
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc
View File

@@ -64,8 +64,8 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc(
DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite;
d_channel = 0;
d_flag_valid_word = false;
d_TOW_at_current_symbol = 0.0;
d_TOW_at_Preamble = 0.0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
//initialize the CNAV frame decoder (libswiftcnav)
cnav_msg_decoder_init(&d_cnav_decoder);
for (int aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++)
@@ -236,47 +236,56 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u
}
//update TOW at the preamble instant
d_TOW_at_Preamble = static_cast<double>(msg.tow) * 6.0;
d_TOW_at_Preamble_ms = msg.tow * 6000;
//* The time of the last input symbol can be computed from the message ToW and
//* delay by the formulae:
//* \code
//* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory
d_TOW_at_current_symbol = (static_cast<double>(msg.tow) * 6.0) + (static_cast<double>(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD;
d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
//d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5i_SYMBOL_PERIOD_MS;
d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5i_SYMBOL_PERIOD_MS;
d_flag_valid_word = true;
}
else
{
d_TOW_at_current_symbol += GPS_L5i_PERIOD;
d_TOW_at_current_symbol_ms += GPS_L5i_PERIOD_MS;
if (current_synchro_data.Flag_valid_symbol_output == false)
{
d_flag_valid_word = false;
}
}
current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_synchro_data.Flag_valid_word = d_flag_valid_word;
if (d_dump == true)
if (d_flag_valid_word == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_synchro_data.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = d_TOW_at_Preamble;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what();
}
}
current_synchro_data.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_synchro_data.Flag_valid_word = d_flag_valid_word;
//3. Make the output (copy the object contents to the GNURadio reserved memory)
out[0] = current_synchro_data;
return 1;
if (d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_synchro_data.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what();
}
}
//3. Make the output (copy the object contents to the GNURadio reserved memory)
out[0] = current_synchro_data;
return 1;
}
else
{
return 0;
}
}

7
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h
View File

@@ -41,8 +41,7 @@
#include <utility>
#include <vector>
extern "C"
{
extern "C" {
#include "cnav_msg.h"
#include "edc.h"
#include "bits.h"
@@ -85,8 +84,8 @@ private:
cnav_msg_decoder_t d_cnav_decoder;
double d_TOW_at_current_symbol;
double d_TOW_at_Preamble;
unsigned int d_TOW_at_current_symbol_ms;
unsigned int d_TOW_at_Preamble_ms;
bool d_flag_valid_word;
Gps_CNAV_Navigation_Message d_CNAV_Message;

10
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@@ -363,6 +363,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
}
// --- Initializations ---
multicorrelator_cpu.set_fast_resampler(trk_parameters.use_fast_resampler);
// Initial code frequency basis of NCO
d_code_freq_chips = d_code_chip_rate;
// Residual code phase (in chips)
@@ -742,8 +743,7 @@ void dll_pll_veml_tracking::run_dll_pll()
d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz);
// New carrier Doppler frequency estimation
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz;
// New code Doppler frequency estimation
d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate;
// ################## DLL ##########################################################
// DLL discriminator
@@ -757,6 +757,9 @@ void dll_pll_veml_tracking::run_dll_pll()
}
// Code discriminator filter
d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second]
// New code Doppler frequency estimation
d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips;
}
@@ -778,13 +781,12 @@ void dll_pll_veml_tracking::update_tracking_vars()
{
T_chip_seconds = 1.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * static_cast<double>(d_code_length_chips);
double code_error_filt_secs = T_prn_seconds * d_code_error_filt_chips * T_chip_seconds; //[seconds]
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// keep alignment parameters for the next input buffer
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
T_prn_samples = T_prn_seconds * trk_parameters.fs_in;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * trk_parameters.fs_in;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples;
//d_current_prn_length_samples = static_cast<int>(round(K_blk_samples)); // round to a discrete number of samples
d_current_prn_length_samples = static_cast<int>(std::floor(K_blk_samples)); // round to a discrete number of samples

42
src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc
View File

@@ -37,7 +37,6 @@
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <cmath>
cpu_multicorrelator_real_codes::cpu_multicorrelator_real_codes()
{
d_sig_in = nullptr;
@@ -47,6 +46,7 @@ cpu_multicorrelator_real_codes::cpu_multicorrelator_real_codes()
d_local_codes_resampled = nullptr;
d_code_length_chips = 0;
d_n_correlators = 0;
d_use_fast_resampler = true;
}
@@ -84,6 +84,7 @@ bool cpu_multicorrelator_real_codes::set_local_code_and_taps(
d_local_code_in = local_code_in;
d_shifts_chips = shifts_chips;
d_code_length_chips = code_length_chips;
return true;
}
@@ -97,16 +98,31 @@ bool cpu_multicorrelator_real_codes::set_input_output_vectors(std::complex<float
}
void cpu_multicorrelator_real_codes::update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips)
void cpu_multicorrelator_real_codes::update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips)
{
volk_gnsssdr_32f_xn_resampler_32f_xn(d_local_codes_resampled,
d_local_code_in,
rem_code_phase_chips,
code_phase_step_chips,
d_shifts_chips,
d_code_length_chips,
d_n_correlators,
correlator_length_samples);
if (d_use_fast_resampler)
{
volk_gnsssdr_32f_xn_fast_resampler_32f_xn(d_local_codes_resampled,
d_local_code_in,
rem_code_phase_chips,
code_phase_step_chips,
code_phase_rate_step_chips,
d_shifts_chips,
d_code_length_chips,
d_n_correlators,
correlator_length_samples);
}
else
{
volk_gnsssdr_32f_xn_resampler_32f_xn(d_local_codes_resampled,
d_local_code_in,
rem_code_phase_chips,
code_phase_step_chips,
d_shifts_chips,
d_code_length_chips,
d_n_correlators,
correlator_length_samples);
}
}
@@ -141,3 +157,9 @@ bool cpu_multicorrelator_real_codes::free()
}
return true;
}
void cpu_multicorrelator_real_codes::set_fast_resampler(
bool use_fast_resampler)
{
d_use_fast_resampler = use_fast_resampler;
}

4
src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h
View File

@@ -46,11 +46,12 @@ class cpu_multicorrelator_real_codes
{
public:
cpu_multicorrelator_real_codes();
void set_fast_resampler(bool use_fast_resampler);
~cpu_multicorrelator_real_codes();
bool init(int max_signal_length_samples, int n_correlators);
bool set_local_code_and_taps(int code_length_chips, const float *local_code_in, float *shifts_chips);
bool set_input_output_vectors(std::complex<float> *corr_out, const std::complex<float> *sig_in);
void update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips);
void update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips = 0.0);
bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, int signal_length_samples);
bool free();
@@ -61,6 +62,7 @@ private:
const float *d_local_code_in;
std::complex<float> *d_corr_out;
float *d_shifts_chips;
bool d_use_fast_resampler;
int d_code_length_chips;
int d_n_correlators;
};

5
src/algorithms/tracking/libs/dll_pll_conf.cc
View File

@@ -36,11 +36,14 @@
Dll_Pll_Conf::Dll_Pll_Conf()
{
/* DLL/PLL tracking configuration */
use_fast_resampler = true;
fs_in = 0.0;
vector_length = 0;
dump = false;
dump_filename = "./dll_pll_dump.dat";
pll_bw_hz = 40.0;
pll_pull_in_bw_hz = 50.0;
dll_pull_in_bw_hz = 3.0;
pll_bw_hz = 35.0;
dll_bw_hz = 2.0;
pll_bw_narrow_hz = 5.0;
dll_bw_narrow_hz = 0.75;

3
src/algorithms/tracking/libs/dll_pll_conf.h
View File

@@ -44,6 +44,8 @@ public:
unsigned int vector_length;
bool dump;
std::string dump_filename;
float pll_pull_in_bw_hz;
float dll_pull_in_bw_hz;
float pll_bw_hz;
float dll_bw_hz;
float pll_bw_narrow_hz;
@@ -53,6 +55,7 @@ public:
float early_late_space_narrow_chips;
float very_early_late_space_narrow_chips;
int extend_correlation_symbols;
bool use_fast_resampler;
int cn0_samples;
int carrier_lock_det_mav_samples;
int cn0_min;