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_ */