mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-08-07 18:58:53 +00:00
Code Issues Releases Wiki Activity

Update variable names, apply clang-tidy and clang-format

Browse Source
This commit is contained in:
Carles Fernandez
2019-09-19 21:41:18 +02:00
parent e430d5b76f
commit 0eacfbfd19
11 changed files with 241 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/algorithms/tracking/adapters/galileo_e1_joint_veml_tracking.cc
+5 -5
View File
@@ -49,7 +49,7 @@ GalileoE1JointVemlTracking::GalileoE1JointVemlTracking(
{
Dll_Pll_Conf trk_param = Dll_Pll_Conf();
DLOG(INFO) << "role " << role;
//################# CONFIGURATION PARAMETERS ########################
// ################# CONFIGURATION PARAMETERS ########################
std::string default_item_type = "gr_complex";
std::string item_type = configuration->property(role + ".item_type", default_item_type);
int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
@@ -156,7 +156,7 @@ GalileoE1JointVemlTracking::GalileoE1JointVemlTracking(
}
trk_param.track_pilot = track_pilot;
trk_param.extend_correlation_symbols = extend_correlation_symbols;
int vector_length = std::round(fs_in / (GALILEO_E1_CODE_CHIP_RATE_HZ / GALILEO_E1_B_CODE_LENGTH_CHIPS));
int vector_length = std::round(fs_in / (GALILEO_E1_CODE_CHIP_RATE_CPS / GALILEO_E1_B_CODE_LENGTH_CHIPS));
trk_param.vector_length = vector_length;
trk_param.system = 'E';
char sig_[3] = "1B";
@@ -167,7 +167,7 @@ GalileoE1JointVemlTracking::GalileoE1JointVemlTracking(
trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail);
trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th);
//################# MAKE TRACKING GNURadio object ###################
// ################# MAKE TRACKING GNURadio object ###################
if (item_type == "gr_complex")
{
item_size_ = sizeof(gr_complex);
@@ -227,7 +227,7 @@ void GalileoE1JointVemlTracking::connect(gr::top_block_sptr top_block)
if (top_block)
{ /* top_block is not null */
};
//nothing to connect, now the tracking uses gr_sync_decimator
// nothing to connect, now the tracking uses gr_sync_decimator
}
@@ -236,7 +236,7 @@ void GalileoE1JointVemlTracking::disconnect(gr::top_block_sptr top_block)
if (top_block)
{ /* top_block is not null */
};
//nothing to disconnect, now the tracking uses gr_sync_decimator
// nothing to disconnect, now the tracking uses gr_sync_decimator
}
src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc
+5 -5
View File
@@ -49,7 +49,7 @@ GalileoE1MixedVemlTracking::GalileoE1MixedVemlTracking(
{
Dll_Pll_Conf trk_param = Dll_Pll_Conf();
DLOG(INFO) << "role " << role;
//################# CONFIGURATION PARAMETERS ########################
// ################# CONFIGURATION PARAMETERS ########################
std::string default_item_type = "gr_complex";
std::string item_type = configuration->property(role + ".item_type", default_item_type);
int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
@@ -156,7 +156,7 @@ GalileoE1MixedVemlTracking::GalileoE1MixedVemlTracking(
}
trk_param.track_pilot = track_pilot;
trk_param.extend_correlation_symbols = extend_correlation_symbols;
int vector_length = std::round(fs_in / (GALILEO_E1_CODE_CHIP_RATE_HZ / GALILEO_E1_B_CODE_LENGTH_CHIPS));
int vector_length = std::round(fs_in / (GALILEO_E1_CODE_CHIP_RATE_CPS / GALILEO_E1_B_CODE_LENGTH_CHIPS));
trk_param.vector_length = vector_length;
trk_param.system = 'E';
char sig_[3] = "1B";
@@ -167,7 +167,7 @@ GalileoE1MixedVemlTracking::GalileoE1MixedVemlTracking(
trk_param.max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", trk_param.max_carrier_lock_fail);
trk_param.carrier_lock_th = configuration->property(role + ".carrier_lock_th", trk_param.carrier_lock_th);
//################# MAKE TRACKING GNURadio object ###################
// ################# MAKE TRACKING GNURadio object ###################
if (item_type == "gr_complex")
{
item_size_ = sizeof(gr_complex);
@@ -227,7 +227,7 @@ void GalileoE1MixedVemlTracking::connect(gr::top_block_sptr top_block)
if (top_block)
{ /* top_block is not null */
};
//nothing to connect, now the tracking uses gr_sync_decimator
// nothing to connect, now the tracking uses gr_sync_decimator
}
@@ -236,7 +236,7 @@ void GalileoE1MixedVemlTracking::disconnect(gr::top_block_sptr top_block)
if (top_block)
{ /* top_block is not null */
};
//nothing to disconnect, now the tracking uses gr_sync_decimator
// nothing to disconnect, now the tracking uses gr_sync_decimator
}
src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.cc
+77 -73
View File
@@ -58,14 +58,14 @@
#include <glog/logging.h>
#include <gnuradio/io_signature.h> // for io_signature
#include <gnuradio/thread/thread.h> // for scoped_lock
#include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp
#include <gsl/gsl>
#include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <algorithm> // for fill_n
#include <cmath> // for fmod, round, floor
#include <exception> // for exception
#include <gsl/gsl>
#include <iostream> // for cout, cerr
#include <iostream> // for cout, cerr
#include <map>
#include <numeric>
#include <vector>
@@ -91,9 +91,9 @@ joint_veml_tracking_sptr joint_veml_make_tracking(const Dll_Pll_Conf &conf_)
joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("joint_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
//prevent telemetry symbols accumulation in output buffers
// prevent telemetry symbols accumulation in output buffers
this->set_max_noutput_items(1);
trk_parameters = conf_;
// Telemetry bit synchronization message port input
@@ -137,8 +137,8 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
if (signal_type == "1C")
{
d_signal_carrier_freq = GPS_L1_FREQ_HZ;
d_code_period = GPS_L1_CA_CODE_PERIOD;
d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ;
d_code_period = GPS_L1_CA_CODE_PERIOD_S;
d_code_chip_rate = GPS_L1_CA_CODE_RATE_CPS;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_code_length_chips = static_cast<uint32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS);
@@ -154,8 +154,8 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
else if (signal_type == "2S")
{
d_signal_carrier_freq = GPS_L2_FREQ_HZ;
d_code_period = GPS_L2_M_PERIOD;
d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ;
d_code_period = GPS_L2_M_PERIOD_S;
d_code_chip_rate = GPS_L2_M_CODE_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(GPS_L2_M_CODE_LENGTH_CHIPS);
// GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required
d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL;
@@ -168,8 +168,8 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
else if (signal_type == "L5")
{
d_signal_carrier_freq = GPS_L5_FREQ_HZ;
d_code_period = GPS_L5I_PERIOD;
d_code_chip_rate = GPS_L5I_CODE_RATE_HZ;
d_code_period = GPS_L5I_PERIOD_S;
d_code_chip_rate = GPS_L5I_CODE_RATE_CPS;
// symbol integration: 10 trk symbols (10 ms) = 1 tlm bit
d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL;
d_correlation_length_ms = 1;
@@ -215,8 +215,8 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
if (signal_type == "1B")
{
d_signal_carrier_freq = GALILEO_E1_FREQ_HZ;
d_code_period = GALILEO_E1_CODE_PERIOD;
d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ;
d_code_period = GALILEO_E1_CODE_PERIOD_S;
d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(GALILEO_E1_B_CODE_LENGTH_CHIPS);
// Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required
d_symbols_per_bit = 1;
@@ -240,8 +240,8 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
else if (signal_type == "5X")
{
d_signal_carrier_freq = GALILEO_E5A_FREQ_HZ;
d_code_period = GALILEO_E5A_CODE_PERIOD;
d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_HZ;
d_code_period = GALILEO_E5A_CODE_PERIOD_S;
d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_CPS;
d_symbols_per_bit = 20;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -284,10 +284,10 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
{
// GEO Satellites use different secondary code
d_signal_carrier_freq = BEIDOU_B1I_FREQ_HZ;
d_code_period = BEIDOU_B1I_CODE_PERIOD;
d_code_chip_rate = BEIDOU_B1I_CODE_RATE_HZ;
d_code_period = BEIDOU_B1I_CODE_PERIOD_S;
d_code_chip_rate = BEIDOU_B1I_CODE_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(BEIDOU_B1I_CODE_LENGTH_CHIPS);
//d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -296,17 +296,17 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
}
else if (signal_type == "B3")
{
// GEO Satellites use different secondary code
d_signal_carrier_freq = BEIDOU_B3I_FREQ_HZ;
d_code_period = BEIDOU_B3I_CODE_PERIOD;
d_code_chip_rate = BEIDOU_B3I_CODE_RATE_HZ;
d_code_period = BEIDOU_B3I_CODE_PERIOD_S;
d_code_chip_rate = BEIDOU_B3I_CODE_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(BEIDOU_B3I_CODE_LENGTH_CHIPS);
//d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -314,8 +314,8 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
trk_parameters.track_pilot = false;
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
}
else
{
@@ -350,20 +350,20 @@ joint_veml_tracking::joint_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
// Initialize tracking ==========================================
state_init = arma::zeros<arma::vec>(5);
state_cov_init.eye(5,5);
state_cov_init.eye(5, 5);
arma::vec q_process = arma::ones<arma::vec>(5);
q_process(0) = std::pow( static_cast<float>(d_code_period), 6 );
q_process(1) = std::pow( static_cast<float>(d_code_period), 4 );
q_process(2) = std::pow( static_cast<float>(d_code_period), 2 );
q_process(3) = std::pow( static_cast<float>(d_code_period), 2 );
q_process(4) = std::pow( static_cast<float>(d_code_period), 2 );
ncov_process.eye(5,5);
ncov_process.diag() = q_process ;
ncov_measurement.eye(6,6);
q_process(0) = std::pow(static_cast<float>(d_code_period), 6);
q_process(1) = std::pow(static_cast<float>(d_code_period), 4);
q_process(2) = std::pow(static_cast<float>(d_code_period), 2);
q_process(3) = std::pow(static_cast<float>(d_code_period), 2);
q_process(4) = std::pow(static_cast<float>(d_code_period), 2);
ncov_process.eye(5, 5);
ncov_process.diag() = q_process;
ncov_measurement.eye(6, 6);
d_carrier_code_filter.set_params( state_init, state_cov_init, ncov_process, ncov_measurement );
d_carrier_evolution_model.set_samp_length( static_cast<float>(d_code_period) );
d_carrier_evolution_model.set_chip_length( static_cast<float>(d_signal_carrier_freq), static_cast<float>(d_code_period) );
d_carrier_code_filter.set_params(state_init, state_cov_init, ncov_process, ncov_measurement);
d_carrier_evolution_model.set_samp_length(static_cast<float>(d_code_period));
d_carrier_evolution_model.set_chip_length(static_cast<float>(d_signal_carrier_freq), static_cast<float>(d_code_period));
d_carrier_code_filter.set_model(&d_carrier_evolution_model, &d_correlator_output_model);
// Initialization of local code replica
@@ -565,7 +565,7 @@ void joint_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg)
{
DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel;
gr::thread::scoped_lock lock(d_setlock);
d_carrier_lock_fail_counter = 200000; //force loss-of-lock condition
d_carrier_lock_fail_counter = 200000; // force loss-of-lock condition
break;
}
default:
@@ -666,7 +666,7 @@ void joint_veml_tracking::start_tracking()
// GEO Satellites use different secondary code
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
//d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;// todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -680,7 +680,7 @@ void joint_veml_tracking::start_tracking()
}
else
{
//d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;// todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -689,8 +689,8 @@ void joint_veml_tracking::start_tracking()
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
}
}
@@ -701,7 +701,7 @@ void joint_veml_tracking::start_tracking()
// Update secondary code settings for geo satellites
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
//d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;// todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -715,7 +715,7 @@ void joint_veml_tracking::start_tracking()
}
else
{
//d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -724,8 +724,8 @@ void joint_veml_tracking::start_tracking()
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
}
}
@@ -759,13 +759,13 @@ void joint_veml_tracking::start_tracking()
d_current_correlation_time_s = d_code_period;
// Gaussian filter initialization
state_init = arma::zeros<arma::vec>( arma::size(state_init) );
state_init = arma::zeros<arma::vec>(arma::size(state_init));
state_init(0) = 0.0;
state_init(1) = static_cast<float>(d_acq_carrier_doppler_hz);
state_init(2) = 0.0;
state_init(3) = 1.0;
state_init(4) = 1.0;
state_cov_init = arma::zeros<arma::mat>( arma::size(state_cov_init) );
state_cov_init = arma::zeros<arma::mat>(arma::size(state_cov_init));
state_cov_init(0, 0) = 1.1;
state_cov_init(1, 1) = 1.1;
state_cov_init(2, 2) = 1.1;
@@ -971,21 +971,21 @@ void joint_veml_tracking::run_dll_pll(const gr_complex *input_samples)
// ################## CKF ##########################################################
// Setup model parameters
d_correlator_output_model.set_local_code_and_input_parameters(d_tracking_code,
input_samples,
static_cast<uint32_t>(trk_parameters.vector_length),
static_cast<uint32_t>(d_code_samples_per_chip),
static_cast<uint32_t>(d_code_length_chips),
static_cast<float>(d_code_phase_step_chips),
static_cast<float>(d_code_phase_rate_step_chips),
static_cast<float>(d_carrier_phase_step_rad),
static_cast<float>(d_carrier_phase_rate_step_rad),
static_cast<float>(trk_parameters.early_late_space_chips) );
d_carrier_evolution_model.set_samp_length( static_cast<float>(d_code_period) );
d_carrier_evolution_model.set_chip_length( static_cast<float>(d_signal_carrier_freq),
static_cast<float>(d_code_period) );
input_samples,
static_cast<uint32_t>(trk_parameters.vector_length),
static_cast<uint32_t>(d_code_samples_per_chip),
static_cast<uint32_t>(d_code_length_chips),
static_cast<float>(d_code_phase_step_chips),
static_cast<float>(d_code_phase_rate_step_chips),
static_cast<float>(d_carrier_phase_step_rad),
static_cast<float>(d_carrier_phase_rate_step_rad),
static_cast<float>(trk_parameters.early_late_space_chips));
d_carrier_evolution_model.set_samp_length(static_cast<float>(d_code_period));
d_carrier_evolution_model.set_chip_length(static_cast<float>(d_signal_carrier_freq),
static_cast<float>(d_code_period));
// Carrier correlator output filter
arma::vec correlator_vec = arma::zeros<arma::vec>(6,1);
arma::vec correlator_vec = arma::zeros<arma::vec>(6, 1);
correlator_vec(0, 0) = std::pow(d_Early->real(), 2) - std::pow(d_Early->imag(), 2);
correlator_vec(2, 0) = std::pow(d_Prompt->real(), 2) - std::pow(d_Prompt->imag(), 2);
correlator_vec(4, 0) = std::pow(d_Late->real(), 2) - std::pow(d_Late->imag(), 2);
@@ -1063,7 +1063,7 @@ void joint_veml_tracking::update_tracking_vars()
K_blk_samples = T_prn_samples + d_rem_code_phase_samples;
d_current_prn_length_samples = static_cast<int32_t>(std::floor(K_blk_samples)); // round to a discrete number of samples
//################### PLL COMMANDS #################################################
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / trk_parameters.fs_in;
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
@@ -1097,7 +1097,7 @@ void joint_veml_tracking::update_tracking_vars()
// std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl;
d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
//################### DLL COMMANDS #################################################
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
d_code_phase_step_chips = d_code_freq_chips / trk_parameters.fs_in;
if (trk_parameters.high_dyn)
@@ -1208,7 +1208,7 @@ void joint_veml_tracking::save_correlation_results()
else
{
d_P_data_accu += *d_Prompt;
//std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl;
// std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl;
}
d_current_data_symbol++;
d_current_data_symbol %= d_symbols_per_bit;
@@ -1245,7 +1245,11 @@ void joint_veml_tracking::log_data()
// Dump results to file
float prompt_I;
float prompt_Q;
float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL;
float tmp_VE;
float tmp_E;
float tmp_P;
float tmp_L;
float tmp_VL;
float tmp_float;
double tmp_double;
uint64_t tmp_long_int;
@@ -1551,8 +1555,8 @@ void joint_veml_tracking::set_channel(uint32_t channel)
{
try
{
//trk_parameters.dump_filename.append(boost::lexical_cast<std::string>(d_channel));
//trk_parameters.dump_filename.append(".dat");
// trk_parameters.dump_filename.append(boost::lexical_cast<std::string>(d_channel));
// trk_parameters.dump_filename.append(".dat");
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(dump_filename_.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << dump_filename_.c_str();
@@ -1651,10 +1655,10 @@ int joint_veml_tracking::general_work(int noutput_items __attribute__((unused)),
d_P_accu = *d_Prompt;
d_L_accu = *d_Late;
//fail-safe: check if the secondary code or bit synchronization has not succedded in a limited time period
// fail-safe: check if the secondary code or bit synchronization has not succedded in a limited time period
if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast<int>(trk_parameters.fs_in))
{
d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition
d_carrier_lock_fail_counter = 300000; // force loss-of-lock condition
LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
}
@@ -1692,9 +1696,9 @@ int joint_veml_tracking::general_work(int noutput_items __attribute__((unused)),
}
}
}
else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change
else if (d_symbols_per_bit > 1) // Signal does not have secondary code. Search a bit transition by sign change
{
//******* preamble correlation ********
// ******* preamble correlation ********
d_Prompt_circular_buffer.push_back(*d_Prompt);
if (d_Prompt_circular_buffer.size() == d_secondary_code_length)
{
@@ -1715,7 +1719,7 @@ int joint_veml_tracking::general_work(int noutput_items __attribute__((unused)),
}
else
{
next_state = false; //keep in state 2 during pull-in transitory
next_state = false; // keep in state 2 during pull-in transitory
}
if (next_state)
{ // reset extended correlator
src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.h
+44 -45
View File
@@ -33,25 +33,24 @@
#ifndef GNSS_SDR_JOINT_VEML_TRACKING_H
#define GNSS_SDR_JOINT_VEML_TRACKING_H
//#include "tracking_models.h"
#include "tracking_Gaussian_filter.h"
#include "cpu_multicorrelator_real_codes.h"
#include "cpu_autocorrelator_real_codes.h"
#include "cpu_multicorrelator_real_codes.h"
#include "dll_pll_conf.h"
#include "exponential_smoother.h"
#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter
#include "tracking_loop_filter.h" // for DLL filter
#include "tracking_Gaussian_filter.h"
#include "tracking_loop_filter.h" // for DLL filter
#include <boost/circular_buffer.hpp>
#include <boost/shared_ptr.hpp> // for boost::shared_ptr
#include <gnuradio/block.h> // for block
#include <gnuradio/gr_complex.h> // for gr_complex
#include <gnuradio/types.h> // for gr_vector_int, gr_vector...
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <pmt/pmt.h> // for pmt_t
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <utility> // for pair
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <string>
#include <utility> // for pair
#include <vector>
template <class OutputType>
@@ -59,23 +58,24 @@ class JointCarrierTransitionModel : public ModelFunction<OutputType>
{
public:
// explicit CarrierTransitionModel(const float carrier_pdi) { pdi = carrier_pdi; };
OutputType operator()(const arma::vec& input) override {
OutputType operator()(const arma::vec &input) override
{
// input(0) = A_k;
// input(1) = dZ_k;
// input(2) = dPsi_k;
// input(3) = dot_dPsi_k;
// input(4) = ddot_dPsi_k)
OutputType output = arma::zeros<OutputType>(5,1);
OutputType output = arma::zeros<OutputType>(5, 1);
output(0, 0) = input(0);
output(1, 0) = input(1) + beta*t_samp*input(3) + 0.5*beta*std::pow(t_samp,2)*input(4);
output(2, 0) = input(2) + t_samp*input(3) + 0.5*std::pow(t_samp,2)*input(4);
output(3, 0) = input(3) + t_samp*input(4);
output(1, 0) = input(1) + beta * t_samp * input(3) + 0.5 * beta * std::pow(t_samp, 2) * input(4);
output(2, 0) = input(2) + t_samp * input(3) + 0.5 * std::pow(t_samp, 2) * input(4);
output(3, 0) = input(3) + t_samp * input(4);
output(4, 0) = input(4);
return output;
};
void set_samp_length(const float ts) { t_samp = ts; };
void set_chip_length(const float f_carr, const float tc) { beta = f_carr*tc; };
void set_chip_length(const float f_carr, const float tc) { beta = f_carr * tc; };
private:
float t_samp;
@@ -86,7 +86,8 @@ template <class OutputType>
class JointCarrierMeasurementModel : public ModelFunction<OutputType>
{
public:
OutputType operator()(const arma::vec& input) override {
OutputType operator()(const arma::vec &input) override
{
using namespace std::complex_literals;
// Allocate memory for correlator outputs
@@ -99,24 +100,24 @@ public:
d_local_code_shift_chips[2] = d_early_late_space_chips * static_cast<float>(d_code_samples_per_chip);
// Setup correlator
multicorrelator_cpu.init( 2 * d_vector_length, 3);
multicorrelator_cpu.init(2 * d_vector_length, 3);
multicorrelator_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_tracking_code, d_local_code_shift_chips);
// Perform correlation
perform_correlation(input(2), input(1) );
perform_correlation(input(2), input(1));
gr_complex R_Early = std::complex<float>(d_correlator_outs[0], 0.0);
gr_complex R_Prompt = std::complex<float>(d_correlator_outs[1], 0.0);
gr_complex R_Late = std::complex<float>(d_correlator_outs[2], 0.0);
arma::mat output = arma::zeros<arma::mat>(6,1);
gr_complex Y_Gain = static_cast<gr_complex>( input(0) * std::exp( 1i * input(2) ) );
output(0, 0) = std::real( std::pow(Y_Gain * R_Early, 2) );
output(2, 0) = std::real( std::pow(Y_Gain * R_Prompt, 2) );
output(4, 0) = std::real( std::pow(Y_Gain * R_Late, 2) );
output(1, 0) = std::imag( std::pow(Y_Gain * R_Early, 2) );
output(3, 0) = std::imag( std::pow(Y_Gain * R_Prompt, 2) );
output(5, 0) = std::imag( std::pow(Y_Gain * R_Late, 2) );
arma::mat output = arma::zeros<arma::mat>(6, 1);
gr_complex Y_Gain = static_cast<gr_complex>(input(0) * std::exp(1i * input(2)));
output(0, 0) = std::real(std::pow(Y_Gain * R_Early, 2));
output(2, 0) = std::real(std::pow(Y_Gain * R_Prompt, 2));
output(4, 0) = std::real(std::pow(Y_Gain * R_Late, 2));
output(1, 0) = std::imag(std::pow(Y_Gain * R_Early, 2));
output(3, 0) = std::imag(std::pow(Y_Gain * R_Prompt, 2));
output(5, 0) = std::imag(std::pow(Y_Gain * R_Late, 2));
free(d_correlator_outs);
free(d_local_code_shift_chips);
@@ -125,16 +126,16 @@ public:
};
void set_local_code_and_input_parameters(
const float* local_code_in,
const gr_complex* input_buffer,
const uint32_t vector_length,
const uint32_t samples_per_chip,
const uint32_t code_length_chips,
const float code_phase_step,
const float code_phase_rate_step,
const float carrier_step,
const float carrier_rate_step,
const float early_late_space_chips)
const float *local_code_in,
const gr_complex *input_buffer,
const uint32_t vector_length,
const uint32_t samples_per_chip,
const uint32_t code_length_chips,
const float code_phase_step,
const float code_phase_rate_step,
const float carrier_step,
const float carrier_rate_step,
const float early_late_space_chips)
{
// Set Input Buffers
d_input_buffer = input_buffer;
@@ -157,14 +158,14 @@ public:
}
// Destructor
~JointCarrierMeasurementModel() {
~JointCarrierMeasurementModel()
{
free(d_correlator_outs);
free(d_local_code_shift_chips);
multicorrelator_cpu.free();
};
private:
void perform_correlation(double rem_carr_phase_rad, double rem_code_phase_chips)
{
// Compute Early, Prompt and Late correlation
@@ -176,10 +177,9 @@ private:
static_cast<float>(d_code_phase_step_chips) * static_cast<float>(d_code_samples_per_chip),
static_cast<float>(d_code_phase_rate_step_chips) * static_cast<float>(d_code_samples_per_chip),
d_vector_length);
};
Cpu_Autocorrelator_Real_Codes multicorrelator_cpu; // Multicorrelator_cpu
Cpu_Autocorrelator_Real_Codes multicorrelator_cpu; // Multicorrelator_cpu
float *d_correlator_outs;
float *d_local_code_shift_chips;
@@ -196,7 +196,6 @@ private:
float d_carrier_phase_rate_step_rad;
float d_early_late_space_chips;
};
@@ -287,7 +286,7 @@ private:
float *d_local_code_shift_chips;
float *d_prompt_data_shift;
Cpu_Multicorrelator_Real_Codes multicorrelator_cpu;
Cpu_Multicorrelator_Real_Codes correlator_data_cpu; //for data channel
Cpu_Multicorrelator_Real_Codes correlator_data_cpu; // for data channel
/* TODO: currently the multicorrelator does not support adding extra correlator
with different local code, thus we need extra multicorrelator instance.
@@ -322,7 +321,7 @@ private:
double d_carrier_phase_step_rad;
double d_carrier_phase_rate_step_rad;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
float d_rem_carr_phase_rad;
src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc
+62 -58
View File
@@ -58,14 +58,14 @@
#include <glog/logging.h>
#include <gnuradio/io_signature.h> // for io_signature
#include <gnuradio/thread/thread.h> // for scoped_lock
#include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp
#include <gsl/gsl>
#include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <algorithm> // for fill_n
#include <cmath> // for fmod, round, floor
#include <exception> // for exception
#include <gsl/gsl>
#include <iostream> // for cout, cerr
#include <iostream> // for cout, cerr
#include <map>
#include <numeric>
#include <vector>
@@ -91,9 +91,9 @@ mixed_veml_tracking_sptr mixed_veml_make_tracking(const Dll_Pll_Conf &conf_)
mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("mixed_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
//prevent telemetry symbols accumulation in output buffers
// prevent telemetry symbols accumulation in output buffers
this->set_max_noutput_items(1);
trk_parameters = conf_;
// Telemetry bit synchronization message port input
@@ -137,8 +137,8 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
if (signal_type == "1C")
{
d_signal_carrier_freq = GPS_L1_FREQ_HZ;
d_code_period = GPS_L1_CA_CODE_PERIOD;
d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ;
d_code_period = GPS_L1_CA_CODE_PERIOD_S;
d_code_chip_rate = GPS_L1_CA_CODE_RATE_CPS;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_code_length_chips = static_cast<uint32_t>(GPS_L1_CA_CODE_LENGTH_CHIPS);
@@ -154,8 +154,8 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
else if (signal_type == "2S")
{
d_signal_carrier_freq = GPS_L2_FREQ_HZ;
d_code_period = GPS_L2_M_PERIOD;
d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ;
d_code_period = GPS_L2_M_PERIOD_S;
d_code_chip_rate = GPS_L2_M_CODE_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(GPS_L2_M_CODE_LENGTH_CHIPS);
// GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required
d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL;
@@ -168,8 +168,8 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
else if (signal_type == "L5")
{
d_signal_carrier_freq = GPS_L5_FREQ_HZ;
d_code_period = GPS_L5I_PERIOD;
d_code_chip_rate = GPS_L5I_CODE_RATE_HZ;
d_code_period = GPS_L5I_PERIOD_S;
d_code_chip_rate = GPS_L5I_CODE_RATE_CPS;
// symbol integration: 10 trk symbols (10 ms) = 1 tlm bit
d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL;
d_correlation_length_ms = 1;
@@ -215,8 +215,8 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
if (signal_type == "1B")
{
d_signal_carrier_freq = GALILEO_E1_FREQ_HZ;
d_code_period = GALILEO_E1_CODE_PERIOD;
d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ;
d_code_period = GALILEO_E1_CODE_PERIOD_S;
d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(GALILEO_E1_B_CODE_LENGTH_CHIPS);
// Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required
d_symbols_per_bit = 1;
@@ -240,8 +240,8 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
else if (signal_type == "5X")
{
d_signal_carrier_freq = GALILEO_E5A_FREQ_HZ;
d_code_period = GALILEO_E5A_CODE_PERIOD;
d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_HZ;
d_code_period = GALILEO_E5A_CODE_PERIOD_S;
d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_CPS;
d_symbols_per_bit = 20;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -284,10 +284,10 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
{
// GEO Satellites use different secondary code
d_signal_carrier_freq = BEIDOU_B1I_FREQ_HZ;
d_code_period = BEIDOU_B1I_CODE_PERIOD;
d_code_chip_rate = BEIDOU_B1I_CODE_RATE_HZ;
d_code_period = BEIDOU_B1I_CODE_PERIOD_S;
d_code_chip_rate = BEIDOU_B1I_CODE_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(BEIDOU_B1I_CODE_LENGTH_CHIPS);
//d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -296,17 +296,17 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
}
else if (signal_type == "B3")
{
// GEO Satellites use different secondary code
d_signal_carrier_freq = BEIDOU_B3I_FREQ_HZ;
d_code_period = BEIDOU_B3I_CODE_PERIOD;
d_code_chip_rate = BEIDOU_B3I_CODE_RATE_HZ;
d_code_period = BEIDOU_B3I_CODE_PERIOD_S;
d_code_chip_rate = BEIDOU_B3I_CODE_RATE_CPS;
d_code_length_chips = static_cast<uint32_t>(BEIDOU_B3I_CODE_LENGTH_CHIPS);
//d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -314,8 +314,8 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
trk_parameters.track_pilot = false;
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
}
else
{
@@ -352,18 +352,18 @@ mixed_veml_tracking::mixed_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block(
d_code_loop_filter = Tracking_loop_filter(d_code_period, trk_parameters.dll_bw_hz, trk_parameters.dll_filter_order, false);
// Initialize kalman tracking ==========================================
state_init = arma::zeros<arma::vec>(4,1);
state_cov_init.eye(4,4);
arma::vec q_process = arma::ones<arma::vec>(4,1);
q_process(0) = std::pow( static_cast<float>(d_code_period), 6 );
q_process(1) = std::pow( static_cast<float>(d_code_period), 4 );
q_process(2) = std::pow( static_cast<float>(d_code_period), 2 );
q_process(3) = std::pow( static_cast<float>(d_code_period), 2 );
ncov_process.eye(4,4);
state_init = arma::zeros<arma::vec>(4, 1);
state_cov_init.eye(4, 4);
arma::vec q_process = arma::ones<arma::vec>(4, 1);
q_process(0) = std::pow(static_cast<float>(d_code_period), 6);
q_process(1) = std::pow(static_cast<float>(d_code_period), 4);
q_process(2) = std::pow(static_cast<float>(d_code_period), 2);
q_process(3) = std::pow(static_cast<float>(d_code_period), 2);
ncov_process.eye(4, 4);
ncov_process.diag() = q_process;
ncov_measurement.eye(2,2);
ncov_measurement.eye(2, 2);
d_carrier_loop_filter.set_params( state_init, state_cov_init, ncov_process, ncov_measurement );
d_carrier_loop_filter.set_params(state_init, state_cov_init, ncov_process, ncov_measurement);
d_carrier_evolution_model.set_code_period(static_cast<float>(d_code_period));
d_carrier_loop_filter.set_model(&d_carrier_evolution_model, &d_correlator_output_model);
@@ -566,7 +566,7 @@ void mixed_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg)
{
DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel;
gr::thread::scoped_lock lock(d_setlock);
d_carrier_lock_fail_counter = 200000; //force loss-of-lock condition
d_carrier_lock_fail_counter = 200000; // force loss-of-lock condition
break;
}
default:
@@ -667,7 +667,7 @@ void mixed_veml_tracking::start_tracking()
// GEO Satellites use different secondary code
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
//d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;// todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -681,7 +681,7 @@ void mixed_veml_tracking::start_tracking()
}
else
{
//d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;// todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -690,8 +690,8 @@ void mixed_veml_tracking::start_tracking()
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
}
}
@@ -702,7 +702,7 @@ void mixed_veml_tracking::start_tracking()
// Update secondary code settings for geo satellites
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
//d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;// todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -716,7 +716,7 @@ void mixed_veml_tracking::start_tracking()
}
else
{
//d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
// d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
@@ -725,8 +725,8 @@ void mixed_veml_tracking::start_tracking()
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
// d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
// d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
}
}
@@ -763,14 +763,14 @@ void mixed_veml_tracking::start_tracking()
d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_hz);
d_code_loop_filter.set_update_interval(d_code_period);
// DLL/PLL filter initialization
d_code_loop_filter.initialize(); // initialize the code filter
d_code_loop_filter.initialize(); // initialize the code filter
// Carrier Gaussian filter initialization
state_init = arma::zeros<arma::vec>( arma::size(state_init) );
state_init = arma::zeros<arma::vec>(arma::size(state_init));
state_init(0) = 0.0;
state_init(1) = static_cast<float>(d_acq_carrier_doppler_hz);
state_init(2) = 0.0;
state_init(3) = 1.0;
state_cov_init = arma::zeros<arma::mat>( arma::size(state_cov_init) );
state_cov_init = arma::zeros<arma::mat>(arma::size(state_cov_init));
state_cov_init(0, 0) = std::pow(0.5 * PI_2 / 3.0, 2);
state_cov_init(1, 1) = std::pow(450.0 / 3.0, 2);
state_cov_init(2, 2) = std::pow(4.0 * PI_2, 2) / 12.0;
@@ -975,7 +975,7 @@ void mixed_veml_tracking::run_dll_pll()
{
// ################## CKF ##########################################################
// Carrier correlator output filter
arma::vec correlator_vec = arma::zeros<arma::vec>(2,1);
arma::vec correlator_vec = arma::zeros<arma::vec>(2, 1);
correlator_vec(0) = std::pow(d_Prompt->real(), 2) - std::pow(d_Prompt->imag(), 2);
correlator_vec(1) = 2.0 * d_Prompt->real() * d_Prompt->imag();
arma::vec carrier_nco = d_carrier_loop_filter.get_carrier_nco(correlator_vec);
@@ -1209,7 +1209,7 @@ void mixed_veml_tracking::save_correlation_results()
else
{
d_P_data_accu += *d_Prompt;
//std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl;
// std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl;
}
d_current_data_symbol++;
d_current_data_symbol %= d_symbols_per_bit;
@@ -1246,7 +1246,11 @@ void mixed_veml_tracking::log_data()
// Dump results to file
float prompt_I;
float prompt_Q;
float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL;
float tmp_VE;
float tmp_E;
float tmp_P;
float tmp_L;
float tmp_VL;
float tmp_float;
double tmp_double;
uint64_t tmp_long_int;
@@ -1552,8 +1556,8 @@ void mixed_veml_tracking::set_channel(uint32_t channel)
{
try
{
//trk_parameters.dump_filename.append(boost::lexical_cast<std::string>(d_channel));
//trk_parameters.dump_filename.append(".dat");
// trk_parameters.dump_filename.append(boost::lexical_cast<std::string>(d_channel));
// trk_parameters.dump_filename.append(".dat");
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(dump_filename_.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << dump_filename_.c_str();
@@ -1652,10 +1656,10 @@ int mixed_veml_tracking::general_work(int noutput_items __attribute__((unused)),
d_P_accu = *d_Prompt;
d_L_accu = *d_Late;
//fail-safe: check if the secondary code or bit synchronization has not succedded in a limited time period
// fail-safe: check if the secondary code or bit synchronization has not succedded in a limited time period
if (trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast<int>(trk_parameters.fs_in))
{
d_carrier_lock_fail_counter = 300000; //force loss-of-lock condition
d_carrier_lock_fail_counter = 300000; // force loss-of-lock condition
LOG(INFO) << systemName << " " << signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel
<< " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl;
}
@@ -1693,9 +1697,9 @@ int mixed_veml_tracking::general_work(int noutput_items __attribute__((unused)),
}
}
}
else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change
else if (d_symbols_per_bit > 1) // Signal does not have secondary code. Search a bit transition by sign change
{
//******* preamble correlation ********
// ******* preamble correlation ********
d_Prompt_circular_buffer.push_back(*d_Prompt);
if (d_Prompt_circular_buffer.size() == d_secondary_code_length)
{
@@ -1716,7 +1720,7 @@ int mixed_veml_tracking::general_work(int noutput_items __attribute__((unused)),
}
else
{
next_state = false; //keep in state 2 during pull-in transitory
next_state = false; // keep in state 2 during pull-in transitory
}
if (next_state)
{ // reset extended correlator
src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h
+20 -17
View File
@@ -33,14 +33,12 @@
#ifndef GNSS_SDR_MIXED_VEML_TRACKING_H
#define GNSS_SDR_MIXED_VEML_TRACKING_H
//#include "tracking_models.h"
#include "tracking_Gaussian_filter.h"
#include "cpu_multicorrelator_real_codes.h"
#include "dll_pll_conf.h"
#include "exponential_smoother.h"
#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter
#include "tracking_loop_filter.h" // for DLL filter
#include "tracking_Gaussian_filter.h"
#include "tracking_loop_filter.h" // for DLL filter
#include <boost/circular_buffer.hpp>
#include <boost/shared_ptr.hpp> // for boost::shared_ptr
#include <gnuradio/block.h> // for block
@@ -49,35 +47,39 @@
#include <pmt/pmt.h> // for pmt_t
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <utility> // for pair
#include <string>
#include <utility> // for pair
#include <vector>
class MixedCarrierTransitionModel : public ModelFunction<arma::vec>
{
public:
arma::vec operator()(const arma::vec& input) override {
/*
arma::vec operator()(const arma::vec &input) override
{
/*
* input/output(0) - Carrier Phase
* input/output(1) - Carrier Doppler
* input/output(2) - Carrier Doppler Rate
* input/output(3) - Squared Correlator Output Amplitude
*/
arma::vec output = arma::zeros(4,1);
output(0, 0) = input(0) + PI_2*pdi*input(1) + 0.5*PI_2*std::pow(pdi, 2)*input(2);
output(1, 0) = input(1) + pdi*input(2);
arma::vec output = arma::zeros(4, 1);
output(0, 0) = input(0) + PI_2 * pdi * input(1) + 0.5 * PI_2 * std::pow(pdi, 2) * input(2);
output(1, 0) = input(1) + pdi * input(2);
output(2, 0) = input(2);
output(3, 0) = input(3);
return output;
};
void set_code_period(const float carrier_pdi) { pdi = carrier_pdi; };
private:
float pdi;
};
class MixedCarrierMeasurementModel : public ModelFunction<arma::vec>
{
public:
arma::vec operator()(const arma::vec& input) override {
/*
arma::vec operator()(const arma::vec &input) override
{
/*
* input(0) - Carrier Phase
* input(1) - Carrier Doppler
* input(2) - Carrier Doppler Rate
@@ -87,11 +89,12 @@ public:
* output(1) - Imag component of squared Prompt
*/
using namespace std::complex_literals;
arma::vec output = arma::zeros<arma::vec>(2,1);
output(0) = static_cast<double>(input(3)) * std::cos( 2.0 * static_cast<double>(input(0)) );
output(1) = static_cast<double>(input(3)) * (-1) * std::sin( 2.0 * static_cast<double>(input(0)) );
arma::vec output = arma::zeros<arma::vec>(2, 1);
output(0) = static_cast<double>(input(3)) * std::cos(2.0 * static_cast<double>(input(0)));
output(1) = static_cast<double>(input(3)) * (-1) * std::sin(2.0 * static_cast<double>(input(0)));
return output;
};
private:
};
@@ -183,7 +186,7 @@ private:
float *d_local_code_shift_chips;
float *d_prompt_data_shift;
Cpu_Multicorrelator_Real_Codes multicorrelator_cpu;
Cpu_Multicorrelator_Real_Codes correlator_data_cpu; //for data channel
Cpu_Multicorrelator_Real_Codes correlator_data_cpu; // for data channel
/* TODO: currently the multicorrelator does not support adding extra correlator
with different local code, thus we need extra multicorrelator instance.
@@ -218,7 +221,7 @@ private:
double d_carrier_phase_step_rad;
double d_carrier_phase_rate_step_rad;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
float d_rem_carr_phase_rad;
src/algorithms/tracking/libs/cpu_autocorrelator_real_codes.cc
+1 -2
View File
@@ -67,7 +67,7 @@ bool Cpu_Autocorrelator_Real_Codes::init(
// ALLOCATE MEMORY FOR INTERNAL vectors
size_t size = max_signal_length_samples * sizeof(float);
d_corr_out_cx = static_cast<std::complex<float>*>(volk_gnsssdr_malloc(n_correlators * sizeof(std::complex<float>),volk_gnsssdr_get_alignment()));
d_corr_out_cx = static_cast<std::complex<float>*>(volk_gnsssdr_malloc(n_correlators * sizeof(std::complex<float>), volk_gnsssdr_get_alignment()));
d_local_code_cx = static_cast<std::complex<float>*>(volk_gnsssdr_malloc(size * sizeof(std::complex<float>), volk_gnsssdr_get_alignment()));
d_local_codes_resampled = static_cast<float**>(volk_gnsssdr_malloc(n_correlators * sizeof(float*), volk_gnsssdr_get_alignment()));
@@ -131,7 +131,6 @@ void Cpu_Autocorrelator_Real_Codes::update_local_code(int correlator_length_samp
{
d_local_code_cx[n] = std::complex<float>(d_local_code_in[n], 0.0);
}
}
bool Cpu_Autocorrelator_Real_Codes::Local_code_multi_autocorrelator_resampler(
src/algorithms/tracking/libs/nonlinear_tracking.h
+4 -4
View File
@@ -47,9 +47,9 @@
#define ARMA_NO_DEBUG 1
#endif
#include "tracking_models.h"
#include <armadillo>
#include <gnuradio/gr_complex.h>
#include "tracking_models.h"
class GaussianFilter
{
@@ -70,7 +70,7 @@ public:
arma::mat get_P_x_est() const;
// Prediction and Estimation
//void run_sequential(const arma::vec& z_upd, const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, ModelFunction* measurement_fcn, const arma::mat& process_covariance, const arma::mat& measurement_covariance);
// void run_sequential(const arma::vec& z_upd, const arma::vec& x_post, const arma::mat& P_x_post, ModelFunction* transition_fcn, ModelFunction* measurement_fcn, const arma::mat& process_covariance, const arma::mat& measurement_covariance);
protected:
arma::vec x_pred_out;
@@ -193,8 +193,8 @@ void CubatureFilter::update_sequential(const OutputType& z_upd, const arma::vec&
arma::cx_mat W_k = P_xz_pred * arma::inv(P_zz_pred);
// Compute and store the updated mean and error covariance
x_est = x_pred + arma::real( W_k * (z_upd - z_pred) );
P_x_est = P_x_pred - arma::real( W_k * P_zz_pred * W_k.t() );
x_est = x_pred + arma::real(W_k * (z_upd - z_pred));
P_x_est = P_x_pred - arma::real(W_k * P_zz_pred * W_k.t());
}
/***************** END CUBATURE KALMAN FILTER *****************/
src/algorithms/tracking/libs/tracking_Gaussian_filter.cc
+9 -8
View File
@@ -38,31 +38,32 @@
*/
#include "tracking_Gaussian_filter.h"
#include <utility>
void TrackingGaussianFilter::set_ncov_measurement(arma::mat ncov)
{
d_ncov_measurement = ncov;
d_ncov_measurement = std::move(ncov);
}
void TrackingGaussianFilter::set_ncov_process(arma::mat ncov)
{
d_ncov_process = ncov;
d_ncov_process = std::move(ncov);
}
void TrackingGaussianFilter::set_state(arma::vec state)
{
d_state = state;
d_state = std::move(state);
}
void TrackingGaussianFilter::set_state_cov(arma::mat state_cov)
{
d_state_cov = state_cov;
d_state_cov = std::move(state_cov);
}
void TrackingGaussianFilter::set_params(arma::vec state, arma::mat state_cov, arma::mat p_ncov, arma::mat m_ncov)
{
set_ncov_process(p_ncov);
set_ncov_measurement(m_ncov);
set_state(state);
set_state_cov(state_cov);
set_ncov_process(std::move(p_ncov));
set_ncov_measurement(std::move(m_ncov));
set_state(std::move(state));
set_state_cov(std::move(state_cov));
}
src/algorithms/tracking/libs/tracking_Gaussian_filter.h
+9 -9
View File
@@ -40,11 +40,11 @@
#ifndef GNSS_SDR_TRACKING_GAUSSIAN_FILTER_H_
#define GNSS_SDR_TRACKING_GAUSSIAN_FILTER_H_
#include "MATH_CONSTANTS.h"
#include "nonlinear_tracking.h"
#include "tracking_models.h"
#include <armadillo>
#include <gnuradio/gr_complex.h>
#include "tracking_models.h"
#include "nonlinear_tracking.h"
#include "MATH_CONSTANTS.h"
/*!
* \brief This class implements a standard Gaussian filter for carrier tracking.
@@ -63,8 +63,8 @@ public:
void set_params(arma::vec state, arma::mat state_cov, arma::mat p_ncov, arma::mat m_ncov);
protected:
arma::vec d_state; /* state vector */
arma::mat d_state_cov; /* state error covariance matrix */
arma::vec d_state; /* state vector */
arma::mat d_state_cov; /* state error covariance matrix */
arma::mat d_ncov_process; /* model error covariance matrix */
arma::mat d_ncov_measurement; /* measurement error covariance matrix */
};
@@ -74,10 +74,11 @@ class TrackingNonlinearFilter : public TrackingGaussianFilter
{
public:
arma::vec get_carrier_nco(const OutputType2 meas_in);
void set_transition_model(ModelFunction<OutputType1>* ft) { func_transition = ft; };
void set_measurement_model(ModelFunction<OutputType2>* fm) { func_measurement = fm; };
void set_model(ModelFunction<OutputType1>* ft, ModelFunction<OutputType2>* fm) {
void set_model(ModelFunction<OutputType1>* ft, ModelFunction<OutputType2>* fm)
{
set_transition_model(ft);
set_measurement_model(fm);
};
@@ -87,7 +88,6 @@ private:
ModelFunction<OutputType2>* func_measurement;
NonlinearFilter GaussFilt;
};
/* Template definitions */
@@ -98,7 +98,7 @@ private:
* The output is in [Hz/s].
*/
template <class NonlinearFilter, class OutputType1, class OutputType2>
arma::vec TrackingNonlinearFilter<NonlinearFilter,OutputType1,OutputType2>::get_carrier_nco(const OutputType2 meas_in)
arma::vec TrackingNonlinearFilter<NonlinearFilter, OutputType1, OutputType2>::get_carrier_nco(const OutputType2 meas_in)
{
arma::vec state_pred;
arma::mat state_cov_pred;
src/algorithms/tracking/libs/tracking_models.h
+5 -4
View File
@@ -37,15 +37,16 @@
#include "exponential_smoother.h"
#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter
#include "tracking_loop_filter.h" // for DLL filter
#include <armadillo>
#include <boost/circular_buffer.hpp>
#include <boost/shared_ptr.hpp> // for boost::shared_ptr
#include <gnuradio/block.h> // for block
#include <gnuradio/gr_complex.h> // for gr_complex
#include <gnuradio/types.h> // for gr_vector_int, gr_vector...
#include <pmt/pmt.h>
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <utility> // for pair
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <utility> // for pair
#include <vector>
// Abstract model function
@@ -70,7 +71,7 @@ class CarrierTransitionModel : public ModelFunction
{
public:
explicit CarrierTransitionModel(const float carrier_pdi) { pdi = carrier_pdi; };
arma::vec operator()(const arma::vec& input) override {
arma::vec operator()(const arma::vec& input) override {
arma::vec output = arma::zeros(3,1);
output(0, 0) = input(0) + PI_2*pdi*input(1) + 0.5*PI_2*std::pow(pdi, 2)*input(2);
output(0, 0) = input(1) + pdi*input(2);