diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index a6f0bba1c..814edef22 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -106,6 +106,7 @@ Channel::Channel(ConfigurationInterface* configuration, uint32_t channel, std::s channel_fsm_->set_acquisition(acq_); channel_fsm_->set_tracking(trk_); + channel_fsm_->set_telemetry(nav_); channel_fsm_->set_channel(channel_); channel_fsm_->set_queue(queue_); @@ -129,6 +130,8 @@ void Channel::connect(gr::top_block_sptr top_block) //Synchronous ports top_block->connect(trk_->get_right_block(), 0, nav_->get_left_block(), 0); + // Message ports + top_block->msg_connect(nav_->get_left_block(), pmt::mp("telemetry_to_trk"), trk_->get_right_block(), pmt::mp("telemetry_to_trk")); DLOG(INFO) << "tracking -> telemetry_decoder"; // Message ports diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index cd8e97b26..533bbd2d4 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -157,7 +157,11 @@ void ChannelFsm::set_tracking(std::shared_ptr tracking) trk_ = std::move(tracking); } - +void ChannelFsm::set_telemetry(std::shared_ptr telemetry) +{ + std::lock_guard lk(mx); + nav_ = std::move(telemetry); +} void ChannelFsm::set_queue(gr::msg_queue::sptr queue) { std::lock_guard lk(mx); @@ -186,6 +190,7 @@ void ChannelFsm::stop_tracking() void ChannelFsm::start_acquisition() { acq_->reset(); + nav_->reset(); } diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 8242058cb..bb26b3c08 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -33,6 +33,7 @@ #define GNSS_SDR_CHANNEL_FSM_H #include "acquisition_interface.h" +#include "telemetry_decoder_interface.h" #include "tracking_interface.h" #include #include @@ -51,6 +52,7 @@ public: void set_acquisition(std::shared_ptr acquisition); void set_tracking(std::shared_ptr tracking); + void set_telemetry(std::shared_ptr telemetry); void set_queue(gr::msg_queue::sptr queue); void set_channel(uint32_t channel); @@ -72,10 +74,11 @@ private: std::shared_ptr acq_; std::shared_ptr trk_; + std::shared_ptr nav_; gr::msg_queue::sptr queue_; uint32_t channel_; uint32_t d_state; std::mutex mx; }; -#endif // GNSS_SDR_CHANNEL_FSM_H +#endif // GNSS_SDR_CHANNEL_FSM_H diff --git a/src/algorithms/telemetry_decoder/adapters/beidou_b1i_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/beidou_b1i_telemetry_decoder.h index cfc09ebea..5dcab7f0f 100644 --- a/src/algorithms/telemetry_decoder/adapters/beidou_b1i_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/beidou_b1i_telemetry_decoder.h @@ -74,9 +74,9 @@ public: void set_satellite(const Gnss_Satellite& satellite) override; inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } - inline void reset() override { + telemetry_decoder_->reset(); return; } diff --git a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h index 1d39b37e4..5a00b5887 100644 --- a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h @@ -80,6 +80,7 @@ public: inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } inline void reset() override { + telemetry_decoder_->reset(); return; } diff --git a/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h index 1bfc7a6e4..55ae5a6e8 100644 --- a/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h @@ -81,6 +81,7 @@ public: inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } inline void reset() override { + telemetry_decoder_->reset(); return; } diff --git a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h index 3b5e13d15..ae031a3f0 100644 --- a/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/glonass_l1_ca_telemetry_decoder.h @@ -71,8 +71,9 @@ public: gr::basic_block_sptr get_right_block() override; void set_satellite(const Gnss_Satellite& satellite) override; void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } - void reset() override + inline void reset() override { + telemetry_decoder_->reset(); return; } size_t item_size() override diff --git a/src/algorithms/telemetry_decoder/adapters/glonass_l2_ca_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/glonass_l2_ca_telemetry_decoder.h index 3a36b7deb..cdd6be49d 100644 --- a/src/algorithms/telemetry_decoder/adapters/glonass_l2_ca_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/glonass_l2_ca_telemetry_decoder.h @@ -33,8 +33,8 @@ #ifndef GNSS_SDR_GLONASS_L2_CA_TELEMETRY_DECODER_H_ #define GNSS_SDR_GLONASS_L2_CA_TELEMETRY_DECODER_H_ -#include "gnss_satellite.h" // for Gnss_Satellite #include "glonass_l2_ca_telemetry_decoder_gs.h" +#include "gnss_satellite.h" // for Gnss_Satellite #include "telemetry_decoder_interface.h" #include // for basic_block_sptr, top_block_sptr #include // for size_t @@ -70,8 +70,9 @@ public: gr::basic_block_sptr get_right_block() override; void set_satellite(const Gnss_Satellite& satellite) override; void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } - void reset() override + inline void reset() override { + telemetry_decoder_->reset(); return; } size_t item_size() override diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.h index b43f8f426..af1959226 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.h @@ -73,9 +73,9 @@ public: void set_satellite(const Gnss_Satellite& satellite) override; inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } - inline void reset() override { + telemetry_decoder_->reset(); return; } diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.h index 83263a0c8..b1ff4d866 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.h @@ -73,9 +73,9 @@ public: void set_satellite(const Gnss_Satellite& satellite) override; inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } - inline void reset() override { + telemetry_decoder_->reset(); return; } inline size_t item_size() override diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h index 56fc3afd3..40d4bd62e 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h @@ -75,9 +75,9 @@ public: void set_satellite(const Gnss_Satellite& satellite) override; inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } - inline void reset() override { + telemetry_decoder_->reset(); return; } inline size_t item_size() override diff --git a/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.h index b6f9a8564..94671da4b 100644 --- a/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.h @@ -77,9 +77,9 @@ public: void set_satellite(const Gnss_Satellite& satellite) override; inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } - inline void reset() override { + telemetry_decoder_->reset(); return; } 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 45a9f7667..690a95a26 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 @@ -65,6 +65,8 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs( { // Ephemeris data port out 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()); 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 dc281e84d..1c7e18a9c 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,7 +62,10 @@ public: ~beidou_b1i_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ 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 f2a938114..5c1fab382 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 @@ -68,6 +68,12 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( { // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); + // Control messages to tracking block + this->message_port_register_out(pmt::mp("telemetry_to_trk")); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + + // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); @@ -91,6 +97,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; + d_max_symbols_without_valid_frame = GALILEO_INAV_PAGE_PART_SYMBOLS * 10; //rise alarm 10 seconds without valid tlm + break; } case 2: // FNAV @@ -119,6 +127,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_secondary_code_samples[i] = -1; } } + d_max_symbols_without_valid_frame = GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_FNAV_SYMBOLS_PER_PAGE * 10; //rise alarm 10 seconds without valid tlm break; } default: @@ -133,6 +142,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_frame_length_symbols = 0U; CodeLength = 0; DataLength = 0; + d_max_symbols_without_valid_frame = 0; std::cout << "Galileo unified telemetry decoder error: Unknown frame type " << std::endl; } @@ -431,12 +441,21 @@ void galileo_telemetry_decoder_gs::decode_FNAV_word(double *page_symbols, int32_ void galileo_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satellite) { + gr::thread::scoped_lock lock(d_setlock); d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); + d_last_valid_preamble = d_sample_counter; + d_sent_tlm_failed_msg = false; DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite; DLOG(INFO) << "Navigation Satellite set to " << d_satellite; } +void galileo_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_sent_tlm_failed_msg = false; + DLOG(INFO) << "Telemetry decoder reset for satellite " << d_satellite; +} void galileo_telemetry_decoder_gs::set_channel(int32_t channel) { d_channel = channel; @@ -478,10 +497,20 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( current_symbol = in[0][0]; // add new symbol to the symbol queue d_symbol_history.push_back(current_symbol.Prompt_I); - d_sample_counter++; // count for the processed samples + d_sample_counter++; // count for the processed symbols consume_each(1); d_flag_preamble = false; + // check if there is a problem with the telemetry of the current satellite + if (d_stat < 1 and d_sent_tlm_failed_msg == false) + { + if ((d_sample_counter - d_last_valid_preamble) > d_max_symbols_without_valid_frame) + { + int message = 1; //bad telemetry + this->message_port_pub(pmt::mp("telemetry_to_trk"), pmt::make_any(message)); + d_sent_tlm_failed_msg = true; + } + } if (d_symbol_history.size() > d_required_symbols) { // ******* preamble correlation ******** @@ -604,6 +633,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_CRC_error_counter = 0; d_flag_preamble = true; // valid preamble indicator (initialized to false every work()) d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P) + d_last_valid_preamble = d_sample_counter; if (!d_flag_frame_sync) { d_flag_frame_sync = true; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index f08c37b81..65f65f62e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -59,6 +59,7 @@ public: ~galileo_telemetry_decoder_gs(); void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int32_t channel); //!< Set receiver's channel + void reset(); int32_t flag_even_word_arrived; /*! @@ -95,6 +96,10 @@ private: uint64_t d_sample_counter; uint64_t d_preamble_index; + uint64_t d_last_valid_preamble; + uint32_t d_max_symbols_without_valid_frame; + + bool d_sent_tlm_failed_msg; uint32_t d_stat; bool d_flag_frame_sync; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index e39c84852..7629c2810 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -62,6 +62,8 @@ glonass_l1_ca_telemetry_decoder_gs::glonass_l1_ca_telemetry_decoder_gs( { // Ephemeris data port out 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()); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h index 3b82aa5f2..bd37b4905 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h @@ -64,7 +64,10 @@ public: ~glonass_l1_ca_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int32_t channel); //!< Set receiver's channel - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index 6f671bb9b..144554491 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -62,6 +62,8 @@ glonass_l2_ca_telemetry_decoder_gs::glonass_l2_ca_telemetry_decoder_gs( { // Ephemeris data port out 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()); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h index 40737fd85..ffe3f1ca4 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h @@ -62,7 +62,10 @@ public: ~glonass_l2_ca_telemetry_decoder_gs(); //!< Class destructor void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int32_t channel); //!< Set receiver's channel - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ 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 9125f12de..ee875b9a4 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 @@ -64,6 +64,12 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( { // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); + // Control messages to tracking block + this->message_port_register_out(pmt::mp("telemetry_to_trk")); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + d_max_symbols_without_valid_frame = GPS_SUBFRAME_BITS * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT * 5; //rise alarm if 5 consecutive subframes have no valid CRC + // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); @@ -103,6 +109,7 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( d_symbol_history.set_capacity(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); d_crc_error_synchronization_counter = 0; d_current_subframe_symbol = 0; + d_sample_counter = 0; } @@ -317,6 +324,12 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe() return subframe_synchro_confirmation; } +void gps_l1_ca_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_sent_tlm_failed_msg = false; + DLOG(INFO) << "Telemetry decoder reset for satellite " << d_satellite; +} int gps_l1_ca_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) @@ -342,6 +355,19 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ d_flag_preamble = false; + // check if there is a problem with the telemetry of the current satellite + d_sample_counter++; // count for the processed symbols + if (d_sent_tlm_failed_msg == false) + { + if ((d_sample_counter - d_last_valid_preamble) > d_max_symbols_without_valid_frame) + { + int message = 1; //bad telemetry + this->message_port_pub(pmt::mp("telemetry_to_trk"), pmt::make_any(message)); + d_sent_tlm_failed_msg = true; + } + } + + // ******* preamble correlation ******** int32_t corr_value = 0; if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) @@ -442,6 +468,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ d_TOW_at_Preamble_ms = static_cast(d_nav.d_TOW * 1000.0); flag_TOW_set = true; d_flag_new_tow_available = false; + d_last_valid_preamble = d_sample_counter; } else { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h index ef22e17ca..2f910cd3c 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h @@ -33,8 +33,8 @@ #include "GPS_L1_CA.h" #include "gnss_satellite.h" -#include "gps_navigation_message.h" #include "gnss_synchro.h" +#include "gps_navigation_message.h" #include #include // for boost::shared_ptr #include // for block @@ -61,7 +61,7 @@ public: ~gps_l1_ca_telemetry_decoder_gs(); void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - + void reset(); /*! * \brief This is where all signal processing takes place */ @@ -79,6 +79,10 @@ private: bool decode_subframe(); bool new_decoder(); int d_crc_error_synchronization_counter; + uint64_t d_sample_counter; + bool d_sent_tlm_failed_msg; + uint64_t d_last_valid_preamble; + uint32_t d_max_symbols_without_valid_frame; int *d_preambles_symbols; uint32_t d_stat; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc index 246173919..83d39d8be 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc @@ -62,6 +62,13 @@ gps_l2c_telemetry_decoder_gs::gps_l2c_telemetry_decoder_gs( { // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); + // Control messages to tracking block + this->message_port_register_out(pmt::mp("telemetry_to_trk")); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + d_max_symbols_without_valid_frame = GPS_L2_CNAV_DATA_PAGE_BITS * GPS_L2_SYMBOLS_PER_BIT * 5; //rise alarm if 5 consecutive subframes have no valid CRC + + // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); @@ -76,6 +83,8 @@ gps_l2c_telemetry_decoder_gs::gps_l2c_telemetry_decoder_gs( // initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); + + d_sample_counter = 0; } @@ -130,6 +139,13 @@ void gps_l2c_telemetry_decoder_gs::set_channel(int channel) } +void gps_l2c_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_sent_tlm_failed_msg = false; + DLOG(INFO) << "Telemetry decoder reset for satellite " << d_satellite; +} + int gps_l2c_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) { @@ -147,6 +163,18 @@ int gps_l2c_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( consume_each(1); // one by one + // check if there is a problem with the telemetry of the current satellite + d_sample_counter++; // count for the processed symbols + if (d_sent_tlm_failed_msg == false) + { + if ((d_sample_counter - d_last_valid_preamble) > d_max_symbols_without_valid_frame) + { + int message = 1; //bad telemetry + this->message_port_pub(pmt::mp("telemetry_to_trk"), pmt::make_any(message)); + d_sent_tlm_failed_msg = true; + } + } + // UPDATE GNSS SYNCHRO DATA Gnss_Synchro current_synchro_data{}; // structure to save the synchronization information and send the output object to the next block @@ -190,6 +218,7 @@ int gps_l2c_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( // update TOW at the preamble instant d_TOW_at_Preamble = static_cast(msg.tow); + d_last_valid_preamble = d_sample_counter; // The time of the last input symbol can be computed from the message ToW and // delay by the formulae: // \code diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h index 241ac81e7..7f7737429 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h @@ -41,8 +41,7 @@ #include #include -extern "C" -{ +extern "C" { #include "cnav_msg.h" } @@ -64,7 +63,7 @@ public: ~gps_l2c_telemetry_decoder_gs(); void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int32_t channel); //!< Set receiver's channel - + void reset(); /*! * \brief This is where all signal processing takes place */ @@ -87,6 +86,10 @@ private: int32_t d_state; int32_t d_crc_error_count; + uint64_t d_sample_counter; + bool d_sent_tlm_failed_msg; + uint64_t d_last_valid_preamble; + uint32_t d_max_symbols_without_valid_frame; double d_TOW_at_current_symbol; double d_TOW_at_Preamble; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index a70aaebb1..d9c33d80f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -60,6 +60,12 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( { // Ephemeris data port out this->message_port_register_out(pmt::mp("telemetry")); + // Control messages to tracking block + this->message_port_register_out(pmt::mp("telemetry_to_trk")); + d_last_valid_preamble = 0; + d_sent_tlm_failed_msg = false; + d_max_symbols_without_valid_frame = GPS_L5_CNAV_DATA_PAGE_BITS * GPS_L5_SAMPLES_PER_SYMBOL * GPS_L5_SYMBOLS_PER_BIT * 5; //rise alarm if 5 consecutive subframes have no valid CRC + // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); @@ -85,6 +91,8 @@ gps_l5_telemetry_decoder_gs::gps_l5_telemetry_decoder_gs( } sync_NH = false; new_sym = false; + + d_sample_counter = 0; } @@ -140,6 +148,13 @@ void gps_l5_telemetry_decoder_gs::set_channel(int32_t channel) } } +void gps_l5_telemetry_decoder_gs::reset() +{ + d_last_valid_preamble = d_sample_counter; + d_sent_tlm_failed_msg = false; + DLOG(INFO) << "Telemetry decoder reset for satellite " << d_satellite; +} + int gps_l5_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) @@ -157,6 +172,18 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u int32_t corr_NH = 0; int32_t symbol_value = 0; + // check if there is a problem with the telemetry of the current satellite + d_sample_counter++; // count for the processed symbols + if (d_sent_tlm_failed_msg == false) + { + if ((d_sample_counter - d_last_valid_preamble) > d_max_symbols_without_valid_frame) + { + int message = 1; //bad telemetry + this->message_port_pub(pmt::mp("telemetry_to_trk"), pmt::make_any(message)); + d_sent_tlm_failed_msg = true; + } + } + // Search correlation with Neuman-Hofman Code (see IS-GPS-705D) if (sym_hist.size() == GPS_L5I_NH_CODE_LENGTH) { @@ -240,6 +267,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u // update TOW at the preamble instant d_TOW_at_Preamble_ms = msg.tow * 6000; + d_last_valid_preamble = d_sample_counter; // The time of the last input symbol can be computed from the message ToW and // delay by the formulae: // \code diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h index e3fbd2e05..1971f3c09 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h @@ -42,8 +42,7 @@ #include #include -extern "C" -{ +extern "C" { #include "cnav_msg.h" } @@ -65,6 +64,7 @@ public: ~gps_l5_telemetry_decoder_gs(); void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int32_t channel); //!< Set receiver's channel + void reset(); int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); @@ -85,6 +85,10 @@ private: uint32_t d_TOW_at_current_symbol_ms; uint32_t d_TOW_at_Preamble_ms; bool d_flag_valid_word; + uint64_t d_sample_counter; + bool d_sent_tlm_failed_msg; + uint64_t d_last_valid_preamble; + uint32_t d_max_symbols_without_valid_frame; Gps_CNAV_Navigation_Message d_CNAV_Message; float bits_NH[GPS_L5I_NH_CODE_LENGTH]{}; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc index 30da62d4f..cfa68d2be 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc @@ -60,6 +60,8 @@ sbas_l1_telemetry_decoder_gs::sbas_l1_telemetry_decoder_gs( { // Ephemeris data port out 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()); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.h index 35306340c..ae27ec8c7 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.h @@ -63,7 +63,10 @@ public: ~sbas_l1_telemetry_decoder_gs(); void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN void set_channel(int32_t channel); //!< Set receiver's channel - + inline void reset() + { + return; + } /*! * \brief This is where all signal processing takes place */ 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 df7c6c867..7ad6535ae 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -45,7 +45,6 @@ #include "beidou_b1i_signal_processing.h" #include "galileo_e1_signal_processing.h" #include "galileo_e5_signal_processing.h" -#include "gnss_satellite.h" #include "gnss_sdr_create_directory.h" #include "gnss_synchro.h" #include "gps_l2c_signal.h" @@ -62,11 +61,9 @@ #include #include // for fill_n #include // for fmod, round, floor -#include // for complex -#include // for abs, size_t #include // for exception #include // for cout, cerr -#include // for map +#include dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_) @@ -84,7 +81,36 @@ void dll_pll_veml_tracking::forecast(int noutput_items, } } +void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) +{ + try + { + if (pmt::any_ref(msg).type() == typeid(int)) + { + int tlm_event; + tlm_event = boost::any_cast(pmt::any_ref(msg)); + switch (tlm_event) + { + case 1: //tlm fault in current channel + { + DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; + gr::thread::scoped_lock lock(d_setlock); + d_carrier_lock_fail_counter = 10000; //force loss-of-lock condition + break; + } + default: + { + break; + } + } + } + } + catch (boost::bad_any_cast &e) + { + LOG(WARNING) << "msg_handler_telemetry_to_trk Bad any cast!"; + } +} dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { @@ -93,12 +119,14 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl this->message_port_register_out(pmt::mp("events")); this->set_relative_rate(1.0 / static_cast(trk_parameters.vector_length)); - // Telemetry bit synchronization message port input (mainly for GPS L1 CA) - this->message_port_register_in(pmt::mp("preamble_samplestamp")); + // Telemetry message port input + this->message_port_register_in(pmt::mp("telemetry_to_trk")); + 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_veml = false; d_cloop = true; + d_pull_in_transitory = true; d_code_chip_rate = 0.0; d_secondary_code_length = 0U; d_secondary_code_string = nullptr; @@ -332,10 +360,8 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl K_blk_samples = 0.0; // Initialize tracking ========================================== - d_code_loop_filter = Tracking_2nd_DLL_filter(static_cast(d_code_period)); - d_carrier_loop_filter = Tracking_2nd_PLL_filter(static_cast(d_code_period)); - d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_hz); + d_code_loop_filter = Tracking_loop_filter(d_code_period, trk_parameters.dll_bw_hz, trk_parameters.dll_filter_order, false); + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_hz, trk_parameters.pll_filter_order); // Initialization of local code replica // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip @@ -446,7 +472,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_carrier_phase_step_rad = 0.0; d_carrier_phase_rate_step_rad = 0.0; d_rem_code_phase_chips = 0.0; - d_last_prompt = gr_complex(0.0, 0.0); d_state = 0; // initial state: standby clear_tracking_vars(); if (trk_parameters.smoother_length > 0) @@ -513,8 +538,8 @@ void dll_pll_veml_tracking::start_tracking() d_carr_ph_history.clear(); d_code_ph_history.clear(); // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(); // initialize the carrier filter - d_code_loop_filter.initialize(); // initialize the code filter + d_carrier_loop_filter.initialize(static_cast(d_acq_carrier_doppler_hz)); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter if (systemName == "GPS" and signal_type == "1C") { @@ -642,10 +667,10 @@ void dll_pll_veml_tracking::start_tracking() d_local_code_shift_chips[2] = trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); } - d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_hz); - d_carrier_loop_filter.set_pdi(static_cast(d_code_period)); - d_code_loop_filter.set_pdi(static_cast(d_code_period)); + d_current_correlation_time_s = d_code_period; + + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_hz); + d_code_loop_filter.set_update_interval(d_code_period); // DEBUG OUTPUT std::cout << "Tracking of " << systemName << " " << signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; @@ -654,8 +679,8 @@ void dll_pll_veml_tracking::start_tracking() // enable tracking pull-in d_state = 1; d_cloop = true; + d_pull_in_transitory = true; d_Prompt_circular_buffer.clear(); - d_last_prompt = gr_complex(0.0, 0.0); } @@ -764,15 +789,18 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_ // Carrier lock indicator d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, trk_parameters.cn0_samples); // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) + if (!d_pull_in_transitory) { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < trk_parameters.cn0_min) { - d_carrier_lock_fail_counter--; + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) + { + d_carrier_lock_fail_counter--; + } } } if (d_carrier_lock_fail_counter > trk_parameters.max_lock_fail) @@ -827,20 +855,43 @@ void dll_pll_veml_tracking::run_dll_pll() if (d_cloop) { // Costas loop discriminator, insensitive to 180 deg phase transitions - d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; + d_carr_phase_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; } else { // Secondary code acquired. No symbols transition should be present in the signal - d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; + d_carr_phase_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; + } + + if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true) or trk_parameters.enable_fll_steady_state) + { + // FLL discriminator + d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_P_accu_old = d_P_accu; + //std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; + // Carrier discriminator filter + if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true)) + { + //pure FLL, disable PLL + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, 0, d_current_correlation_time_s); + } + else + { + //FLL-aided PLL + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(d_carr_freq_error_hz, d_carr_phase_error_hz, d_current_correlation_time_s); + } + } + else + { + // Carrier discriminator filter + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_error(0, d_carr_phase_error_hz, d_current_correlation_time_s); } - // Carrier discriminator filter - d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz); // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; - + d_carrier_doppler_hz = d_carr_error_filt_hz; + // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << std::endl; + // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << std::endl; // ################## DLL ########################################################## // DLL discriminator if (d_veml) @@ -852,7 +903,7 @@ void dll_pll_veml_tracking::run_dll_pll() d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu); // [chips/Ti] } // Code discriminator filter - d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] + d_code_error_filt_chips = d_code_loop_filter.apply(d_code_error_chips); // [chips/second] // New code Doppler frequency estimation d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips; @@ -866,13 +917,14 @@ void dll_pll_veml_tracking::clear_tracking_vars() { d_Prompt_Data[0] = gr_complex(0.0, 0.0); } - d_carr_error_hz = 0.0; + d_P_accu_old = gr_complex(0.0, 0.0); + d_carr_phase_error_hz = 0.0; + d_carr_freq_error_hz = 0.0; d_carr_error_filt_hz = 0.0; d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; d_Prompt_circular_buffer.clear(); - d_last_prompt = gr_complex(0.0, 0.0); d_carrier_phase_rate_step_rad = 0.0; d_code_phase_rate_step_chips = 0.0; d_carr_ph_history.clear(); @@ -1104,7 +1156,7 @@ void dll_pll_veml_tracking::log_data(bool integrating) tmp_float = d_code_phase_rate_step_chips * trk_parameters.fs_in * trk_parameters.fs_in; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // PLL commands - tmp_float = d_carr_error_hz; + tmp_float = d_carr_phase_error_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_carr_error_filt_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); @@ -1434,6 +1486,13 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) auto **out = reinterpret_cast(&output_items[0]); Gnss_Synchro current_synchro_data = Gnss_Synchro(); + if (d_pull_in_transitory == true) + { + if (trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(trk_parameters.fs_in)) + { + d_pull_in_transitory = false; + } + } switch (d_state) { case 0: // Standby - Consume samples at full throttle, do nothing @@ -1609,7 +1668,6 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_P_accu = gr_complex(0.0, 0.0); d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); - d_last_prompt = gr_complex(0.0, 0.0); d_Prompt_circular_buffer.clear(); d_current_symbol = 0; @@ -1617,9 +1675,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { // UPDATE INTEGRATION TIME d_extend_correlation_symbols_count = 0; - float new_correlation_time = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); - d_carrier_loop_filter.set_pdi(new_correlation_time); - d_code_loop_filter.set_pdi(new_correlation_time); + d_current_correlation_time_s = static_cast(trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); d_state = 3; // next state is the extended correlator integrator LOG(INFO) << "Enabled " << trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " << d_channel @@ -1628,8 +1684,9 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; // Set narrow taps delay values [chips] - d_code_loop_filter.set_DLL_BW(trk_parameters.dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(trk_parameters.pll_bw_narrow_hz); + d_code_loop_filter.set_update_interval(d_current_correlation_time_s); + d_code_loop_filter.set_noise_bandwidth(trk_parameters.dll_bw_narrow_hz); + d_carrier_loop_filter.set_params(trk_parameters.fll_bw_hz, trk_parameters.pll_bw_narrow_hz, trk_parameters.pll_filter_order); if (d_veml) { d_local_code_shift_chips[0] = -trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index c1d197da9..43d95f3f6 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -34,8 +34,8 @@ #include "cpu_multicorrelator_real_codes.h" #include "dll_pll_conf.h" -#include "tracking_2nd_DLL_filter.h" -#include "tracking_2nd_PLL_filter.h" +#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter +#include "tracking_loop_filter.h" // for DLL filter #include #include // for boost::shared_ptr #include // for block @@ -44,10 +44,8 @@ #include // for pmt_t #include // for int32_t #include // for string, ofstream -#include // for string #include // for pair - class Gnss_Synchro; class dll_pll_veml_tracking; @@ -75,9 +73,8 @@ public: private: friend dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_); - + void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg); dll_pll_veml_tracking(const Dll_Pll_Conf &conf_); - void msg_handler_preamble_index(pmt::pmt_t msg); bool cn0_and_tracking_lock_status(double coh_integration_time_s); bool acquire_secondary(); @@ -147,9 +144,9 @@ private: gr_complex d_VE_accu; gr_complex d_E_accu; gr_complex d_P_accu; + gr_complex d_P_accu_old; gr_complex d_L_accu; gr_complex d_VL_accu; - gr_complex d_last_prompt; gr_complex *d_Prompt_Data; @@ -163,16 +160,18 @@ private: double d_rem_code_phase_samples; float d_rem_carr_phase_rad; - // PLL and DLL filter library - Tracking_2nd_DLL_filter d_code_loop_filter; - Tracking_2nd_PLL_filter d_carrier_loop_filter; + Tracking_loop_filter d_code_loop_filter; + Tracking_FLL_PLL_filter d_carrier_loop_filter; // acquisition double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; // tracking vars - double d_carr_error_hz; + bool d_pull_in_transitory; + double d_current_correlation_time_s; + double d_carr_phase_error_hz; + double d_carr_freq_error_hz; double d_carr_error_filt_hz; double d_code_error_chips; double d_code_error_filt_chips; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index 77c0b48a3..a2e224d7d 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -43,6 +43,13 @@ Dll_Pll_Conf::Dll_Pll_Conf() dump = false; dump_mat = true; dump_filename = std::string("./dll_pll_dump.dat"); + enable_fll_pull_in = false; + enable_fll_steady_state = false; + pull_in_time_s = 2; + fll_filter_order = 1; + pll_filter_order = 3; + dll_filter_order = 2; + fll_bw_hz = 35.0; pll_pull_in_bw_hz = 50.0; dll_pull_in_bw_hz = 3.0; pll_bw_hz = 35.0; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index 2304404b1..4a04c02e9 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -40,6 +40,14 @@ class Dll_Pll_Conf { public: /* DLL/PLL tracking configuration */ + + int fll_filter_order; + bool enable_fll_pull_in; + bool enable_fll_steady_state; + unsigned int pull_in_time_s; + int pll_filter_order; + int dll_filter_order; + double fs_in; uint32_t vector_length; bool dump; @@ -47,6 +55,7 @@ public: std::string dump_filename; float pll_pull_in_bw_hz; float dll_pull_in_bw_hz; + float fll_bw_hz; float pll_bw_hz; float dll_bw_hz; float pll_bw_narrow_hz; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 898461d16..dd5bdf02d 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -215,7 +215,7 @@ void GNSSFlowgraph::connect() } DLOG(INFO) << "blocks connected internally"; - // Signal Source (i) > Signal conditioner (i) > +// Signal Source (i) > Signal conditioner (i) > #ifndef ENABLE_FPGA @@ -469,9 +469,6 @@ void GNSSFlowgraph::connect() top_block_->connect(acq_resamplers_.at(map_key), 0, channels_.at(i)->get_left_block_acq(), 0); - top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, - channels_.at(i)->get_left_block_trk(), 0); - std::shared_ptr channel_ptr; channel_ptr = std::dynamic_pointer_cast(channels_.at(i)); channel_ptr->acquisition()->set_resampler_latency((taps.size() - 1) / 2); @@ -482,8 +479,6 @@ void GNSSFlowgraph::connect() //resampler not required! top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, channels_.at(i)->get_left_block_acq(), 0); - top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, - channels_.at(i)->get_left_block_trk(), 0); } } else @@ -491,17 +486,15 @@ void GNSSFlowgraph::connect() LOG(INFO) << "Disabled acquisition resampler because the input sampling frequency is too low"; top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, channels_.at(i)->get_left_block_acq(), 0); - top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, - channels_.at(i)->get_left_block_trk(), 0); } } else { top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, channels_.at(i)->get_left_block_acq(), 0); - top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, - channels_.at(i)->get_left_block_trk(), 0); } + top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, + channels_.at(i)->get_left_block_trk(), 0); } catch (const std::exception& e) {