From c45257a26be7ca062ea41cb7aaeb9041b04a6151 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 25 Jul 2019 16:38:19 +0200 Subject: [PATCH] Bug fix: Restore telecommand functionality --- .../galileo_e1_dll_pll_veml_tracking.cc | 1 + .../galileo_e1_dll_pll_veml_tracking_fpga.cc | 1 + .../adapters/galileo_e5a_dll_pll_tracking.cc | 1 + .../galileo_e5a_dll_pll_tracking_fpga.cc | 1 + .../gps_l1_ca_dll_pll_tracking_fpga.cc | 1 + .../adapters/gps_l2_m_dll_pll_tracking.cc | 1 + .../gps_l2_m_dll_pll_tracking_fpga.cc | 1 + .../adapters/gps_l5_dll_pll_tracking.cc | 1 + .../adapters/gps_l5_dll_pll_tracking_fpga.cc | 1 + src/core/receiver/control_thread.cc | 34 ++++++++++++++----- src/core/receiver/control_thread.h | 1 + src/core/receiver/gnss_flowgraph.cc | 23 +++---------- 12 files changed, 40 insertions(+), 27 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index 754b3ec3a..2c435b66b 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -194,6 +194,7 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( void GalileoE1DllPllVemlTracking::stop_tracking() { + tracking_->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc index e3b3a3bb6..db4c429c5 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking_fpga.cc @@ -300,6 +300,7 @@ GalileoE1DllPllVemlTrackingFpga::~GalileoE1DllPllVemlTrackingFpga() void GalileoE1DllPllVemlTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index 201597bbf..97181abfb 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -190,6 +190,7 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( void GalileoE5aDllPllTracking::stop_tracking() { + tracking_->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc index 2c2fe882d..3a4a91176 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking_fpga.cc @@ -290,6 +290,7 @@ void GalileoE5aDllPllTrackingFpga::start_tracking() void GalileoE5aDllPllTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc index bbd2b1014..46219425f 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_fpga.cc @@ -252,6 +252,7 @@ void GpsL1CaDllPllTrackingFpga::start_tracking() void GpsL1CaDllPllTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index 5d6587d78..954524a9c 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -172,6 +172,7 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( void GpsL2MDllPllTracking::stop_tracking() { + tracking_->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc index 177a9dc13..387da089b 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking_fpga.cc @@ -163,6 +163,7 @@ void GpsL2MDllPllTrackingFpga::start_tracking() void GpsL2MDllPllTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc index d7c4d0454..9eba7c538 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc @@ -192,6 +192,7 @@ GpsL5DllPllTracking::GpsL5DllPllTracking( void GpsL5DllPllTracking::stop_tracking() { + tracking_->stop_tracking(); } diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc index 587cc940b..9baf9e8f1 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking_fpga.cc @@ -311,6 +311,7 @@ void GpsL5DllPllTrackingFpga::start_tracking() void GpsL5DllPllTrackingFpga::stop_tracking() { + tracking_fpga_sc->stop_tracking(); } diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index 629957959..ba72efb94 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -194,6 +194,8 @@ void ControlThread::init() agnss_ref_time_.valid = false; } } + + receiver_on_standby_ = false; } @@ -234,11 +236,14 @@ void ControlThread::event_dispatcher(bool &valid_event, pmt::pmt_t &msg) processed_control_messages_++; if (pmt::any_ref(msg).type() == typeid(channel_event_sptr)) { - channel_event_sptr new_event; - new_event = boost::any_cast(pmt::any_ref(msg)); - DLOG(INFO) << "New channel event rx from ch id: " << new_event->channel_id - << " what: " << new_event->event_type; - flowgraph_->apply_action(new_event->channel_id, new_event->event_type); + if (receiver_on_standby_ == false) + { + channel_event_sptr new_event; + new_event = boost::any_cast(pmt::any_ref(msg)); + DLOG(INFO) << "New channel event rx from ch id: " << new_event->channel_id + << " what: " << new_event->event_type; + flowgraph_->apply_action(new_event->channel_id, new_event->event_type); + } } else if (pmt::any_ref(msg).type() == typeid(command_event_sptr)) { @@ -267,8 +272,11 @@ void ControlThread::event_dispatcher(bool &valid_event, pmt::pmt_t &msg) } else { - //perform non-priority tasks - flowgraph_->acquisition_manager(0); //start acquisition of untracked satellites + if (receiver_on_standby_ == false) + { + //perform non-priority tasks + flowgraph_->acquisition_manager(0); //start acquisition of untracked satellites + } } } @@ -852,6 +860,10 @@ void ControlThread::apply_action(unsigned int what) stop_ = true; restart_ = true; break; + case 10: // request standby mode + LOG(INFO) << "TC request standby mode"; + receiver_on_standby_ = true; + break; case 11: LOG(INFO) << "Receiver action COLDSTART"; // delete all ephemeris and almanac information from maps (also the PVT map queue) @@ -859,13 +871,16 @@ void ControlThread::apply_action(unsigned int what) pvt_ptr->clear_ephemeris(); // todo: reorder the satellite queues to the receiver default startup order. // This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites + // start again the satellite acquisitions + receiver_on_standby_ = false; break; case 12: LOG(INFO) << "Receiver action HOTSTART"; visible_satellites = get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH()); // reorder the satellite queue to acquire first those visible satellites flowgraph_->priorize_satellites(visible_satellites); - // start again the satellite acquisitions (done in chained apply_action to flowgraph) + // start again the satellite acquisitions + receiver_on_standby_ = false; break; case 13: LOG(INFO) << "Receiver action WARMSTART"; @@ -879,7 +894,8 @@ void ControlThread::apply_action(unsigned int what) get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH()); // reorder the satellite queue to acquire first those visible satellites flowgraph_->priorize_satellites(visible_satellites); - // start again the satellite acquisitions (done in chained apply_action to flowgraph) + // start again the satellite acquisitions + receiver_on_standby_ = false; break; default: LOG(INFO) << "Unrecognized action."; diff --git a/src/core/receiver/control_thread.h b/src/core/receiver/control_thread.h index 0459acd50..dae3c3022 100644 --- a/src/core/receiver/control_thread.h +++ b/src/core/receiver/control_thread.h @@ -129,6 +129,7 @@ private: /* * New receiver event dispatcher */ + bool receiver_on_standby_; void event_dispatcher(bool &valid_event, pmt::pmt_t &msg); std::thread cmd_interface_thread_; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 2562e094b..0c0171e18 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1288,8 +1288,8 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) std::lock_guard lock(signal_list_mutex); DLOG(INFO) << "Received " << what << " from " << who; - Gnss_Signal gs = channels_[who]->get_signal(); unsigned int sat = 0; + Gnss_Signal gs; if (who < 200) { try @@ -1304,6 +1304,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) switch (what) { case 0: + gs = channels_[who]->get_signal(); DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << gs.get_satellite() << ", Signal " << gs.get_signal_str(); channels_state_[who] = 0; if (acq_channels_count_ > 0) @@ -1319,6 +1320,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } break; case 1: + gs = channels_[who]->get_signal(); DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << gs.get_satellite(); // If the satellite is in the list of available ones, remove it. remove_signal(gs); @@ -1333,6 +1335,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) break; case 2: + gs = channels_[who]->get_signal(); DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << gs.get_satellite(); if (acq_channels_count_ < max_acq_channels_) { @@ -1359,7 +1362,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } break; case 10: // request standby mode - LOG(INFO) << "TC request standby mode"; for (size_t n = 0; n < channels_.size(); n++) { if (channels_state_[n] == 1 or channels_state_[n] == 2) // channel in acquisition or in tracking @@ -1372,22 +1374,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_state_[n] = 0; } } - acq_channels_count_ = 0; // all channels are in standby now - break; - case 11: // request coldstart mode - LOG(INFO) << "TC request flowgraph coldstart"; - // call the acquisition manager to assign new satellite and start next acquisition (if required) - acquisition_manager(who); - break; - case 12: // request hotstart mode - LOG(INFO) << "TC request flowgraph hotstart"; - // call the acquisition manager to assign new satellite and start next acquisition (if required) - acquisition_manager(who); - break; - case 13: // request warmstart mode - LOG(INFO) << "TC request flowgraph warmstart"; - // call the acquisition manager to assign new satellite and start next acquisition (if required) - acquisition_manager(who); + acq_channels_count_ = 0; // all channels are in standby now and no new acquisition should be started break; default: break;