diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 187d419f4..44ee7bce5 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -150,23 +150,27 @@ void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); //std::cout<<"Ev_channel_start_acquisition launched"<process_event(Ev_channel_valid_acquisition()); + DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; } void ChannelFsm::Event_failed_acquisition_repeat() { this->process_event(Ev_channel_failed_acquisition_repeat()); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; } void ChannelFsm::Event_failed_acquisition_no_repeat() { this->process_event(Ev_channel_failed_acquisition_no_repeat()); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; } @@ -174,6 +178,7 @@ void ChannelFsm::Event_failed_acquisition_no_repeat() void ChannelFsm::Event_failed_tracking_standby() { this->process_event(Ev_channel_failed_tracking_standby()); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; } //void ChannelFsm::Event_failed_tracking_reacq() { diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index ed505b5f9..a535c90fd 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -374,11 +374,10 @@ bool GNSSFlowgraph::send_telemetry_msg(pmt::pmt_t msg) void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) { DLOG(INFO) << "received " << what << " from " << who; - switch (what) { case 0: - LOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); available_GNSS_signals_.push_back(channels_.at(who)->get_signal()); //TODO: Optimize the channel and signal matching! while ( channels_.at(who)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) @@ -388,15 +387,33 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } channels_.at(who)->set_signal(available_GNSS_signals_.front()); available_GNSS_signals_.pop_front(); - usleep(100); - LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); channels_.at(who)->start_acquisition(); break; + case 1: - LOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); + DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); channels_state_[who] = 2; acq_channels_count_--; - if (!available_GNSS_signals_.empty() && acq_channels_count_ < max_acq_channels_) + for (unsigned int i = 0; i < channels_count_; i++) + { + if(!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_) && (channels_state_[i] == 0)) + { + channels_state_[i] = 1; + while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0) + { + available_GNSS_signals_.push_back(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + } + channels_.at(i)->set_signal(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + acq_channels_count_++; + channels_.at(i)->start_acquisition(); + } + DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; + } + /* + if (!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_)) { for (unsigned int i = 0; i < channels_count_; i++) { @@ -417,11 +434,12 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } } - + */ break; case 2: - LOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + DLOG(INFO) << "Number of channels in acquisition = " << acq_channels_count_; if (acq_channels_count_ < max_acq_channels_) { channels_state_[who] = 1; @@ -433,17 +451,13 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_state_[who] = 0; available_GNSS_signals_.push_back( channels_.at(who)->get_signal() ); } - - // for (unsigned int i = 0; i < channels_count_; i++) - // { - // LOG(INFO) << "Channel " << i << " in state " << channels_state_[i] << std::endl; - // } break; default: break; } DLOG(INFO) << "Number of available signals: " << available_GNSS_signals_.size(); + applied_actions_++; } @@ -555,7 +569,6 @@ void GNSSFlowgraph::init() set_signals_list(); set_channels_state(); applied_actions_ = 0; - DLOG(INFO) << "Blocks instantiated. " << channels_count_ << " channels."; } @@ -704,7 +717,7 @@ void GNSSFlowgraph::set_signals_list() if (configuration_->property("Channels_1B.count", 0) > 0) { /* - * Loop to create the list of Galileo E1 B signals + * Loop to create the list of Galileo E1B signals */ for (available_gnss_prn_iter = available_galileo_prn.cbegin(); available_gnss_prn_iter != available_galileo_prn.cend(); @@ -718,7 +731,7 @@ void GNSSFlowgraph::set_signals_list() if (configuration_->property("Channels_5X.count", 0) > 0 ) { /* - * Loop to create the list of Galileo E1 B signals + * Loop to create the list of Galileo E5a signals */ for (available_gnss_prn_iter = available_galileo_prn.cbegin(); available_gnss_prn_iter != available_galileo_prn.cend(); @@ -754,14 +767,6 @@ void GNSSFlowgraph::set_signals_list() gnss_it = available_GNSS_signals_.insert(gnss_it, signal_value); } } - - // **** FOR DEBUGGING THE LIST OF GNSS SIGNALS **** - // std::list::const_iterator available_gnss_list_iter; - // for (available_gnss_list_iter = available_GNSS_signals_.cbegin(); available_gnss_list_iter - // != available_GNSS_signals_.cend(); available_gnss_list_iter++) - // { - // std::cout << *available_gnss_list_iter << std::endl; - // } } @@ -776,12 +781,8 @@ void GNSSFlowgraph::set_channels_state() channels_state_.reserve(channels_count_); for (unsigned int i = 0; i < channels_count_; i++) { - if (i < max_acq_channels_) - { - channels_state_.push_back(1); - } - else - channels_state_.push_back(0); + if (i < max_acq_channels_) {channels_state_.push_back(1);} + else {channels_state_.push_back(0);} DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } acq_channels_count_ = max_acq_channels_; diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index 4922e5399..25288a704 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -51,7 +51,7 @@ public: Gnss_Signal(const std::string& signal_); Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_); ~Gnss_Signal(); - std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1B" for Galileo E1B, "5X" for Galileo E5a} + std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1B" for Galileo E1B, "5X" for Galileo E5a, "L5" for GPS L5} Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal friend bool operator== (const Gnss_Signal &, const Gnss_Signal &); //!< operator== for comparison friend std::ostream& operator<<(std::ostream &, const Gnss_Signal &); //!< operator<< for pretty printing