From b2b889ec79445be41d48c12d256b6faf82755836 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 18 Dec 2017 11:07:29 +0100 Subject: [PATCH 01/18] Minor changes in Telemetry comments --- .../gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc | 2 +- .../gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc index 242aca3ac..01b4bed0b 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc @@ -154,7 +154,7 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( //* The time of the last input symbol can be computed from the message ToW and //* delay by the formulae: //* \code - //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; d_flag_valid_word = true; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index 30d549508..2d604b221 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -192,7 +192,7 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( //* The time of the last input symbol can be computed from the message ToW and //* delay by the formulae: //* \code - //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory d_TOW_at_current_symbol = (static_cast(msg.tow) * 6.0) + (static_cast(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; d_flag_valid_word = true; From cef7ae5f7bacfbf523d5bcb63c33cd42d6cba62e Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 18 Dec 2017 15:40:14 +0100 Subject: [PATCH 02/18] Add LOG lines in channel_fsm --- src/algorithms/channel/libs/channel_fsm.cc | 5 ++ src/core/receiver/gnss_flowgraph.cc | 61 +++++++++++----------- src/core/system_parameters/gnss_signal.h | 2 +- 3 files changed, 37 insertions(+), 31 deletions(-) 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 From 8bb0c880c032c2a4045058203cf17947d18624b2 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 20 Dec 2017 16:12:08 +0100 Subject: [PATCH 03/18] Minor changes in DLOG lines --- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- src/algorithms/channel/libs/channel_fsm.cc | 3 +-- .../channel/libs/channel_msg_receiver_cc.cc | 4 ---- src/core/receiver/gnss_flowgraph.cc | 15 ++++++++------- 4 files changed, 10 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 726225998..93685eb24 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -349,7 +349,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); if(result == 0) { - LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; + DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; this->set_time_offset_s(0.0); //reset rx time estimation this->set_num_valid_observations(0); } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 44ee7bce5..85905bfd8 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -115,8 +115,7 @@ public: typedef sc::transition reactions; - channel_waiting_fsm_S3(my_context ctx) : - my_base(ctx) + channel_waiting_fsm_S3(my_context ctx) : my_base(ctx) { //std::cout << "Enter Channel_waiting_S3 " << std::endl; context ().request_satellite(); diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc index 573c1aa84..d22f2dd73 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc @@ -50,13 +50,9 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) switch (message) { case 1: //positive acquisition - //DLOG(INFO) << "Channel " << channel_ << " ACQ SUCCESS satellite " << - // gnss_synchro_.System << " " << gnss_synchro_.PRN; d_channel_fsm->Event_valid_acquisition(); break; case 2: //negative acquisition - //DLOG(INFO) << "Channel " << channel_ - // << " ACQ FAILED satellite " << gnss_synchro_.System << " " << gnss_synchro_.PRN; if (d_repeat == true) { d_channel_fsm->Event_failed_acquisition_repeat(); diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index a535c90fd..609908097 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -91,12 +91,6 @@ void GNSSFlowgraph::start() void GNSSFlowgraph::stop() { - // for (unsigned int i = 0; i < channels_count_; i++) - // { - // channels_.at(i)->stop_channel(); - // LOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; - // } - // LOG(INFO) << "Threads finished. Return to main program."; top_block_->stop(); running_ = false; } @@ -373,11 +367,13 @@ 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; + DLOG(INFO) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_; + VLOG(-100) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_; switch (what) { case 0: DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + VLOG(-100) << "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,11 +384,13 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_.at(who)->set_signal(available_GNSS_signals_.front()); available_GNSS_signals_.pop_front(); DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + VLOG(-100) << "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: DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); + VLOG(-100) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); channels_state_[who] = 2; acq_channels_count_--; for (unsigned int i = 0; i < channels_count_; i++) @@ -440,6 +438,9 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) case 2: DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); DLOG(INFO) << "Number of channels in acquisition = " << acq_channels_count_; + VLOG(-100) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + VLOG(-100) << "Number of channels in acquisition = " << acq_channels_count_; + if (acq_channels_count_ < max_acq_channels_) { channels_state_[who] = 1; From 2d8141d9b518853e02cb3fb5883f78b115c34e24 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 21 Dec 2017 13:05:21 +0100 Subject: [PATCH 04/18] Migrate msg_queue boost smart pointers to gr sptr --- src/algorithms/channel/adapters/channel.cc | 2 +- src/algorithms/channel/adapters/channel.h | 2 +- src/algorithms/channel/libs/channel_fsm.cc | 2 +- src/algorithms/channel/libs/channel_fsm.h | 4 ++-- src/core/receiver/control_message_factory.cc | 6 +++--- src/core/receiver/control_message_factory.h | 2 +- src/core/receiver/control_thread.cc | 4 ++-- src/core/receiver/control_thread.h | 4 ++-- src/core/receiver/gnss_flowgraph.cc | 3 +-- src/core/receiver/gnss_flowgraph.h | 2 +- 10 files changed, 15 insertions(+), 16 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 425dbb968..f66353f7e 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -40,7 +40,7 @@ using google::LogMessage; Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, std::shared_ptr pass_through, std::shared_ptr acq, std::shared_ptr trk, std::shared_ptr nav, - std::string role, std::string implementation, boost::shared_ptr queue) + std::string role, std::string implementation, gr::msg_queue::sptr queue) { pass_through_ = pass_through; acq_ = acq; diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index a05697eed..8e0893843 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -105,7 +105,7 @@ private: bool connected_; bool repeat_; ChannelFsm channel_fsm_; - boost::shared_ptr queue_; + gr::msg_queue::sptr queue_; }; #endif /*GNSS_SDR_CHANNEL_H_*/ diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 85905bfd8..f147a57ef 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -194,7 +194,7 @@ void ChannelFsm::set_tracking(std::shared_ptr tracking) trk_ = tracking; } -void ChannelFsm::set_queue(boost::shared_ptr queue) +void ChannelFsm::set_queue(gr::msg_queue::sptr queue) { queue_ = queue; } diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 868388b7b..997056723 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -59,7 +59,7 @@ public: void set_acquisition(std::shared_ptr acquisition); void set_tracking(std::shared_ptr tracking); - void set_queue(boost::shared_ptr queue); + void set_queue(gr::msg_queue::sptr queue); void set_channel(unsigned int channel); void start_acquisition(); void start_tracking(); @@ -77,7 +77,7 @@ public: private: std::shared_ptr acq_; std::shared_ptr trk_; - boost::shared_ptr queue_; + gr::msg_queue::sptr queue_; unsigned int channel_; }; diff --git a/src/core/receiver/control_message_factory.cc b/src/core/receiver/control_message_factory.cc index 6f7994c29..a7da90ab1 100644 --- a/src/core/receiver/control_message_factory.cc +++ b/src/core/receiver/control_message_factory.cc @@ -44,18 +44,18 @@ ControlMessageFactory::~ControlMessageFactory() {} -boost::shared_ptr ControlMessageFactory::GetQueueMessage(unsigned int who, unsigned int what) +gr::message::sptr ControlMessageFactory::GetQueueMessage(unsigned int who, unsigned int what) { std::shared_ptr control_message = std::make_shared(); control_message->who = who; control_message->what = what; - boost::shared_ptr queue_message = gr::message::make(0, 0, 0, sizeof(ControlMessage)); + gr::message::sptr queue_message = gr::message::make(0, 0, 0, sizeof(ControlMessage)); memcpy(queue_message->msg(), control_message.get(), sizeof(ControlMessage)); return queue_message; } -std::shared_ptr>> ControlMessageFactory::GetControlMessages(boost::shared_ptr queue_message) +std::shared_ptr>> ControlMessageFactory::GetControlMessages(gr::message::sptr queue_message) { std::shared_ptr>> control_messages = std::make_shared>>(); unsigned int control_messages_count = queue_message->length() / sizeof(ControlMessage); diff --git a/src/core/receiver/control_message_factory.h b/src/core/receiver/control_message_factory.h index 2ff294a8c..9bdd49d32 100644 --- a/src/core/receiver/control_message_factory.h +++ b/src/core/receiver/control_message_factory.h @@ -58,7 +58,7 @@ public: //! Virtual destructor virtual ~ControlMessageFactory(); - boost::shared_ptr GetQueueMessage(unsigned int who, unsigned int what); + gr::message::sptr GetQueueMessage(unsigned int who, unsigned int what); std::shared_ptr>> GetControlMessages(gr::message::sptr queue_message); }; diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index 44cdc36d3..73b05651c 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -158,7 +158,7 @@ void ControlThread::run() } -void ControlThread::set_control_queue(boost::shared_ptr control_queue) +void ControlThread::set_control_queue(gr::msg_queue::sptr control_queue) { if (flowgraph_->running()) { @@ -445,7 +445,7 @@ void ControlThread::init() void ControlThread::read_control_messages() { DLOG(INFO) << "Reading control messages from queue"; - boost::shared_ptr queue_message = control_queue_->delete_head(); + gr::message::sptr queue_message = control_queue_->delete_head(); if (queue_message != 0) { control_messages_ = control_message_factory_->GetControlMessages(queue_message); diff --git a/src/core/receiver/control_thread.h b/src/core/receiver/control_thread.h index 1580695df..ea0decec8 100644 --- a/src/core/receiver/control_thread.h +++ b/src/core/receiver/control_thread.h @@ -89,7 +89,7 @@ public: * * \param[in] boost::shared_ptr control_queue */ - void set_control_queue(boost::shared_ptr control_queue); + void set_control_queue(gr::msg_queue::sptr control_queue); unsigned int processed_control_messages() @@ -146,7 +146,7 @@ private: void apply_action(unsigned int what); std::shared_ptr flowgraph_; std::shared_ptr configuration_; - boost::shared_ptr control_queue_; + gr::msg_queue::sptr control_queue_; std::shared_ptr control_message_factory_; std::shared_ptr>> control_messages_; bool stop_; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 609908097..8bbd9a20c 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -51,8 +51,7 @@ using google::LogMessage; -GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr configuration, - boost::shared_ptr queue) +GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr configuration, gr::msg_queue::sptr queue) { connected_ = false; running_ = false; diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index d765d3cf4..4998d7170 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -139,7 +139,7 @@ private: std::vector> channels_; gnss_sdr_sample_counter_sptr ch_out_sample_counter; gr::top_block_sptr top_block_; - boost::shared_ptr queue_; + gr::msg_queue::sptr queue_; std::list available_GNSS_signals_; std::vector channels_state_; }; From 8932e9a51bcf6ce68fbf1d51027e4837eb95147c Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 21 Dec 2017 16:20:11 +0100 Subject: [PATCH 05/18] Clean acquisition interface --- src/core/interfaces/acquisition_interface.h | 1 - src/core/receiver/gnss_block_factory.cc | 16 ++++++++-------- src/core/receiver/gnss_block_factory.h | 18 +++++++++--------- 3 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/core/interfaces/acquisition_interface.h b/src/core/interfaces/acquisition_interface.h index 722b99777..b6d9e4e72 100644 --- a/src/core/interfaces/acquisition_interface.h +++ b/src/core/interfaces/acquisition_interface.h @@ -53,7 +53,6 @@ templateclass concurrent_queue; class AcquisitionInterface: public GNSSBlockInterface { public: - //virtual void set_active(bool active) = 0; virtual void set_gnss_synchro(Gnss_Synchro* gnss_synchro) = 0; virtual void set_channel(unsigned int channel) = 0; virtual void set_threshold(float threshold) = 0; diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index c3e3889fb..bdf34dc22 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -151,7 +151,7 @@ GNSSBlockFactory::~GNSSBlockFactory() std::unique_ptr GNSSBlockFactory::GetSignalSource( - std::shared_ptr configuration, boost::shared_ptr queue, int ID) + std::shared_ptr configuration, gr::msg_queue::sptr queue, int ID) { std::string default_implementation = "File_Signal_Source"; std::string role = "SignalSource"; //backwards compatibility for old conf files @@ -264,7 +264,7 @@ std::unique_ptr GNSSBlockFactory::GetPVT(std::shared_ptr GNSSBlockFactory::GetChannel_1C( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { //"appendix" is added to the "role" with the aim of Acquisition, Tracking and Telemetry Decoder adapters //can find their specific configurations when they read the config @@ -332,7 +332,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_1C( std::unique_ptr GNSSBlockFactory::GetChannel_2S( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { LOG(INFO) << "Instantiating Channel " << channel << " with Acquisition Implementation: " @@ -397,7 +397,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_2S( std::unique_ptr GNSSBlockFactory::GetChannel_1B( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { std::stringstream stream; stream << channel; @@ -464,7 +464,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_1B( std::unique_ptr GNSSBlockFactory::GetChannel_5X( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { std::stringstream stream; stream << channel; @@ -531,7 +531,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_5X( std::unique_ptr GNSSBlockFactory::GetChannel_L5( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { std::stringstream stream; stream << channel; @@ -597,7 +597,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_L5( std::unique_ptr>> GNSSBlockFactory::GetChannels( - std::shared_ptr configuration, boost::shared_ptr queue) + std::shared_ptr configuration, gr::msg_queue::sptr queue) { std::string default_implementation = "Pass_Through"; std::string tracking_implementation; @@ -783,7 +783,7 @@ std::unique_ptr GNSSBlockFactory::GetBlock( std::shared_ptr configuration, std::string role, std::string implementation, unsigned int in_streams, - unsigned int out_streams, boost::shared_ptr queue) + unsigned int out_streams, gr::msg_queue::sptr queue) { std::unique_ptr block; diff --git a/src/core/receiver/gnss_block_factory.h b/src/core/receiver/gnss_block_factory.h index 516b9e8e9..bba584d7a 100644 --- a/src/core/receiver/gnss_block_factory.h +++ b/src/core/receiver/gnss_block_factory.h @@ -57,7 +57,7 @@ public: GNSSBlockFactory(); virtual ~GNSSBlockFactory(); std::unique_ptr GetSignalSource(std::shared_ptr configuration, - boost::shared_ptr queue, int ID = -1); + gr::msg_queue::sptr queue, int ID = -1); std::unique_ptr GetSignalConditioner(std::shared_ptr configuration, int ID = -1); @@ -66,7 +66,7 @@ public: std::unique_ptr GetObservables(std::shared_ptr configuration); std::unique_ptr>> GetChannels(std::shared_ptr configuration, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); /* * \brief Returns the block with the required configuration and implementation @@ -74,29 +74,29 @@ public: std::unique_ptr GetBlock(std::shared_ptr configuration, std::string role, std::string implementation, unsigned int in_streams, unsigned int out_streams, - boost::shared_ptr queue = nullptr); + gr::msg_queue::sptr queue = nullptr); private: std::unique_ptr GetChannel_1C(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_2S(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_1B(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_5X(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_L5(std::shared_ptr configuration, - std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + std::string acq, std::string trk, std::string tlm, int channel, + gr::msg_queue::sptr queue); std::unique_ptr GetAcqBlock( std::shared_ptr configuration, From 3eafe2047aa2753d66d23bc4edbb1763eb9b74df Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 2 Jan 2018 12:29:15 +0100 Subject: [PATCH 06/18] Fix deadlock flowgraph --- src/algorithms/channel/libs/channel_fsm.cc | 19 +++++-------------- .../gps_l1_ca_telemetry_decoder_cc.cc | 2 +- src/core/receiver/gnss_flowgraph.cc | 18 +++++++----------- 3 files changed, 13 insertions(+), 26 deletions(-) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index f147a57ef..daef2f9ad 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -62,10 +62,8 @@ struct channel_idle_fsm_S0: public sc::state public: // sc::transition(event, next state) typedef sc::transition reactions; - channel_idle_fsm_S0(my_context ctx) : my_base(ctx) - { - //std::cout << "Enter Channel_Idle_S0 " << std::endl; - } + channel_idle_fsm_S0(my_context ctx) : my_base(ctx){} + }; @@ -78,13 +76,10 @@ public: channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx) { - //std::cout << "Enter Channel_Acq_S1 " << std::endl; context ().start_acquisition(); } - ~channel_acquiring_fsm_S1() - { - //std::cout << "Exit Channel_Acq_S1 " << std::endl; - } + ~channel_acquiring_fsm_S1(){} + }; @@ -96,13 +91,11 @@ public: channel_tracking_fsm_S2(my_context ctx) : my_base(ctx) { - //std::cout << "Enter Channel_tracking_S2 " << std::endl; context ().start_tracking(); } ~channel_tracking_fsm_S2() { - //std::cout << "Exit Channel_tracking_S2 " << std::endl; context ().notify_stop_tracking(); } @@ -117,10 +110,9 @@ public: channel_waiting_fsm_S3(my_context ctx) : my_base(ctx) { - //std::cout << "Enter Channel_waiting_S3 " << std::endl; context ().request_satellite(); } - // ~channel_waiting_fsm_S3(){} + ~channel_waiting_fsm_S3(){} }; @@ -148,7 +140,6 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); - //std::cout<<"Ev_channel_start_acquisition launched"<start_acquisition(); available_GNSS_signals_.pop_front(); - LOG(INFO) << "Channel " << i << " assigned to " << available_GNSS_signals_.front(); + LOG(INFO) << "Channel " << i << " assigned to " << channels_.at(i)->get_signal(); LOG(INFO) << "Channel " << i << " connected to observables and ready for acquisition"; } else @@ -367,12 +367,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 << ". Number of applied actions = " << applied_actions_; - VLOG(-100) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_; switch (what) { case 0: - DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); - VLOG(-100) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + LOG(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 ) @@ -382,14 +380,12 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } channels_.at(who)->set_signal(available_GNSS_signals_.front()); available_GNSS_signals_.pop_front(); - DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); - VLOG(-100) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + LOG(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: - DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); - VLOG(-100) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); + LOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); channels_state_[who] = 2; acq_channels_count_--; for (unsigned int i = 0; i < channels_count_; i++) @@ -435,20 +431,20 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) break; case 2: - DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + LOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); DLOG(INFO) << "Number of channels in acquisition = " << acq_channels_count_; - VLOG(-100) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); - VLOG(-100) << "Number of channels in acquisition = " << acq_channels_count_; if (acq_channels_count_ < max_acq_channels_) { channels_state_[who] = 1; acq_channels_count_++; + LOG(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(); } else { channels_state_[who] = 0; + LOG(INFO) << "Channel "<< who << " Idle state"; available_GNSS_signals_.push_back( channels_.at(who)->get_signal() ); } break; From b28c244df5cea69f9f1d8236271d14d7001979d9 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 11:40:17 +0100 Subject: [PATCH 07/18] Debug log lines --- .../acquisition/gnuradio_blocks/pcps_acquisition_cc.cc | 4 +++- .../acquisition/gnuradio_blocks/pcps_acquisition_cc.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index a0461f9c5..fdcd895b4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -329,6 +329,7 @@ int pcps_acquisition_cc::general_work(int noutput_items, d_input_power = 0.0; d_test_statistics = 0.0; d_state = 1; + LOG(INFO) << "ACQ. general_work changing to state 1 (active)"; } d_sample_counter += d_fft_size * ninput_items[0]; // sample counter @@ -368,8 +369,9 @@ int pcps_acquisition_cc::general_work(int noutput_items, if( d_blocking ) { + LOG(INFO) << "ACQ. general_work waiting for acquisition_core"; d_cond.wait( lk, [&]{ return !this->d_new_data_available; } ); - + LOG(INFO) << "ACQ. Exiting general_work"; } } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 486468379..1c6162492 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -60,6 +60,7 @@ #include #include #include "gnss_synchro.h" +#include class pcps_acquisition_cc; @@ -194,6 +195,7 @@ public: { gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_active = active; + LOG(INFO) << "ACQ RESET"; } /*! From 47c752c141ba95bb3c25c424c7d6f5c4c8ea1e5b Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 12:44:58 +0100 Subject: [PATCH 08/18] More debug lines --- src/algorithms/channel/adapters/channel.cc | 1 + src/algorithms/channel/libs/channel_fsm.cc | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index f66353f7e..ed8832c3f 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -202,5 +202,6 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { channel_fsm_.Event_start_acquisition(); + LOG(INFO) << "Channel start_acquisition()"; } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index daef2f9ad..0d5a1606e 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -140,6 +140,7 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); + LOG(INFO) << "FSM Event_start_acquisition" DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; } @@ -198,6 +199,7 @@ void ChannelFsm::set_channel(unsigned int channel) void ChannelFsm::start_acquisition() { acq_->reset(); + LOG(INFO) << "FSM. start_acquisition()"; } void ChannelFsm::start_tracking() From 3ddea680ad9ad9437e5420b3f2489c263e26154b Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 12:48:04 +0100 Subject: [PATCH 09/18] debug1 --- src/algorithms/channel/libs/channel_fsm.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 0d5a1606e..dd3dbbd29 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -140,7 +140,7 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); - LOG(INFO) << "FSM Event_start_acquisition" + LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; } From 6dd3abfdea8e0bf6cbb2ef4fbf5000a95f015c04 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 15:47:16 +0100 Subject: [PATCH 10/18] debug2 --- src/algorithms/channel/libs/channel_fsm.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index dd3dbbd29..68e0798aa 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -139,6 +139,7 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { + initiate(); this->process_event(Ev_channel_start_acquisition()); LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; From 540221e227d0cba95bed6a6f6d1504801e6d9b6d Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 4 Jan 2018 10:51:47 +0100 Subject: [PATCH 11/18] New Channel FSM --- src/algorithms/channel/adapters/channel.cc | 6 +-- src/algorithms/channel/libs/channel_fsm.cc | 49 ++++++++++++---------- src/algorithms/channel/libs/channel_fsm.h | 30 ++++++------- 3 files changed, 42 insertions(+), 43 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index ed8832c3f..8689a8d94 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -103,10 +103,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, // Destructor -Channel::~Channel() -{ - channel_fsm_.terminate(); -} +Channel::~Channel(){} void Channel::connect(gr::top_block_sptr top_block) @@ -139,7 +136,6 @@ void Channel::connect(gr::top_block_sptr top_block) top_block->msg_connect(nav_->get_left_block(), pmt::mp("preamble_timestamp_s"), trk_->get_right_block(), pmt::mp("preamble_timestamp_s")); DLOG(INFO) << "MSG FEEDBACK CHANNEL telemetry_decoder -> tracking"; - //std::cout<<"has port: "<get_right_block()->has_msg_port(pmt::mp("events"))<msg_connect(acq_->get_right_block(), pmt::mp("events"), channel_msg_rx, pmt::mp("events")); top_block->msg_connect(trk_->get_right_block(), pmt::mp("events"), channel_msg_rx, pmt::mp("events")); diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 68e0798aa..e5cb34493 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -30,15 +30,10 @@ #include "channel_fsm.h" #include -#include -#include -#include -#include -#include #include #include "control_message_factory.h" - +/* struct Ev_channel_start_acquisition: sc::event {}; @@ -53,10 +48,9 @@ struct Ev_channel_failed_acquisition_no_repeat: sc::event {}; +*/ -//struct Ev_channel_failed_tracking_reacq: sc::event -//{}; - +/* struct channel_idle_fsm_S0: public sc::state { public: @@ -114,7 +108,7 @@ public: } ~channel_waiting_fsm_S3(){} }; - +*/ ChannelFsm::ChannelFsm() @@ -122,7 +116,7 @@ ChannelFsm::ChannelFsm() acq_ = nullptr; trk_ = nullptr; channel_ = 0; - initiate(); //start the FSM + d_state = 0; } @@ -132,51 +126,60 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : { trk_ = nullptr; channel_ = 0; - initiate(); //start the FSM + d_state = 0; } void ChannelFsm::Event_start_acquisition() { - initiate(); - this->process_event(Ev_channel_start_acquisition()); + mx.lock(); + d_state = 1; + start_acquisition(); LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; + mx.unlock(); } void ChannelFsm::Event_valid_acquisition() { - this->process_event(Ev_channel_valid_acquisition()); + mx.lock(); + d_state = 2; + start_tracking(); DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; + mx.unlock(); } void ChannelFsm::Event_failed_acquisition_repeat() { - this->process_event(Ev_channel_failed_acquisition_repeat()); + mx.lock(); + d_state = 1; + start_acquisition(); DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; + mx.unlock(); } void ChannelFsm::Event_failed_acquisition_no_repeat() { - this->process_event(Ev_channel_failed_acquisition_no_repeat()); + mx.lock(); + d_state = 3; + request_satellite(); DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; + mx.unlock(); } -// Something is wrong here, we are using a memory after it ts freed void ChannelFsm::Event_failed_tracking_standby() { - this->process_event(Ev_channel_failed_tracking_standby()); + mx.lock(); + d_state = 0; + notify_stop_tracking(); DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; + mx.unlock(); } -//void ChannelFsm::Event_failed_tracking_reacq() { -// this->process_event(Ev_channel_failed_tracking_reacq()); -//} - void ChannelFsm::set_acquisition(std::shared_ptr acquisition) { acq_ = acquisition; diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 997056723..02be4f1fc 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -1,12 +1,12 @@ /*! * \file channel_fsm.h - * \brief Interface of the State Machine for channel using boost::statechart - * \author Luis Esteve, 2011. luis(at)epsilon-formacion.com + * \brief Interface of the State Machine for channel + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es * * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -32,26 +32,23 @@ #ifndef GNSS_SDR_CHANNEL_FSM_H #define GNSS_SDR_CHANNEL_FSM_H - -#include +#include #include #include "acquisition_interface.h" #include "tracking_interface.h" #include "telemetry_decoder_interface.h" - -namespace sc = boost::statechart; -namespace mpl = boost::mpl; - +/* struct channel_idle_fsm_S0; struct channel_acquiring_fsm_S1; struct channel_tracking_fsm_S2; struct channel_waiting_fsm_S3; +*/ /*! * \brief This class implements a State Machine for channel using boost::statechart */ -class ChannelFsm: public sc::state_machine +class ChannelFsm { public: ChannelFsm(); @@ -61,24 +58,27 @@ public: void set_tracking(std::shared_ptr tracking); void set_queue(gr::msg_queue::sptr queue); void set_channel(unsigned int channel); - void start_acquisition(); - void start_tracking(); - void request_satellite(); - void notify_stop_tracking(); //FSM EVENTS void Event_start_acquisition(); void Event_valid_acquisition(); void Event_failed_acquisition_repeat(); void Event_failed_acquisition_no_repeat(); - //void Event_gps_failed_tracking_reacq(); void Event_failed_tracking_standby(); private: + + void start_acquisition(); + void start_tracking(); + void request_satellite(); + void notify_stop_tracking(); + std::shared_ptr acq_; std::shared_ptr trk_; gr::msg_queue::sptr queue_; unsigned int channel_; + unsigned int d_state; + std::mutex mx; }; #endif /*GNSS_SDR_CHANNEL_FSM_H*/ From bbf8587970d6bfd4ac2d0b9a7df238b80a441d98 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 4 Jan 2018 12:16:12 +0100 Subject: [PATCH 12/18] Clean code --- .../gnuradio_blocks/pcps_acquisition_cc.cc | 3 - .../gnuradio_blocks/pcps_acquisition_cc.h | 1 - src/algorithms/channel/libs/channel_fsm.cc | 85 +------------------ src/algorithms/channel/libs/channel_fsm.h | 9 +- src/core/receiver/gnss_flowgraph.cc | 23 ----- 5 files changed, 4 insertions(+), 117 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index fdcd895b4..60ad2e82c 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -329,7 +329,6 @@ int pcps_acquisition_cc::general_work(int noutput_items, d_input_power = 0.0; d_test_statistics = 0.0; d_state = 1; - LOG(INFO) << "ACQ. general_work changing to state 1 (active)"; } d_sample_counter += d_fft_size * ninput_items[0]; // sample counter @@ -369,9 +368,7 @@ int pcps_acquisition_cc::general_work(int noutput_items, if( d_blocking ) { - LOG(INFO) << "ACQ. general_work waiting for acquisition_core"; d_cond.wait( lk, [&]{ return !this->d_new_data_available; } ); - LOG(INFO) << "ACQ. Exiting general_work"; } } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 1c6162492..e33746f2b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -195,7 +195,6 @@ public: { gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_active = active; - LOG(INFO) << "ACQ RESET"; } /*! diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index e5cb34493..75ac0e3da 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -1,11 +1,11 @@ /*! * \file channel_fsm.cc - * \brief Implementation of a State Machine for channel using boost::statechart - * \author Luis Esteve, 2011. luis(at)epsilon-formacion.com + * \brief Implementation of a State Machine for channel + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -33,83 +33,6 @@ #include #include "control_message_factory.h" -/* -struct Ev_channel_start_acquisition: sc::event -{}; - -struct Ev_channel_valid_acquisition: sc::event -{}; - -struct Ev_channel_failed_acquisition_repeat: sc::event -{}; - -struct Ev_channel_failed_acquisition_no_repeat: sc::event -{}; - -struct Ev_channel_failed_tracking_standby: sc::event -{}; -*/ - -/* -struct channel_idle_fsm_S0: public sc::state -{ -public: - // sc::transition(event, next state) - typedef sc::transition reactions; - channel_idle_fsm_S0(my_context ctx) : my_base(ctx){} - -}; - - -struct channel_acquiring_fsm_S1: public sc::state -{ -public: - typedef mpl::list, - sc::transition, - sc::transition > reactions; - - channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx) - { - context ().start_acquisition(); - } - ~channel_acquiring_fsm_S1(){} - -}; - - -struct channel_tracking_fsm_S2: public sc::state -{ -public: - typedef mpl::list, - sc::transition> reactions; - - channel_tracking_fsm_S2(my_context ctx) : my_base(ctx) - { - context ().start_tracking(); - } - - ~channel_tracking_fsm_S2() - { - context ().notify_stop_tracking(); - } - -}; - - -struct channel_waiting_fsm_S3: public sc::state -{ -public: - typedef sc::transition reactions; - - channel_waiting_fsm_S3(my_context ctx) : my_base(ctx) - { - context ().request_satellite(); - } - ~channel_waiting_fsm_S3(){} -}; -*/ - ChannelFsm::ChannelFsm() { @@ -136,7 +59,6 @@ void ChannelFsm::Event_start_acquisition() mx.lock(); d_state = 1; start_acquisition(); - LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; mx.unlock(); } @@ -203,7 +125,6 @@ void ChannelFsm::set_channel(unsigned int channel) void ChannelFsm::start_acquisition() { acq_->reset(); - LOG(INFO) << "FSM. start_acquisition()"; } void ChannelFsm::start_tracking() diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 02be4f1fc..55068dc85 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -38,15 +38,8 @@ #include "tracking_interface.h" #include "telemetry_decoder_interface.h" -/* -struct channel_idle_fsm_S0; -struct channel_acquiring_fsm_S1; -struct channel_tracking_fsm_S2; -struct channel_waiting_fsm_S3; -*/ - /*! - * \brief This class implements a State Machine for channel using boost::statechart + * \brief This class implements a State Machine for channel */ class ChannelFsm { diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index ed47b1a3e..c63b56435 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -405,29 +405,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } 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++) - { - if (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(); - break; - } - DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; - } - } - */ break; case 2: From 3a37345d4c4bf8269f192ff30de10d82529e8e2e Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 4 Jan 2018 12:33:13 +0100 Subject: [PATCH 13/18] Minor changes --- src/algorithms/channel/adapters/channel.cc | 2 +- src/algorithms/channel/libs/channel_fsm.cc | 3 ++- src/algorithms/channel/libs/channel_fsm.h | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 8689a8d94..f4c7fa77d 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -198,6 +198,6 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { channel_fsm_.Event_start_acquisition(); - LOG(INFO) << "Channel start_acquisition()"; + DLOG(INFO) << "Channel start_acquisition()"; } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 75ac0e3da..f218981c6 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -1,7 +1,8 @@ /*! * \file channel_fsm.cc * \brief Implementation of a State Machine for channel - * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * \authors Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * Luis Esteve, 2011. luis(at)epsilon-formacion.com * * ------------------------------------------------------------------------- * diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 55068dc85..5cca3b53e 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -1,8 +1,8 @@ /*! * \file channel_fsm.h * \brief Interface of the State Machine for channel - * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es - * + * \authors Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * Luis Esteve, 2011. luis(at)epsilon-formacion.com * * ------------------------------------------------------------------------- * From cfc0a4a498b856060933426d1408b96fe3ead774 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 5 Jan 2018 11:50:52 +0100 Subject: [PATCH 14/18] Clean flowgraph --- src/algorithms/channel/libs/channel_fsm.cc | 1 - src/algorithms/channel/libs/channel_fsm.h | 1 + src/core/receiver/gnss_flowgraph.cc | 41 +++++++++------------- src/core/receiver/gnss_flowgraph.h | 4 +-- 4 files changed, 20 insertions(+), 27 deletions(-) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index f218981c6..499d55881 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -30,7 +30,6 @@ */ #include "channel_fsm.h" -#include #include #include "control_message_factory.h" diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 5cca3b53e..29f741bfc 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 +#include #include #include "acquisition_interface.h" #include "tracking_interface.h" diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index c63b56435..cf1226102 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -33,7 +33,6 @@ */ #include "gnss_flowgraph.h" -#include #include #include #include @@ -284,12 +283,7 @@ void GNSSFlowgraph::connect() } std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str(); // use channel's implicit signal! - while (gnss_signal.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()); + channels_.at(i)->set_signal(search_next_signal(gnss_signal, false)); if (channels_state_[i] == 1) { @@ -370,17 +364,10 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) 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 ) - { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - } - channels_.at(who)->set_signal(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + channels_.at(who)->set_signal(search_next_signal(channels_.at(who)->get_signal().get_signal_str(), true)); + 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; @@ -393,13 +380,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) 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(); + channels_.at(i)->set_signal(search_next_signal(channels_.at(i)->get_signal().get_signal_str(), true)); acq_channels_count_++; channels_.at(i)->start_acquisition(); } @@ -761,3 +742,15 @@ void GNSSFlowgraph::set_channels_state() acq_channels_count_ = max_acq_channels_; DLOG(INFO) << acq_channels_count_ << " channels in acquisition state"; } + +Gnss_Signal GNSSFlowgraph::search_next_signal(std::string searched_signal, bool pop) +{ + while(searched_signal.compare(available_GNSS_signals_.front().get_signal_str()) != 0) + { + available_GNSS_signals_.push_back(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + } + Gnss_Signal result = available_GNSS_signals_.front(); + if(pop){available_GNSS_signals_.pop_front();} + return result; +} diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index 4998d7170..abb4a6ef5 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -63,8 +63,7 @@ public: /*! * \brief Constructor that initializes the receiver flowgraph */ - GNSSFlowgraph(std::shared_ptr configuration, - boost::shared_ptr queue); + GNSSFlowgraph(std::shared_ptr configuration, gr::msg_queue::sptr queue); /*! * \brief Virtual destructor @@ -119,6 +118,7 @@ private: void set_signals_list(); void set_channels_state(); // Initializes the channels state (start acquisition or keep standby) // using the configuration parameters (number of channels and max channels in acquisition) + Gnss_Signal search_next_signal(std::string searched_signal, bool pop); bool connected_; bool running_; int sources_count_; From 74a1f76282fee1b42407820b3cf18deb09bf7052 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 8 Jan 2018 10:57:01 +0100 Subject: [PATCH 15/18] Avoid multithreading collision --- src/algorithms/channel/adapters/channel.cc | 8 +- src/algorithms/channel/libs/channel_fsm.cc | 103 ++++++++++++------ src/algorithms/channel/libs/channel_fsm.h | 10 +- .../channel/libs/channel_msg_receiver_cc.cc | 13 ++- 4 files changed, 90 insertions(+), 44 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index f4c7fa77d..353f5a080 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -197,7 +197,13 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { - channel_fsm_.Event_start_acquisition(); + bool result = false; + result = channel_fsm_.Event_start_acquisition(); + if(!result) + { + LOG(WARNING) << "Invalid channel event"; + return; + } DLOG(INFO) << "Channel start_acquisition()"; } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 499d55881..935c696ce 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -53,53 +53,88 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : } - -void ChannelFsm::Event_start_acquisition() +bool ChannelFsm::Event_start_acquisition() { - mx.lock(); - d_state = 1; - start_acquisition(); - DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; - mx.unlock(); + std::lock_guard lk(mx); + if((d_state == 1) || (d_state == 2)) + { + return false; + } + else + { + d_state = 1; + start_acquisition(); + DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; + return true; + } } -void ChannelFsm::Event_valid_acquisition() +bool ChannelFsm::Event_valid_acquisition() { - mx.lock(); - d_state = 2; - start_tracking(); - DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; - mx.unlock(); + std::lock_guard lk(mx); + if(d_state != 1) + { + return false; + } + else + { + d_state = 2; + start_tracking(); + DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; + return true; + } } -void ChannelFsm::Event_failed_acquisition_repeat() +bool ChannelFsm::Event_failed_acquisition_repeat() { - mx.lock(); - d_state = 1; - start_acquisition(); - DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; - mx.unlock(); -} - -void ChannelFsm::Event_failed_acquisition_no_repeat() -{ - mx.lock(); - d_state = 3; - request_satellite(); - DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; - mx.unlock(); + std::lock_guard lk(mx); + if(d_state != 1) + { + return false; + } + else + { + d_state = 1; + start_acquisition(); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; + return true; + } } -void ChannelFsm::Event_failed_tracking_standby() +bool ChannelFsm::Event_failed_acquisition_no_repeat() { - mx.lock(); - d_state = 0; - notify_stop_tracking(); - DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; - mx.unlock(); + std::lock_guard lk(mx); + if(d_state != 1) + { + return false; + } + else + { + d_state = 3; + request_satellite(); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; + return true; + } +} + + +bool ChannelFsm::Event_failed_tracking_standby() +{ + std::lock_guard lk(mx); + if(d_state != 2) + { + return false; + } + else + { + d_state = 0; + notify_stop_tracking(); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; + return true; + } } void ChannelFsm::set_acquisition(std::shared_ptr acquisition) diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 29f741bfc..23b92c394 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -54,11 +54,11 @@ public: void set_channel(unsigned int channel); //FSM EVENTS - void Event_start_acquisition(); - void Event_valid_acquisition(); - void Event_failed_acquisition_repeat(); - void Event_failed_acquisition_no_repeat(); - void Event_failed_tracking_standby(); + bool Event_start_acquisition(); + bool Event_valid_acquisition(); + bool Event_failed_acquisition_repeat(); + bool Event_failed_acquisition_no_repeat(); + bool Event_failed_tracking_standby(); private: diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc index d22f2dd73..a88fc6d88 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc @@ -44,26 +44,27 @@ channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fs void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) { + bool result = false; try { long int message = pmt::to_long(msg); switch (message) { case 1: //positive acquisition - d_channel_fsm->Event_valid_acquisition(); + result = d_channel_fsm->Event_valid_acquisition(); break; case 2: //negative acquisition if (d_repeat == true) { - d_channel_fsm->Event_failed_acquisition_repeat(); + result = d_channel_fsm->Event_failed_acquisition_repeat(); } else { - d_channel_fsm->Event_failed_acquisition_no_repeat(); + result = d_channel_fsm->Event_failed_acquisition_no_repeat(); } break; case 3: // tracking loss of lock event - d_channel_fsm->Event_failed_tracking_standby(); + result = d_channel_fsm->Event_failed_tracking_standby(); break; default: LOG(WARNING) << "Default case, invalid message."; @@ -74,6 +75,10 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) { LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; } + if(!result) + { + LOG(WARNING) << "msg_handler_telemetry invalid event"; + } } From 320b4e2d7bc9e7a9510df1458c14b1066399e1de Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 8 Jan 2018 12:03:32 +0100 Subject: [PATCH 16/18] Migrate channel_fsm pointer to std::shared_ptr --- src/algorithms/channel/adapters/channel.cc | 13 +++++++------ src/algorithms/channel/adapters/channel.h | 2 +- .../channel/libs/channel_msg_receiver_cc.cc | 4 ++-- .../channel/libs/channel_msg_receiver_cc.h | 8 ++++---- 4 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 353f5a080..45e2c0833 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -50,6 +50,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, implementation_ = implementation; channel_ = channel; queue_ = queue; + channel_fsm_ = std::make_shared(); flag_enable_fpga = configuration->property("Channel.enable_FPGA", false); acq_->set_channel(channel_); @@ -89,16 +90,16 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, repeat_ = configuration->property("Acquisition_" + implementation_ + boost::lexical_cast(channel_) + ".repeat_satellite", false); DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_; - channel_fsm_.set_acquisition(acq_); - channel_fsm_.set_tracking(trk_); - channel_fsm_.set_channel(channel_); - channel_fsm_.set_queue(queue_); + channel_fsm_->set_acquisition(acq_); + channel_fsm_->set_tracking(trk_); + channel_fsm_->set_channel(channel_); + channel_fsm_->set_queue(queue_); connected_ = false; gnss_signal_ = Gnss_Signal(implementation_); - channel_msg_rx = channel_msg_receiver_make_cc(&channel_fsm_, repeat_); + channel_msg_rx = channel_msg_receiver_make_cc(channel_fsm_, repeat_); } @@ -198,7 +199,7 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { bool result = false; - result = channel_fsm_.Event_start_acquisition(); + result = channel_fsm_->Event_start_acquisition(); if(!result) { LOG(WARNING) << "Invalid channel event"; diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index 8e0893843..405385814 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -104,7 +104,7 @@ private: Gnss_Signal gnss_signal_; bool connected_; bool repeat_; - ChannelFsm channel_fsm_; + std::shared_ptr channel_fsm_; gr::msg_queue::sptr queue_; }; diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc index a88fc6d88..7a80cfceb 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc @@ -37,7 +37,7 @@ using google::LogMessage; -channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat) +channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat) { return channel_msg_receiver_cc_sptr(new channel_msg_receiver_cc(channel_fsm, repeat)); } @@ -82,7 +82,7 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) } -channel_msg_receiver_cc::channel_msg_receiver_cc(ChannelFsm* channel_fsm, bool repeat) : +channel_msg_receiver_cc::channel_msg_receiver_cc(std::shared_ptr channel_fsm, bool repeat) : gr::block("channel_msg_receiver_cc", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) { this->message_port_register_in(pmt::mp("events")); diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.h b/src/algorithms/channel/libs/channel_msg_receiver_cc.h index 195d41e7c..77a91379a 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.h +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.h @@ -38,7 +38,7 @@ class channel_msg_receiver_cc; typedef boost::shared_ptr channel_msg_receiver_cc_sptr; -channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat); +channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat); /*! * \brief GNU Radio block that receives asynchronous channel messages from acquisition and tracking blocks @@ -46,11 +46,11 @@ channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fs class channel_msg_receiver_cc : public gr::block { private: - ChannelFsm* d_channel_fsm; + std::shared_ptr d_channel_fsm; bool d_repeat; // todo: change FSM to include repeat value - friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat); + friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat); void msg_handler_events(pmt::pmt_t msg); - channel_msg_receiver_cc(ChannelFsm* channel_fsm, bool repeat); + channel_msg_receiver_cc(std::shared_ptr channel_fsm, bool repeat); public: ~channel_msg_receiver_cc (); //!< Default destructor From a90edd025cf12d6eeff9492be177370a33b249dc Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 8 Jan 2018 12:54:35 +0100 Subject: [PATCH 17/18] Minor changes --- src/algorithms/channel/adapters/channel.cc | 2 ++ src/algorithms/channel/adapters/channel.h | 6 ++++-- src/algorithms/channel/libs/channel_fsm.cc | 4 ++++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 45e2c0833..e66074b7a 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -184,6 +184,7 @@ gr::basic_block_sptr Channel::get_right_block() void Channel::set_signal(const Gnss_Signal& gnss_signal) { + std::lock_guard lk(mx); gnss_signal_ = gnss_signal; std::string str_aux = gnss_signal_.get_signal_str(); const char * str = str_aux.c_str(); // get a C style null terminated string @@ -198,6 +199,7 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { + std::lock_guard lk(mx); bool result = false; result = channel_fsm_->Event_start_acquisition(); if(!result) diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index 405385814..8740b5ad9 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -37,6 +37,7 @@ #include #include +#include #include #include #include "channel_interface.h" @@ -63,8 +64,7 @@ public: Channel(ConfigurationInterface *configuration, unsigned int channel, std::shared_ptr pass_through, std::shared_ptr acq, std::shared_ptr trk, std::shared_ptr nav, - std::string role, std::string implementation, - boost::shared_ptr queue); + std::string role, std::string implementation, gr::msg_queue::sptr queue); //! Virtual destructor virtual ~Channel(); @@ -79,6 +79,7 @@ public: inline std::string implementation() override { return implementation_; } inline size_t item_size() override { return 0; } + inline Gnss_Signal get_signal() const override { return gnss_signal_; } void start_acquisition() override; //!< Start the State Machine @@ -106,6 +107,7 @@ private: bool repeat_; std::shared_ptr channel_fsm_; gr::msg_queue::sptr queue_; + std::mutex mx; }; #endif /*GNSS_SDR_CHANNEL_H_*/ diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 935c696ce..b6f23cbeb 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -139,21 +139,25 @@ bool ChannelFsm::Event_failed_tracking_standby() void ChannelFsm::set_acquisition(std::shared_ptr acquisition) { + std::lock_guard lk(mx); acq_ = acquisition; } void ChannelFsm::set_tracking(std::shared_ptr tracking) { + std::lock_guard lk(mx); trk_ = tracking; } void ChannelFsm::set_queue(gr::msg_queue::sptr queue) { + std::lock_guard lk(mx); queue_ = queue; } void ChannelFsm::set_channel(unsigned int channel) { + std::lock_guard lk(mx); channel_ = channel; } From 25127a0b273ac96f7381fe2256724cc88cccbf41 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 10 Jan 2018 16:24:09 +0100 Subject: [PATCH 18/18] Minor changes --- .../tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc | 2 ++ src/core/receiver/gnss_flowgraph.cc | 1 + 2 files changed, 3 insertions(+) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index ec5004c56..8911db3db 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -185,6 +185,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() { + gr::thread::scoped_lock lk(d_setlock); /* * correct the code phase according to the delay between acq and trk */ @@ -521,6 +522,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc() int Gps_L1_Ca_Dll_Pll_Tracking_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) { + gr::thread::scoped_lock lk(d_setlock); // process vars double carr_error_hz = 0.0; double carr_error_filt_hz = 0.0; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index cf1226102..a59d4e5d1 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -382,6 +382,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_state_[i] = 1; channels_.at(i)->set_signal(search_next_signal(channels_.at(i)->get_signal().get_signal_str(), true)); acq_channels_count_++; + DLOG(INFO) << "Channel "<< i << " Starting acquisition " << channels_.at(i)->get_signal().get_satellite() << ", Signal " << channels_.at(i)->get_signal().get_signal_str(); channels_.at(i)->start_acquisition(); } DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i];