From 0c4b8b25f14c5755204feb763ea476178a564110 Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Sun, 23 Dec 2018 12:51:28 -0600 Subject: [PATCH] bds_b1i: Adding new tel dec block structure --- conf/gnss-sdr_BDS_B1I_byte.conf | 8 +- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- .../libs/rtklib/rtklib_conversions.cc | 18 +- .../beidou_b1i_telemetry_decoder_cc.cc | 143 ++++++++++++---- .../beidou_b1i_telemetry_decoder_cc.h | 10 +- src/core/system_parameters/Beidou_B1I.h | 4 +- .../beidou_dnav_navigation_message.cc | 158 ++++++++++++------ .../beidou_dnav_navigation_message.h | 44 +++-- 8 files changed, 272 insertions(+), 115 deletions(-) diff --git a/conf/gnss-sdr_BDS_B1I_byte.conf b/conf/gnss-sdr_BDS_B1I_byte.conf index 1ec800c1e..0c55931a1 100644 --- a/conf/gnss-sdr_BDS_B1I_byte.conf +++ b/conf/gnss-sdr_BDS_B1I_byte.conf @@ -73,9 +73,9 @@ Channel9.satellite = 5; Acquisition_B1.implementation=BEIDOU_B1I_PCPS_Acquisition Acquisition_B1.item_type=gr_complex Acquisition_B1.coherent_integration_time_ms=1 -Acquisition_B1.threshold=0.004 +Acquisition_B1.threshold=0.0038 ;Acquisition_B1.pfa=0.0000001; -Acquisition_B1.doppler_max=12000 +Acquisition_B1.doppler_max=15000 Acquisition_B1.doppler_step=100 Acquisition_B1.dump=true Acquisition_B1.dump_filename=./bds_acq @@ -87,8 +87,8 @@ Acquisition_B1.bit_transition_flag = false; ;######### TRACKING GLOBAL CONFIG ############ Tracking_B1.implementation=BEIDOU_B1I_DLL_PLL_Tracking Tracking_B1.item_type=gr_complex -Tracking_B1.pll_bw_hz=30.0; -Tracking_B1.dll_bw_hz=3.0; +Tracking_B1.pll_bw_hz=25.0; +Tracking_B1.dll_bw_hz=2.50; Tracking_B1.dump=true; Tracking_B1.dump_filename=./epl_tracking_ch_ diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 32e8421a8..28a770faa 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -775,7 +775,7 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}}; obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, - beidou_ephemeris_iter->second.i_BEIDOU_week, + beidou_ephemeris_iter->second.i_BEIDOU_week + 1356, 0); valid_obs++; } diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 876dff92b..719e50b47 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -75,14 +75,16 @@ obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro // Mote that BeiDou week numbers do not need adjustment for foreseeable future. Consider change // to more elegant solution - if(gnss_synchro.System == 'C') - { - rtklib_obs.time = bdt2gpst(bdt2time(week, gnss_synchro.RX_time)); - } - else - { - rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); - } +// if(gnss_synchro.System == 'C') +// { +// rtklib_obs.time = bdt2gpst(bdt2time(week, gnss_synchro.RX_time)); +// } +// else +// { +// rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); +// } +// + rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); rtklib_obs.rcv = 1; return rtklib_obs; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.cc index cbbc961dd..452f74107 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.cc @@ -76,21 +76,21 @@ beidou_b1i_telemetry_decoder_cc::beidou_b1i_telemetry_decoder_cc( d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; // preamble symbols to samples - d_secondary_code_samples = static_cast(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); + 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_samples_per_symbol; - d_subframe_length_symbols = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS - BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_subframe_length_symbols = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; // 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_samples[i] = 1; + d_secondary_code_symbols[i] = 1; } else { - d_secondary_code_samples[i] = -1; + d_secondary_code_symbols[i] = -1; } } @@ -103,7 +103,7 @@ beidou_b1i_telemetry_decoder_cc::beidou_b1i_telemetry_decoder_cc( { for (uint32_t j = 0; j < d_samples_per_symbol; j++) { - d_preamble_samples[n] = d_secondary_code_samples[m]; + d_preamble_samples[n] = d_secondary_code_symbols[m]; n++; m++; m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; @@ -113,7 +113,7 @@ beidou_b1i_telemetry_decoder_cc::beidou_b1i_telemetry_decoder_cc( { for (uint32_t j = 0; j < d_samples_per_symbol; j++) { - d_preamble_samples[n] = -d_secondary_code_samples[m]; + d_preamble_samples[n] = -d_secondary_code_symbols[m]; n++; m++; m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; @@ -122,7 +122,7 @@ beidou_b1i_telemetry_decoder_cc::beidou_b1i_telemetry_decoder_cc( } d_subframe_symbols = static_cast(volk_gnsssdr_malloc(d_subframe_length_symbols * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS*d_samples_per_symbol; + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS*d_samples_per_symbol + d_samples_per_preamble; d_sample_counter = 0; d_stat = 0; d_preamble_index = 0; @@ -134,7 +134,7 @@ beidou_b1i_telemetry_decoder_cc::beidou_b1i_telemetry_decoder_cc( d_CRC_error_counter = 0; d_flag_preamble = false; d_channel = 0; - flag_TOW_set = false; + flag_SOW_set = false; } @@ -142,7 +142,7 @@ beidou_b1i_telemetry_decoder_cc::beidou_b1i_telemetry_decoder_cc( beidou_b1i_telemetry_decoder_cc::~beidou_b1i_telemetry_decoder_cc() { volk_gnsssdr_free(d_preamble_samples); - volk_gnsssdr_free(d_secondary_code_samples); + volk_gnsssdr_free(d_secondary_code_symbols); volk_gnsssdr_free(d_subframe_symbols); if (d_dump_file.is_open() == true) @@ -159,14 +159,95 @@ beidou_b1i_telemetry_decoder_cc::~beidou_b1i_telemetry_decoder_cc() } +void beidou_b1i_telemetry_decoder_cc::decode_bch15_11_01(int32_t *bits, int32_t *decbits) +{ + int bit, err, reg[4] = {1, 1, 1, 1}; + int errind[15] = {14, 13, 10, 12, 6, 9, 4, 11, 0, 5, 7, 8, 1, 3, 2}; + + for (unsigned int i = 0; i < 15; i++) + { + decbits[i] = bits[i]; + } + + for (unsigned int i = 0; i < 15; i++) + { + bit = reg[3]; + reg[3] = reg[2]; + reg[2] = reg[1]; + reg[1] = reg[0]; + reg[0] = bits[i] * bit; + reg[1] *= bit; + } + + err = errind[reg[0] + reg[1]*2 + reg[2]*4 + reg[3]*8]; + + if (err > 0) + { + decbits[err - 1] *= -1; + } +} + +void beidou_b1i_telemetry_decoder_cc::decode_word( + int32_t word_counter, + double* enc_word_symbols, + int32_t* dec_word_symbols) +{ + int32_t bitsbch[30], first_branch[15], second_branch[15]; + + if (word_counter == 1) + { + for (unsigned int j = 0; j < 30; j++) + { + dec_word_symbols[j] = (int32_t)(enc_word_symbols[j] > 0) ? (1) : (-1); + } + } + else + { + for (unsigned int r = 0; r < 2; r++) + { + for (unsigned int c = 0; c < 15; c++) + { + bitsbch[r*15 + c] = (int32_t)(enc_word_symbols[c*2 + r] > 0) ? (1) : (-1); + } + } + + decode_bch15_11_01(&bitsbch[0], first_branch); + decode_bch15_11_01(&bitsbch[15], second_branch); + + for (unsigned int j = 0; j < 11; j++) + { + dec_word_symbols[j] = first_branch[j]; + dec_word_symbols[j + 11] = second_branch[j]; + } + + for (unsigned int j = 0; j < 4; j++) + { + dec_word_symbols[j + 22] = first_branch[11 + j]; + dec_word_symbols[j + 26] = second_branch[11 + j]; + } + } + + +} + + void beidou_b1i_telemetry_decoder_cc::decode_subframe(double *frame_symbols, int32_t frame_length) { - // 1. Transform from symbols to bits + // 1. Transform from symbols to bits std::string data_bits; + int32_t dec_word_bits[30]; - // we want data_bits = frame_symbols[24:24+288] - for (uint32_t ii = 0; ii < (BEIDOU_DNAV_DATA_BITS); ii++) { - data_bits.push_back( (frame_symbols[ii] > 0) ? ('1') : ('0') ); + // Decode each word in subframe + for(uint32_t ii = 0; ii < BEIDOU_DNAV_WORDS_SUBFRAME; ii++) + { + // decode the word + decode_word((ii+1), &frame_symbols[ii*30], dec_word_bits); + + // Save word to string format + for (uint32_t jj = 0; jj < (BEIDOU_DNAV_WORD_LENGTH_BITS); jj++) + { + data_bits.push_back( (dec_word_bits[jj] > 0) ? ('1') : ('0') ); + } } d_nav.subframe_decoder(data_bits); @@ -207,10 +288,10 @@ void beidou_b1i_telemetry_decoder_cc::decode_subframe(double *frame_symbols, int } if (d_nav.have_new_almanac() == true) { - unsigned int slot_nbr = d_nav.i_alm_satellite_PRN; - 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 << " in slot number " << slot_nbr; +// unsigned int slot_nbr = d_nav.i_alm_satellite_PRN; +// 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; } } @@ -335,7 +416,7 @@ int beidou_b1i_telemetry_decoder_cc::general_work(int noutput_items __attribute_ for (uint32_t m = 0; m < d_samples_per_symbol; m++) { // because last symbol of the preamble is just received now! - d_subframe_symbols[i] += static_cast(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); + 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; } @@ -351,7 +432,7 @@ int beidou_b1i_telemetry_decoder_cc::general_work(int noutput_items __attribute_ for (uint32_t m = 0; m < d_samples_per_symbol; m++) { // because last symbol of the preamble is just received now! - d_subframe_symbols[i] -= static_cast(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); + 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; } @@ -359,7 +440,7 @@ int beidou_b1i_telemetry_decoder_cc::general_work(int noutput_items __attribute_ } //call the decoder - decode_subframe(d_frame_symbols, d_subframe_length_symbols); + decode_subframe(d_subframe_symbols, d_subframe_length_symbols); if (d_nav.flag_crc_test == true) { d_CRC_error_counter = 0; @@ -380,6 +461,7 @@ int beidou_b1i_telemetry_decoder_cc::general_work(int noutput_items __attribute_ LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; + flag_SOW_set = false; } } } @@ -387,27 +469,22 @@ int beidou_b1i_telemetry_decoder_cc::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_TOW_set == true) + if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true) //update TOW at the preamble instant { - if (d_nav.flag_SOW == true) - { - d_TOW_at_Preamble_ms = static_cast(d_nav.d_SOW * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * BEIDOU_B1I_CODE_PERIOD_MS); - d_nav.flag_SOW = 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);; - } + // 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); + 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);; + d_TOW_at_current_symbol_ms += static_cast(BEIDOU_B1I_CODE_PERIOD_MS); } - if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) + if (d_flag_frame_sync == true and flag_SOW_set == true) { current_symbol.Flag_valid_word = true; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.h index d11f85423..80931c46f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.h @@ -2,7 +2,8 @@ * \file beidou_b1i_telemetry_decoder_cc.h * \brief Implementation of an adapter of a BEIDOU BI1 DNAV data decoder block * to a TelemetryDecoderInterface - * \note Code added as part of GSoC 2018 program + * \details Code added as part of GSoC 2018 program. However new modifications included to mimic + * decoding of existing signals * \author Damian Miralles, 2018. dmiralles2009(at)gmail.com * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.es * @@ -78,11 +79,14 @@ private: beidou_b1i_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump); void decode_subframe(double *symbols, int32_t frame_length); + void decode_word(int32_t word_counter, double* enc_word_symbols, int32_t* dec_word_symbols); + void decode_bch15_11_01(int32_t *bits, int32_t *decbits); + //!< Preamble decoding unsigned short int d_preambles_symbols[BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS]; int32_t *d_preamble_samples; - int32_t *d_secondary_code_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; @@ -101,7 +105,7 @@ private: bool d_flag_frame_sync; //!< Indicate when a frame sync is achieved bool d_flag_preamble; //!< Flag indicating when preamble was found int32_t d_CRC_error_counter; //!< Number of failed CRC operations - bool flag_TOW_set; //!< Indicates when time of week is set + bool flag_SOW_set; //!< Indicates when time of week is set //!< Navigation Message variable Beidou_Dnav_Navigation_Message d_nav; diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index 7a0123a05..5f9bbd270 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -85,7 +85,7 @@ const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; // ************* 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 -const int BEIDOU_SUBFRAME_BITS = 300; //!< Number of bits per subframe in the NAV message [bits] +const int BEIDOU_DNAV_SUBFRAME_DATA_BITS = 300; //!< Number of bits per subframe in the NAV message [bits] const int BEIDOU_SUBFRAME_SECONDS = 6; //!< Subframe duration [seconds] const int BEIDOU_SUBFRAME_MS = 6000; //!< Subframe duration [miliseconds] const int BEIDOU_WORD_BITS = 30; //!< Number of bits per word in the NAV message [bits] @@ -96,6 +96,8 @@ const int BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS = 11; // ************** const double BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS = 300; const double BEIDOU_DNAV_SUBFRAME_SYMBOLS = 300; const double BEIDOU_DNAV_DATA_BITS = 300; +const double BEIDOU_DNAV_WORDS_SUBFRAME = 10; +const double BEIDOU_DNAV_WORD_LENGTH_BITS = 30; const double BEIDOU_B1I_SYMBOL_RATE_SPS = 50; //BEIDOU symbol rate const double BEIDOU_B1I_PREAMBLE_PERIOD_SYMBOLS = 300; diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.cc b/src/core/system_parameters/beidou_dnav_navigation_message.cc index bdb55dc5a..091e7edc9 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.cc +++ b/src/core/system_parameters/beidou_dnav_navigation_message.cc @@ -33,12 +33,20 @@ m * \file beidou_navigation_message.cc #include #include +#include // for boost::crc_basic, boost::crc_optimal +#include +#include #include void Beidou_Dnav_Navigation_Message::reset() { - b_valid_ephemeris_set_flag = false; + // Control variable for message decoding + flag_eph_valid = false; + flag_iono_valid = false; + flag_utc_model_valid = false; + flag_crc_test = false; + d_SOW = 0; d_SOW_SF1 = 0; d_SOW_SF2 = 0; @@ -176,11 +184,11 @@ void Beidou_Dnav_Navigation_Message::print_beidou_word_bytes(unsigned int BEIDOU std::cout << std::endl; } -bool Beidou_Dnav_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) +bool Beidou_Dnav_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) { bool value; - if (bits[BEIDOU_SUBFRAME_BITS - parameter[0].first] == 1) + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) { value = true; } @@ -191,7 +199,7 @@ bool Beidou_Dnav_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) +unsigned long int Beidou_Dnav_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector> parameter) { unsigned long int value = 0; int num_of_slices = parameter.size(); @@ -200,7 +208,7 @@ unsigned long int Beidou_Dnav_Navigation_Message::read_navigation_unsigned(std:: for (int j = 0; j < parameter[i].second; j++) { value <<= 1; //shift left - if (bits[BEIDOU_SUBFRAME_BITS - parameter[i].first - j] == 1) + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) { value += 1; // insert the bit } @@ -209,7 +217,7 @@ unsigned long int Beidou_Dnav_Navigation_Message::read_navigation_unsigned(std:: return value; } -signed long int Beidou_Dnav_Navigation_Message::read_navigation_signed(std::bitset bits, const std::vector> parameter) +signed long int Beidou_Dnav_Navigation_Message::read_navigation_signed(std::bitset bits, const std::vector> parameter) { signed long int value = 0; int num_of_slices = parameter.size(); @@ -218,7 +226,7 @@ signed long int Beidou_Dnav_Navigation_Message::read_navigation_signed(std::bits if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system { // read the MSB and perform the sign extension - if (bits[BEIDOU_SUBFRAME_BITS - parameter[0].first] == 1) + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) { value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable } @@ -233,7 +241,7 @@ signed long int Beidou_Dnav_Navigation_Message::read_navigation_signed(std::bits { value <<= 1; //shift left value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable) - if (bits[BEIDOU_SUBFRAME_BITS - parameter[i].first - j] == 1) + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) { value += 1; // insert the bit } @@ -243,7 +251,7 @@ signed long int Beidou_Dnav_Navigation_Message::read_navigation_signed(std::bits else // we assume we are in a 32 bits system { // read the MSB and perform the sign extension - if (bits[BEIDOU_SUBFRAME_BITS - parameter[0].first] == 1) + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) { value ^= 0xFFFFFFFF; } @@ -258,7 +266,7 @@ signed long int Beidou_Dnav_Navigation_Message::read_navigation_signed(std::bits { value <<= 1; //shift left value &= 0xFFFFFFFE; //reset the corresponding bit - if (bits[BEIDOU_SUBFRAME_BITS - parameter[i].first - j] == 1) + if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) { value += 1; // insert the bit } @@ -387,28 +395,16 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime) d_satvel_Z = d_satpos_Y * sin(i); } -int Beidou_Dnav_Navigation_Message::subframe_decoder(char *subframe) +int Beidou_Dnav_Navigation_Message::subframe_decoder(std::string const &subframe) { int subframe_ID = 0; - std::bitset mysubframe_bits; - - unsigned int beidou_word; - - std::bitset subframe_bits; - std::bitset word_bits; - for (int i = 0; i < 10; i++) - { - memcpy(&beidou_word, &subframe[i * 4], sizeof(char) * 4); - word_bits = std::bitset<(BEIDOU_WORD_BITS + 2) > (beidou_word); - for (int j = 0; j < BEIDOU_WORD_BITS; j++) - { - subframe_bits[BEIDOU_WORD_BITS * (9 - i) + j] = word_bits[j]; - } - - } + std::bitset subframe_bits(subframe); subframe_ID = static_cast(read_navigation_unsigned(subframe_bits, D1_FRAID)); + // Perform crc computtaion (tbd) + flag_crc_test = true; + // Decode all 5 sub-frames switch (subframe_ID) { @@ -476,13 +472,12 @@ int Beidou_Dnav_Navigation_Message::subframe_decoder(char *subframe) d_a1 = d_a1 * D1_A1_LSB; d_AODE = static_cast(read_navigation_unsigned(subframe_bits, D1_AODE)); + + // Set system flags for message reception + flag_sf_1 = true; flag_iono_valid = true; - //d_A_f0 = static_cast(read_navigation_signed(subframe_bits, A_F0)); - //d_A_f0 = d_A_f0 * A_F0_LSB; - //d_A_f1 = static_cast(read_navigation_signed(subframe_bits, A_F1)); - //d_A_f1 = d_A_f1 * A_F1_LSB; - //d_A_f2 = static_cast(read_navigation_signed(subframe_bits, A_F2)); - //d_A_f2 = d_A_f2 * A_F2_LSB; + flag_utc_model_valid = true; + flag_new_SOW_available = true; break; @@ -516,13 +511,9 @@ int Beidou_Dnav_Navigation_Message::subframe_decoder(char *subframe) d_Toe_sf2 = static_cast(read_navigation_unsigned(subframe_bits, D1_TOE_SF2)); d_Toe_sf2 = static_cast((static_cast(d_Toe_sf2) << 15)); -// d_SOW = d_SOW_SF2; // Set transmission time -// b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); -// b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); -// b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); -// b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG); -// i_AODO = static_cast(read_navigation_unsigned(subframe_bits, AODO)); -// i_AODO = i_AODO * AODO_LSB; + // Set system flags for message reception + flag_sf_2 = true; + flag_new_SOW_available = true; break; @@ -554,14 +545,9 @@ int Beidou_Dnav_Navigation_Message::subframe_decoder(char *subframe) d_OMEGA = static_cast(read_navigation_signed(subframe_bits, D1_OMEGA)); d_OMEGA = d_OMEGA * D1_OMEGA_LSB; - //d_SOW_SF3 = static_cast(read_navigation_unsigned(subframe_bits, SOW)); - - //d_SOW_SF3 = d_SOW_SF3 * 6; - //d_SOW = d_SOW_SF3; // Set transmission time - //b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); - //b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); - //b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); - //d_AODE_SF3 = static_cast(read_navigation_unsigned(subframe_bits, AODE_SF3)); + // Set system flags for message reception + flag_sf_3 = true; + flag_new_SOW_available = true; break; @@ -599,6 +585,11 @@ int Beidou_Dnav_Navigation_Message::subframe_decoder(char *subframe) d_M0_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_M0)); d_M0_ALMANAC = d_M0_ALMANAC * D1_M0_ALMANAC_LSB; + // Set system flags for message reception + flag_sf_4 = true; + flag_new_SOW_available = true; + + break; /* b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); @@ -762,7 +753,9 @@ int Beidou_Dnav_Navigation_Message::subframe_decoder(char *subframe) d_A1UTC = d_A1UTC * D1_A1UTC_LSB; } - + // Set system flags for message reception + flag_sf_5 = true; + flag_new_SOW_available = true; break; @@ -904,10 +897,73 @@ Beidou_Dnav_Utc_Model Beidou_Dnav_Navigation_Message::get_utc_model() return utc_model; } + +bool Beidou_Dnav_Navigation_Message::have_new_ephemeris() // Check if we have a new ephemeris stored in the galileo navigation class +{ + if ((flag_sf_1 == true) and (flag_sf_2 == true) and (flag_sf_3 == true)) + { + // if all ephemeris pages have the same IOD, then they belong to the same block + if (d_previous_aode != d_AODE) + { + flag_sf_1 = false; // clear the flag + flag_sf_2 = false; // clear the flag + flag_sf_3 = false; // clear the flag + flag_eph_valid = true; + // Update the time of ephemeris information + d_previous_aode = d_AODE; + DLOG(INFO) << "Beidou Dnav Ephemeris (1, 2) have been received and belong to the same batch" << std::endl; + return true; + + } + } + return false; +} + + +bool Beidou_Dnav_Navigation_Message::have_new_iono() +{ + // the condition on flag_utc_model is added to have a time stamp for iono + if (flag_iono_valid == true) + { + flag_iono_valid = false; // clear the flag + return true; + } + + return false; +} + + +bool Beidou_Dnav_Navigation_Message::have_new_utc_model() +{ + if (flag_utc_model_valid == true) + { + flag_utc_model_valid = false; // clear the flag + return true; + } + + return false; +} + + +bool Beidou_Dnav_Navigation_Message::have_new_almanac() +{ + if ((flag_sf_4 == true) and (flag_sf_5 == true)) + { + // All almanac have been received + flag_sf_4 = false; + flag_sf_5 = false; + + return true; + } + + return false; +} + + bool Beidou_Dnav_Navigation_Message::satellite_validation() { bool flag_data_valid = false; - b_valid_ephemeris_set_flag = false; + flag_eph_valid = false; // First Step: // check Issue Of Ephemeris Data (AODE AODC..) to find a possible interrupted reception @@ -917,7 +973,7 @@ bool Beidou_Dnav_Navigation_Message::satellite_validation() if (d_AODC!= -1) { flag_data_valid = true; - b_valid_ephemeris_set_flag = true; + flag_eph_valid = true; } } return flag_data_valid; diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.h b/src/core/system_parameters/beidou_dnav_navigation_message.h index d72c4d4c2..6ffdb882a 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.h +++ b/src/core/system_parameters/beidou_dnav_navigation_message.h @@ -54,9 +54,9 @@ class Beidou_Dnav_Navigation_Message { private: - unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); - signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); - bool read_navigation_bool(std::bitset bits, const std::vector> parameter); + unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); + signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); + bool read_navigation_bool(std::bitset bits, const std::vector> parameter); void print_beidou_word_bytes(unsigned int BEIDOU_word); /* * Accounts for the beginning or end of week crossover @@ -68,13 +68,18 @@ private: double check_t(double time); public: - bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check + // System flags for data processing + bool flag_eph_valid; + bool flag_utc_model_valid; + bool flag_iono_valid; bool flag_sf_1; bool flag_sf_2; bool flag_sf_3; bool flag_sf_4; bool flag_sf_5; - bool flag_SOW; + bool flag_new_SOW_available; + bool flag_crc_test; + double d_previous_aode; //broadcast orbit 1 double d_SOW; //!< Time of BeiDou Week of the ephemeris set (taken from subframes SOW) [s] @@ -172,7 +177,6 @@ public: double d_subframe_timestamp_ms; //[ms] // Ionospheric parameters - bool flag_iono_valid; //!< If set, it indicates that the ionospheric parameters are filled (page 18 has arrived and decoded) double d_alpha0; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s] double d_alpha1; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle] double d_alpha2; //!< Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2] @@ -183,7 +187,6 @@ public: double d_beta3; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3] // UTC parameters - bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled double d_A2UTC; double d_A1UTC; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200E) [s/s] double d_A0UTC; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200E) [s] @@ -213,12 +216,6 @@ public: int almanac_WN; double d_toa2; - - - - - - // Satellite velocity double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m] double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m] @@ -247,7 +244,7 @@ public: /*! * \brief Decodes the GPS NAV message */ - int subframe_decoder(char *subframe); + int subframe_decoder(std::string const &subframe); /*! * \brief Computes the position of the satellite @@ -270,6 +267,25 @@ public: bool satellite_validation(); + /* + * \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed + */ + bool have_new_ephemeris(); + + /* + * \brief Returns true if new Iono model has arrived. The flag is set to false when the function is executed + */ + bool have_new_iono(); + + /* + * \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed + */ + bool have_new_utc_model(); + + /* + * \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed + */ + bool have_new_almanac(); /*! * Default constructor */