From e17472d986a2e0b3d80c61d5cc84025e920ceac9 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Mon, 1 Jul 2019 11:00:38 +0200 Subject: [PATCH 01/24] Partial merge commit, to be fixed in next commit --- .../beidou_b1i_telemetry_decoder_gs.cc | 135 ++-- .../beidou_b1i_telemetry_decoder_gs.h | 10 +- .../galileo_telemetry_decoder_gs.cc | 109 ++-- .../galileo_telemetry_decoder_gs.h | 2 - .../gps_l1_ca_telemetry_decoder_gs.cc | 133 ++-- .../gps_l5_telemetry_decoder_gs.cc | 75 +-- .../gps_l5_telemetry_decoder_gs.h | 4 - .../gnuradio_blocks/dll_pll_veml_tracking.cc | 590 ++++++++---------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 9 +- src/core/system_parameters/Beidou_B1I.h | 8 +- src/core/system_parameters/Beidou_B3I.h | 2 + src/core/system_parameters/GPS_L1_CA.h | 2 + 12 files changed, 413 insertions(+), 666 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index 9417cd510..0ca5f5e74 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -72,56 +72,27 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); LOG(INFO) << "Initializing BeiDou B1I Telemetry Decoding for satellite " << this->d_satellite; - d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D1NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - - // Setting samples of secondary code - for (int32_t i = 0; i < BEIDOU_B1I_SECONDARY_CODE_LENGTH; i++) - { - if (BEIDOU_B1I_SECONDARY_CODE.at(i) == '1') - { - d_secondary_code_symbols[i] = 1; - } - else - { - d_secondary_code_symbols[i] = -1; - } - } + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - int32_t m = 0; if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); // Generic settings d_sample_counter = 0; @@ -141,7 +112,6 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -189,7 +159,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, in void beidou_b1i_telemetry_decoder_gs::decode_word( int32_t word_counter, - const double *enc_word_symbols, + const float *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -229,7 +199,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_word( } -void beidou_b1i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) +void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -312,48 +282,36 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell // Update satellite information for DNAV decoder sat_prn = d_satellite.get_PRN(); d_nav.i_satellite_PRN = sat_prn; - d_nav.i_signal_type = 1; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) + d_nav.i_signal_type = 1; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) // Update tel dec parameters for D2 NAV Messages if (sat_prn > 0 and sat_prn < 6) { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); - d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D2NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = nullptr; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = 1; - n++; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -1; - n++; - } + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); } } @@ -403,7 +361,8 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_flag_preamble = false; - if (d_symbol_history.size() > d_required_symbols) + //std::cout << "size: " << d_symbol_history.size() << " in " << current_symbol.Prompt_I << std::endl; + if (d_symbol_history.size() >= d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -416,9 +375,15 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { corr_value += d_preamble_samples[i]; } + //std::cout << "corr: " << corr_value << ","; } + //std::cout << " final corr: " << corr_value << std::endl; } + if (abs(corr_value) >= d_samples_per_preamble) + { + std::cout << " preamble corr: " << corr_value << std::endl; + } //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -461,53 +426,38 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ // ******* SAMPLES TO SYMBOLS ******* if (corr_value > 0) //normal PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = d_symbol_history.at(i); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = d_symbol_history.at(i); } } } else // 180 deg. inverted carrier phase PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = -d_symbol_history.at(i); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] = -d_symbol_history.at(i); } } } + // call the decoder decode_subframe(d_subframe_symbols); @@ -536,7 +486,6 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ } } } - // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h index 3a7b8debf..9180a40b8 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h @@ -39,7 +39,7 @@ #include #include // for boost::shared_ptr #include // for block -#include // for gr_vector_const_void_star +#include // for gr_vector_const_void_star #include #include #include @@ -77,19 +77,17 @@ private: beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); beidou_b1i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(double *symbols); - void decode_word(int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols); + void decode_subframe(float *symbols); + void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols); void decode_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; - int32_t *d_secondary_code_symbols; - uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - double *d_subframe_symbols; + float *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index a94aebcf8..4bfcc06d3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -85,15 +85,13 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( case 1: // INAV { d_PRN_code_period_ms = static_cast(GALILEO_E1_CODE_PERIOD_MS); - d_samples_per_symbol = GALILEO_E1_B_SAMPLES_PER_SYMBOL; d_bits_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS; // set the preamble - d_samples_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol; + d_samples_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS; d_preamble_period_symbols = GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS; d_required_symbols = static_cast(GALILEO_INAV_PAGE_SYMBOLS) + d_samples_per_preamble; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_secondary_code_samples = nullptr; d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; @@ -103,31 +101,18 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( } case 2: // FNAV { - d_PRN_code_period_ms = static_cast(GALILEO_E5A_CODE_PERIOD_MS); - d_samples_per_symbol = GALILEO_FNAV_CODES_PER_SYMBOL; + d_PRN_code_period_ms = static_cast(GALILEO_E5A_CODE_PERIOD_MS * GALILEO_E5A_I_SECONDARY_CODE_LENGTH); d_bits_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; // set the preamble - d_samples_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol; - d_preamble_period_symbols = GALILEO_FNAV_CODES_PER_PAGE; - d_required_symbols = static_cast(GALILEO_FNAV_SYMBOLS_PER_PAGE) * d_samples_per_symbol + d_samples_per_preamble; + d_samples_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; + d_preamble_period_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE; + d_required_symbols = static_cast(GALILEO_FNAV_SYMBOLS_PER_PAGE) + d_samples_per_preamble; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_secondary_code_samples = static_cast(volk_gnsssdr_malloc(GALILEO_E5A_I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; - for (int32_t i = 0; i < GALILEO_E5A_I_SECONDARY_CODE_LENGTH; i++) - { - if (GALILEO_E5A_I_SECONDARY_CODE.at(i) == '1') - { - d_secondary_code_samples[i] = 1; - } - else - { - d_secondary_code_samples[i] = -1; - } - } - d_max_symbols_without_valid_frame = GALILEO_FNAV_CODES_PER_PAGE * 10; //rise alarm 100 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_FNAV_SYMBOLS_PER_PAGE * 10; //rise alarm 100 seconds without valid tlm break; } default: @@ -135,8 +120,6 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_samples_per_preamble = 0; d_preamble_period_symbols = 0; d_preamble_samples = nullptr; - d_secondary_code_samples = nullptr; - d_samples_per_symbol = 0U; d_PRN_code_period_ms = 0U; d_required_symbols = 0U; d_frame_length_symbols = 0U; @@ -147,7 +130,6 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( } d_page_part_symbols = static_cast(volk_gnsssdr_malloc(d_frame_length_symbols * sizeof(double), volk_gnsssdr_get_alignment())); - int32_t n = 0; for (int32_t i = 0; i < d_bits_per_preamble; i++) { switch (d_frame_type) @@ -156,45 +138,23 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( { if (GALILEO_INAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = 1; - n++; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -1; - n++; - } + d_preamble_samples[i] = -1; } break; } case 2: // FNAV for E5a-I { - // Galileo E5a data channel (E5a-I) still has a secondary code - int m = 0; if (GALILEO_FNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = d_secondary_code_samples[m]; - n++; - m++; - m = m % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -d_secondary_code_samples[m]; - n++; - m++; - m = m % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = -1; } break; } @@ -235,10 +195,6 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( galileo_telemetry_decoder_gs::~galileo_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); - if (d_frame_type == 2) - { - volk_gnsssdr_free(d_secondary_code_samples); - } volk_gnsssdr_free(d_page_part_symbols); volk_gnsssdr_free(out0); volk_gnsssdr_free(out1); @@ -497,7 +453,24 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( // 1. Copy the current tracking output current_symbol = in[0][0]; // add new symbol to the symbol queue - d_symbol_history.push_back(current_symbol.Prompt_I); + switch (d_frame_type) + { + case 1: // INAV + { + d_symbol_history.push_back(current_symbol.Prompt_I); + break; + } + case 2: //FNAV + { + d_symbol_history.push_back(current_symbol.Prompt_Q); + break; + } + default: + { + d_symbol_history.push_back(current_symbol.Prompt_I); + break; + } + } d_sample_counter++; // count for the processed symbols consume_each(1); d_flag_preamble = false; @@ -626,29 +599,21 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( // 0. fetch the symbols into an array if (flag_PLL_180_deg_phase_locked == false) // normal PLL lock { - int k = 0; for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] = 0; - for (uint32_t m = 0; m < d_samples_per_symbol; m++) + for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] += static_cast(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); // because last symbol of the preamble is just received now! - k++; - k = k % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + d_page_part_symbols[i] = d_symbol_history.at(i + d_samples_per_preamble); // because last symbol of the preamble is just received now! } } } else // 180 deg. inverted carrier phase PLL lock { - int k = 0; for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] = 0; - for (uint32_t m = 0; m < d_samples_per_symbol; m++) // integrate samples into symbols + for (uint32_t i = 0; i < d_frame_length_symbols; i++) { - d_page_part_symbols[i] -= static_cast(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); // because last symbol of the preamble is just received now! - k++; - k = k % GALILEO_E5A_I_SECONDARY_CODE_LENGTH; + d_page_part_symbols[i] = -d_symbol_history.at(i + d_samples_per_preamble); // because last symbol of the preamble is just received now! } } } @@ -732,7 +697,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( if (d_fnav_nav.flag_TOW_1 == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_1 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_1 = false; } @@ -740,26 +705,26 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_2 * 1000.0); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_2 = false; } else if (d_fnav_nav.flag_TOW_3 == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_3 * 1000.0); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_3 = false; } else if (d_fnav_nav.flag_TOW_4 == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.FNAV_TOW_4 * 1000.0); //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.flag_TOW_4 = false; } else { - d_TOW_at_current_symbol_ms += static_cast(GALILEO_E5A_CODE_PERIOD_MS); + d_TOW_at_current_symbol_ms += static_cast(GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); } break; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index 33e936d04..9d3ed657d 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -85,8 +85,6 @@ private: int32_t d_samples_per_preamble; int32_t d_preamble_period_symbols; int32_t *d_preamble_samples; - int32_t *d_secondary_code_samples; - uint32_t d_samples_per_symbol; uint32_t d_PRN_code_period_ms; uint32_t d_required_symbols; uint32_t d_frame_length_symbols; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index eb5d43f4f..612045e31 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -76,10 +76,11 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( DLOG(INFO) << "Initializing GPS L1 TELEMETRY DECODER"; d_bits_per_preamble = GPS_CA_PREAMBLE_LENGTH_BITS; - d_samples_per_preamble = d_bits_per_preamble * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; - d_preamble_period_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + //d_samples_per_preamble = d_bits_per_preamble * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_samples_per_preamble = d_bits_per_preamble; + d_preamble_period_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; // set the preamble - d_required_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_required_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_frame_length_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; @@ -89,19 +90,19 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( { if (GPS_CA_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - d_preamble_samples[n] = 1; - n++; - } + // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) + // { + d_preamble_samples[n] = 1; + n++; + // } } else { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - d_preamble_samples[n] = -1; - n++; - } + // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) + // { + d_preamble_samples[n] = -1; + n++; + // } } } d_sample_counter = 0ULL; @@ -209,74 +210,58 @@ void gps_l1_ca_telemetry_decoder_gs::set_channel(int32_t channel) bool gps_l1_ca_telemetry_decoder_gs::decode_subframe() { char subframe[GPS_SUBFRAME_LENGTH]; - - int32_t symbol_accumulator_counter = 0; int32_t frame_bit_index = 0; int32_t word_index = 0; uint32_t GPS_frame_4bytes = 0; - float symbol_accumulator = 0; bool subframe_synchro_confirmation = true; for (float subframe_symbol : d_symbol_history) { // ******* SYMBOL TO BIT ******* - // extended correlation to bit period is enabled in tracking! - symbol_accumulator += subframe_symbol; // accumulate the input value in d_symbol_accumulator - symbol_accumulator_counter++; - if (symbol_accumulator_counter == 20) + // symbol to bit + if (subframe_symbol > 0) { - // symbol to bit - if (symbol_accumulator > 0) - { - GPS_frame_4bytes += 1; // insert the telemetry bit in LSB - //std::cout << "1"; - } - else - { - //std::cout << "0"; - } - symbol_accumulator = 0; - symbol_accumulator_counter = 0; + GPS_frame_4bytes += 1; // insert the telemetry bit in LSB + } - // ******* bits to words ****** - frame_bit_index++; - if (frame_bit_index == 30) + // ******* bits to words ****** + frame_bit_index++; + if (frame_bit_index == 30) + { + frame_bit_index = 0; + // parity check + // Each word in wordbuff is composed of: + // Bits 0 to 29 = the GPS data word + // Bits 30 to 31 = 2 LSBs of the GPS word ahead. + // prepare the extended frame [-2 -1 0 ... 30] + if (d_prev_GPS_frame_4bytes & 0x00000001) { - frame_bit_index = 0; - // parity check - // Each word in wordbuff is composed of: - // Bits 0 to 29 = the GPS data word - // Bits 30 to 31 = 2 LSBs of the GPS word ahead. - // prepare the extended frame [-2 -1 0 ... 30] - if (d_prev_GPS_frame_4bytes & 0x00000001) - { - GPS_frame_4bytes = GPS_frame_4bytes | 0x40000000; - } - if (d_prev_GPS_frame_4bytes & 0x00000002) - { - GPS_frame_4bytes = GPS_frame_4bytes | 0x80000000; - } - // Check that the 2 most recently logged words pass parity. Have to first - // invert the data bits according to bit 30 of the previous word. - if (GPS_frame_4bytes & 0x40000000) - { - GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR) - } - // check parity. If ANY word inside the subframe fails the parity, set subframe_synchro_confirmation = false - if (not gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(GPS_frame_4bytes)) - { - subframe_synchro_confirmation = false; - } - // add word to subframe - // insert the word in the correct position of the subframe - std::memcpy(&subframe[word_index * GPS_WORD_LENGTH], &GPS_frame_4bytes, sizeof(uint32_t)); - word_index++; - d_prev_GPS_frame_4bytes = GPS_frame_4bytes; // save the actual frame - GPS_frame_4bytes = 0; + GPS_frame_4bytes = GPS_frame_4bytes | 0x40000000; } - else + if (d_prev_GPS_frame_4bytes & 0x00000002) { - GPS_frame_4bytes <<= 1; // shift 1 bit left the telemetry word + GPS_frame_4bytes = GPS_frame_4bytes | 0x80000000; } + // Check that the 2 most recently logged words pass parity. Have to first + // invert the data bits according to bit 30 of the previous word. + if (GPS_frame_4bytes & 0x40000000) + { + GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR) + } + // check parity. If ANY word inside the subframe fails the parity, set subframe_synchro_confirmation = false + if (not gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(GPS_frame_4bytes)) + { + subframe_synchro_confirmation = false; + } + // add word to subframe + // insert the word in the correct position of the subframe + std::memcpy(&subframe[word_index * GPS_WORD_LENGTH], &GPS_frame_4bytes, sizeof(uint32_t)); + word_index++; + d_prev_GPS_frame_4bytes = GPS_frame_4bytes; // save the actual frame + GPS_frame_4bytes = 0; + } + else + { + GPS_frame_4bytes <<= 1; // shift 1 bit left the telemetry word } } @@ -378,10 +363,10 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { // correlate with preamble int32_t corr_value = 0; - if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS) + if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_BITS) { // ******* preamble correlation ******** - for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) + for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++) { if (d_symbol_history[i] < 0.0) // symbols clipping { @@ -397,6 +382,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { d_preamble_index = d_sample_counter; // record the preamble sample stamp DLOG(INFO) << "Preamble detection for GPS L1 satellite " << this->d_satellite; + decode_subframe(); d_stat = 1; // enter into frame pre-detection status } flag_TOW_set = false; @@ -407,10 +393,10 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ // correlate with preamble int32_t corr_value = 0; int32_t preamble_diff = 0; - if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS) + if (d_symbol_history.size() >= GPS_CA_PREAMBLE_LENGTH_BITS) { // ******* preamble correlation ******** - for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) + for (int32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++) { if (d_symbol_history[i] < 0.0) // symbols clipping { @@ -438,6 +424,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { flag_PLL_180_deg_phase_locked = false; } + decode_subframe(); d_stat = 2; } else @@ -510,7 +497,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { if (flag_TOW_set == true) { - d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; + d_TOW_at_current_symbol_ms += GPS_L1_CA_BIT_PERIOD_MS; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index ea8a87e44..891f12c27 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -64,7 +64,7 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry_to_trk")); d_last_valid_preamble = 0; d_sent_tlm_failed_msg = false; - d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SAMPLES_PER_SYMBOL * GPS_L5_SYMBOLS_PER_BIT * 10; //rise alarm if 20 consecutive subframes have no valid CRC + d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SYMBOLS_PER_BIT * 10; //rise alarm if 20 consecutive subframes have no valid CRC // initialize internal vars d_dump = dump; @@ -74,23 +74,8 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( d_flag_valid_word = false; d_TOW_at_current_symbol_ms = 0U; d_TOW_at_Preamble_ms = 0U; - sym_hist.set_capacity(GPS_L5I_NH_CODE_LENGTH); - // initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); - for (int32_t aux = 0; aux < GPS_L5I_NH_CODE_LENGTH; aux++) - { - if (GPS_L5I_NH_CODE[aux] == 0) - { - bits_NH[aux] = -1.0; - } - else - { - bits_NH[aux] = 1.0; - } - } - sync_NH = false; - new_sym = false; d_sample_counter = 0; } @@ -171,9 +156,6 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u // 1. Copy the current tracking output current_synchro_data = in[0]; consume_each(1); // one by one - sym_hist.push_back(in[0].Prompt_I); - int32_t corr_NH = 0; - int32_t symbol_value = 0; // check if there is a problem with the telemetry of the current satellite d_sample_counter++; // count for the processed symbols @@ -187,61 +169,18 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u } } - // Search correlation with Neuman-Hofman Code (see IS-GPS-705D) - if (sym_hist.size() == GPS_L5I_NH_CODE_LENGTH) - { - for (int32_t i = 0; i < GPS_L5I_NH_CODE_LENGTH; i++) - { - if ((bits_NH[i] * sym_hist[i]) > 0.0) - { - corr_NH += 1; - } - else - { - corr_NH -= 1; - } - } - if (abs(corr_NH) == GPS_L5I_NH_CODE_LENGTH) - { - sync_NH = true; - if (corr_NH > 0) - { - symbol_value = 1; - } - else - { - symbol_value = -1; - } - new_sym = true; - //sym_hist.clear(); - } - else - { - sync_NH = false; - new_sym = false; - } - } - - bool flag_new_cnav_frame = false; cnav_msg_t msg; - uint32_t delay = 0; - - // add the symbol to the decoder - if (new_sym) - { - uint8_t symbol_clip = static_cast(symbol_value > 0) * 255; - flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); - new_sym = false; - } + uint32_t delay; + uint8_t symbol_clip = static_cast(current_synchro_data.Prompt_Q > 0) * 255; // 2. Add the telemetry decoder information // check if new CNAV frame is available - if (flag_new_cnav_frame == true) + if (cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay) == true) { std::bitset raw_bits; // Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder for (uint32_t i = 0; i < GPS_L5_CNAV_DATA_PAGE_BITS; i++) { - raw_bits[GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1U); + raw_bits[GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i / 8] >> (7 - i % 8)) & 1u); } d_CNAV_Message.decode_page(raw_bits); @@ -279,7 +218,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u //check TOW update consistency uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5I_SYMBOL_PERIOD_MS; - if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > 1) + if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > GPS_L5I_SYMBOL_PERIOD_MS) { DLOG(INFO) << "Warning: GPS L5 TOW update in ch " << d_channel << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms " @@ -298,7 +237,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u { if (d_flag_valid_word) { - d_TOW_at_current_symbol_ms += GPS_L5I_PERIOD_MS; + d_TOW_at_current_symbol_ms += GPS_L5I_SYMBOL_PERIOD_MS; if (current_synchro_data.Flag_valid_symbol_output == false) { d_flag_valid_word = false; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h index 1971f3c09..9c49694d3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h @@ -91,10 +91,6 @@ private: uint32_t d_max_symbols_without_valid_frame; Gps_CNAV_Navigation_Message d_CNAV_Message; - float bits_NH[GPS_L5I_NH_CODE_LENGTH]{}; - boost::circular_buffer sym_hist; - bool sync_NH; - bool new_sym; }; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index e26712291..c90301a33 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -63,11 +63,9 @@ #include // for fill_n #include // for fmod, round, floor #include // for exception -#include -#include // for cout, cerr +#include // for cout, cerr #include #include -#include #if HAS_STD_FILESYSTEM #if HAS_STD_FILESYSTEM_EXPERIMENTAL @@ -109,6 +107,8 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_chip_rate = 0.0; d_secondary_code_length = 0U; d_secondary_code_string = nullptr; + d_data_secondary_code_length = 0U; + d_data_secondary_code_string = nullptr; d_preambles_symbols = nullptr; d_preamble_length_symbols = 0; signal_type = std::string(trk_parameters.signal); @@ -134,39 +134,17 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl 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_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); // GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - - // set the preamble - uint16_t preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE; - - // preamble bits to sampled symbols - d_preamble_length_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - for (uint16_t preambles_bit : preambles_bits) - { - for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer + // symbol integration: 20 trk symbols (20 ms) = 1 tlm bit + // set the preamble in the secondary code acquisition to obtain tlm symbol synchronization + d_secondary_code_length = static_cast(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&GPS_CA_PREAMBLE_SYMBOLS_STR); + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; } else if (signal_type == "2S") { @@ -174,19 +152,20 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; 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; d_correlation_length_ms = 20; d_code_samples_per_chip = 1; // GPS L2 does not have pilot component nor secondary code d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; } 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; + // symbol integration: 10 trk symbols (10 ms) = 1 tlm bit d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; @@ -194,17 +173,22 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = true; if (trk_parameters.track_pilot) { + //synchronize pilot secondary code d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); + //remove data secondary code + //remove Neuman-Hofman Code (see IS-GPS-705D) + d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; - interchange_iq = true; } else { + //synchronize and remove data secondary code + //remove Neuman-Hofman Code (see IS-GPS-705D) d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; } } else @@ -213,7 +197,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cerr << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0U; @@ -230,6 +213,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GALILEO_E1_CODE_PERIOD; d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ; 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; d_correlation_length_ms = 4; d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip @@ -246,7 +230,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = false; signal_pretty_name = signal_pretty_name + "B"; } - interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. } else if (signal_type == "5X") { @@ -257,20 +241,22 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GALILEO_E5A_CODE_LENGTH_CHIPS); - + d_secondary = true; if (trk_parameters.track_pilot) { - d_secondary = true; + //synchronize pilot secondary code d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); signal_pretty_name = signal_pretty_name + "Q"; - interchange_iq = true; + //remove data secondary code + d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); } else { - //Do not acquire secondary code in data component. It is done in telemetry decoder - d_secondary = false; + //synchronize and remove data secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); signal_pretty_name = signal_pretty_name + "I"; - interchange_iq = false; } } else @@ -279,7 +265,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0U; @@ -302,9 +287,11 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = true; trk_parameters.track_pilot = false; - interchange_iq = false; + //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); } else if (signal_type == "B3") { @@ -318,9 +305,10 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = true; trk_parameters.track_pilot = false; - interchange_iq = 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); } else { @@ -328,7 +316,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0; @@ -343,7 +330,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl std::cerr << "Invalid System argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; - interchange_iq = false; d_signal_carrier_freq = 0.0; d_code_period = 0.0; d_code_length_chips = 0U; @@ -589,52 +575,48 @@ void dll_pll_veml_tracking::start_tracking() d_carrier_phase_rate_step_rad = 0.0; d_carr_ph_history.clear(); d_code_ph_history.clear(); - std::array Signal_; - std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3); if (systemName == "GPS" and signal_type == "1C") { - gps_l1_ca_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); } else if (systemName == "GPS" and signal_type == "2S") { - gps_l2c_m_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); } else if (systemName == "GPS" and signal_type == "L5") { if (trk_parameters.track_pilot) { - gps_l5q_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); - gps_l5i_code_gen_float(gsl::span(d_data_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - gps_l5i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "1B") { if (trk_parameters.track_pilot) { - std::array pilot_signal = {{'1', 'C', '\0'}}; - - galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), pilot_signal, d_acquisition_gnss_synchro->PRN); - galileo_e1_code_gen_sinboc11_float(gsl::span(d_data_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); + char pilot_signal[3] = "1C"; + galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "5X") { auto *aux_code = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); - std::array signal_type_ = {{'5', 'X', '\0'}}; - galileo_e5_a_code_gen_complex_primary(gsl::span(aux_code, d_code_length_chips), d_acquisition_gnss_synchro->PRN, signal_type_); + galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); if (trk_parameters.track_pilot) { d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); @@ -657,7 +639,7 @@ void dll_pll_veml_tracking::start_tracking() } else if (systemName == "Beidou" and signal_type == "B1") { - beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -666,38 +648,17 @@ void dll_pll_veml_tracking::start_tracking() d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - d_secondary_code_length = 0; - d_secondary_code_string = const_cast(&BEIDOU_B1I_D2_SECONDARY_CODE_STR); - // preamble bits to sampled symbols - d_preamble_length_symbols = 22; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(22 * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - uint32_t preambles_bits[BEIDOU_B1I_PREAMBLE_LENGTH_BITS] = {1, 1, 1, 0, 0, 0, 1, 0, 0, 1, 0}; - for (uint32_t preambles_bit : preambles_bits) - { - for (int32_t j = 0; j < d_symbols_per_bit; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.resize(22); // Change fixed buffer size - d_symbol_history.clear(); + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR); + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } else if (systemName == "Beidou" and signal_type == "B3") { - beidou_b3i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); + beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -706,32 +667,11 @@ void dll_pll_veml_tracking::start_tracking() d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - interchange_iq = false; - d_secondary_code_length = 0; - d_secondary_code_string = const_cast(&BEIDOU_B3I_D2_SECONDARY_CODE_STR); - // preamble bits to sampled symbols - d_preamble_length_symbols = 22; - d_preambles_symbols = static_cast(volk_gnsssdr_malloc(22 * sizeof(int32_t), volk_gnsssdr_get_alignment())); - int32_t n = 0; - uint32_t preambles_bits[BEIDOU_B3I_PREAMBLE_LENGTH_BITS] = {1, 1, 1, 0, 0, 0, 1, 0, 0, 1, 0}; - for (uint32_t preambles_bit : preambles_bits) - { - for (int32_t j = 0; j < d_symbols_per_bit; j++) - { - if (preambles_bit == 1) - { - d_preambles_symbols[n] = 1; - } - else - { - d_preambles_symbols[n] = -1; - } - n++; - } - } - d_symbol_history.resize(22); // Change fixed buffer size - d_symbol_history.clear(); + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = const_cast(&BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR); + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -1053,6 +993,7 @@ void dll_pll_veml_tracking::clear_tracking_vars() if (trk_parameters.track_pilot) { d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); } d_P_accu_old = gr_complex(0.0, 0.0); d_carr_phase_error_hz = 0.0; @@ -1061,6 +1002,7 @@ void dll_pll_veml_tracking::clear_tracking_vars() d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; + d_current_data_symbol = 0; d_Prompt_circular_buffer.clear(); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; @@ -1184,12 +1126,69 @@ void dll_pll_veml_tracking::save_correlation_results() d_E_accu += *d_Early; d_P_accu += *d_Prompt; d_L_accu += *d_Late; - d_current_symbol++; - d_current_symbol %= d_symbols_per_bit; } - // If tracking pilot, disable Costas loop + + //data secondary code roll-up + if (d_symbols_per_bit > 1) + { + if (d_data_secondary_code_length > 0) + { + if (trk_parameters.track_pilot) + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu -= *d_Prompt_Data; + } + } + else + { + if (d_data_secondary_code_string->at(d_current_data_symbol) == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + //std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + d_current_data_symbol++; + // data secondary code roll-up + d_current_data_symbol %= d_data_secondary_code_length; + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu += *d_Prompt_Data; + } + else + { + d_P_data_accu += *d_Prompt; + } + d_current_data_symbol++; + d_current_data_symbol %= d_symbols_per_bit; + } + } + else + { + if (trk_parameters.track_pilot) + { + d_P_data_accu = *d_Prompt_Data; + } + else + { + d_P_data_accu = *d_Prompt; + } + } + if (trk_parameters.track_pilot) { + // If tracking pilot, disable Costas loop d_cloop = false; } else @@ -1198,8 +1197,7 @@ void dll_pll_veml_tracking::save_correlation_results() } } - -void dll_pll_veml_tracking::log_data(bool integrating) +void dll_pll_veml_tracking::log_data() { if (d_dump) { @@ -1212,29 +1210,13 @@ void dll_pll_veml_tracking::log_data(bool integrating) uint64_t tmp_long_int; if (trk_parameters.track_pilot) { - if (interchange_iq) - { - prompt_I = d_Prompt_Data->imag(); - prompt_Q = d_Prompt_Data->real(); - } - else - { - prompt_I = d_Prompt_Data->real(); - prompt_Q = d_Prompt_Data->imag(); - } + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); } else { - if (interchange_iq) - { - prompt_I = d_Prompt->imag(); - prompt_Q = d_Prompt->real(); - } - else - { - prompt_I = d_Prompt->real(); - prompt_Q = d_Prompt->imag(); - } + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); } if (d_veml) { @@ -1249,20 +1231,6 @@ void dll_pll_veml_tracking::log_data(bool integrating) tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); - if (integrating) - { - //TODO: Improve this solution! - // It compensates the amplitude difference while integrating - if (d_extend_correlation_symbols_count > 0) - { - float scale_factor = static_cast(trk_parameters.extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); - tmp_VE *= scale_factor; - tmp_E *= scale_factor; - tmp_P *= scale_factor; - tmp_L *= scale_factor; - tmp_VL *= scale_factor; - } - } try { @@ -1361,28 +1329,28 @@ int32_t dll_pll_veml_tracking::save_matfile() { return 1; } - auto abs_VE = std::vector(num_epoch); - auto abs_E = std::vector(num_epoch); - auto abs_P = std::vector(num_epoch); - auto abs_L = std::vector(num_epoch); - auto abs_VL = std::vector(num_epoch); - auto Prompt_I = std::vector(num_epoch); - auto Prompt_Q = std::vector(num_epoch); - auto PRN_start_sample_count = std::vector(num_epoch); - auto acc_carrier_phase_rad = std::vector(num_epoch); - auto carrier_doppler_hz = std::vector(num_epoch); - auto carrier_doppler_rate_hz = std::vector(num_epoch); - auto code_freq_chips = std::vector(num_epoch); - auto code_freq_rate_chips = std::vector(num_epoch); - auto carr_error_hz = std::vector(num_epoch); - auto carr_error_filt_hz = std::vector(num_epoch); - auto code_error_chips = std::vector(num_epoch); - auto code_error_filt_chips = std::vector(num_epoch); - auto CN0_SNV_dB_Hz = std::vector(num_epoch); - auto carrier_lock_test = std::vector(num_epoch); - auto aux1 = std::vector(num_epoch); - auto aux2 = std::vector(num_epoch); - auto PRN = std::vector(num_epoch); + auto *abs_VE = new float[num_epoch]; + auto *abs_E = new float[num_epoch]; + auto *abs_P = new float[num_epoch]; + auto *abs_L = new float[num_epoch]; + auto *abs_VL = new float[num_epoch]; + auto *Prompt_I = new float[num_epoch]; + auto *Prompt_Q = new float[num_epoch]; + auto *PRN_start_sample_count = new uint64_t[num_epoch]; + auto *acc_carrier_phase_rad = new float[num_epoch]; + auto *carrier_doppler_hz = new float[num_epoch]; + auto *carrier_doppler_rate_hz = new float[num_epoch]; + auto *code_freq_chips = new float[num_epoch]; + auto *code_freq_rate_chips = new float[num_epoch]; + auto *carr_error_hz = new float[num_epoch]; + auto *carr_error_filt_hz = new float[num_epoch]; + auto *code_error_chips = new float[num_epoch]; + auto *code_error_filt_chips = new float[num_epoch]; + auto *CN0_SNV_dB_Hz = new float[num_epoch]; + auto *carrier_lock_test = new float[num_epoch]; + auto *aux1 = new float[num_epoch]; + auto *aux2 = new double[num_epoch]; + auto *PRN = new uint32_t[num_epoch]; try { @@ -1419,6 +1387,28 @@ int32_t dll_pll_veml_tracking::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] carrier_doppler_rate_hz; + delete[] code_freq_chips; + delete[] code_freq_rate_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; return 1; } @@ -1432,95 +1422,117 @@ int32_t dll_pll_veml_tracking::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0); + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0); + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] carrier_doppler_rate_hz; + delete[] code_freq_chips; + delete[] code_freq_rate_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; return 0; } @@ -1557,21 +1569,18 @@ void dll_pll_veml_tracking::set_channel(uint32_t channel) } } - void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { gr::thread::scoped_lock l(d_setlock); d_acquisition_gnss_synchro = p_gnss_synchro; } - void dll_pll_veml_tracking::stop_tracking() { gr::thread::scoped_lock l(d_setlock); d_state = 0; } - int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { @@ -1579,6 +1588,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) const auto *in = reinterpret_cast(input_items[0]); auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); + current_synchro_data.Flag_valid_symbol_output = false; if (d_pull_in_transitory == true) { @@ -1656,7 +1666,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) update_tracking_vars(); // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); + log_data(); if (!d_pull_in_transitory) { @@ -1678,42 +1688,18 @@ int dll_pll_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 { - float current_tracking_time_s = static_cast(d_sample_counter - d_acq_sample_stamp) / trk_parameters.fs_in; - if (current_tracking_time_s > 10) + //******* preamble correlation ******** + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) { - d_symbol_history.push_back(d_Prompt->real()); - //******* preamble correlation ******** - int32_t corr_value = 0; - if ((static_cast(d_symbol_history.size()) == d_preamble_length_symbols)) // and (d_make_correlation or !d_flag_frame_sync)) + next_state = acquire_secondary(); + if (next_state) { - int i = 0; - for (const auto &iter : d_symbol_history) - { - if (iter < 0.0) // symbols clipping - { - corr_value -= d_preambles_symbols[i]; - } - else - { - corr_value += d_preambles_symbols[i]; - } - i++; - } - } - if (corr_value == d_preamble_length_symbols) - { - LOG(INFO) << systemName << " " << signal_pretty_name << " tracking preamble detected in channel " << d_channel + LOG(INFO) << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + std::cout << systemName << " " << signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - next_state = true; } - else - { - next_state = false; - } - } - else - { - next_state = false; } } else @@ -1725,52 +1711,17 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { next_state = false; //keep in state 2 during pull-in transitory } - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - if (next_state) { // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); d_P_accu = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); d_Prompt_circular_buffer.clear(); d_current_symbol = 0; + d_current_data_symbol = 0; if (d_enable_extended_integration) { @@ -1811,48 +1762,25 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } case 3: // coherent integration (correlation time extension) { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; // perform a correlation step do_correlation_step(in); - update_tracking_vars(); save_correlation_results(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) + update_tracking_vars(); + if (d_current_data_symbol == 0) { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; d_extend_correlation_symbols_count++; if (d_extend_correlation_symbols_count == (trk_parameters.extend_correlation_symbols - 1)) { @@ -1863,9 +1791,6 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } case 4: // narrow tracking { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // perform a correlation step do_correlation_step(in); save_correlation_results(); @@ -1880,44 +1805,23 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { run_dll_pll(); update_tracking_vars(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) + if (d_current_data_symbol == 0) { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); } - else - { - if (trk_parameters.track_pilot) - { - // Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); + log_data(); // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 170a3ddb5..4005cc88b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -85,7 +85,7 @@ private: void update_tracking_vars(); void clear_tracking_vars(); void save_correlation_results(); - void log_data(bool integrating); + void log_data(); int32_t save_matfile(); // tracking configuration vars @@ -97,22 +97,23 @@ private: // Signal parameters bool d_secondary; - bool interchange_iq; double d_signal_carrier_freq; double d_code_period; double d_code_chip_rate; uint32_t d_secondary_code_length; + uint32_t d_data_secondary_code_length; uint32_t d_code_length_chips; uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) int32_t d_symbols_per_bit; std::string systemName; std::string signal_type; std::string *d_secondary_code_string; + std::string *d_data_secondary_code_string; std::string signal_pretty_name; int32_t *d_preambles_symbols; int32_t d_preamble_length_symbols; - boost::circular_buffer d_symbol_history; + //boost::circular_buffer d_symbol_history; // dll filter buffer boost::circular_buffer d_dll_filt_history; @@ -144,6 +145,7 @@ private: bool d_enable_extended_integration; int32_t d_extend_correlation_symbols_count; int32_t d_current_symbol; + int32_t d_current_data_symbol; gr_complex d_VE_accu; gr_complex d_E_accu; @@ -152,6 +154,7 @@ private: gr_complex d_L_accu; gr_complex d_VL_accu; + gr_complex d_P_data_accu; gr_complex *d_Prompt_Data; double d_code_phase_step_chips; diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index 7ee713625..48dd4ea78 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -57,13 +57,17 @@ const double BEIDOU_B1I_CHIP_PERIOD = 4.8875e-07; //!< beidou b1I chip period const int32_t BEIDOU_B1I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B1I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B1I_SECONDARY_CODE_STR = "00000100110101001110"; +const std::string BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR = {"1111110000001100001100"}; +const int32_t BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS = 22; + const std::string BEIDOU_B1I_D2_SECONDARY_CODE_STR = "00"; const int BEIDOU_B1I_PREAMBLE_LENGTH_BITS = 11; const int BEIDOU_B1I_PREAMBLE_LENGTH_SYMBOLS = 220; // ************** const double BEIDOU_B1I_PREAMBLE_DURATION_S = 0.220; const int BEIDOU_B1I_PREAMBLE_DURATION_MS = 220; -const int BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] -const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; // ************* +const int BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] +const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; +const int BEIDOU_B1I_TELEMETRY_SYMBOL_PERIOD_MS = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; const int BEIDOU_B1I_TELEMETRY_RATE_SYMBOLS_SECOND = BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND * BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //************!< NAV message bit rate [symbols/s] const int BEIDOU_WORD_LENGTH = 4; //**************!< CRC + BEIDOU WORD (-2 -1 0 ... 29) Bits = 4 bytes const int BEIDOU_SUBFRAME_LENGTH = 40; //**************!< BEIDOU_WORD_LENGTH x 10 = 40 bytes diff --git a/src/core/system_parameters/Beidou_B3I.h b/src/core/system_parameters/Beidou_B3I.h index 13e4aa2ba..bd58aeae0 100644 --- a/src/core/system_parameters/Beidou_B3I.h +++ b/src/core/system_parameters/Beidou_B3I.h @@ -45,6 +45,8 @@ const uint32_t BEIDOU_B3I_CODE_PERIOD_MS = 1; //!< GPS L1 C/A code perio const int32_t BEIDOU_B3I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B3I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B3I_SECONDARY_CODE_STR = "00000100110101001110"; +const std::string BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR = {"1111110000001100001100"}; +const int32_t BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS = 22; const std::string BEIDOU_B3I_D2_SECONDARY_CODE_STR = "00"; const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_BITS = 11; const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_SYMBOLS = 220; // ************** diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 44579c185..2c6c7465f 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -55,6 +55,7 @@ const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [c const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] const uint32_t GPS_L1_CA_CODE_PERIOD_MS = 1U; //!< GPS L1 C/A code period [ms] +const uint32_t GPS_L1_CA_BIT_PERIOD_MS = 20U; //!< GPS L1 C/A bit period [ms] const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period [seconds] //optimum parameters @@ -84,6 +85,7 @@ const int32_t GPS_L1_CA_HISTORY_DEEP = 100; 1, 0, 0, 0, 1, 0, 1, 1 \ } const std::string GPS_CA_PREAMBLE = {"10001011"}; +const std::string GPS_CA_PREAMBLE_SYMBOLS_STR = {"1111111111111111111100000000000000000000000000000000000000000000000000000000000011111111111111111111000000000000000000001111111111111111111111111111111111111111"}; const int32_t GPS_CA_PREAMBLE_LENGTH_BITS = 8; const int32_t GPS_CA_PREAMBLE_LENGTH_SYMBOLS = 160; const double GPS_CA_PREAMBLE_DURATION_S = 0.160; From 658b001accdacf635bb40bf73d56f0e8265fa2f1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 1 Jul 2019 11:44:04 +0200 Subject: [PATCH 02/24] Fix building --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 220 ++++++++---------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 3 +- 2 files changed, 93 insertions(+), 130 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index c90301a33..1a03f987d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -63,9 +63,11 @@ #include // for fill_n #include // for fmod, round, floor #include // for exception -#include // for cout, cerr +#include +#include // for cout, cerr #include #include +#include #if HAS_STD_FILESYSTEM #if HAS_STD_FILESYSTEM_EXPERIMENTAL @@ -152,7 +154,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; 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 + // GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 20; d_code_samples_per_chip = 1; @@ -173,19 +175,19 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = true; if (trk_parameters.track_pilot) { - //synchronize pilot secondary code + // synchronize pilot secondary code d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5Q_NH_CODE_STR); - //remove data secondary code - //remove Neuman-Hofman Code (see IS-GPS-705D) + // remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_data_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "Q"; } else { - //synchronize and remove data secondary code - //remove Neuman-Hofman Code (see IS-GPS-705D) + // synchronize and remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); d_secondary_code_string = const_cast(&GPS_L5I_NH_CODE_STR); signal_pretty_name = signal_pretty_name + "I"; @@ -213,7 +215,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = GALILEO_E1_CODE_PERIOD; d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_HZ; 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 + // Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required d_symbols_per_bit = 1; d_correlation_length_ms = 4; d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip @@ -244,16 +246,16 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_secondary = true; if (trk_parameters.track_pilot) { - //synchronize pilot secondary code + // synchronize pilot secondary code d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); signal_pretty_name = signal_pretty_name + "Q"; - //remove data secondary code + // remove data secondary code d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); d_data_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); } else { - //synchronize and remove data secondary code + // synchronize and remove data secondary code d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&GALILEO_E5A_I_SECONDARY_CODE); signal_pretty_name = signal_pretty_name + "I"; @@ -287,7 +289,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_samples_per_chip = 1; d_secondary = true; trk_parameters.track_pilot = false; - //synchronize and remove data secondary code + // 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); @@ -540,7 +542,7 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) switch (tlm_event) { - case 1: //tlm fault in current channel + case 1: // tlm fault in current channel { DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; gr::thread::scoped_lock lock(d_setlock); @@ -565,7 +567,7 @@ void dll_pll_veml_tracking::start_tracking() { gr::thread::scoped_lock l(d_setlock); - // correct the code phase according to the delay between acq and trk + // correct the code phase according to the delay between acq and trk d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; @@ -575,55 +577,58 @@ void dll_pll_veml_tracking::start_tracking() d_carrier_phase_rate_step_rad = 0.0; d_carr_ph_history.clear(); d_code_ph_history.clear(); + std::array Signal_; + std::memcpy(Signal_.data(), d_acquisition_gnss_synchro->Signal, 3); if (systemName == "GPS" and signal_type == "1C") { - gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + gps_l1_ca_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); } else if (systemName == "GPS" and signal_type == "2S") { - gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l2c_m_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); } else if (systemName == "GPS" and signal_type == "L5") { if (trk_parameters.track_pilot) { - gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); - gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); + gps_l5q_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(gsl::span(d_data_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "1B") { if (trk_parameters.track_pilot) { - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); - galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + std::array pilot_signal = {{'1', 'C', '\0'}}; + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(d_data_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), Signal_, d_acquisition_gnss_synchro->PRN); } } else if (systemName == "Galileo" and signal_type == "5X") { auto *aux_code = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); - galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); + std::array signal_type_ = {{'5', 'X', '\0'}}; + galileo_e5_a_code_gen_complex_primary(gsl::span(aux_code, d_code_length_chips), d_acquisition_gnss_synchro->PRN, signal_type_); if (trk_parameters.track_pilot) { d_secondary_code_string = const_cast(&GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); for (uint32_t i = 0; i < d_code_length_chips; i++) { d_tracking_code[i] = aux_code[i].imag(); - d_data_code[i] = aux_code[i].real(); //the same because it is generated the full signal (E5aI + E5aQ) + d_data_code[i] = aux_code[i].real(); // the same because it is generated the full signal (E5aI + E5aQ) } d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); @@ -639,7 +644,7 @@ void dll_pll_veml_tracking::start_tracking() } else if (systemName == "Beidou" and signal_type == "B1") { - beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -658,7 +663,7 @@ void dll_pll_veml_tracking::start_tracking() else if (systemName == "Beidou" and signal_type == "B3") { - beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + beidou_b3i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { @@ -830,11 +835,9 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_ d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); // Carrier lock indicator d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); - //d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); // Loss of lock detection if (!d_pull_in_transitory) { - //d_carrier_lock_test < d_carrier_lock_threshold or if (d_carrier_lock_test < d_carrier_lock_threshold) { d_carrier_lock_fail_counter++; @@ -925,20 +928,20 @@ void dll_pll_veml_tracking::run_dll_pll() if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true) or trk_parameters.enable_fll_steady_state) { // FLL discriminator - //d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + // d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; d_P_accu_old = d_P_accu; - //std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; + // std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; // Carrier discriminator filter if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true)) { - //pure FLL, disable PLL + // pure FLL, disable PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, 0, d_current_correlation_time_s); } else { - //FLL-aided PLL + // FLL-aided PLL d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, d_carr_phase_error_hz, d_current_correlation_time_s); } } @@ -953,6 +956,7 @@ void dll_pll_veml_tracking::run_dll_pll() // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl; // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl; + // ################## DLL ########################################################## // DLL discriminator if (d_veml) @@ -1021,7 +1025,6 @@ void dll_pll_veml_tracking::update_tracking_vars() // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation T_prn_samples = T_prn_seconds * trk_parameters.fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples; - //d_current_prn_length_samples = static_cast(round(K_blk_samples)); // round to a discrete number of samples d_current_prn_length_samples = static_cast(std::floor(K_blk_samples)); // round to a discrete number of samples //################### PLL COMMANDS ################################################# @@ -1047,15 +1050,15 @@ void dll_pll_veml_tracking::update_tracking_vars() d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples; } } - //std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; + // std::cout << d_carrier_phase_rate_step_rad * trk_parameters.fs_in * trk_parameters.fs_in / PI_2 << std::endl; // remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad += static_cast(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)); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); // carrier phase accumulator - //double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); - //double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); - //std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl; + // double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + // double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + // 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 ################################################# @@ -1128,7 +1131,7 @@ void dll_pll_veml_tracking::save_correlation_results() d_L_accu += *d_Late; } - //data secondary code roll-up + // data secondary code roll-up if (d_symbols_per_bit > 1) { if (d_data_secondary_code_length > 0) @@ -1155,7 +1158,7 @@ void dll_pll_veml_tracking::save_correlation_results() 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++; // data secondary code roll-up d_current_data_symbol %= d_data_secondary_code_length; @@ -1197,6 +1200,7 @@ void dll_pll_veml_tracking::save_correlation_results() } } + void dll_pll_veml_tracking::log_data() { if (d_dump) @@ -1329,29 +1333,28 @@ int32_t dll_pll_veml_tracking::save_matfile() { return 1; } - auto *abs_VE = new float[num_epoch]; - auto *abs_E = new float[num_epoch]; - auto *abs_P = new float[num_epoch]; - auto *abs_L = new float[num_epoch]; - auto *abs_VL = new float[num_epoch]; - auto *Prompt_I = new float[num_epoch]; - auto *Prompt_Q = new float[num_epoch]; - auto *PRN_start_sample_count = new uint64_t[num_epoch]; - auto *acc_carrier_phase_rad = new float[num_epoch]; - auto *carrier_doppler_hz = new float[num_epoch]; - auto *carrier_doppler_rate_hz = new float[num_epoch]; - auto *code_freq_chips = new float[num_epoch]; - auto *code_freq_rate_chips = new float[num_epoch]; - auto *carr_error_hz = new float[num_epoch]; - auto *carr_error_filt_hz = new float[num_epoch]; - auto *code_error_chips = new float[num_epoch]; - auto *code_error_filt_chips = new float[num_epoch]; - auto *CN0_SNV_dB_Hz = new float[num_epoch]; - auto *carrier_lock_test = new float[num_epoch]; - auto *aux1 = new float[num_epoch]; - auto *aux2 = new double[num_epoch]; - auto *PRN = new uint32_t[num_epoch]; - + auto abs_VE = std::vector(num_epoch); + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto abs_VL = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto carrier_doppler_rate_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto code_freq_rate_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(num_epoch); try { if (dump_file.is_open()) @@ -1387,28 +1390,6 @@ int32_t dll_pll_veml_tracking::save_matfile() catch (const std::ifstream::failure &e) { std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 1; } @@ -1422,117 +1403,95 @@ int32_t dll_pll_veml_tracking::save_matfile() if (reinterpret_cast(matfp) != nullptr) { size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0); + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz, 0); + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_rate_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips, 0); + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_rate_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] carrier_doppler_rate_hz; - delete[] code_freq_chips; - delete[] code_freq_rate_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; return 0; } @@ -1569,18 +1528,21 @@ void dll_pll_veml_tracking::set_channel(uint32_t channel) } } + void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { gr::thread::scoped_lock l(d_setlock); d_acquisition_gnss_synchro = p_gnss_synchro; } + void dll_pll_veml_tracking::stop_tracking() { gr::thread::scoped_lock l(d_setlock); d_state = 0; } + int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 4005cc88b..d565e8fa1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -113,7 +113,6 @@ private: int32_t *d_preambles_symbols; int32_t d_preamble_length_symbols; - //boost::circular_buffer d_symbol_history; // dll filter buffer boost::circular_buffer d_dll_filt_history; @@ -130,6 +129,7 @@ private: float *d_prompt_data_shift; Cpu_Multicorrelator_Real_Codes multicorrelator_cpu; 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. Implement this functionality inside multicorrelator class @@ -163,6 +163,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; From cf967be25277a45f246b02b02ed5277cf5f0c2dc Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 3 Jul 2019 18:57:26 +0200 Subject: [PATCH 03/24] Consolidating unified dll pll veml tracking to synchronize symbols for GPS L1, L5, Galileo E1, E5 and Beidou B1, B3. Beidou bug fixes in acquisition. Adapting all telemetry decoders to use 1 sample per symbol --- .../adapters/beidou_b1i_pcps_acquisition.cc | 89 ++--- .../adapters/beidou_b1i_pcps_acquisition.h | 20 +- .../adapters/beidou_b3i_pcps_acquisition.cc | 101 +++-- .../adapters/beidou_b3i_pcps_acquisition.h | 4 +- src/algorithms/libs/gnss_sdr_flags.cc | 2 +- .../beidou_b1i_telemetry_decoder_gs.cc | 226 +++++++---- .../beidou_b1i_telemetry_decoder_gs.h | 14 +- .../beidou_b3i_telemetry_decoder_gs.cc | 378 +++++++++--------- .../beidou_b3i_telemetry_decoder_gs.h | 21 +- .../galileo_telemetry_decoder_gs.cc | 8 +- .../gps_l1_ca_telemetry_decoder_gs.cc | 14 +- .../adapters/beidou_b3i_dll_pll_tracking.cc | 48 ++- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 64 ++- src/algorithms/tracking/libs/dll_pll_conf.cc | 1 + src/algorithms/tracking/libs/dll_pll_conf.h | 1 + src/core/system_parameters/Beidou_B1I.h | 1 + src/core/system_parameters/Beidou_B3I.h | 5 +- 17 files changed, 543 insertions(+), 454 deletions(-) diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc index 1c266882b..27dad02cd 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc @@ -51,48 +51,37 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( in_streams_(in_streams), out_streams_(out_streams) { - Acq_Conf acq_parameters = Acq_Conf(); configuration_ = configuration; std::string default_item_type = "gr_complex"; - std::string default_dump_filename = "./data/acquisition.dat"; + std::string default_dump_filename = "./acquisition.mat"; - DLOG(INFO) << "role " << role; + LOG(INFO) << "role " << role; item_type_ = configuration_->property(role + ".item_type", default_item_type); + int64_t fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - acq_parameters.fs_in = fs_in_; + acq_parameters_.fs_in = fs_in_; dump_ = configuration_->property(role + ".dump", false); - acq_parameters.dump = dump_; + acq_parameters_.dump = dump_; + acq_parameters_.dump_channel = configuration_->property(role + ".dump_channel", 0); blocking_ = configuration_->property(role + ".blocking", true); - acq_parameters.blocking = blocking_; - doppler_max_ = configuration_->property(role + ".doppler_max", 5000); + acq_parameters_.blocking = blocking_; + doppler_max_ = configuration->property(role + ".doppler_max", 5000); if (FLAGS_doppler_max != 0) { doppler_max_ = FLAGS_doppler_max; } - acq_parameters.doppler_max = doppler_max_; - sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); - acq_parameters.sampled_ms = sampled_ms_; + acq_parameters_.doppler_max = doppler_max_; bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); - acq_parameters.bit_transition_flag = bit_transition_flag_; + acq_parameters_.bit_transition_flag = bit_transition_flag_; use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions - acq_parameters.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; + acq_parameters_.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; max_dwells_ = configuration_->property(role + ".max_dwells", 1); - acq_parameters.max_dwells = max_dwells_; + acq_parameters_.max_dwells = max_dwells_; dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); - acq_parameters.dump_filename = dump_filename_; - //--- Find number of samples per spreading code ------------------------- - code_length_ = static_cast(std::round(static_cast(fs_in_) / (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS))); - - vector_length_ = code_length_ * sampled_ms_; - - if (bit_transition_flag_) - { - vector_length_ *= 2; - } - - code_ = std::vector>(vector_length_); + acq_parameters_.dump_filename = dump_filename_; + acq_parameters_.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1); if (item_type_ == "cshort") { @@ -102,18 +91,28 @@ BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( { item_size_ = sizeof(gr_complex); } - acq_parameters.it_size = item_size_; - acq_parameters.sampled_ms = sampled_ms_; - acq_parameters.samples_per_ms = code_length_; - acq_parameters.samples_per_code = code_length_; - acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); - acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); - acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); - acquisition_ = pcps_make_acquisition(acq_parameters); - DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; - stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); - DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; + acq_parameters_.ms_per_code = 1; + acq_parameters_.it_size = item_size_; + num_codes_ = acq_parameters_.sampled_ms; + acq_parameters_.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters_.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters_.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters_.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false); + acq_parameters_.use_automatic_resampler = configuration_->property("GNSS-SDR.use_acquisition_resampler", false); + + acq_parameters_.resampled_fs = fs_in_; + //--- Find number of samples per spreading code ------------------------- + code_length_ = static_cast(std::floor(static_cast(fs_in_) / (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS))); + acq_parameters_.samples_per_ms = static_cast(fs_in_) * 0.001; + acq_parameters_.samples_per_chip = static_cast(ceil((1.0 / BEIDOU_B1I_CODE_RATE_HZ) * static_cast(acq_parameters_.fs_in))); + + + acq_parameters_.samples_per_code = acq_parameters_.samples_per_ms * static_cast(BEIDOU_B1I_CODE_PERIOD * 1000.0); + vector_length_ = std::floor(acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms) * (acq_parameters_.bit_transition_flag ? 2 : 1); + code_ = std::vector>(vector_length_); + acquisition_ = pcps_make_acquisition(acq_parameters_); + DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; if (item_type_ == "cbyte") { @@ -208,7 +207,7 @@ void BeidouB1iPcpsAcquisition::set_local_code() beidou_b1i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); gsl::span code_span(code_.data(), vector_length_); - for (unsigned int i = 0; i < sampled_ms_; i++) + for (unsigned int i = 0; i < num_codes_; i++) { std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } @@ -233,15 +232,7 @@ float BeidouB1iPcpsAcquisition::calculate_threshold(float pfa) { //Calculate the threshold uint32_t frequency_bins = 0; - /* - for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_) - { - frequency_bins++; - } - */ - frequency_bins = (2 * doppler_max_ + doppler_step_) / doppler_step_; - DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; uint32_t ncells = vector_length_ * frequency_bins; double exponent = 1 / static_cast(ncells); @@ -266,9 +257,11 @@ void BeidouB1iPcpsAcquisition::connect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { + // Since a byte-based acq implementation is not available, + // we just convert cshorts to gr_complex top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->connect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->connect(float_to_complex_, 0, acquisition_, 0); } else { @@ -289,11 +282,9 @@ void BeidouB1iPcpsAcquisition::disconnect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { - // Since a byte-based acq implementation is not available, - // we just convert cshorts to gr_complex top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->disconnect(float_to_complex_, 0, acquisition_, 0); } else { diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h index 503b6409e..0c2e611e2 100644 --- a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h @@ -163,22 +163,21 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; - gr::blocks::stream_to_vector::sptr stream_to_vector_; + Acq_Conf acq_parameters_; gr::blocks::float_to_complex::sptr float_to_complex_; complex_byte_to_float_x2_sptr cbyte_to_float_x2_; size_t item_size_; std::string item_type_; - uint32_t vector_length_; - uint32_t code_length_; + unsigned int vector_length_; + unsigned int code_length_; bool bit_transition_flag_; bool use_CFAR_algorithm_flag_; - uint32_t channel_; + unsigned int channel_; std::weak_ptr channel_fsm_; float threshold_; - uint32_t doppler_max_; - uint32_t doppler_step_; - uint32_t sampled_ms_; - uint32_t max_dwells_; + unsigned int doppler_max_; + unsigned int doppler_step_; + unsigned int max_dwells_; int64_t fs_in_; bool dump_; bool blocking_; @@ -186,8 +185,9 @@ private: std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; - uint32_t in_streams_; - uint32_t out_streams_; + unsigned int num_codes_; + unsigned int in_streams_; + unsigned int out_streams_; float calculate_threshold(float pfa); }; diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc index 3a51a2a77..bdeede24e 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.cc @@ -49,49 +49,37 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( in_streams_(in_streams), out_streams_(out_streams) { - Acq_Conf acq_parameters = Acq_Conf(); configuration_ = configuration; std::string default_item_type = "gr_complex"; - std::string default_dump_filename = "./data/acquisition.dat"; + std::string default_dump_filename = "./acquisition.mat"; - DLOG(INFO) << "role " << role; + LOG(INFO) << "role " << role; item_type_ = configuration_->property(role + ".item_type", default_item_type); int64_t fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - acq_parameters.fs_in = fs_in_; + acq_parameters_.fs_in = fs_in_; dump_ = configuration_->property(role + ".dump", false); - acq_parameters.dump = dump_; + acq_parameters_.dump = dump_; + acq_parameters_.dump_channel = configuration_->property(role + ".dump_channel", 0); blocking_ = configuration_->property(role + ".blocking", true); - acq_parameters.blocking = blocking_; - doppler_max_ = configuration_->property(role + ".doppler_max", 5000); + acq_parameters_.blocking = blocking_; + doppler_max_ = configuration->property(role + ".doppler_max", 5000); if (FLAGS_doppler_max != 0) { doppler_max_ = FLAGS_doppler_max; } - acq_parameters.doppler_max = doppler_max_; - sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); - acq_parameters.sampled_ms = sampled_ms_; + acq_parameters_.doppler_max = doppler_max_; bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); - acq_parameters.bit_transition_flag = bit_transition_flag_; + acq_parameters_.bit_transition_flag = bit_transition_flag_; use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions - acq_parameters.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; + acq_parameters_.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; max_dwells_ = configuration_->property(role + ".max_dwells", 1); - acq_parameters.max_dwells = max_dwells_; + acq_parameters_.max_dwells = max_dwells_; dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); - acq_parameters.dump_filename = dump_filename_; - //--- Find number of samples per spreading code ------------------------- - code_length_ = static_cast(std::round(static_cast(fs_in_) / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS))); - - vector_length_ = code_length_ * sampled_ms_; - - if (bit_transition_flag_) - { - vector_length_ *= 2; - } - - code_ = std::vector>(vector_length_); + acq_parameters_.dump_filename = dump_filename_; + acq_parameters_.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1); if (item_type_ == "cshort") { @@ -101,18 +89,28 @@ BeidouB3iPcpsAcquisition::BeidouB3iPcpsAcquisition( { item_size_ = sizeof(gr_complex); } - acq_parameters.it_size = item_size_; - acq_parameters.sampled_ms = sampled_ms_; - acq_parameters.samples_per_ms = code_length_; - acq_parameters.samples_per_code = code_length_; - acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); - acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); - acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); - acquisition_ = pcps_make_acquisition(acq_parameters); - DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; - stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); - DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; + acq_parameters_.ms_per_code = 1; + acq_parameters_.it_size = item_size_; + num_codes_ = acq_parameters_.sampled_ms; + acq_parameters_.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters_.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters_.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acq_parameters_.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false); + acq_parameters_.use_automatic_resampler = configuration_->property("GNSS-SDR.use_acquisition_resampler", false); + + acq_parameters_.resampled_fs = fs_in_; + //--- Find number of samples per spreading code ------------------------- + code_length_ = static_cast(std::floor(static_cast(fs_in_) / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS))); + acq_parameters_.samples_per_ms = static_cast(fs_in_) * 0.001; + acq_parameters_.samples_per_chip = static_cast(ceil((1.0 / BEIDOU_B3I_CODE_RATE_HZ) * static_cast(acq_parameters_.fs_in))); + + + acq_parameters_.samples_per_code = acq_parameters_.samples_per_ms * static_cast(BEIDOU_B3I_CODE_PERIOD * 1000.0); + vector_length_ = std::floor(acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms) * (acq_parameters_.bit_transition_flag ? 2 : 1); + code_ = std::vector>(vector_length_); + acquisition_ = pcps_make_acquisition(acq_parameters_); + DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; if (item_type_ == "cbyte") { @@ -146,8 +144,12 @@ void BeidouB3iPcpsAcquisition::stop_acquisition() void BeidouB3iPcpsAcquisition::set_threshold(float threshold) { - float pfa = configuration_->property(role_ + ".pfa", 0.0); + float pfa = configuration_->property(role_ + std::to_string(channel_) + ".pfa", 0.0); + if (pfa == 0.0) + { + pfa = configuration_->property(role_ + ".pfa", 0.0); + } if (pfa == 0.0) { threshold_ = threshold; @@ -196,7 +198,6 @@ signed int BeidouB3iPcpsAcquisition::mag() void BeidouB3iPcpsAcquisition::init() { acquisition_->init(); - set_local_code(); } @@ -207,7 +208,7 @@ void BeidouB3iPcpsAcquisition::set_local_code() beidou_b3i_code_gen_complex_sampled(gsl::span>(code, code_length_), gnss_synchro_->PRN, fs_in_, 0); gsl::span code_span(code_.data(), vector_length_); - for (unsigned int i = 0; i < sampled_ms_; i++) + for (unsigned int i = 0; i < num_codes_; i++) { std::copy_n(code.get(), code_length_, code_span.subspan(i * code_length_, code_length_).data()); } @@ -232,20 +233,12 @@ float BeidouB3iPcpsAcquisition::calculate_threshold(float pfa) { //Calculate the threshold unsigned int frequency_bins = 0; - /* - for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_) - { - frequency_bins++; - } - */ - frequency_bins = (2 * doppler_max_ + doppler_step_) / doppler_step_; - DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; unsigned int ncells = vector_length_ * frequency_bins; - double exponent = 1 / static_cast(ncells); + double exponent = 1.0 / static_cast(ncells); double val = pow(1.0 - pfa, exponent); - auto lambda = static_cast(vector_length_); + auto lambda = double(vector_length_); boost::math::exponential_distribution mydist(lambda); auto threshold = static_cast(quantile(mydist, val)); @@ -265,9 +258,11 @@ void BeidouB3iPcpsAcquisition::connect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { + // Since a byte-based acq implementation is not available, + // we just convert cshorts to gr_complex top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->connect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->connect(float_to_complex_, 0, acquisition_, 0); } else { @@ -288,11 +283,9 @@ void BeidouB3iPcpsAcquisition::disconnect(gr::top_block_sptr top_block) } else if (item_type_ == "cbyte") { - // Since a byte-based acq implementation is not available, - // we just convert cshorts to gr_complex top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0); top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1); - top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->disconnect(float_to_complex_, 0, acquisition_, 0); } else { @@ -307,7 +300,7 @@ gr::basic_block_sptr BeidouB3iPcpsAcquisition::get_left_block() { return acquisition_; } - else if (item_type_ == "cshort") + if (item_type_ == "cshort") { return acquisition_; } diff --git a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h index db2c6fbaa..c57f28127 100644 --- a/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h +++ b/src/algorithms/acquisition/adapters/beidou_b3i_pcps_acquisition.h @@ -162,7 +162,7 @@ public: private: ConfigurationInterface* configuration_; pcps_acquisition_sptr acquisition_; - gr::blocks::stream_to_vector::sptr stream_to_vector_; + Acq_Conf acq_parameters_; gr::blocks::float_to_complex::sptr float_to_complex_; complex_byte_to_float_x2_sptr cbyte_to_float_x2_; size_t item_size_; @@ -176,7 +176,6 @@ private: float threshold_; unsigned int doppler_max_; unsigned int doppler_step_; - unsigned int sampled_ms_; unsigned int max_dwells_; int64_t fs_in_; bool dump_; @@ -185,6 +184,7 @@ private: std::vector> code_; Gnss_Synchro* gnss_synchro_; std::string role_; + unsigned int num_codes_; unsigned int in_streams_; unsigned int out_streams_; float calculate_threshold(float pfa); diff --git a/src/algorithms/libs/gnss_sdr_flags.cc b/src/algorithms/libs/gnss_sdr_flags.cc index 871776a52..6e021b73c 100644 --- a/src/algorithms/libs/gnss_sdr_flags.cc +++ b/src/algorithms/libs/gnss_sdr_flags.cc @@ -67,7 +67,7 @@ DEFINE_int32(cn0_samples, 20, "Number of correlator outputs used for CN0 estimat DEFINE_int32(cn0_min, 25, "Minimum valid CN0 (in dB-Hz)."); -DEFINE_int32(max_carrier_lock_fail, 10000, "Maximum number of carrier lock failures before dropping a satellite."); +DEFINE_int32(max_carrier_lock_fail, 5000, "Maximum number of carrier lock failures before dropping a satellite."); DEFINE_int32(max_lock_fail, 50, "Maximum number of code lock failures before dropping a satellite."); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index 0ca5f5e74..0f56d1614 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -29,10 +29,10 @@ * * ------------------------------------------------------------------------- */ - - #include "beidou_b1i_telemetry_decoder_gs.h" #include "Beidou_B1I.h" +#include "Beidou_DNAV.h" +#include "beidou_dnav_almanac.h" #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" @@ -56,7 +56,6 @@ beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) return beidou_b1i_telemetry_decoder_gs_sptr(new beidou_b1i_telemetry_decoder_gs(satellite, dump)); } - beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( const Gnss_Satellite &satellite, bool dump) : gr::block("beidou_b1i_telemetry_decoder_gs", @@ -72,6 +71,7 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); LOG(INFO) << "Initializing BeiDou B1I Telemetry Decoding for satellite " << this->d_satellite; + d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); @@ -94,12 +94,15 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; d_symbol_history.set_capacity(d_required_symbols); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; d_preamble_index = 0; d_flag_frame_sync = false; - d_TOW_at_current_symbol_ms = 0; + d_TOW_at_current_symbol_ms = 0U; d_TOW_at_Preamble_ms = 0U; Flag_valid_word = false; d_CRC_error_counter = 0; @@ -150,7 +153,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, in err = errind[reg[0] + reg[1] * 2 + reg[2] * 4 + reg[3] * 8]; - if (err > 0) + if (err > 0 and err < 16) { decbits[err - 1] *= -1; } @@ -230,11 +233,13 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // 3. Check operation executed correctly if (d_nav.flag_crc_test == true) { - LOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel + << " from satellite " << d_satellite; } else { - LOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel + << " from satellite " << d_satellite; } // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) @@ -296,6 +301,36 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + // Setting samples of preamble code + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + d_preamble_samples[i] = 1; + } + else + { + d_preamble_samples[i] = -1; + } + } + + d_symbol_duration_ms = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); + } + else + { + // Clear values from previous declaration + volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_subframe_symbols); + //back to normal satellites + d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + // Setting samples of preamble code for (int32_t i = 0; i < d_symbols_per_preamble; i++) { @@ -342,6 +377,15 @@ void beidou_b1i_telemetry_decoder_gs::set_channel(int32_t channel) } } +void beidou_b1i_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_TOW_at_current_symbol_ms = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; + DLOG(INFO) << "Beidou B1I Telemetry decoder reset for satellite " << d_satellite; + return; +} int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -358,10 +402,8 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); - d_flag_preamble = false; - //std::cout << "size: " << d_symbol_history.size() << " in " << current_symbol.Prompt_I << std::endl; if (d_symbol_history.size() >= d_required_symbols) { //******* preamble correlation ******** @@ -375,14 +417,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { corr_value += d_preamble_samples[i]; } - //std::cout << "corr: " << corr_value << ","; } - //std::cout << " final corr: " << corr_value << std::endl; - } - - if (abs(corr_value) >= d_samples_per_preamble) - { - std::cout << " preamble corr: " << corr_value << std::endl; } //******* frame sync ****************** if (d_stat == 0) // no preamble information @@ -391,7 +426,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - LOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; + DLOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -405,9 +440,54 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; + DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; d_preamble_index = d_sample_counter; //record the preamble sample stamp + + d_stat = 2; + + // ******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) //normal PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = d_symbol_history.at(i); + } + } + else // 180 deg. inverted carrier phase PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = -d_symbol_history.at(i); + } + } + + // call the decoder + decode_subframe(d_subframe_symbols); + + if (d_nav.flag_crc_test == true) + { + d_CRC_error_counter = 0; + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + if (!d_flag_frame_sync) + { + d_flag_frame_sync = true; + DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " << this->d_satellite; + } + } + else + { + d_CRC_error_counter++; + d_preamble_index = d_sample_counter; // record the preamble sample stamp + if (d_CRC_error_counter > CRC_ERROR_LIMIT) + { + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; + d_flag_frame_sync = false; + d_stat = 0; + flag_SOW_set = false; + } + } } else { @@ -428,36 +508,17 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = d_symbol_history.at(i); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = d_symbol_history.at(i); - } + d_subframe_symbols[i] = d_symbol_history.at(i); } } else // 180 deg. inverted carrier phase PLL lock { for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = -d_symbol_history.at(i); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] = -d_symbol_history.at(i); - } + d_subframe_symbols[i] = -d_symbol_history.at(i); } } - // call the decoder decode_subframe(d_subframe_symbols); @@ -478,7 +539,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -493,52 +554,67 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * BEIDOU_B1I_CODE_PERIOD_MS); + //check TOW update consistency + uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + //compute new TOW + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; flag_SOW_set = true; d_nav.flag_new_SOW_available = false; - } - else // if there is not a new preamble, we define the TOW of the current symbol - { - d_TOW_at_current_symbol_ms += static_cast(BEIDOU_B1I_CODE_PERIOD_MS); - } + if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) + { + LOG(INFO) << "Warning: BEIDOU B1I TOW update in ch " << d_channel + << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; - if (d_flag_frame_sync == true and flag_SOW_set == true) - { - current_symbol.Flag_valid_word = true; + d_TOW_at_current_symbol_ms = 0; + d_flag_valid_word = false; + } + else + { + d_last_valid_preamble = d_sample_counter; + d_flag_valid_word = true; + } } else { - current_symbol.Flag_valid_word = false; + if (d_flag_valid_word) + { + d_TOW_at_current_symbol_ms += d_symbol_duration_ms; + if (current_symbol.Flag_valid_symbol_output == false) + { + d_flag_valid_word = false; + } + } } - current_symbol.PRN = this->d_satellite.get_PRN(); - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - - if (d_dump == true) + if (d_flag_valid_word == true) { - // MULTIPLEXED FILE RECORDING - Record results to file - try + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + current_symbol.Flag_valid_word = d_flag_valid_word; + + if (d_dump == true) { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = d_nav.d_SOW; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = static_cast(d_required_symbols); - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); + } } + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + return 1; } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - - return 1; + return 0; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h index 9180a40b8..9485008fd 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h @@ -62,10 +62,8 @@ public: ~beidou_b1i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - inline void reset() - { - return; - } + void reset(); + /*! * \brief This is where all signal processing takes place */ @@ -105,12 +103,16 @@ private: //!< Navigation Message variable Beidou_Dnav_Navigation_Message d_nav; - //!< Values to populate gnss synchronization structure + // Values to populate gnss synchronization structure + uint32_t d_symbol_duration_ms; uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; + uint64_t d_last_valid_preamble; + bool d_flag_valid_word; + bool d_sent_tlm_failed_msg; bool Flag_valid_word; - //!< Satellite Information and logging capacity + // Satellite Information and logging capacity Gnss_Satellite d_satellite; int32_t d_channel; bool d_dump; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc index 9f1396b8b..2e6d6b6af 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc @@ -52,8 +52,7 @@ beidou_b3i_telemetry_decoder_gs_sptr beidou_b3i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) { - return beidou_b3i_telemetry_decoder_gs_sptr( - new beidou_b3i_telemetry_decoder_gs(satellite, dump)); + return beidou_b3i_telemetry_decoder_gs_sptr(new beidou_b3i_telemetry_decoder_gs(satellite, dump)); } @@ -67,74 +66,37 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); - // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " - << this->d_satellite; + LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " << this->d_satellite; - d_samples_per_symbol = - (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / - BEIDOU_D1NAV_SYMBOL_RATE_SPS; + d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = - BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = static_cast( - volk_gnsssdr_malloc(BEIDOU_B3I_SECONDARY_CODE_LENGTH * sizeof(int32_t), - volk_gnsssdr_get_alignment())); - d_preamble_samples = static_cast(volk_gnsssdr_malloc( - d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - - // Setting samples of secondary code - for (int32_t i = 0; i < BEIDOU_B3I_SECONDARY_CODE_LENGTH; i++) - { - if (BEIDOU_B3I_SECONDARY_CODE.at(i) == '1') - { - d_secondary_code_symbols[i] = 1; - } - else - { - d_secondary_code_symbols[i] = -1; - } - } + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - int32_t m = 0; if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast( - volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), - volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + - d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; @@ -153,7 +115,6 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -164,8 +125,7 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() } catch (const std::exception &ex) { - LOG(WARNING) << "Exception in destructor closing the dump file " - << ex.what(); + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } } @@ -194,7 +154,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, err = errind[reg[0] + reg[1] * 2 + reg[2] * 4 + reg[3] * 8]; - if (err > 0) + if (err > 0 and err < 16) { decbits[err - 1] *= -1; } @@ -202,7 +162,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, void beidou_b3i_telemetry_decoder_gs::decode_word( - int32_t word_counter, const double *enc_word_symbols, + int32_t word_counter, + const float *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -211,8 +172,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t j = 0; j < 30; j++) { - dec_word_symbols[j] = - static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); + dec_word_symbols[j] = static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); } } else @@ -221,8 +181,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t c = 0; c < 15; c++) { - bitsbch[r * 15 + c] = - static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); + bitsbch[r * 15 + c] = static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); } } @@ -244,7 +203,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( } -void beidou_b3i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) +void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -275,13 +234,13 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) // 3. Check operation executed correctly if (d_nav.flag_crc_test == true) { - LOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel + << " from satellite " << d_satellite; } else { - LOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel + << " from satellite " << d_satellite; } // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) @@ -353,50 +312,62 @@ void beidou_b3i_telemetry_decoder_gs::set_satellite( { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); - d_samples_per_symbol = - (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / - BEIDOU_D2NAV_SYMBOL_RATE_SPS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = - BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = nullptr; - d_preamble_samples = static_cast( - volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), - volk_gnsssdr_get_alignment())); - d_preamble_period_samples = - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), + volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // Setting samples of preamble code - int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = 1; - n++; - } + d_preamble_samples[i] = 1; } else { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -1; - n++; - } + d_preamble_samples[i] = -1; + } + } + d_symbol_duration_ms = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; + d_subframe_symbols = static_cast(volk_gnsssdr_malloc( + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), + volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); + } + else + { + // Clear values from previous declaration + volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_subframe_symbols); + //back to normal satellites + d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + + // Setting samples of preamble code + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + d_preamble_samples[i] = 1; + } + else + { + d_preamble_samples[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc( - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), - volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + - d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols + 1); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols); } } @@ -431,6 +402,15 @@ void beidou_b3i_telemetry_decoder_gs::set_channel(int32_t channel) } } +void beidou_b3i_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_TOW_at_current_symbol_ms = 0; + d_sent_tlm_failed_msg = false; + d_flag_valid_word = false; + DLOG(INFO) << "Beidou B3I Telemetry decoder reset for satellite " << d_satellite; + return; +} int beidou_b3i_telemetry_decoder_gs::general_work( int noutput_items __attribute__((unused)), @@ -451,10 +431,9 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); - d_flag_preamble = false; - if (d_symbol_history.size() > d_required_symbols) + if (d_symbol_history.size() >= d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -469,7 +448,6 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } - //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -477,8 +455,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - LOG(INFO) << "Preamble detection for BEIDOU B3I SAT " - << this->d_satellite; + DLOG(INFO) << "Preamble detection for BEIDOU B3I SAT " << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -492,10 +469,55 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " - << this->d_satellite; + DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " + << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp d_stat = 2; + + // ******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) //normal PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = d_symbol_history.at(i); + } + } + else // 180 deg. inverted carrier phase PLL lock + { + for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) + { + d_subframe_symbols[i] = -d_symbol_history.at(i); + } + } + + // call the decoder + decode_subframe(d_subframe_symbols); + + if (d_nav.flag_crc_test == true) + { + d_CRC_error_counter = 0; + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + if (!d_flag_frame_sync) + { + d_flag_frame_sync = true; + DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " + << this->d_satellite; + } + } + else + { + d_CRC_error_counter++; + d_preamble_index = d_sample_counter; // record the preamble sample stamp + if (d_CRC_error_counter > CRC_ERROR_LIMIT) + { + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " + << this->d_satellite; + d_flag_frame_sync = false; + d_stat = 0; + flag_SOW_set = false; + } + } } else { @@ -510,62 +532,21 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } else if (d_stat == 2) // preamble acquired { - if (d_sample_counter == - d_preamble_index + static_cast(d_preamble_period_samples)) + if (d_sample_counter == d_preamble_index + static_cast(d_preamble_period_samples)) { - //******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) // normal PLL lock + // ******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) //normal PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) - { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += - d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += - static_cast(d_secondary_code_symbols[k]) * - d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } - } + d_subframe_symbols[i] = d_symbol_history.at(i); } } else // 180 deg. inverted carrier phase PLL lock { - int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = 0; - // integrate samples into symbols - for (uint32_t m = 0; m < d_samples_per_symbol; m++) - { - if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= - d_symbol_history.at(i * d_samples_per_symbol + m); - } - else - { - // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= - static_cast(d_secondary_code_symbols[k]) * - d_symbol_history.at(i * d_samples_per_symbol + m); - k++; - k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; - } - } + d_subframe_symbols[i] = -d_symbol_history.at(i); } } @@ -575,10 +556,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (d_nav.flag_crc_test == true) { d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to - // false every work()) - d_preamble_index = - d_sample_counter; // record the preamble sample stamp (t_P) + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) if (!d_flag_frame_sync) { d_flag_frame_sync = true; @@ -592,8 +571,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " - << this->d_satellite; + DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " + << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -601,7 +580,6 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } - // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) @@ -609,55 +587,67 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - d_TOW_at_current_symbol_ms = - d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * - BEIDOU_B3I_CODE_PERIOD_MS); + //check TOW update consistency + uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + //compute new TOW + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; flag_SOW_set = true; d_nav.flag_new_SOW_available = false; - } - else // if there is not a new preamble, we define the TOW of the current - // symbol - { - d_TOW_at_current_symbol_ms += - static_cast(BEIDOU_B3I_CODE_PERIOD_MS); - } - if (d_flag_frame_sync == true and flag_SOW_set == true) - { - current_symbol.Flag_valid_word = true; + if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) + { + LOG(INFO) << "Warning: BEIDOU B3I TOW update in ch " << d_channel + << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; + + d_TOW_at_current_symbol_ms = 0; + d_flag_valid_word = false; + } + else + { + d_last_valid_preamble = d_sample_counter; + d_flag_valid_word = true; + } } else { - current_symbol.Flag_valid_word = false; + if (d_flag_valid_word) + { + d_TOW_at_current_symbol_ms += d_symbol_duration_ms; + if (current_symbol.Flag_valid_symbol_output == false) + { + d_flag_valid_word = false; + } + } } - current_symbol.PRN = this->d_satellite.get_PRN(); - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - - if (d_dump == true) + if (d_flag_valid_word == true) { - // MULTIPLEXED FILE RECORDING - Record results to file - try + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + current_symbol.Flag_valid_word = d_flag_valid_word; + + if (d_dump == true) { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = d_nav.d_SOW; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = static_cast(d_required_symbols); - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); + } } + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + return 1; } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - - return 1; + return 0; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h index 54bac3adb..d08f8388e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h @@ -60,10 +60,8 @@ public: ~beidou_b3i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - inline void reset() - { - return; - } + void reset(); + /*! * \brief This is where all signal processing takes place */ @@ -77,27 +75,24 @@ private: bool dump); beidou_b3i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(double *symbols); - void decode_word(int32_t word_counter, const double *enc_word_symbols, + void decode_subframe(float *symbols); + void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols); void decode_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; - int32_t *d_secondary_code_symbols; - uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - double *d_subframe_symbols; + float *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data boost::circular_buffer d_symbol_history; // Variables for internal functionality - uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) - // indicating number of samples processed + uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed uint64_t d_preamble_index; // Index of sample number where preamble was found uint32_t d_stat; // Status of decoder bool d_flag_frame_sync; // Indicate when a frame sync is achieved @@ -109,8 +104,12 @@ private: Beidou_Dnav_Navigation_Message d_nav; // Values to populate gnss synchronization structure + uint32_t d_symbol_duration_ms; uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; + uint64_t d_last_valid_preamble; + bool d_flag_valid_word; + bool d_sent_tlm_failed_msg; bool Flag_valid_word; // Satellite Information and logging capacity diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 4bfcc06d3..254993c49 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -112,7 +112,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; - d_max_symbols_without_valid_frame = GALILEO_FNAV_SYMBOLS_PER_PAGE * 10; //rise alarm 100 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_FNAV_SYMBOLS_PER_PAGE * 5; //rise alarm 100 seconds without valid tlm break; } default: @@ -623,12 +623,11 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( return -1; break; } - + d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) if (d_inav_nav.flag_CRC_test == true or d_fnav_nav.flag_CRC_test == true) { d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) d_last_valid_preamble = d_sample_counter; if (!d_flag_frame_sync) { @@ -639,7 +638,6 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( else { d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 612045e31..3e406a544 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -76,33 +76,26 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( DLOG(INFO) << "Initializing GPS L1 TELEMETRY DECODER"; d_bits_per_preamble = GPS_CA_PREAMBLE_LENGTH_BITS; - //d_samples_per_preamble = d_bits_per_preamble * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; d_samples_per_preamble = d_bits_per_preamble; - d_preamble_period_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_preamble_period_symbols = GPS_SUBFRAME_BITS; // set the preamble - d_required_symbols = GPS_SUBFRAME_BITS; // * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + d_required_symbols = GPS_SUBFRAME_BITS; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_frame_length_symbols = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; - d_max_symbols_without_valid_frame = d_required_symbols * 10; // rise alarm 1 minute without valid tlm + d_max_symbols_without_valid_frame = d_required_symbols * 20; // rise alarm 120 segs without valid tlm int32_t n = 0; for (int32_t i = 0; i < d_bits_per_preamble; i++) { if (GPS_CA_PREAMBLE.at(i) == '1') { - // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - // { d_preamble_samples[n] = 1; n++; - // } } else { - // for (uint32_t j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++) - // { d_preamble_samples[n] = -1; n++; - // } } } d_sample_counter = 0ULL; @@ -462,7 +455,6 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ else { d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > 2) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; diff --git a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc index fb998ab81..c311d1ef2 100644 --- a/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/beidou_b3i_dll_pll_tracking.cc @@ -58,22 +58,39 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( trk_param.fs_in = fs_in; bool dump = configuration->property(role + ".dump", false); trk_param.dump = dump; + std::string default_dump_filename = "./track_ch"; + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + trk_param.dump_filename = dump_filename; + bool dump_mat = configuration->property(role + ".dump_mat", true); + trk_param.dump_mat = dump_mat; + trk_param.high_dyn = configuration->property(role + ".high_dyn", false); + if (configuration->property(role + ".smoother_length", 10) < 1) + { + trk_param.smoother_length = 1; + std::cout << TEXT_RED << "WARNING: BEIDOU B3I. smoother_length must be bigger than 0. It has been set to 1" << TEXT_RESET << std::endl; + } + else + { + trk_param.smoother_length = configuration->property(role + ".smoother_length", 10); + } float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) { pll_bw_hz = static_cast(FLAGS_pll_bw_hz); } trk_param.pll_bw_hz = pll_bw_hz; - float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); - trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; - float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); - trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); if (FLAGS_dll_bw_hz != 0.0) { dll_bw_hz = static_cast(FLAGS_dll_bw_hz); } trk_param.dll_bw_hz = dll_bw_hz; + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + trk_param.pll_bw_narrow_hz = pll_bw_narrow_hz; + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + trk_param.dll_bw_narrow_hz = dll_bw_narrow_hz; + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + trk_param.early_late_space_chips = early_late_space_chips; int dll_filter_order = configuration->property(role + ".dll_filter_order", 2); if (dll_filter_order < 1) @@ -116,16 +133,12 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( trk_param.fll_bw_hz = fll_bw_hz; trk_param.pull_in_time_s = configuration->property(role + ".pull_in_time_s", trk_param.pull_in_time_s); - float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); - trk_param.early_late_space_chips = early_late_space_chips; - float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); - trk_param.early_late_space_narrow_chips = early_late_space_narrow_chips; - std::string default_dump_filename = "./track_ch"; - std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); - trk_param.dump_filename = dump_filename; - int vector_length = std::round(fs_in / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS)); + int vector_length = std::round(static_cast(fs_in) / (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS)); trk_param.vector_length = vector_length; int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); + trk_param.early_late_space_narrow_chips = early_late_space_narrow_chips; + bool track_pilot = configuration->property(role + ".track_pilot", false); if (symbols_extended_correlator < 1) { symbols_extended_correlator = 1; @@ -137,18 +150,9 @@ BeidouB3iDllPllTracking::BeidouB3iDllPllTracking( std::cout << TEXT_RED << "WARNING: BEIDOU B3I. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } trk_param.extend_correlation_symbols = symbols_extended_correlator; - bool track_pilot = configuration->property(role + ".track_pilot", false); - if (track_pilot) - { - std::cout << TEXT_RED << "WARNING: BEIDOU B3I does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; - } - if ((symbols_extended_correlator > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) - { - std::cout << TEXT_RED << "WARNING: BEIDOU B3I. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; - } + trk_param.track_pilot = track_pilot; trk_param.very_early_late_space_chips = 0.0; trk_param.very_early_late_space_narrow_chips = 0.0; - trk_param.track_pilot = false; trk_param.system = 'C'; char sig_[3] = "B3"; std::memcpy(trk_param.signal, sig_, 3); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 1a03f987d..a50b02310 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -102,7 +102,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl this->set_msg_handler(pmt::mp("telemetry_to_trk"), boost::bind(&dll_pll_veml_tracking::msg_handler_telemetry_to_trk, this, _1)); // initialize internal vars - d_dll_filt_history.set_capacity(2000); + d_dll_filt_history.set_capacity(1000); d_veml = false; d_cloop = true; d_pull_in_transitory = true; @@ -546,7 +546,7 @@ void dll_pll_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 = 100000; //force loss-of-lock condition + d_carrier_lock_fail_counter = 200000; //force loss-of-lock condition break; } default: @@ -645,18 +645,32 @@ void dll_pll_veml_tracking::start_tracking() else if (systemName == "Beidou" and signal_type == "B1") { beidou_b1i_code_gen_float(gsl::span(d_tracking_code, 2 * d_code_length_chips), d_acquisition_gnss_synchro->PRN, 0); - // Update secondary code settings for geo satellites + // GEO Satellites use different secondary code if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) { - d_symbols_per_bit = 2; + d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - // preamble bits to sampled symbols // set the preamble in the secondary code acquisition d_secondary_code_length = static_cast(BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS); d_secondary_code_string = const_cast(&BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR); + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = true; + trk_parameters.track_pilot = false; + // 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_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -667,15 +681,29 @@ void dll_pll_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 = 2; + d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_secondary = false; trk_parameters.track_pilot = false; - // preamble bits to sampled symbols // set the preamble in the secondary code acquisition d_secondary_code_length = static_cast(BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS); d_secondary_code_string = const_cast(&BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR); + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = true; + trk_parameters.track_pilot = false; + // 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_Prompt_circular_buffer.set_capacity(d_secondary_code_length); } } @@ -976,16 +1004,18 @@ void dll_pll_veml_tracking::run_dll_pll() if (d_pull_in_transitory == false and d_corrected_doppler == false) { d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + if (d_dll_filt_history.full()) { - float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0) / static_cast(d_dll_filt_history.capacity()); - if (fabs(avg_code_error_chips_s) > 1.0) + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 0.025) { float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); d_corrected_doppler = true; } + d_dll_filt_history.clear(); } } } @@ -1158,7 +1188,7 @@ void dll_pll_veml_tracking::save_correlation_results() d_P_data_accu -= *d_Prompt; } } - // std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << std::endl; + d_current_data_symbol++; // data secondary code roll-up d_current_data_symbol %= d_data_secondary_code_length; @@ -1172,6 +1202,7 @@ void dll_pll_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; } d_current_data_symbol++; d_current_data_symbol %= d_symbols_per_bit; @@ -1614,6 +1645,13 @@ int dll_pll_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 + 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 + 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; + } // Check lock status if (!cn0_and_tracking_lock_status(d_code_period)) { @@ -1730,6 +1768,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) update_tracking_vars(); if (d_current_data_symbol == 0) { + log_data(); // ########### Output the tracking results to Telemetry block ########## // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -1769,6 +1808,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) update_tracking_vars(); if (d_current_data_symbol == 0) { + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); // ########### Output the tracking results to Telemetry block ########## // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -1782,8 +1823,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) current_synchro_data.Flag_valid_symbol_output = true; d_P_data_accu = gr_complex(0.0, 0.0); } - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); + // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index 8bb114e96..f31bccc4e 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -47,6 +47,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() enable_fll_pull_in = false; enable_fll_steady_state = false; pull_in_time_s = 10; + bit_synchronization_time_limit_s = pull_in_time_s + 60; fll_filter_order = 1; pll_filter_order = 3; dll_filter_order = 2; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index 94ac5e270..d84e8f204 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -45,6 +45,7 @@ public: bool enable_fll_pull_in; bool enable_fll_steady_state; unsigned int pull_in_time_s; + unsigned int bit_synchronization_time_limit_s; int pll_filter_order; int dll_filter_order; diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index 48dd4ea78..bdc5ff40f 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -67,6 +67,7 @@ const double BEIDOU_B1I_PREAMBLE_DURATION_S = 0.220; const int BEIDOU_B1I_PREAMBLE_DURATION_MS = 220; const int BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; +const int BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT = 2; const int BEIDOU_B1I_TELEMETRY_SYMBOL_PERIOD_MS = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; const int BEIDOU_B1I_TELEMETRY_RATE_SYMBOLS_SECOND = BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND * BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //************!< NAV message bit rate [symbols/s] const int BEIDOU_WORD_LENGTH = 4; //**************!< CRC + BEIDOU WORD (-2 -1 0 ... 29) Bits = 4 bytes diff --git a/src/core/system_parameters/Beidou_B3I.h b/src/core/system_parameters/Beidou_B3I.h index bd58aeae0..3eceab185 100644 --- a/src/core/system_parameters/Beidou_B3I.h +++ b/src/core/system_parameters/Beidou_B3I.h @@ -52,8 +52,9 @@ const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_BITS = 11; const uint32_t BEIDOU_B3I_PREAMBLE_LENGTH_SYMBOLS = 220; // ************** const double BEIDOU_B3I_PREAMBLE_DURATION_S = 0.220; const int32_t BEIDOU_B3I_PREAMBLE_DURATION_MS = 220; -const int32_t BEIDOU_B3I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] -const int32_t BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT = 20; // ************* +const int32_t BEIDOU_B3I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] +const int32_t BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT = 20; +const int32_t BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT = 2; // ************* const int32_t BEIDOU_B3I_TELEMETRY_RATE_SYMBOLS_SECOND = BEIDOU_B3I_TELEMETRY_RATE_BITS_SECOND * BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //************!< NAV message bit rate [symbols/s] #endif /* GNSS_SDR_BEIDOU_B3I_H_ */ From 7877754cb28be88e2534634e11a229ee0d2c6b8d Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 4 Jul 2019 12:16:37 +0200 Subject: [PATCH 04/24] Debug gnuradio buffer latency --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc | 4 ++++ .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 4 +++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 3e406a544..90a209fa5 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -62,6 +62,10 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("gps_navigation_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + this->set_max_output_buffer(128); + LOG(INFO) << "current tlm output buffer set to " << this->max_output_buffer(0); + std::cout << "current tlm output buffer set to " << this->max_output_buffer(0) << "\n"; + // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index a50b02310..6ec7ccf02 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -92,6 +92,9 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_) dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + this->set_max_output_buffer(128); + LOG(INFO) << "current tracking output buffer set to " << this->max_output_buffer(0); + std::cout << "current tracking output buffer set to " << this->max_output_buffer(0) << "\n"; trk_parameters = conf_; // Telemetry bit synchronization message port input this->message_port_register_out(pmt::mp("events")); @@ -566,7 +569,6 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) void dll_pll_veml_tracking::start_tracking() { gr::thread::scoped_lock l(d_setlock); - // correct the code phase according to the delay between acq and trk d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; From 951e179bb85f501174c5e930a9895191be032d04 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 4 Jul 2019 12:54:45 +0200 Subject: [PATCH 05/24] Debug gnuradio buffer latency test 2 --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc | 5 ++--- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 4 +--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 90a209fa5..2fd99ae09 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -62,9 +62,8 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("gps_navigation_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - this->set_max_output_buffer(128); - LOG(INFO) << "current tlm output buffer set to " << this->max_output_buffer(0); - std::cout << "current tlm output buffer set to " << this->max_output_buffer(0) << "\n"; + //this->set_max_output_buffer(128); + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 6ec7ccf02..7450782b4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -92,9 +92,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_) dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - this->set_max_output_buffer(128); - LOG(INFO) << "current tracking output buffer set to " << this->max_output_buffer(0); - std::cout << "current tracking output buffer set to " << this->max_output_buffer(0) << "\n"; + this->set_max_noutput_items(1); trk_parameters = conf_; // Telemetry bit synchronization message port input this->message_port_register_out(pmt::mp("events")); From 29f13e5e3128dcc67112894d76ddbc807ac791ca Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 4 Jul 2019 17:19:33 +0200 Subject: [PATCH 06/24] Set noutput items limit in telemetry decoders and some minor changes --- .../beidou_b1i_telemetry_decoder_gs.cc | 11 +++++++---- .../beidou_b3i_telemetry_decoder_gs.cc | 19 +++++++++++-------- .../galileo_telemetry_decoder_gs.cc | 2 ++ .../glonass_l1_ca_telemetry_decoder_gs.cc | 2 ++ .../glonass_l2_ca_telemetry_decoder_gs.cc | 2 ++ .../gps_l1_ca_telemetry_decoder_gs.cc | 2 +- .../gps_l2c_telemetry_decoder_gs.cc | 2 ++ .../gps_l5_telemetry_decoder_gs.cc | 2 ++ .../sbas_l1_telemetry_decoder_gs.cc | 2 ++ .../gnuradio_blocks/dll_pll_veml_tracking.cc | 1 + 10 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index 0f56d1614..b597504b3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -36,6 +36,7 @@ #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" +#include "display.h" #include "gnss_synchro.h" #include #include @@ -62,6 +63,8 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block @@ -248,7 +251,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B1I DNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_utc_model() == true) { @@ -256,7 +259,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_utc_model()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B1I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_iono() == true) { @@ -264,7 +267,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_iono()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " << d_satellite << std::endl; + std::cout << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_almanac() == true) { @@ -272,7 +275,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // std::shared_ptr tmp_obj = std::make_shared(d_nav.get_almanac(slot_nbr)); // this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Almanac have been received in channel" << d_channel << " from satellite " << d_satellite << std::endl; - std::cout << "New BEIDOU B1I DNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << std::endl; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc index 2e6d6b6af..419aff764 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc @@ -35,6 +35,7 @@ #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" +#include "display.h" #include "gnss_synchro.h" #include #include @@ -62,6 +63,8 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block @@ -251,8 +254,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B3I DNAV message received in channel " << d_channel - << ": ephemeris from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV message received in channel " << d_channel + << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_utc_model() == true) { @@ -262,9 +265,9 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B3I DNAV utc model message received in channel " + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " - << d_satellite << std::endl; + << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_iono() == true) { @@ -274,9 +277,9 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B3I DNAV Iono message received in channel " + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " - << d_satellite << std::endl; + << d_satellite << TEXT_RESET << std::endl; } if (d_nav.have_new_almanac() == true) { @@ -287,8 +290,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Almanac have been received in channel" << d_channel << " from satellite " << d_satellite << std::endl; - std::cout << "New BEIDOU B3I DNAV almanac received in channel " << d_channel - << " from satellite " << d_satellite << std::endl; + std::cout << TEXT_YELLOW << "New BEIDOU B3I DNAV almanac received in channel " << d_channel + << " from satellite " << d_satellite << TEXT_RESET << std::endl; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 254993c49..965100242 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -66,6 +66,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( bool dump) : gr::block("galileo_telemetry_decoder_gs", 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index 7629c2810..3f6fdabcb 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -60,6 +60,8 @@ glonass_l1_ca_telemetry_decoder_gs::glonass_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("glonass_l1_ca_telemetry_decoder_gs", 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index 144554491..e26592279 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -60,6 +60,8 @@ glonass_l2_ca_telemetry_decoder_gs::glonass_l2_ca_telemetry_decoder_gs( bool dump) : gr::block("glonass_l2_ca_telemetry_decoder_gs", 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 2fd99ae09..dc4eeed36 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -62,7 +62,7 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( bool dump) : gr::block("gps_navigation_gs", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - //this->set_max_output_buffer(128); + //prevent telemetry symbols accumulation in output buffers this->set_max_noutput_items(1); // Ephemeris data port out diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc index 44cd5cd40..e2702b374 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc @@ -60,6 +60,8 @@ gps_l2c_telemetry_decoder_gs::gps_l2c_telemetry_decoder_gs( 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index 891f12c27..deccd3c54 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -58,6 +58,8 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc index cfa68d2be..daef06b6e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc @@ -58,6 +58,8 @@ sbas_l1_telemetry_decoder_gs::sbas_l1_telemetry_decoder_gs( 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 + this->set_max_noutput_items(1); // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 7450782b4..4f324af8b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -92,6 +92,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_) dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + //prevent telemetry symbols accumulation in output buffers this->set_max_noutput_items(1); trk_parameters = conf_; // Telemetry bit synchronization message port input From 52c08e3ab474bfb6a0db321cad5334034a7df13b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 13:08:00 +0200 Subject: [PATCH 07/24] Add copy constructor, copy assignment operator, move constructor and move assignment operator --- src/core/system_parameters/gnss_satellite.cc | 255 +++++++++++-------- src/core/system_parameters/gnss_satellite.h | 8 +- 2 files changed, 154 insertions(+), 109 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index bfebef376..c928ce069 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -89,29 +89,60 @@ bool operator==(const Gnss_Satellite& sat1, const Gnss_Satellite& sat2) { if (sat1.get_PRN() == sat2.get_PRN()) { - equal = true; + if (sat1.get_rf_link() == sat2.get_rf_link()) + { + equal = true; + } } } return equal; } -/* -Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite &rhs) { +// Copy constructor +Gnss_Satellite::Gnss_Satellite(Gnss_Satellite&& other) +{ + *this = std::move(other); +} + + +// Copy assignment operator +Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) +{ // Only do assignment if RHS is a different object from this. - if (this != &rhs) { + if (this != &rhs) + { // Deallocate, allocate new space, copy values... - const std::string system_ = rhs.get_system(); - const uint32_t PRN_ = rhs.get_PRN(); - const std::string block_ = rhs.get_block(); - // const int32_t rf_link_ = 0; - this->set_system(system_); - this->set_PRN(PRN_); - this->set_block(system_, PRN_); - //this.rf_link = rf_link_; - } + this->reset(); + this->set_system(rhs.get_system()); + this->set_PRN(rhs.get_PRN()); + this->set_block(rhs.get_system(), rhs.get_PRN()); + this->set_rf_link(rhs.get_rf_link()); + } return *this; -}*/ +} + + +// Move constructor +Gnss_Satellite::Gnss_Satellite(const Gnss_Satellite& other) +{ + *this = std::move(other); +} + + +// Move assignment operator +Gnss_Satellite& Gnss_Satellite::operator=(Gnss_Satellite&& other) +{ + if (this != &other) + { + this->reset(); + this->system = std::move(other.get_system()); + this->PRN = std::move(other.get_PRN()); + this->block = std::move(other.get_block()); + this->rf_link = std::move(other.get_rf_link()); + } + return *this; +} void Gnss_Satellite::set_system(const std::string& system_) @@ -260,6 +291,14 @@ int32_t Gnss_Satellite::get_rf_link() const } +void Gnss_Satellite::set_rf_link(int32_t rf_link_) +{ + // Set satellite's rf link. Identifies the GLONASS Frequency Channel + rf_link = rf_link_; + return; +} + + uint32_t Gnss_Satellite::get_PRN() const { // Get satellite's PRN @@ -625,100 +664,100 @@ std::string Gnss_Satellite::what_block(const std::string& system_, uint32_t PRN_ if (system_ == "Beidou") { // Check https://en.wikipedia.org/wiki/List_of_BeiDou_satellites - switch ( PRN_ ) - { - case 1: - block_ = std::string("Compass-G1"); //! system_set; // = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}; void reset(); + void set_rf_link(int32_t rf_link_); }; #endif From 38b91bec13177515084eaf5590a58cee0aa76cfe Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 5 Jul 2019 13:29:23 +0200 Subject: [PATCH 08/24] Fix GPS L2CM tracking and tlm decoding --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 4f324af8b..2f46f0f95 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -1009,7 +1009,7 @@ void dll_pll_veml_tracking::run_dll_pll() if (d_dll_filt_history.full()) { float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); - if (fabs(avg_code_error_chips_s) > 0.025) + if (fabs(avg_code_error_chips_s) > 0.10) { float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); From 1237a29fc90f2e819ca1bddd4f663d83f4f8c25b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 14:42:48 +0200 Subject: [PATCH 09/24] Add copy constructor, copy assignment operator, move constructor and move assignment operator --- src/core/system_parameters/gnss_signal.cc | 40 +++++++++++++++++++++++ src/core/system_parameters/gnss_signal.h | 4 +++ 2 files changed, 44 insertions(+) diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index 0c25a7841..520d7242a 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -53,6 +53,46 @@ Gnss_Signal::Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& si Gnss_Signal::~Gnss_Signal() = default; +// Copy constructor +Gnss_Signal::Gnss_Signal(Gnss_Signal&& other) +{ + *this = std::move(other); +} + + +// Copy assignment operator +Gnss_Signal& Gnss_Signal::operator=(const Gnss_Signal& rhs) +{ + // Only do assignment if RHS is a different object from this. + if (this != &rhs) + { + // Deallocate, allocate new space, copy values... + this->satellite = rhs.get_satellite(); + this->signal = rhs.get_signal_str(); + } + return *this; +} + + +// Move constructor +Gnss_Signal::Gnss_Signal(const Gnss_Signal& other) +{ + *this = std::move(other); +} + + +// Move assignment operator +Gnss_Signal& Gnss_Signal::operator=(Gnss_Signal&& other) +{ + if (this != &other) + { + this->satellite = std::move(other.get_satellite()); + this->signal = std::move(other.get_signal_str()); + } + return *this; +} + + std::string Gnss_Signal::get_signal_str() const { return this->signal; diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index c2f6e37c4..92051444a 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -54,6 +54,10 @@ public: friend bool operator==(const Gnss_Signal& /*sig1*/, const Gnss_Signal& /*sig2*/); //!< operator== for comparison friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing + Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor + Gnss_Signal& operator=(const Gnss_Signal&); //!< Copy assignment operator + Gnss_Signal(const Gnss_Signal& other); //!< Move constructor + Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator private: Gnss_Satellite satellite; std::string signal; From 0e0991a1a55f843603adb26b1a2467e5fd3c542e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 14:44:56 +0200 Subject: [PATCH 10/24] Add move assignment operator --- .../tracking/libs/exponential_smoother.cc | 18 ++++++++++++++++++ .../tracking/libs/exponential_smoother.h | 1 + .../tracking/libs/tracking_loop_filter.cc | 19 +++++++++++++++++++ .../tracking/libs/tracking_loop_filter.h | 16 +++++++++------- 4 files changed, 47 insertions(+), 7 deletions(-) diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc index 204a1afb1..186882d06 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.cc +++ b/src/algorithms/tracking/libs/exponential_smoother.cc @@ -51,6 +51,24 @@ Exponential_Smoother::Exponential_Smoother() Exponential_Smoother::~Exponential_Smoother() = default; +// Move assignment operator +Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) +{ + if (this != &other) + { + this->alpha_ = other.alpha_; + this->old_value_ = other.old_value_; + this->one_minus_alpha_ = 1.0 - other.alpha_; + this->samples_for_initialization_ = other.samples_for_initialization_; + this->initializing_ = other.initializing_; + this->init_counter_ = other.init_counter_; + this->min_value_ = other.min_value_; + this->offset_ = other.offset_; + } + return *this; +} + + void Exponential_Smoother::set_alpha(float alpha) { alpha_ = alpha; diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h index f8ff21182..dc7f222d4 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.h +++ b/src/algorithms/tracking/libs/exponential_smoother.h @@ -56,6 +56,7 @@ public: void set_offset(float offset); float smooth(float raw); double smooth(double raw); + Exponential_Smoother& operator=(Exponential_Smoother&& other); //!< Move assignment operator private: float alpha_; // takes value 0.0001 if not set int samples_for_initialization_; diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index 1d1b679c6..d2b581d2f 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -71,6 +71,25 @@ Tracking_loop_filter::Tracking_loop_filter() Tracking_loop_filter::~Tracking_loop_filter() = default; +// Move assignment operator +Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) +{ + if (this != &other) + { + this->d_inputs = other.d_inputs; + this->d_outputs = other.d_outputs; + this->d_input_coefficients = other.d_input_coefficients; + this->d_output_coefficients = other.d_output_coefficients; + this->d_loop_order = other.d_loop_order; + this->d_current_index = other.d_current_index; + this->d_include_last_integrator = other.d_include_last_integrator; + this->d_noise_bandwidth = other.d_noise_bandwidth; + this->d_update_interval = other.d_update_interval; + } + return *this; +} + + float Tracking_loop_filter::apply(float current_input) { // Now apply the filter coefficients: diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.h b/src/algorithms/tracking/libs/tracking_loop_filter.h index 0151f3c5a..204b19825 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.h +++ b/src/algorithms/tracking/libs/tracking_loop_filter.h @@ -44,6 +44,15 @@ class Tracking_loop_filter { public: + Tracking_loop_filter(); + ~Tracking_loop_filter(); + + Tracking_loop_filter(float update_interval, float noise_bandwidth, + int loop_order = 2, + bool include_last_integrator = false); + + Tracking_loop_filter& operator=(Tracking_loop_filter&& other); //!< Move assignment operator + float get_noise_bandwidth(void) const; float get_update_interval(void) const; bool get_include_last_integrator(void) const; @@ -57,13 +66,6 @@ public: void initialize(float initial_output = 0.0); float apply(float current_input); - Tracking_loop_filter(float update_interval, float noise_bandwidth, - int loop_order = 2, - bool include_last_integrator = false); - - Tracking_loop_filter(); - ~Tracking_loop_filter(); - private: // Store the last inputs and outputs: std::vector d_inputs; From 5d679a3eef73e221ce43e15fff98c71df7889d3a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 15:24:28 +0200 Subject: [PATCH 11/24] Do not use move when it is not needed --- src/core/system_parameters/gnss_satellite.cc | 6 +++--- src/core/system_parameters/gnss_signal.cc | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index c928ce069..8284baf9a 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -126,7 +126,7 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) // Move constructor Gnss_Satellite::Gnss_Satellite(const Gnss_Satellite& other) { - *this = std::move(other); + *this = other; } @@ -137,9 +137,9 @@ Gnss_Satellite& Gnss_Satellite::operator=(Gnss_Satellite&& other) { this->reset(); this->system = std::move(other.get_system()); - this->PRN = std::move(other.get_PRN()); + this->PRN = other.get_PRN(); this->block = std::move(other.get_block()); - this->rf_link = std::move(other.get_rf_link()); + this->rf_link = other.get_rf_link(); } return *this; } diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index 520d7242a..d1e838140 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -77,7 +77,7 @@ Gnss_Signal& Gnss_Signal::operator=(const Gnss_Signal& rhs) // Move constructor Gnss_Signal::Gnss_Signal(const Gnss_Signal& other) { - *this = std::move(other); + *this = other; } From 755dd7901fc01957fba2667c4cb0510677861e24 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 16:13:35 +0200 Subject: [PATCH 12/24] Add copy constructor, move constructor and move assignment operator. Fix memory leak --- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 33 +++++++++++++++++++- src/core/monitor/serdes_gnss_synchro.h | 33 +++++++++++++++++++- 2 files changed, 64 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 490580818..a03e297db 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -48,7 +48,6 @@ public: // Verify that the version of the library that we linked against is // compatible with the version of the headers we compiled against. GOOGLE_PROTOBUF_VERIFY_VERSION; - monitor_.New(); } ~Serdes_Monitor_Pvt() @@ -56,6 +55,36 @@ public: // google::protobuf::ShutdownProtobufLibrary(); } + inline Serdes_Monitor_Pvt(Serdes_Monitor_Pvt&& other) //!< Copy constructor + { + this->monitor_ = other.monitor_; + } + + inline Serdes_Monitor_Pvt& operator=(const Serdes_Monitor_Pvt& rhs) //!< Copy assignment operator + { + // Only do assignment if RHS is a different object from this. + if (this != &rhs) + { + // Deallocate, allocate new space, copy values... + this->monitor_ = rhs.monitor_; + } + return *this; + } + + inline Serdes_Monitor_Pvt(const Serdes_Monitor_Pvt& other) //!< Move constructor + { + this->monitor_ = std::move(other.monitor_); + } + + inline Serdes_Monitor_Pvt& operator=(Serdes_Monitor_Pvt&& other) //!< Move assignment operator + { + if (this != &other) + { + this->monitor_ = std::move(other.monitor_); + } + return *this; + } + inline std::string createProtobuffer(const Monitor_Pvt& monitor) //!< Serialization into a string { monitor_.Clear(); @@ -91,6 +120,8 @@ public: monitor_.set_hdop(monitor.hdop); monitor_.set_vdop(monitor.vdop); + monitor_.CheckInitialized(); + monitor_.SerializeToString(&data); return data; } diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index 0e63aed6c..39ececdab 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -50,7 +50,6 @@ public: // Verify that the version of the library that we linked against is // compatible with the version of the headers we compiled against. GOOGLE_PROTOBUF_VERIFY_VERSION; - observables.New(); } ~Serdes_Gnss_Synchro() @@ -58,6 +57,36 @@ public: google::protobuf::ShutdownProtobufLibrary(); } + inline Serdes_Gnss_Synchro(Serdes_Gnss_Synchro&& other) //!< Copy constructor + { + this->observables = other.observables; + } + + inline Serdes_Gnss_Synchro& operator=(const Serdes_Gnss_Synchro& rhs) //!< Copy assignment operator + { + // Only do assignment if RHS is a different object from this. + if (this != &rhs) + { + // Deallocate, allocate new space, copy values... + this->observables = rhs.observables; + } + return *this; + } + + inline Serdes_Gnss_Synchro(const Serdes_Gnss_Synchro& other) //!< Move constructor + { + this->observables = std::move(other.observables); + } + + inline Serdes_Gnss_Synchro& operator=(Serdes_Gnss_Synchro&& other) //!< Move assignment operator + { + if (this != &other) + { + this->observables = std::move(other.observables); + } + return *this; + } + inline std::string createProtobuffer(const std::vector& vgs) //!< Serialization into a string { observables.Clear(); @@ -101,6 +130,8 @@ public: obs->set_rx_time(gs.RX_time); obs->set_flag_valid_pseudorange(gs.Flag_valid_pseudorange); obs->set_interp_tow_ms(gs.interp_TOW_ms); + + obs->CheckInitialized(); } observables.SerializeToString(&data); return data; From 76a14a86f017b4b6d70967856b671bff53a94147 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Jul 2019 16:14:33 +0200 Subject: [PATCH 13/24] Fix data race condition detected by Coverity Scan --- .../gnuradio_blocks/galileo_telemetry_decoder_gs.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index a94aebcf8..b73690f0e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -664,6 +664,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_CRC_error_counter = 0; d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + gr::thread::scoped_lock lock(d_setlock); d_last_valid_preamble = d_sample_counter; if (!d_flag_frame_sync) { From 0d3299f29b2a02b11d91c84cabe1cc6aa26e46f9 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 5 Jul 2019 20:30:34 +0200 Subject: [PATCH 14/24] Fix Beidou. Disabled symbol integration in tracking for Beidou and disabled experimental tracking carrier Doppler correction for all systems --- .../beidou_b1i_telemetry_decoder_gs.cc | 315 +++++++-------- .../beidou_b1i_telemetry_decoder_gs.h | 24 +- .../beidou_b3i_telemetry_decoder_gs.cc | 374 +++++++++--------- .../beidou_b3i_telemetry_decoder_gs.h | 21 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 67 ++-- src/algorithms/tracking/libs/dll_pll_conf.cc | 1 + src/algorithms/tracking/libs/dll_pll_conf.h | 1 + 7 files changed, 400 insertions(+), 403 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index b597504b3..57f8d5e7b 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -29,10 +29,10 @@ * * ------------------------------------------------------------------------- */ + + #include "beidou_b1i_telemetry_decoder_gs.h" #include "Beidou_B1I.h" -#include "Beidou_DNAV.h" -#include "beidou_dnav_almanac.h" #include "beidou_dnav_ephemeris.h" #include "beidou_dnav_iono.h" #include "beidou_dnav_utc_model.h" @@ -57,6 +57,7 @@ beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) return beidou_b1i_telemetry_decoder_gs_sptr(new beidou_b1i_telemetry_decoder_gs(satellite, dump)); } + beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( const Gnss_Satellite &satellite, bool dump) : gr::block("beidou_b1i_telemetry_decoder_gs", @@ -74,38 +75,63 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); LOG(INFO) << "Initializing BeiDou B1I Telemetry Decoding for satellite " << this->d_satellite; - d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; + d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D1NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) + // Setting samples of secondary code + for (int32_t i = 0; i < BEIDOU_B1I_SECONDARY_CODE_LENGTH; i++) { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + if (BEIDOU_B1I_SECONDARY_CODE.at(i) == '1') { - d_preamble_samples[i] = 1; + d_secondary_code_symbols[i] = 1; } else { - d_preamble_samples[i] = -1; + d_secondary_code_symbols[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + // Setting samples of preamble code + int32_t n = 0; + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + int32_t m = 0; + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } + else + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } + } + + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); - d_last_valid_preamble = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; d_preamble_index = 0; d_flag_frame_sync = false; - d_TOW_at_current_symbol_ms = 0U; + d_TOW_at_current_symbol_ms = 0; d_TOW_at_Preamble_ms = 0U; Flag_valid_word = false; d_CRC_error_counter = 0; @@ -118,6 +144,7 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -165,7 +192,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, in void beidou_b1i_telemetry_decoder_gs::decode_word( int32_t word_counter, - const float *enc_word_symbols, + const double *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -205,7 +232,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_word( } -void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) +void beidou_b1i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -236,13 +263,11 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) // 3. Check operation executed correctly if (d_nav.flag_crc_test == true) { - DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel << " from satellite " << d_satellite; } else { - DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel - << " from satellite " << d_satellite; + DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel << " from satellite " << d_satellite; } // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) @@ -297,59 +322,41 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); + d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D2NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = nullptr; d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; // Setting samples of preamble code + int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - d_preamble_samples[i] = 1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = 1; + n++; + } } else { - d_preamble_samples[i] = -1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -1; + n++; + } } } - d_symbol_duration_ms = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); - } - else - { - // Clear values from previous declaration - volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_subframe_symbols); - //back to normal satellites - d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS; - d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; - - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) - { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') - { - d_preamble_samples[i] = 1; - } - else - { - d_preamble_samples[i] = -1; - } - } - - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); } } @@ -380,15 +387,6 @@ void beidou_b1i_telemetry_decoder_gs::set_channel(int32_t channel) } } -void beidou_b1i_telemetry_decoder_gs::reset() -{ - d_last_valid_preamble = d_sample_counter; - d_TOW_at_current_symbol_ms = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; - DLOG(INFO) << "Beidou B1I Telemetry decoder reset for satellite " << d_satellite; - return; -} int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -405,9 +403,10 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); + d_flag_preamble = false; - if (d_symbol_history.size() >= d_required_symbols) + if (d_symbol_history.size() > d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -422,6 +421,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ } } } + //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -429,7 +429,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - DLOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; + LOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -443,54 +443,9 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; + LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; d_preamble_index = d_sample_counter; //record the preamble sample stamp - - d_stat = 2; - - // ******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) //normal PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = d_symbol_history.at(i); - } - } - else // 180 deg. inverted carrier phase PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = -d_symbol_history.at(i); - } - } - - // call the decoder - decode_subframe(d_subframe_symbols); - - if (d_nav.flag_crc_test == true) - { - d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) - if (!d_flag_frame_sync) - { - d_flag_frame_sync = true; - DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " << this->d_satellite; - } - } - else - { - d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp - if (d_CRC_error_counter > CRC_ERROR_LIMIT) - { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; - d_flag_frame_sync = false; - d_stat = 0; - flag_SOW_set = false; - } - } } else { @@ -509,16 +464,50 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ // ******* SAMPLES TO SYMBOLS ******* if (corr_value > 0) //normal PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } } } else // 180 deg. inverted carrier phase PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = -d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= static_cast(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } } } @@ -542,7 +531,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; + LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -550,6 +539,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ } } } + // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) @@ -557,67 +547,52 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - //check TOW update consistency - uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - //compute new TOW - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * BEIDOU_B1I_CODE_PERIOD_MS); flag_SOW_set = true; d_nav.flag_new_SOW_available = false; + } + else // if there is not a new preamble, we define the TOW of the current symbol + { + d_TOW_at_current_symbol_ms += static_cast(BEIDOU_B1I_CODE_PERIOD_MS); + } - if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) - { - LOG(INFO) << "Warning: BEIDOU B1I TOW update in ch " << d_channel - << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; - d_TOW_at_current_symbol_ms = 0; - d_flag_valid_word = false; - } - else - { - d_last_valid_preamble = d_sample_counter; - d_flag_valid_word = true; - } + if (d_flag_frame_sync == true and flag_SOW_set == true) + { + current_symbol.Flag_valid_word = true; } else { - if (d_flag_valid_word) - { - d_TOW_at_current_symbol_ms += d_symbol_duration_ms; - if (current_symbol.Flag_valid_symbol_output == false) - { - d_flag_valid_word = false; - } - } + current_symbol.Flag_valid_word = false; } - if (d_flag_valid_word == true) + current_symbol.PRN = this->d_satellite.get_PRN(); + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + + if (d_dump == true) { - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - current_symbol.Flag_valid_word = d_flag_valid_word; - - if (d_dump == true) + // MULTIPLEXED FILE RECORDING - Record results to file + try { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); - } + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = d_nav.d_SOW; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = static_cast(d_required_symbols); + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - return 1; } - return 0; + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + + return 1; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h index 9485008fd..3a7b8debf 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h @@ -39,7 +39,7 @@ #include #include // for boost::shared_ptr #include // for block -#include // for gr_vector_const_void_star +#include // for gr_vector_const_void_star #include #include #include @@ -62,8 +62,10 @@ public: ~beidou_b1i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - void reset(); - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ @@ -75,17 +77,19 @@ private: beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); beidou_b1i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(float *symbols); - void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols); + void decode_subframe(double *symbols); + void decode_word(int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols); void decode_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; + int32_t *d_secondary_code_symbols; + uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - float *d_subframe_symbols; + double *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data @@ -103,16 +107,12 @@ private: //!< Navigation Message variable Beidou_Dnav_Navigation_Message d_nav; - // Values to populate gnss synchronization structure - uint32_t d_symbol_duration_ms; + //!< Values to populate gnss synchronization structure uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; - uint64_t d_last_valid_preamble; - bool d_flag_valid_word; - bool d_sent_tlm_failed_msg; bool Flag_valid_word; - // Satellite Information and logging capacity + //!< Satellite Information and logging capacity Gnss_Satellite d_satellite; int32_t d_channel; bool d_dump; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc index 419aff764..01530cd32 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc @@ -53,7 +53,8 @@ beidou_b3i_telemetry_decoder_gs_sptr beidou_b3i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump) { - return beidou_b3i_telemetry_decoder_gs_sptr(new beidou_b3i_telemetry_decoder_gs(satellite, dump)); + return beidou_b3i_telemetry_decoder_gs_sptr( + new beidou_b3i_telemetry_decoder_gs(satellite, dump)); } @@ -69,37 +70,74 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); + // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " << this->d_satellite; + LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " + << this->d_satellite; - d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; + d_samples_per_symbol = + (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / + BEIDOU_D1NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_samples_per_preamble = + BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = static_cast( + volk_gnsssdr_malloc(BEIDOU_B3I_SECONDARY_CODE_LENGTH * sizeof(int32_t), + volk_gnsssdr_get_alignment())); + d_preamble_samples = static_cast(volk_gnsssdr_malloc( + d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) + // Setting samples of secondary code + for (int32_t i = 0; i < BEIDOU_B3I_SECONDARY_CODE_LENGTH; i++) { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + if (BEIDOU_B3I_SECONDARY_CODE.at(i) == '1') { - d_preamble_samples[i] = 1; + d_secondary_code_symbols[i] = 1; } else { - d_preamble_samples[i] = -1; + d_secondary_code_symbols[i] = -1; } } - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + // Setting samples of preamble code + int32_t n = 0; + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + int32_t m = 0; + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } + else + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } + } + + d_subframe_symbols = static_cast( + volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), + volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); - d_last_valid_preamble = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; // Generic settings d_sample_counter = 0; d_stat = 0; @@ -118,6 +156,7 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs( beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() { volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -128,7 +167,8 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() } catch (const std::exception &ex) { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + LOG(WARNING) << "Exception in destructor closing the dump file " + << ex.what(); } } } @@ -165,8 +205,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, void beidou_b3i_telemetry_decoder_gs::decode_word( - int32_t word_counter, - const float *enc_word_symbols, + int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols) { int32_t bitsbch[30], first_branch[15], second_branch[15]; @@ -175,7 +214,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t j = 0; j < 30; j++) { - dec_word_symbols[j] = static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); + dec_word_symbols[j] = + static_cast(enc_word_symbols[j] > 0) ? (1) : (-1); } } else @@ -184,7 +224,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( { for (uint32_t c = 0; c < 15; c++) { - bitsbch[r * 15 + c] = static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); + bitsbch[r * 15 + c] = + static_cast(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1); } } @@ -206,7 +247,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word( } -void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) +void beidou_b3i_telemetry_decoder_gs::decode_subframe(double *frame_symbols) { // 1. Transform from symbols to bits std::string data_bits; @@ -315,62 +356,50 @@ void beidou_b3i_telemetry_decoder_gs::set_satellite( { // Clear values from previous declaration volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); - + d_samples_per_symbol = + (BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) / + BEIDOU_D2NAV_SYMBOL_RATE_SPS; d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), - volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_samples_per_preamble = + BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = nullptr; + d_preamble_samples = static_cast( + volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), + volk_gnsssdr_get_alignment())); + d_preamble_period_samples = + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol; // Setting samples of preamble code + int32_t n = 0; for (int32_t i = 0; i < d_symbols_per_preamble; i++) { if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') { - d_preamble_samples[i] = 1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = 1; + n++; + } } else { - d_preamble_samples[i] = -1; + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -1; + n++; + } } } - d_symbol_duration_ms = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; - d_subframe_symbols = static_cast(volk_gnsssdr_malloc( - BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), + + d_subframe_symbols = static_cast(volk_gnsssdr_malloc( + BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); - } - else - { - // Clear values from previous declaration - volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_subframe_symbols); - //back to normal satellites - d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS; - d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; - - // Setting samples of preamble code - for (int32_t i = 0; i < d_symbols_per_preamble; i++) - { - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') - { - d_preamble_samples[i] = 1; - } - else - { - d_preamble_samples[i] = -1; - } - } - - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble; - d_symbol_history.set_capacity(d_required_symbols); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + + d_samples_per_preamble; + d_symbol_history.set_capacity(d_required_symbols + 1); } } @@ -405,15 +434,6 @@ void beidou_b3i_telemetry_decoder_gs::set_channel(int32_t channel) } } -void beidou_b3i_telemetry_decoder_gs::reset() -{ - d_last_valid_preamble = d_sample_counter; - d_TOW_at_current_symbol_ms = 0; - d_sent_tlm_failed_msg = false; - d_flag_valid_word = false; - DLOG(INFO) << "Beidou B3I Telemetry decoder reset for satellite " << d_satellite; - return; -} int beidou_b3i_telemetry_decoder_gs::general_work( int noutput_items __attribute__((unused)), @@ -434,9 +454,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_counter++; // count for the processed samples consume_each(1); + d_flag_preamble = false; - if (d_symbol_history.size() >= d_required_symbols) + if (d_symbol_history.size() > d_required_symbols) { //******* preamble correlation ******** for (int32_t i = 0; i < d_samples_per_preamble; i++) @@ -451,6 +472,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } + //******* frame sync ****************** if (d_stat == 0) // no preamble information { @@ -458,7 +480,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Record the preamble sample stamp d_preamble_index = d_sample_counter; - DLOG(INFO) << "Preamble detection for BEIDOU B3I SAT " << this->d_satellite; + LOG(INFO) << "Preamble detection for BEIDOU B3I SAT " + << this->d_satellite; // Enter into frame pre-detection status d_stat = 1; } @@ -472,55 +495,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (abs(preamble_diff - d_preamble_period_samples) == 0) { // try to decode frame - DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " - << this->d_satellite; + LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT " + << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp d_stat = 2; - - // ******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) //normal PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = d_symbol_history.at(i); - } - } - else // 180 deg. inverted carrier phase PLL lock - { - for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) - { - d_subframe_symbols[i] = -d_symbol_history.at(i); - } - } - - // call the decoder - decode_subframe(d_subframe_symbols); - - if (d_nav.flag_crc_test == true) - { - d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) - if (!d_flag_frame_sync) - { - d_flag_frame_sync = true; - DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " - << this->d_satellite; - } - } - else - { - d_CRC_error_counter++; - d_preamble_index = d_sample_counter; // record the preamble sample stamp - if (d_CRC_error_counter > CRC_ERROR_LIMIT) - { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " - << this->d_satellite; - d_flag_frame_sync = false; - d_stat = 0; - flag_SOW_set = false; - } - } } else { @@ -535,21 +513,62 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } else if (d_stat == 2) // preamble acquired { - if (d_sample_counter == d_preamble_index + static_cast(d_preamble_period_samples)) + if (d_sample_counter == + d_preamble_index + static_cast(d_preamble_period_samples)) { - // ******* SAMPLES TO SYMBOLS ******* - if (corr_value > 0) //normal PLL lock + //******* SAMPLES TO SYMBOLS ******* + if (corr_value > 0) // normal PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += + d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] += + static_cast(d_secondary_code_symbols[k]) * + d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } } } else // 180 deg. inverted carrier phase PLL lock { + int32_t k = 0; for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++) { - d_subframe_symbols[i] = -d_symbol_history.at(i); + d_subframe_symbols[i] = 0; + // integrate samples into symbols + for (uint32_t m = 0; m < d_samples_per_symbol; m++) + { + if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6) + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= + d_symbol_history.at(i * d_samples_per_symbol + m); + } + else + { + // because last symbol of the preamble is just received now! + d_subframe_symbols[i] -= + static_cast(d_secondary_code_symbols[k]) * + d_symbol_history.at(i * d_samples_per_symbol + m); + k++; + k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH; + } + } } } @@ -559,8 +578,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work( if (d_nav.flag_crc_test == true) { d_CRC_error_counter = 0; - d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) - d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + d_flag_preamble = true; // valid preamble indicator (initialized to + // false every work()) + d_preamble_index = + d_sample_counter; // record the preamble sample stamp (t_P) if (!d_flag_frame_sync) { d_flag_frame_sync = true; @@ -574,8 +595,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work( d_preamble_index = d_sample_counter; // record the preamble sample stamp if (d_CRC_error_counter > CRC_ERROR_LIMIT) { - DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " - << this->d_satellite; + LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " + << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; flag_SOW_set = false; @@ -583,6 +604,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work( } } } + // UPDATE GNSS SYNCHRO DATA // 2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) @@ -590,67 +612,55 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { // Reporting sow as gps time of week d_TOW_at_Preamble_ms = static_cast((d_nav.d_SOW + 14) * 1000.0); - //check TOW update consistency - uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - //compute new TOW - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; + d_TOW_at_current_symbol_ms = + d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * + BEIDOU_B3I_CODE_PERIOD_MS); flag_SOW_set = true; d_nav.flag_new_SOW_available = false; + } + else // if there is not a new preamble, we define the TOW of the current + // symbol + { + d_TOW_at_current_symbol_ms += + static_cast(BEIDOU_B3I_CODE_PERIOD_MS); + } - if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms) - { - LOG(INFO) << "Warning: BEIDOU B3I TOW update in ch " << d_channel - << " does not match the TLM TOW counter " << static_cast(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n"; - - d_TOW_at_current_symbol_ms = 0; - d_flag_valid_word = false; - } - else - { - d_last_valid_preamble = d_sample_counter; - d_flag_valid_word = true; - } + if (d_flag_frame_sync == true and flag_SOW_set == true) + { + current_symbol.Flag_valid_word = true; } else { - if (d_flag_valid_word) - { - d_TOW_at_current_symbol_ms += d_symbol_duration_ms; - if (current_symbol.Flag_valid_symbol_output == false) - { - d_flag_valid_word = false; - } - } + current_symbol.Flag_valid_word = false; } - if (d_flag_valid_word == true) + current_symbol.PRN = this->d_satellite.get_PRN(); + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + + if (d_dump == true) { - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - current_symbol.Flag_valid_word = d_flag_valid_word; - - if (d_dump == true) + // MULTIPLEXED FILE RECORDING - Record results to file + try { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - uint64_t tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); - } + double tmp_double; + uint64_t tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = d_nav.d_SOW; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = static_cast(d_required_symbols); + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); } - - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - return 1; } - return 0; + + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + + return 1; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h index d08f8388e..54bac3adb 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h @@ -60,8 +60,10 @@ public: ~beidou_b3i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - void reset(); - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ @@ -75,24 +77,27 @@ private: bool dump); beidou_b3i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); - void decode_subframe(float *symbols); - void decode_word(int32_t word_counter, const float *enc_word_symbols, + void decode_subframe(double *symbols); + void decode_word(int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols); void decode_bch15_11_01(const int32_t *bits, int32_t *decbits); // Preamble decoding int32_t *d_preamble_samples; + int32_t *d_secondary_code_symbols; + uint32_t d_samples_per_symbol; int32_t d_symbols_per_preamble; int32_t d_samples_per_preamble; int32_t d_preamble_period_samples; - float *d_subframe_symbols; + double *d_subframe_symbols; uint32_t d_required_symbols; // Storage for incoming data boost::circular_buffer d_symbol_history; // Variables for internal functionality - uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed + uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) + // indicating number of samples processed uint64_t d_preamble_index; // Index of sample number where preamble was found uint32_t d_stat; // Status of decoder bool d_flag_frame_sync; // Indicate when a frame sync is achieved @@ -104,12 +109,8 @@ private: Beidou_Dnav_Navigation_Message d_nav; // Values to populate gnss synchronization structure - uint32_t d_symbol_duration_ms; uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; - uint64_t d_last_valid_preamble; - bool d_flag_valid_word; - bool d_sent_tlm_failed_msg; bool Flag_valid_word; // Satellite Information and logging capacity diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 2f46f0f95..940b43284 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -286,16 +286,17 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = BEIDOU_B1I_CODE_PERIOD; d_code_chip_rate = BEIDOU_B1I_CODE_RATE_HZ; d_code_length_chips = static_cast(BEIDOU_B1I_CODE_LENGTH_CHIPS); - d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; + //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; - d_secondary = true; + d_secondary = false; trk_parameters.track_pilot = false; // 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") { @@ -304,15 +305,16 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_code_period = BEIDOU_B3I_CODE_PERIOD; d_code_chip_rate = BEIDOU_B3I_CODE_RATE_HZ; d_code_length_chips = static_cast(BEIDOU_B3I_CODE_LENGTH_CHIPS); - d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; + //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; - d_secondary = true; + d_secondary = false; 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 { @@ -649,7 +651,8 @@ void dll_pll_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; + //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; d_secondary = false; @@ -662,16 +665,17 @@ void dll_pll_veml_tracking::start_tracking() } else { - d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; + //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; - d_secondary = true; + d_secondary = false; trk_parameters.track_pilot = false; // 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); } } @@ -682,7 +686,8 @@ void dll_pll_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; + //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; d_secondary = false; @@ -695,16 +700,17 @@ void dll_pll_veml_tracking::start_tracking() } else { - d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; + //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; - d_secondary = true; + d_secondary = false; trk_parameters.track_pilot = false; // 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); } } @@ -1002,21 +1008,24 @@ void dll_pll_veml_tracking::run_dll_pll() d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips; // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler - if (d_pull_in_transitory == false and d_corrected_doppler == false) + if (trk_parameters.enable_doppler_correction == true) { - d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); - - if (d_dll_filt_history.full()) + if (d_pull_in_transitory == false and d_corrected_doppler == false) { - float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); - if (fabs(avg_code_error_chips_s) > 0.10) + d_dll_filt_history.push_back(static_cast(d_code_error_filt_chips)); + + if (d_dll_filt_history.full()) { - float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); - LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); - d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); - d_corrected_doppler = true; + float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast(d_dll_filt_history.capacity()); + if (fabs(avg_code_error_chips_s) > 1.0) + { + float carrier_doppler_error_hz = static_cast(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast(d_code_chip_rate); + LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz); + d_corrected_doppler = true; + } + d_dll_filt_history.clear(); } - d_dll_filt_history.clear(); } } } diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index f31bccc4e..81ecb99b6 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -69,6 +69,7 @@ Dll_Pll_Conf::Dll_Pll_Conf() max_carrier_lock_fail = FLAGS_max_carrier_lock_fail; max_code_lock_fail = FLAGS_max_lock_fail; carrier_lock_th = FLAGS_carrier_lock_th; + enable_doppler_correction = false; track_pilot = false; system = 'G'; char sig_[3] = "1C"; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index d84e8f204..04f42f055 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -75,6 +75,7 @@ public: uint32_t smoother_length; double carrier_lock_th; bool track_pilot; + bool enable_doppler_correction; char system; char signal[3]{}; From 95ece1e6d4f4da4cb6b14c95b4b89b1d77c621f1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 Jul 2019 12:34:27 +0200 Subject: [PATCH 15/24] Simplify move assignment operator --- .../tracking/libs/exponential_smoother.cc | 16 +--------------- .../tracking/libs/exponential_smoother.h | 8 ++++---- .../tracking/libs/tracking_loop_filter.cc | 17 +---------------- 3 files changed, 6 insertions(+), 35 deletions(-) diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc index 186882d06..5c531fcab 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.cc +++ b/src/algorithms/tracking/libs/exponential_smoother.cc @@ -52,21 +52,7 @@ Exponential_Smoother::~Exponential_Smoother() = default; // Move assignment operator -Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) -{ - if (this != &other) - { - this->alpha_ = other.alpha_; - this->old_value_ = other.old_value_; - this->one_minus_alpha_ = 1.0 - other.alpha_; - this->samples_for_initialization_ = other.samples_for_initialization_; - this->initializing_ = other.initializing_; - this->init_counter_ = other.init_counter_; - this->min_value_ = other.min_value_; - this->offset_ = other.offset_; - } - return *this; -} +Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) = default; void Exponential_Smoother::set_alpha(float alpha) diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h index dc7f222d4..e0cc86af9 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.h +++ b/src/algorithms/tracking/libs/exponential_smoother.h @@ -47,10 +47,10 @@ class Exponential_Smoother { public: - Exponential_Smoother(); //!< Constructor - ~Exponential_Smoother(); //!< Destructor - void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.001 - void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200 + Exponential_Smoother(); //!< Constructor + ~Exponential_Smoother(); //!< Destructor + void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.001 + void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200 void reset(); void set_min_value(float value); void set_offset(float offset); diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index d2b581d2f..821efa67c 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -72,22 +72,7 @@ Tracking_loop_filter::~Tracking_loop_filter() = default; // Move assignment operator -Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) -{ - if (this != &other) - { - this->d_inputs = other.d_inputs; - this->d_outputs = other.d_outputs; - this->d_input_coefficients = other.d_input_coefficients; - this->d_output_coefficients = other.d_output_coefficients; - this->d_loop_order = other.d_loop_order; - this->d_current_index = other.d_current_index; - this->d_include_last_integrator = other.d_include_last_integrator; - this->d_noise_bandwidth = other.d_noise_bandwidth; - this->d_update_interval = other.d_update_interval; - } - return *this; -} +Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) = default; float Tracking_loop_filter::apply(float current_input) From 5f1779c15bf1624b608cdd257acad0062d2caf3c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 Jul 2019 13:55:19 +0200 Subject: [PATCH 16/24] Remove test for speed --- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 2 -- src/core/monitor/serdes_gnss_synchro.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index a03e297db..3294a5fc1 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -120,8 +120,6 @@ public: monitor_.set_hdop(monitor.hdop); monitor_.set_vdop(monitor.vdop); - monitor_.CheckInitialized(); - monitor_.SerializeToString(&data); return data; } diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index 39ececdab..c90e93282 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -130,8 +130,6 @@ public: obs->set_rx_time(gs.RX_time); obs->set_flag_valid_pseudorange(gs.Flag_valid_pseudorange); obs->set_interp_tow_ms(gs.interp_TOW_ms); - - obs->CheckInitialized(); } observables.SerializeToString(&data); return data; From cf8f4af52768467a2813e6816dfc930ce77e1381 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 18:41:50 +0200 Subject: [PATCH 17/24] Try to avoid non-default move & copy constructors --- src/core/system_parameters/gnss_satellite.cc | 24 +++++--------------- src/core/system_parameters/gnss_satellite.h | 14 ++++++------ src/core/system_parameters/gnss_signal.cc | 6 ++--- src/core/system_parameters/gnss_signal.h | 10 ++++---- 4 files changed, 20 insertions(+), 34 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 8284baf9a..089a0877b 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -47,17 +47,8 @@ Gnss_Satellite::Gnss_Satellite(const std::string& system_, uint32_t PRN_) } -Gnss_Satellite::~Gnss_Satellite() = default; - - void Gnss_Satellite::reset() { - system_set = {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"}; - satelliteSystem["GPS"] = "G"; - satelliteSystem["Glonass"] = "R"; - satelliteSystem["SBAS"] = "S"; - satelliteSystem["Galileo"] = "E"; - satelliteSystem["Beidou"] = "C"; PRN = 0; system = std::string(""); block = std::string(""); @@ -102,7 +93,7 @@ bool operator==(const Gnss_Satellite& sat1, const Gnss_Satellite& sat2) // Copy constructor Gnss_Satellite::Gnss_Satellite(Gnss_Satellite&& other) { - *this = std::move(other); + *this = other; } @@ -112,12 +103,10 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) // Only do assignment if RHS is a different object from this. if (this != &rhs) { - // Deallocate, allocate new space, copy values... - this->reset(); - this->set_system(rhs.get_system()); - this->set_PRN(rhs.get_PRN()); - this->set_block(rhs.get_system(), rhs.get_PRN()); - this->set_rf_link(rhs.get_rf_link()); + this->system = rhs.system; + this->PRN = rhs.PRN; + this->block = rhs.block; + this->rf_link = rhs.rf_link; } return *this; } @@ -126,7 +115,7 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite& rhs) // Move constructor Gnss_Satellite::Gnss_Satellite(const Gnss_Satellite& other) { - *this = other; + *this = std::move(other); } @@ -135,7 +124,6 @@ Gnss_Satellite& Gnss_Satellite::operator=(Gnss_Satellite&& other) { if (this != &other) { - this->reset(); this->system = std::move(other.get_system()); this->PRN = other.get_PRN(); this->block = std::move(other.get_block()); diff --git a/src/core/system_parameters/gnss_satellite.h b/src/core/system_parameters/gnss_satellite.h index 79fe9dead..b00c4c561 100644 --- a/src/core/system_parameters/gnss_satellite.h +++ b/src/core/system_parameters/gnss_satellite.h @@ -50,7 +50,7 @@ class Gnss_Satellite public: Gnss_Satellite(); //!< Default Constructor. Gnss_Satellite(const std::string& system_, uint32_t PRN_); //!< Concrete GNSS satellite Constructor. - ~Gnss_Satellite(); //!< Default Destructor. + ~Gnss_Satellite() = default; //!< Default Destructor. void update_PRN(uint32_t PRN); //!< Updates the PRN Number when information is decoded, only applies to GLONASS GNAV messages uint32_t get_PRN() const; //!< Gets satellite's PRN int32_t get_rf_link() const; //!< Gets the satellite's rf link @@ -68,15 +68,15 @@ public: Gnss_Satellite& operator=(Gnss_Satellite&& other); //!< Move assignment operator private: - uint32_t PRN; - std::string system; - std::map satelliteSystem; - std::string block; - int32_t rf_link; + uint32_t PRN{}; + int32_t rf_link{}; + std::string system{}; + std::string block{}; + const std::set system_set = {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"}; + const std::map satelliteSystem = {{"GPS", "G"}, {"Glonass", "R"}, {"SBAS", "S"}, {"Galileo", "E"}, {"Beidou", "C"}}; void set_system(const std::string& system); // Sets the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"}. void set_PRN(uint32_t PRN); // Sets satellite's PRN void set_block(const std::string& system_, uint32_t PRN_); - std::set system_set; // = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}; void reset(); void set_rf_link(int32_t rf_link_); }; diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index d1e838140..c73382d1c 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -50,9 +50,7 @@ Gnss_Signal::Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& si } -Gnss_Signal::~Gnss_Signal() = default; - - +/* // Copy constructor Gnss_Signal::Gnss_Signal(Gnss_Signal&& other) { @@ -91,7 +89,7 @@ Gnss_Signal& Gnss_Signal::operator=(Gnss_Signal&& other) } return *this; } - +*/ std::string Gnss_Signal::get_signal_str() const { diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index 92051444a..62f69b5ee 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -47,20 +47,20 @@ public: Gnss_Signal(); Gnss_Signal(const std::string& signal_); Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_); - ~Gnss_Signal(); + ~Gnss_Signal() = default; std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "L5" for GPS L5, "1G" for GLONASS L1 C/A, "1B" for Galileo E1B, "5X" for Galileo E5a. Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal friend bool operator==(const Gnss_Signal& /*sig1*/, const Gnss_Signal& /*sig2*/); //!< operator== for comparison friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing - Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor + /*Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor Gnss_Signal& operator=(const Gnss_Signal&); //!< Copy assignment operator Gnss_Signal(const Gnss_Signal& other); //!< Move constructor - Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator + Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator */ private: - Gnss_Satellite satellite; - std::string signal; + Gnss_Satellite satellite{}; + std::string signal{}; }; #endif From 29a910df300bdbae55bb451ccb7295a4f643a261 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 19:56:23 +0200 Subject: [PATCH 18/24] Simplify initializations --- .../PVT/libs/monitor_pvt_udp_sink.cc | 28 ----------- .../PVT/libs/monitor_pvt_udp_sink.h | 2 +- src/algorithms/PVT/libs/rtklib_solver.cc | 46 ++----------------- src/algorithms/PVT/libs/rtklib_solver.h | 10 ++-- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 5 +- src/core/monitor/serdes_gnss_synchro.h | 6 +-- src/core/receiver/file_configuration.h | 2 +- src/core/system_parameters/gnss_signal.cc | 41 ----------------- src/core/system_parameters/gnss_signal.h | 4 -- 9 files changed, 17 insertions(+), 127 deletions(-) diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc index 0e962207b..2796f13da 100644 --- a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc @@ -42,34 +42,6 @@ Monitor_Pvt_Udp_Sink::Monitor_Pvt_Udp_Sink(const std::vector& addre boost::asio::ip::udp::endpoint endpoint(boost::asio::ip::address::from_string(address, error), port); endpoints.push_back(endpoint); } - monitor_pvt.TOW_at_current_symbol_ms = 0U; - monitor_pvt.week = 0U; - monitor_pvt.RX_time = 0.0; - monitor_pvt.user_clk_offset = 0.0; - monitor_pvt.pos_x = 0.0; - monitor_pvt.pos_y = 0.0; - monitor_pvt.pos_z = 0.0; - monitor_pvt.vel_x = 0.0; - monitor_pvt.vel_y = 0.0; - monitor_pvt.vel_z = 0.0; - monitor_pvt.cov_xx = 0.0; - monitor_pvt.cov_yy = 0.0; - monitor_pvt.cov_zz = 0.0; - monitor_pvt.cov_xy = 0.0; - monitor_pvt.cov_yz = 0.0; - monitor_pvt.cov_zx = 0.0; - monitor_pvt.latitude = 0.0; - monitor_pvt.longitude = 0.0; - monitor_pvt.height = 0.0; - monitor_pvt.valid_sats = 0; - monitor_pvt.solution_status = 0; - monitor_pvt.solution_type = 0; - monitor_pvt.AR_ratio_factor = 0.0; - monitor_pvt.AR_ratio_threshold = 0.0; - monitor_pvt.gdop = 0.0; - monitor_pvt.pdop = 0.0; - monitor_pvt.hdop = 0.0; - monitor_pvt.vdop = 0.0; use_protobuf = protobuf_enabled; if (use_protobuf) diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h index 85a612358..f1f9146f4 100644 --- a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h @@ -53,7 +53,7 @@ private: boost::asio::ip::udp::socket socket; boost::system::error_code error; std::vector endpoints; - Monitor_Pvt monitor_pvt; + Monitor_Pvt monitor_pvt{}; Serdes_Monitor_Pvt serdes; bool use_protobuf; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index ebb359b0f..0f6e84b06 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -77,16 +77,7 @@ Rtklib_Solver::Rtklib_Solver(int nchannels, std::string dump_filename, bool flag count_valid_position = 0; this->set_averaging_flag(false); rtk_ = rtk; - for (double &i : dop_) - { - i = 0.0; - } - pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0}; - ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{{0, 0}}, {{0, 0}}}, {{}, {}}}; - for (auto &i : pvt_ssat) - { - i = ssat0; - } + // ############# ENABLE DATA FILE LOG ################# if (d_flag_dump_enabled == true) { @@ -104,35 +95,6 @@ Rtklib_Solver::Rtklib_Solver(int nchannels, std::string dump_filename, bool flag } } } - // PVT MONITOR - monitor_pvt.TOW_at_current_symbol_ms = 0U; - monitor_pvt.week = 0U; - monitor_pvt.RX_time = 0.0; - monitor_pvt.user_clk_offset = 0.0; - monitor_pvt.pos_x = 0.0; - monitor_pvt.pos_y = 0.0; - monitor_pvt.pos_z = 0.0; - monitor_pvt.vel_x = 0.0; - monitor_pvt.vel_y = 0.0; - monitor_pvt.vel_z = 0.0; - monitor_pvt.cov_xx = 0.0; - monitor_pvt.cov_yy = 0.0; - monitor_pvt.cov_zz = 0.0; - monitor_pvt.cov_xy = 0.0; - monitor_pvt.cov_yz = 0.0; - monitor_pvt.cov_zx = 0.0; - monitor_pvt.latitude = 0.0; - monitor_pvt.longitude = 0.0; - monitor_pvt.height = 0.0; - monitor_pvt.valid_sats = 0; - monitor_pvt.solution_status = 0; - monitor_pvt.solution_type = 0; - monitor_pvt.AR_ratio_factor = 0.0; - monitor_pvt.AR_ratio_threshold = 0.0; - monitor_pvt.gdop = 0.0; - monitor_pvt.pdop = 0.0; - monitor_pvt.hdop = 0.0; - monitor_pvt.vdop = 0.0; } bool Rtklib_Solver::save_matfile() @@ -450,9 +412,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ int valid_obs = 0; // valid observations counter int glo_valid_obs = 0; // GLONASS L1/L2 valid observations counter - std::array obs_data; - std::array eph_data; - std::array geph_data; + std::array obs_data{}; + std::array eph_data{}; + std::array geph_data{}; // Workaround for NAV/CNAV clash problem bool gps_dual_band = false; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 8e1b94cc7..128d8bebf 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -94,8 +94,8 @@ public: bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); - sol_t pvt_sol; - std::array pvt_ssat; + sol_t pvt_sol{}; + std::array pvt_ssat{}; double get_hdop() const; double get_vdop() const; double get_pdop() const; @@ -129,15 +129,15 @@ public: int count_valid_position; private: - rtk_t rtk_; + rtk_t rtk_{}; std::string d_dump_filename; std::ofstream d_dump_file; bool save_matfile(); bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; int d_nchannels; // Number of available channels for positioning - std::array dop_; - Monitor_Pvt monitor_pvt; + std::array dop_{}; + Monitor_Pvt monitor_pvt{}; }; #endif diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 3294a5fc1..2e5b95a17 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -55,6 +55,7 @@ public: // google::protobuf::ShutdownProtobufLibrary(); } + /* inline Serdes_Monitor_Pvt(Serdes_Monitor_Pvt&& other) //!< Copy constructor { this->monitor_ = other.monitor_; @@ -83,7 +84,7 @@ public: this->monitor_ = std::move(other.monitor_); } return *this; - } + }*/ inline std::string createProtobuffer(const Monitor_Pvt& monitor) //!< Serialization into a string { @@ -161,7 +162,7 @@ public: } private: - gnss_sdr::MonitorPvt monitor_; + gnss_sdr::MonitorPvt monitor_{}; }; #endif // GNSS_SDR_SERDES_MONITOR_PVT_H_ diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index c90e93282..935c292de 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -56,7 +56,7 @@ public: { google::protobuf::ShutdownProtobufLibrary(); } - + /* inline Serdes_Gnss_Synchro(Serdes_Gnss_Synchro&& other) //!< Copy constructor { this->observables = other.observables; @@ -85,7 +85,7 @@ public: this->observables = std::move(other.observables); } return *this; - } + }*/ inline std::string createProtobuffer(const std::vector& vgs) //!< Serialization into a string { @@ -179,7 +179,7 @@ public: } private: - gnss_sdr::Observables observables; + gnss_sdr::Observables observables{}; }; #endif // GNSS_SDR_SERDES_GNSS_SYNCHRO_H_ diff --git a/src/core/receiver/file_configuration.h b/src/core/receiver/file_configuration.h index 0e2ccb53e..c4d480d0b 100644 --- a/src/core/receiver/file_configuration.h +++ b/src/core/receiver/file_configuration.h @@ -79,7 +79,7 @@ private: std::shared_ptr ini_reader_; std::shared_ptr overrided_; std::unique_ptr converter_; - int error_; + int error_{}; }; #endif /*GNSS_SDR_FILE_CONFIGURATION_H_*/ diff --git a/src/core/system_parameters/gnss_signal.cc b/src/core/system_parameters/gnss_signal.cc index c73382d1c..71b0603fc 100644 --- a/src/core/system_parameters/gnss_signal.cc +++ b/src/core/system_parameters/gnss_signal.cc @@ -50,47 +50,6 @@ Gnss_Signal::Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& si } -/* -// Copy constructor -Gnss_Signal::Gnss_Signal(Gnss_Signal&& other) -{ - *this = std::move(other); -} - - -// Copy assignment operator -Gnss_Signal& Gnss_Signal::operator=(const Gnss_Signal& rhs) -{ - // Only do assignment if RHS is a different object from this. - if (this != &rhs) - { - // Deallocate, allocate new space, copy values... - this->satellite = rhs.get_satellite(); - this->signal = rhs.get_signal_str(); - } - return *this; -} - - -// Move constructor -Gnss_Signal::Gnss_Signal(const Gnss_Signal& other) -{ - *this = other; -} - - -// Move assignment operator -Gnss_Signal& Gnss_Signal::operator=(Gnss_Signal&& other) -{ - if (this != &other) - { - this->satellite = std::move(other.get_satellite()); - this->signal = std::move(other.get_signal_str()); - } - return *this; -} -*/ - std::string Gnss_Signal::get_signal_str() const { return this->signal; diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index 62f69b5ee..767a12a0c 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -54,10 +54,6 @@ public: friend bool operator==(const Gnss_Signal& /*sig1*/, const Gnss_Signal& /*sig2*/); //!< operator== for comparison friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing - /*Gnss_Signal(Gnss_Signal&& other); //!< Copy constructor - Gnss_Signal& operator=(const Gnss_Signal&); //!< Copy assignment operator - Gnss_Signal(const Gnss_Signal& other); //!< Move constructor - Gnss_Signal& operator=(Gnss_Signal&& other); //!< Move assignment operator */ private: Gnss_Satellite satellite{}; std::string signal{}; From 6cc1de71185c11305b7e655d99d0e79ab2aeb6bb Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 21:44:58 +0200 Subject: [PATCH 19/24] File cleaning --- src/algorithms/PVT/libs/serdes_monitor_pvt.h | 10 ++------- .../gnuradio_blocks/beamformer.cc | 21 +------------------ .../input_filter/gnuradio_blocks/beamformer.h | 8 +++++-- .../tracking/libs/exponential_smoother.cc | 7 ------- .../tracking/libs/exponential_smoother.h | 5 +++-- .../tracking/libs/tracking_loop_filter.cc | 8 +------ .../tracking/libs/tracking_loop_filter.h | 5 +++-- src/core/monitor/serdes_gnss_synchro.h | 11 +++------- 8 files changed, 19 insertions(+), 56 deletions(-) diff --git a/src/algorithms/PVT/libs/serdes_monitor_pvt.h b/src/algorithms/PVT/libs/serdes_monitor_pvt.h index 2e5b95a17..1b5ca11ca 100644 --- a/src/algorithms/PVT/libs/serdes_monitor_pvt.h +++ b/src/algorithms/PVT/libs/serdes_monitor_pvt.h @@ -55,7 +55,6 @@ public: // google::protobuf::ShutdownProtobufLibrary(); } - /* inline Serdes_Monitor_Pvt(Serdes_Monitor_Pvt&& other) //!< Copy constructor { this->monitor_ = other.monitor_; @@ -63,12 +62,7 @@ public: inline Serdes_Monitor_Pvt& operator=(const Serdes_Monitor_Pvt& rhs) //!< Copy assignment operator { - // Only do assignment if RHS is a different object from this. - if (this != &rhs) - { - // Deallocate, allocate new space, copy values... - this->monitor_ = rhs.monitor_; - } + this->monitor_ = rhs.monitor_; return *this; } @@ -84,7 +78,7 @@ public: this->monitor_ = std::move(other.monitor_); } return *this; - }*/ + } inline std::string createProtobuffer(const Monitor_Pvt& monitor) //!< Serialization into a string { diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc index 9656f57d0..165890e66 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc @@ -31,11 +31,8 @@ #include "beamformer.h" #include -#include -#define GNSS_SDR_BEAMFORMER_CHANNELS 8 - beamformer_sptr make_beamformer_sptr() { return beamformer_sptr(new beamformer()); @@ -47,22 +44,6 @@ beamformer::beamformer() gr::io_signature::make(GNSS_SDR_BEAMFORMER_CHANNELS, GNSS_SDR_BEAMFORMER_CHANNELS, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(gr_complex))) { - //initialize weight vector - - if (posix_memalign(reinterpret_cast(&weight_vector), 16, GNSS_SDR_BEAMFORMER_CHANNELS * sizeof(gr_complex)) == 0) - { - }; - - for (int i = 0; i < GNSS_SDR_BEAMFORMER_CHANNELS; i++) - { - weight_vector[i] = gr_complex(1, 0); - } -} - - -beamformer::~beamformer() -{ - free(weight_vector); } @@ -86,7 +67,7 @@ int beamformer::work(int noutput_items, gr_vector_const_void_star &input_items, for (int n = 0; n < noutput_items; n++) { sum = gr_complex(0, 0); - for (int i = 0; i < GNSS_SDR_BEAMFORMER_CHANNELS; i++) + for (int i = 0; i < weight_vector.size(); i++) { sum = sum + (reinterpret_cast(input_items[i]))[n] * weight_vector[i]; } diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h index b9cdd247a..1dcf76ef5 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.h +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.h @@ -32,26 +32,30 @@ #define GNSS_SDR_BEAMFORMER_H #include +#include class beamformer; + using beamformer_sptr = boost::shared_ptr; beamformer_sptr make_beamformer_sptr(); +const int GNSS_SDR_BEAMFORMER_CHANNELS = 8; + /*! * \brief This class implements a real-time software-defined spatial filter using the CTTC GNSS experimental antenna array input and a set of dynamically reloadable weights */ class beamformer : public gr::sync_block { public: - ~beamformer(); + ~beamformer() = default; int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); private: friend beamformer_sptr make_beamformer_sptr(); beamformer(); - gr_complex *weight_vector; + std::vector weight_vector = std::vector(GNSS_SDR_BEAMFORMER_CHANNELS, gr_complex(1.0, 0.0)); }; #endif diff --git a/src/algorithms/tracking/libs/exponential_smoother.cc b/src/algorithms/tracking/libs/exponential_smoother.cc index 5c531fcab..aa83a494e 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.cc +++ b/src/algorithms/tracking/libs/exponential_smoother.cc @@ -48,13 +48,6 @@ Exponential_Smoother::Exponential_Smoother() } -Exponential_Smoother::~Exponential_Smoother() = default; - - -// Move assignment operator -Exponential_Smoother& Exponential_Smoother::operator=(Exponential_Smoother&& other) = default; - - void Exponential_Smoother::set_alpha(float alpha) { alpha_ = alpha; diff --git a/src/algorithms/tracking/libs/exponential_smoother.h b/src/algorithms/tracking/libs/exponential_smoother.h index e0cc86af9..7b597e08b 100644 --- a/src/algorithms/tracking/libs/exponential_smoother.h +++ b/src/algorithms/tracking/libs/exponential_smoother.h @@ -48,7 +48,7 @@ class Exponential_Smoother { public: Exponential_Smoother(); //!< Constructor - ~Exponential_Smoother(); //!< Destructor + ~Exponential_Smoother() = default; //!< Destructor void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.001 void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200 void reset(); @@ -56,7 +56,8 @@ public: void set_offset(float offset); float smooth(float raw); double smooth(double raw); - Exponential_Smoother& operator=(Exponential_Smoother&& other); //!< Move assignment operator + Exponential_Smoother(Exponential_Smoother&&) = default; //!< Move operator + Exponential_Smoother& operator=(Exponential_Smoother&& /*other*/) = default; //!< Move assignment operator private: float alpha_; // takes value 0.0001 if not set int samples_for_initialization_; diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index 821efa67c..c01cca5f6 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -68,13 +68,6 @@ Tracking_loop_filter::Tracking_loop_filter() } -Tracking_loop_filter::~Tracking_loop_filter() = default; - - -// Move assignment operator -Tracking_loop_filter& Tracking_loop_filter::operator=(Tracking_loop_filter&& other) = default; - - float Tracking_loop_filter::apply(float current_input) { // Now apply the filter coefficients: @@ -238,6 +231,7 @@ void Tracking_loop_filter::set_update_interval(float update_interval) update_coefficients(); } + float Tracking_loop_filter::get_update_interval(void) const { return d_update_interval; diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.h b/src/algorithms/tracking/libs/tracking_loop_filter.h index 204b19825..b7dac4280 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.h +++ b/src/algorithms/tracking/libs/tracking_loop_filter.h @@ -45,13 +45,14 @@ class Tracking_loop_filter { public: Tracking_loop_filter(); - ~Tracking_loop_filter(); + ~Tracking_loop_filter() = default; Tracking_loop_filter(float update_interval, float noise_bandwidth, int loop_order = 2, bool include_last_integrator = false); - Tracking_loop_filter& operator=(Tracking_loop_filter&& other); //!< Move assignment operator + Tracking_loop_filter(Tracking_loop_filter&&) = default; //!< Move operator + Tracking_loop_filter& operator=(Tracking_loop_filter&& /*other*/) = default; //!< Move assignment operator float get_noise_bandwidth(void) const; float get_update_interval(void) const; diff --git a/src/core/monitor/serdes_gnss_synchro.h b/src/core/monitor/serdes_gnss_synchro.h index 935c292de..6cf746636 100644 --- a/src/core/monitor/serdes_gnss_synchro.h +++ b/src/core/monitor/serdes_gnss_synchro.h @@ -56,7 +56,7 @@ public: { google::protobuf::ShutdownProtobufLibrary(); } - /* + inline Serdes_Gnss_Synchro(Serdes_Gnss_Synchro&& other) //!< Copy constructor { this->observables = other.observables; @@ -64,12 +64,7 @@ public: inline Serdes_Gnss_Synchro& operator=(const Serdes_Gnss_Synchro& rhs) //!< Copy assignment operator { - // Only do assignment if RHS is a different object from this. - if (this != &rhs) - { - // Deallocate, allocate new space, copy values... - this->observables = rhs.observables; - } + this->observables = rhs.observables; return *this; } @@ -85,7 +80,7 @@ public: this->observables = std::move(other.observables); } return *this; - }*/ + } inline std::string createProtobuffer(const std::vector& vgs) //!< Serialization into a string { From 8de5ea8737c9a2fb9ba3a3bdab70530bf353c946 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 21:50:03 +0200 Subject: [PATCH 20/24] Fix comparison --- src/algorithms/input_filter/gnuradio_blocks/beamformer.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc index 165890e66..726ae6bd2 100644 --- a/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc +++ b/src/algorithms/input_filter/gnuradio_blocks/beamformer.cc @@ -67,7 +67,7 @@ int beamformer::work(int noutput_items, gr_vector_const_void_star &input_items, for (int n = 0; n < noutput_items; n++) { sum = gr_complex(0, 0); - for (int i = 0; i < weight_vector.size(); i++) + for (unsigned int i = 0; i < weight_vector.size(); i++) { sum = sum + (reinterpret_cast(input_items[i]))[n] * weight_vector[i]; } From 24007b59e52ff14d1c2c3bd5183bf25608a4ccee Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 7 Jul 2019 22:36:29 +0200 Subject: [PATCH 21/24] Remove unused includes, fix docs --- src/core/system_parameters/Beidou_B1I.h | 15 +++++++-------- src/core/system_parameters/Beidou_B3I.h | 5 ++--- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index bdc5ff40f..a341d9a2c 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -34,8 +34,7 @@ #include "MATH_CONSTANTS.h" #include -#include // std::pair -#include +#include // Physical constants const double BEIDOU_C_M_S = 299792458.0; //!< The speed of light, [m/s] @@ -48,12 +47,12 @@ const double BEIDOU_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2) // carrier and code frequencies -const double BEIDOU_B1I_FREQ_HZ = 1.561098e9; //!< b1I [Hz] -const double BEIDOU_B1I_CODE_RATE_HZ = 2.046e6; //!< beidou b1I code rate [chips/s] -const double BEIDOU_B1I_CODE_LENGTH_CHIPS = 2046.0; //!< beidou b1I code length [chips] -const double BEIDOU_B1I_CODE_PERIOD = 0.001; //!< beidou b1I code period [seconds] -const uint32_t BEIDOU_B1I_CODE_PERIOD_MS = 1; //!< beidou b1I L1 C/A code period [ms] -const double BEIDOU_B1I_CHIP_PERIOD = 4.8875e-07; //!< beidou b1I chip period [seconds] +const double BEIDOU_B1I_FREQ_HZ = 1.561098e9; //!< B1I [Hz] +const double BEIDOU_B1I_CODE_RATE_HZ = 2.046e6; //!< Beidou B1I code rate [chips/s] +const double BEIDOU_B1I_CODE_LENGTH_CHIPS = 2046.0; //!< Beidou B1I code length [chips] +const double BEIDOU_B1I_CODE_PERIOD = 0.001; //!< Beidou B1I code period [seconds] +const uint32_t BEIDOU_B1I_CODE_PERIOD_MS = 1; //!< Beidou B1I code period [ms] +const double BEIDOU_B1I_CHIP_PERIOD = 4.8875e-07; //!< Beidou B1I chip period [seconds] const int32_t BEIDOU_B1I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B1I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B1I_SECONDARY_CODE_STR = "00000100110101001110"; diff --git a/src/core/system_parameters/Beidou_B3I.h b/src/core/system_parameters/Beidou_B3I.h index 3eceab185..bf77457f5 100644 --- a/src/core/system_parameters/Beidou_B3I.h +++ b/src/core/system_parameters/Beidou_B3I.h @@ -33,15 +33,14 @@ #include "MATH_CONSTANTS.h" #include -#include // std::pair -#include +#include // carrier and code frequencies const double BEIDOU_B3I_FREQ_HZ = 1.268520e9; //!< BeiDou B3I [Hz] const double BEIDOU_B3I_CODE_RATE_HZ = 10.23e6; //!< BeiDou B3I code rate [chips/s] const double BEIDOU_B3I_CODE_LENGTH_CHIPS = 10230.0; //!< BeiDou B3I code length [chips] const double BEIDOU_B3I_CODE_PERIOD = 0.001; //!< BeiDou B3I code period [seconds] -const uint32_t BEIDOU_B3I_CODE_PERIOD_MS = 1; //!< GPS L1 C/A code period [ms] +const uint32_t BEIDOU_B3I_CODE_PERIOD_MS = 1; //!< BeiDou B3I code period [ms] const int32_t BEIDOU_B3I_SECONDARY_CODE_LENGTH = 20; const std::string BEIDOU_B3I_SECONDARY_CODE = "00000100110101001110"; const std::string BEIDOU_B3I_SECONDARY_CODE_STR = "00000100110101001110"; From faeb99bf6f057811c094843b09ff7c59967be926 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Jul 2019 20:00:29 +0200 Subject: [PATCH 22/24] Fix GLONASS L1 nav message decoding (fixes #289) --- .../glonass_l1_ca_telemetry_decoder_gs.cc | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index 3f6fdabcb..e31ae740a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -100,13 +100,11 @@ glonass_l1_ca_telemetry_decoder_gs::glonass_l1_ca_telemetry_decoder_gs( } } - d_symbol_history.set_capacity(GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS); + d_symbol_history.set_capacity(GLONASS_GNAV_STRING_SYMBOLS); d_sample_counter = 0ULL; d_stat = 0; d_preamble_index = 0ULL; - d_flag_frame_sync = false; - d_flag_parity = false; d_TOW_at_current_symbol = 0; Flag_valid_word = false; @@ -289,13 +287,12 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu d_flag_preamble = false; - if (static_cast(d_symbol_history.size()) == d_symbols_per_preamble) + if (static_cast(d_symbol_history.size()) >= d_symbols_per_preamble) { // ******* preamble correlation ******** - int i = 0; - for (const auto &iter : d_symbol_history) + for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - if (iter.Prompt_I < 0.0) // symbols clipping + if (d_symbol_history[i].Prompt_I < 0.0) // symbols clipping { corr_value -= d_preambles_symbols[i]; } @@ -303,7 +300,6 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu { corr_value += d_preambles_symbols[i]; } - i++; } } From d6dd4cad7793888c887f67b89be055d0d3c28053 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Jul 2019 20:09:34 +0200 Subject: [PATCH 23/24] Fix GLONASS L2 nav message decoding --- .../glonass_l2_ca_telemetry_decoder_gs.cc | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index e26592279..053596048 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -100,7 +100,7 @@ glonass_l2_ca_telemetry_decoder_gs::glonass_l2_ca_telemetry_decoder_gs( } } - d_symbol_history.set_capacity(GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS); + d_symbol_history.set_capacity(GLONASS_GNAV_STRING_SYMBOLS); d_sample_counter = 0ULL; d_stat = 0; d_preamble_index = 0ULL; @@ -289,13 +289,12 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu d_flag_preamble = false; - if (d_symbol_history.size() == GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) + if (static_cast(d_symbol_history.size()) >= d_symbols_per_preamble) { // ******* preamble correlation ******** - int i = 0; - for (const auto &iter : d_symbol_history) + for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - if (iter.Prompt_I < 0.0) // symbols clipping + if (d_symbol_history.[i].Prompt_I < 0.0) // symbols clipping { corr_value -= d_preambles_symbols[i]; } @@ -303,7 +302,6 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu { corr_value += d_preambles_symbols[i]; } - i++; } } From 38559628e890efe984551fb7a74c57710b7c0c8c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 9 Jul 2019 20:21:58 +0200 Subject: [PATCH 24/24] Fix bug --- .../gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index 053596048..3fce9c02e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -294,7 +294,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu // ******* preamble correlation ******** for (int32_t i = 0; i < d_symbols_per_preamble; i++) { - if (d_symbol_history.[i].Prompt_I < 0.0) // symbols clipping + if (d_symbol_history[i].Prompt_I < 0.0) // symbols clipping { corr_value -= d_preambles_symbols[i]; }