From e2c78ccdfa5b1a837923dcf0c88fe554f4ae988c Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 11 Jul 2018 19:38:08 +0200 Subject: [PATCH] Simplification of the GPS L1 CA telemetry decoder and synchronizer. Improving preamble detection reliability --- .../gps_l1_ca_telemetry_decoder_cc.cc | 295 ++++++++++-------- .../gps_l1_ca_telemetry_decoder_cc.h | 24 +- 2 files changed, 172 insertions(+), 147 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 78ff3aaa9..e509a3b90 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -83,26 +83,20 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( } } d_stat = 0; - d_symbol_accumulator = 0; - d_symbol_accumulator_counter = 0; - d_frame_bit_index = 0; d_flag_frame_sync = false; - d_GPS_frame_4bytes = 0; d_prev_GPS_frame_4bytes = 0; - d_flag_parity = false; d_TOW_at_Preamble_ms = 0; flag_TOW_set = false; d_flag_preamble = false; d_flag_new_tow_available = false; - d_word_number = 0; d_channel = 0; flag_PLL_180_deg_phase_locked = false; d_preamble_time_samples = 0; d_TOW_at_current_symbol_ms = 0; - d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS + 1); // Change fixed buffer size - d_symbol_history.clear(); // Clear all the elements in the buffer - d_make_correlation = true; - d_symbol_counter_corr = 0; + d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size + d_symbol_history.clear(); // Clear all the elements in the buffer + d_crc_error_synchronization_counter = 0; + d_current_subframe_symbol = 0; } @@ -150,9 +144,10 @@ bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword) void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite) { + d_nav.reset(); d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite; - d_GPS_FSM.i_satellite_PRN = d_satellite.get_PRN(); + d_nav.i_satellite_PRN = d_satellite.get_PRN(); DLOG(INFO) << "Navigation Satellite set to " << d_satellite; } @@ -160,7 +155,7 @@ void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satelli void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel) { d_channel = channel; - d_GPS_FSM.i_channel_ID = channel; + d_nav.i_channel_ID = channel; DLOG(INFO) << "Navigation channel set to " << channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) @@ -186,10 +181,128 @@ void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel) } +bool gps_l1_ca_telemetry_decoder_cc::decode_subframe() +{ + char subframe[GPS_SUBFRAME_LENGTH]; + + int symbol_accumulator_counter = 0; + int frame_bit_index = 0; + int word_index = 0; + uint32_t GPS_frame_4bytes = 0; + float symbol_accumulator = 0; + bool subframe_synchro_confirmation = false; + bool CRC_ok = true; + + for (int n = 0; n < GPS_SUBFRAME_MS; n++) + { + //******* SYMBOL TO BIT ******* + // extended correlation to bit period is enabled in tracking! + symbol_accumulator += d_subframe_symbols[n]; // accumulate the input value in d_symbol_accumulator + symbol_accumulator_counter++; + if (symbol_accumulator_counter == 20) + { + //symbol to bit + if (symbol_accumulator > 0) GPS_frame_4bytes += 1; //insert the telemetry bit in LSB + symbol_accumulator = 0; + symbol_accumulator_counter = 0; + + //******* bits to words ****** + frame_bit_index++; + if (frame_bit_index == 30) + { + frame_bit_index = 0; + // parity check + // Each word in wordbuff is composed of: + // Bits 0 to 29 = the GPS data word + // Bits 30 to 31 = 2 LSBs of the GPS word ahead. + // prepare the extended frame [-2 -1 0 ... 30] + if (d_prev_GPS_frame_4bytes & 0x00000001) + { + GPS_frame_4bytes = GPS_frame_4bytes | 0x40000000; + } + if (d_prev_GPS_frame_4bytes & 0x00000002) + { + GPS_frame_4bytes = GPS_frame_4bytes | 0x80000000; + } + /* Check that the 2 most recently logged words pass parity. Have to first + invert the data bits according to bit 30 of the previous word. */ + if (GPS_frame_4bytes & 0x40000000) + { + GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR) + } + if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(GPS_frame_4bytes)) + { + subframe_synchro_confirmation = true; + } + else + { + //std::cout << "word invalid sat " << this->d_satellite << std::endl; + CRC_ok = false; + //break; + } + //add word to subframe + // insert the word in the correct position of the subframe + std::memcpy(&subframe[word_index * GPS_WORD_LENGTH], &GPS_frame_4bytes, sizeof(uint32_t)); + word_index++; + d_prev_GPS_frame_4bytes = GPS_frame_4bytes; // save the actual frame + GPS_frame_4bytes = 0; + } + else + { + GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word + } + } + } + + + //decode subframe + // NEW GPS SUBFRAME HAS ARRIVED! + if (CRC_ok) + { + int subframe_ID = d_nav.subframe_decoder(subframe); //decode the subframe + std::cout << "New GPS NAV message received in channel " << this->d_channel << ": " + << "subframe " + << subframe_ID << " from satellite " + << Gnss_Satellite(std::string("GPS"), d_nav.i_satellite_PRN) << std::endl; + + switch (subframe_ID) + { + case 3: //we have a new set of ephemeris data for the current SV + if (d_nav.satellite_validation() == true) + { + // get ephemeris object for this SV (mandatory) + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 4: // Possible IONOSPHERE and UTC model update (page 18) + if (d_nav.flag_iono_valid == true) + { + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_iono()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + if (d_nav.flag_utc_model_valid == true) + { + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_utc_model()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 5: + // get almanac (if available) + //TODO: implement almanac reader in navigation_message + break; + default: + break; + } + d_flag_new_tow_available = true; + } + + return subframe_synchro_confirmation; +} + int gps_l1_ca_telemetry_decoder_cc::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) { - int corr_value = 0; int preamble_diff_ms = 0; Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // Get the output buffer pointer @@ -198,15 +311,25 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block //1. Copy the current tracking output current_symbol = in[0][0]; + + //record the oldest subframe symbol before inserting a new symbol into the circular buffer + if (d_current_subframe_symbol < GPS_SUBFRAME_MS and d_symbol_history.size() > 0) + { + d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history.at(0).Prompt_I; + d_current_subframe_symbol++; + } + d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue consume_each(1); - unsigned int required_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS; d_flag_preamble = false; - if ((d_symbol_history.size() > required_symbols) and (d_make_correlation or !d_flag_frame_sync)) + + //******* preamble correlation ******** + int corr_value = 0; + if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) // and (d_make_correlation or !d_flag_frame_sync)) { - //******* preamble correlation ******** + // std::cout << "-------\n"; for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++) { if (d_symbol_history.at(i).Flag_valid_symbol_output == true) @@ -221,38 +344,27 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ } } } - if (std::abs(corr_value) >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS) - { - d_symbol_counter_corr++; - } } + //******* frame sync ****************** if (std::abs(corr_value) == GPS_CA_PREAMBLE_LENGTH_SYMBOLS) { //TODO: Rewrite with state machine if (d_stat == 0) { - d_GPS_FSM.Event_gps_word_preamble(); //record the preamble sample stamp d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the preamble sample stamp DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter; - //sync the symbol to bits integrator - d_symbol_accumulator = 0; - d_symbol_accumulator_counter = 0; - d_frame_bit_index = 0; d_stat = 1; // enter into frame pre-detection status } else if (d_stat == 1) //check 6 seconds of preamble separation { preamble_diff_ms = std::round(((static_cast(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); - if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1) + if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) % GPS_SUBFRAME_MS == 0) { DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite; - d_GPS_FSM.Event_gps_word_preamble(); d_flag_preamble = true; - d_make_correlation = false; - d_symbol_counter_corr = 0; d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the PRN start sample index associated to the preamble if (!d_flag_frame_sync) { @@ -269,131 +381,46 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << static_cast(d_preamble_time_samples) / static_cast(d_symbol_history.at(0).fs) << " [s]"; } + + //try to decode the subframe: + if (decode_subframe() == false) + { + d_crc_error_synchronization_counter++; + if (d_crc_error_synchronization_counter > 3) + { + DLOG(INFO) << "TOO MANY CRC ERRORS: Lost of frame sync SAT " << this->d_satellite << std::endl; + d_stat = 0; //lost of frame sync + d_flag_frame_sync = false; + flag_TOW_set = false; + d_crc_error_synchronization_counter = 0; + } + } + d_current_subframe_symbol = 0; } } } else { - d_symbol_counter_corr++; - if (d_symbol_counter_corr > (GPS_SUBFRAME_MS - GPS_CA_TELEMETRY_SYMBOLS_PER_BIT)) - { - d_make_correlation = true; - } if (d_stat == 1) { preamble_diff_ms = round(((static_cast(d_symbol_history.at(0).Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); - if (preamble_diff_ms > GPS_SUBFRAME_MS + 1) + if (preamble_diff_ms > GPS_SUBFRAME_MS) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms; + // std::cout << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms << std::endl; d_stat = 0; //lost of frame sync d_flag_frame_sync = false; flag_TOW_set = false; - d_make_correlation = true; - d_symbol_counter_corr = 0; + d_current_subframe_symbol = 0; + d_crc_error_synchronization_counter = 0; } } } - //******* SYMBOL TO BIT ******* - if (d_symbol_history.at(0).Flag_valid_symbol_output == true) - { - // extended correlation to bit period is enabled in tracking! - d_symbol_accumulator += d_symbol_history.at(0).Prompt_I; // accumulate the input value in d_symbol_accumulator - d_symbol_accumulator_counter += d_symbol_history.at(0).correlation_length_ms; - } - if (d_symbol_accumulator_counter >= 20) - { - if (d_symbol_accumulator > 0) - { //symbol to bit - d_GPS_frame_4bytes += 1; //insert the telemetry bit in LSB - } - d_symbol_accumulator = 0; - d_symbol_accumulator_counter = 0; - //******* bits to words ****** - d_frame_bit_index++; - if (d_frame_bit_index == 30) - { - d_frame_bit_index = 0; - // parity check - // Each word in wordbuff is composed of: - // Bits 0 to 29 = the GPS data word - // Bits 30 to 31 = 2 LSBs of the GPS word ahead. - // prepare the extended frame [-2 -1 0 ... 30] - if (d_prev_GPS_frame_4bytes & 0x00000001) - { - d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x40000000; - } - if (d_prev_GPS_frame_4bytes & 0x00000002) - { - d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x80000000; - } - /* Check that the 2 most recently logged words pass parity. Have to first - invert the data bits according to bit 30 of the previous word. */ - if (d_GPS_frame_4bytes & 0x40000000) - { - d_GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR) - } - if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes)) - { - memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char) * 4); - //d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0; - d_GPS_FSM.Event_gps_word_valid(); - // send TLM data to PVT using asynchronous message queues - if (d_GPS_FSM.d_flag_new_subframe == true) - { - switch (d_GPS_FSM.d_subframe_ID) - { - case 3: //we have a new set of ephemeris data for the current SV - if (d_GPS_FSM.d_nav.satellite_validation() == true) - { - // get ephemeris object for this SV (mandatory) - std::shared_ptr tmp_obj = std::make_shared(d_GPS_FSM.d_nav.get_ephemeris()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - break; - case 4: // Possible IONOSPHERE and UTC model update (page 18) - if (d_GPS_FSM.d_nav.flag_iono_valid == true) - { - std::shared_ptr tmp_obj = std::make_shared(d_GPS_FSM.d_nav.get_iono()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - if (d_GPS_FSM.d_nav.flag_utc_model_valid == true) - { - std::shared_ptr tmp_obj = std::make_shared(d_GPS_FSM.d_nav.get_utc_model()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - break; - case 5: - // get almanac (if available) - //TODO: implement almanac reader in navigation_message - break; - default: - break; - } - d_GPS_FSM.clear_flag_new_subframe(); - d_flag_new_tow_available = true; - } - - d_flag_parity = true; - } - else - { - d_GPS_FSM.Event_gps_word_invalid(); - d_flag_parity = false; - } - d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame - d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0; - } - else - { - d_GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word - } - } - //2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_flag_new_tow_available == true) { - d_TOW_at_current_symbol_ms = static_cast(d_GPS_FSM.d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS; + d_TOW_at_current_symbol_ms = static_cast(d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS; d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms; flag_TOW_set = true; d_flag_new_tow_available = false; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index 52f1f1049..5c6a07f52 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -72,7 +72,9 @@ private: bool gps_word_parityCheck(unsigned int gpsword); - // class private vars + bool decode_subframe(); + bool new_decoder(); + int d_crc_error_synchronization_counter; int *d_preambles_symbols; unsigned int d_stat; @@ -80,26 +82,22 @@ private: // symbols boost::circular_buffer d_symbol_history; - - double d_symbol_accumulator; - short int d_symbol_accumulator_counter; - - // symbol counting - bool d_make_correlation; - unsigned int d_symbol_counter_corr; + float d_subframe_symbols[GPS_SUBFRAME_MS]; //symbols per subframe + int d_current_subframe_symbol; + //double d_symbol_accumulator; + //short int d_symbol_accumulator_counter; //bits and frame - unsigned short int d_frame_bit_index; - unsigned int d_GPS_frame_4bytes; + //unsigned short int d_frame_bit_index; + //unsigned int d_GPS_frame_4bytes; unsigned int d_prev_GPS_frame_4bytes; - bool d_flag_parity; + //bool d_flag_parity; bool d_flag_preamble; bool d_flag_new_tow_available; - int d_word_number; + //int d_word_number; // navigation message vars Gps_Navigation_Message d_nav; - GpsL1CaSubframeFsm d_GPS_FSM; bool d_dump; Gnss_Satellite d_satellite;