diff --git a/src/algorithms/tracking/adapters/galileo_e1_joint_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_joint_veml_tracking.cc index 0edd3ebe8..623e6161c 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_joint_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_joint_veml_tracking.cc @@ -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 } diff --git a/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc index 90858ae02..0585a4492 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_mixed_veml_tracking.cc @@ -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 } diff --git a/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.cc index cfcf7c538..7aea77618 100644 --- a/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.cc @@ -58,14 +58,14 @@ #include #include // for io_signature #include // for scoped_lock -#include // for Mat_VarCreate -#include // for mp +#include +#include // for Mat_VarCreate +#include // for mp #include #include // for fill_n #include // for fmod, round, floor #include // for exception -#include -#include // for cout, cerr +#include // for cout, cerr #include #include #include @@ -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(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(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(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(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(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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(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(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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(5); - state_cov_init.eye(5,5); + state_cov_init.eye(5, 5); arma::vec q_process = arma::ones(5); - q_process(0) = std::pow( static_cast(d_code_period), 6 ); - q_process(1) = std::pow( static_cast(d_code_period), 4 ); - q_process(2) = std::pow( static_cast(d_code_period), 2 ); - q_process(3) = std::pow( static_cast(d_code_period), 2 ); - q_process(4) = std::pow( static_cast(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(d_code_period), 6); + q_process(1) = std::pow(static_cast(d_code_period), 4); + q_process(2) = std::pow(static_cast(d_code_period), 2); + q_process(3) = std::pow(static_cast(d_code_period), 2); + q_process(4) = std::pow(static_cast(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(d_code_period) ); - d_carrier_evolution_model.set_chip_length( static_cast(d_signal_carrier_freq), static_cast(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(d_code_period)); + d_carrier_evolution_model.set_chip_length(static_cast(d_signal_carrier_freq), static_cast(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(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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::size(state_init) ); + state_init = arma::zeros(arma::size(state_init)); state_init(0) = 0.0; state_init(1) = static_cast(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::size(state_cov_init) ); + state_cov_init = arma::zeros(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(trk_parameters.vector_length), - static_cast(d_code_samples_per_chip), - static_cast(d_code_length_chips), - static_cast(d_code_phase_step_chips), - static_cast(d_code_phase_rate_step_chips), - static_cast(d_carrier_phase_step_rad), - static_cast(d_carrier_phase_rate_step_rad), - static_cast(trk_parameters.early_late_space_chips) ); - d_carrier_evolution_model.set_samp_length( static_cast(d_code_period) ); - d_carrier_evolution_model.set_chip_length( static_cast(d_signal_carrier_freq), - static_cast(d_code_period) ); + input_samples, + static_cast(trk_parameters.vector_length), + static_cast(d_code_samples_per_chip), + static_cast(d_code_length_chips), + static_cast(d_code_phase_step_chips), + static_cast(d_code_phase_rate_step_chips), + static_cast(d_carrier_phase_step_rad), + static_cast(d_carrier_phase_rate_step_rad), + static_cast(trk_parameters.early_late_space_chips)); + d_carrier_evolution_model.set_samp_length(static_cast(d_code_period)); + d_carrier_evolution_model.set_chip_length(static_cast(d_signal_carrier_freq), + static_cast(d_code_period)); // Carrier correlator output filter - arma::vec correlator_vec = arma::zeros(6,1); + arma::vec correlator_vec = arma::zeros(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(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(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(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(d_channel)); - //trk_parameters.dump_filename.append(".dat"); + // trk_parameters.dump_filename.append(boost::lexical_cast(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(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 diff --git a/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.h index b7efab995..a93336136 100644 --- a/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/joint_veml_tracking.h @@ -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 #include // for boost::shared_ptr #include // for block #include // for gr_complex #include // for gr_vector_int, gr_vector... -#include #include // for pmt_t -#include // for int32_t -#include // for string, ofstream -#include // for pair +#include +#include // for int32_t +#include // for string, ofstream +#include +#include // for pair #include template @@ -59,23 +58,24 @@ class JointCarrierTransitionModel : public ModelFunction { 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(5,1); + OutputType output = arma::zeros(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 JointCarrierMeasurementModel : public ModelFunction { 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(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(d_correlator_outs[0], 0.0); gr_complex R_Prompt = std::complex(d_correlator_outs[1], 0.0); gr_complex R_Late = std::complex(d_correlator_outs[2], 0.0); - arma::mat output = arma::zeros(6,1); - gr_complex Y_Gain = static_cast( 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(6, 1); + gr_complex Y_Gain = static_cast(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(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_rate_step_chips) * static_cast(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> 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; diff --git a/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc index 114327327..ef8e3933a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.cc @@ -58,14 +58,14 @@ #include #include // for io_signature #include // for scoped_lock -#include // for Mat_VarCreate -#include // for mp +#include +#include // for Mat_VarCreate +#include // for mp #include #include // for fill_n #include // for fmod, round, floor #include // for exception -#include -#include // for cout, cerr +#include // for cout, cerr #include #include #include @@ -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(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(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(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(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(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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(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(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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(4,1); - state_cov_init.eye(4,4); - arma::vec q_process = arma::ones(4,1); - q_process(0) = std::pow( static_cast(d_code_period), 6 ); - q_process(1) = std::pow( static_cast(d_code_period), 4 ); - q_process(2) = std::pow( static_cast(d_code_period), 2 ); - q_process(3) = std::pow( static_cast(d_code_period), 2 ); - ncov_process.eye(4,4); + state_init = arma::zeros(4, 1); + state_cov_init.eye(4, 4); + arma::vec q_process = arma::ones(4, 1); + q_process(0) = std::pow(static_cast(d_code_period), 6); + q_process(1) = std::pow(static_cast(d_code_period), 4); + q_process(2) = std::pow(static_cast(d_code_period), 2); + q_process(3) = std::pow(static_cast(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(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(BEIDOU_B1I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B1I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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(BEIDOU_B3I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); - //d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); - //d_data_secondary_code_string = const_cast(&BEIDOU_B3I_SECONDARY_CODE_STR); + // d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + // d_data_secondary_code_string = const_cast(&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::size(state_init) ); + state_init = arma::zeros(arma::size(state_init)); state_init(0) = 0.0; state_init(1) = static_cast(d_acq_carrier_doppler_hz); state_init(2) = 0.0; state_init(3) = 1.0; - state_cov_init = arma::zeros( arma::size(state_cov_init) ); + state_cov_init = arma::zeros(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(2,1); + arma::vec correlator_vec = arma::zeros(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(d_channel)); - //trk_parameters.dump_filename.append(".dat"); + // trk_parameters.dump_filename.append(boost::lexical_cast(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(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 diff --git a/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h index fe6ade4a6..b9d192104 100644 --- a/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/mixed_veml_tracking.h @@ -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 #include // for boost::shared_ptr #include // for block @@ -49,35 +47,39 @@ #include // for pmt_t #include // for int32_t #include // for string, ofstream -#include // for pair +#include +#include // for pair #include class MixedCarrierTransitionModel : public ModelFunction { 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 { 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(2,1); - output(0) = static_cast(input(3)) * std::cos( 2.0 * static_cast(input(0)) ); - output(1) = static_cast(input(3)) * (-1) * std::sin( 2.0 * static_cast(input(0)) ); + arma::vec output = arma::zeros(2, 1); + output(0) = static_cast(input(3)) * std::cos(2.0 * static_cast(input(0))); + output(1) = static_cast(input(3)) * (-1) * std::sin(2.0 * static_cast(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> 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; diff --git a/src/algorithms/tracking/libs/cpu_autocorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_autocorrelator_real_codes.cc index b4e5f3b04..e57c9e443 100644 --- a/src/algorithms/tracking/libs/cpu_autocorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_autocorrelator_real_codes.cc @@ -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*>(volk_gnsssdr_malloc(n_correlators * sizeof(std::complex),volk_gnsssdr_get_alignment())); + d_corr_out_cx = static_cast*>(volk_gnsssdr_malloc(n_correlators * sizeof(std::complex), volk_gnsssdr_get_alignment())); d_local_code_cx = static_cast*>(volk_gnsssdr_malloc(size * sizeof(std::complex), volk_gnsssdr_get_alignment())); d_local_codes_resampled = static_cast(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(d_local_code_in[n], 0.0); } - } bool Cpu_Autocorrelator_Real_Codes::Local_code_multi_autocorrelator_resampler( diff --git a/src/algorithms/tracking/libs/nonlinear_tracking.h b/src/algorithms/tracking/libs/nonlinear_tracking.h index ddcd04eeb..414294ffe 100644 --- a/src/algorithms/tracking/libs/nonlinear_tracking.h +++ b/src/algorithms/tracking/libs/nonlinear_tracking.h @@ -47,9 +47,9 @@ #define ARMA_NO_DEBUG 1 #endif +#include "tracking_models.h" #include #include -#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 *****************/ diff --git a/src/algorithms/tracking/libs/tracking_Gaussian_filter.cc b/src/algorithms/tracking/libs/tracking_Gaussian_filter.cc index eab781a23..58545f59f 100644 --- a/src/algorithms/tracking/libs/tracking_Gaussian_filter.cc +++ b/src/algorithms/tracking/libs/tracking_Gaussian_filter.cc @@ -38,31 +38,32 @@ */ #include "tracking_Gaussian_filter.h" +#include 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)); } diff --git a/src/algorithms/tracking/libs/tracking_Gaussian_filter.h b/src/algorithms/tracking/libs/tracking_Gaussian_filter.h index 85ac7bb2a..20daa8f04 100644 --- a/src/algorithms/tracking/libs/tracking_Gaussian_filter.h +++ b/src/algorithms/tracking/libs/tracking_Gaussian_filter.h @@ -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 #include -#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* ft) { func_transition = ft; }; void set_measurement_model(ModelFunction* fm) { func_measurement = fm; }; - void set_model(ModelFunction* ft, ModelFunction* fm) { + void set_model(ModelFunction* ft, ModelFunction* fm) + { set_transition_model(ft); set_measurement_model(fm); }; @@ -87,7 +88,6 @@ private: ModelFunction* func_measurement; NonlinearFilter GaussFilt; - }; /* Template definitions */ @@ -98,7 +98,7 @@ private: * The output is in [Hz/s]. */ template -arma::vec TrackingNonlinearFilter::get_carrier_nco(const OutputType2 meas_in) +arma::vec TrackingNonlinearFilter::get_carrier_nco(const OutputType2 meas_in) { arma::vec state_pred; arma::mat state_cov_pred; diff --git a/src/algorithms/tracking/libs/tracking_models.h b/src/algorithms/tracking/libs/tracking_models.h index 62813de38..398d473e8 100644 --- a/src/algorithms/tracking/libs/tracking_models.h +++ b/src/algorithms/tracking/libs/tracking_models.h @@ -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 #include #include // for boost::shared_ptr #include // for block #include // for gr_complex #include // for gr_vector_int, gr_vector... #include -#include // for int32_t -#include // for string, ofstream -#include // for pair +#include // for int32_t +#include // for string, ofstream +#include // for pair #include // 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);