bcm_sync.h
Go to the documentation of this file.
1 #ifndef H_BCM_SYNC
2 #define H_BCM_SYNC
3 
7 
8 namespace canopen {
9 
10 template<typename T > std::string join(const T &container, const std::string &delim){
11  if(container.empty()) return std::string();
12  std::stringstream sstr;
13  typename T::const_iterator it = container.begin();
14  sstr << *it;
15  for(++it; it != container.end(); ++it){
16  sstr << delim << *it;
17  }
18  return sstr.str();
19 }
20 
21 class BCMsync : public Layer {
22  boost::mutex mutex_;
23 
24  std::string device_;
25 
26  std::set<int> ignored_nodes_;
27  std::set<int> monitored_nodes_;
28  std::set<int> known_nodes_;
29  std::set<int> started_nodes_;
30 
34  uint16_t sync_ms_;
36 
37  std::vector<can::Frame> sync_frames_;
38 
39  bool skipNode(uint8_t id){
40  if(ignored_nodes_.find(id) != ignored_nodes_.end()) return true;
41  if(!monitored_nodes_.empty() && monitored_nodes_.find(id) == monitored_nodes_.end()) return true;
42  known_nodes_.insert(id);
43  return false;
44  }
45 
46  void handleFrame(const can::Frame &frame){
47  boost::mutex::scoped_lock lock(mutex_);
48 
49  if(frame.id == NMT_ID){
50  if(frame.dlc > 1){
51  uint8_t cmd = frame.data[0];
52  uint8_t id = frame.data[1];
53  if(skipNode(id)) return;
54 
55  if(cmd == 1){ // started
56  if(id != 0) started_nodes_.insert(id);
57  else started_nodes_.insert(known_nodes_.begin(), known_nodes_.end()); // start all
58  }else{
59  if(id != 0) started_nodes_.erase(id);
60  else started_nodes_.clear(); // stop all
61  }
62  }
63  }else if((frame.id & ~ALL_NODES_MASK) == HEARTBEAT_ID){
64  int id = frame.id & ALL_NODES_MASK;
65  if(skipNode(id)) return;
66 
67  if(frame.dlc > 0 && frame.data[0] == canopen::Node::Stopped) started_nodes_.erase(id);
68  }
69 
70  // toggle sync if needed
71  if(started_nodes_.empty() && sync_running_){
72  sync_running_ = !bcm_.stopTX(sync_frames_.front());
73  }else if(!started_nodes_.empty() && !sync_running_){
74  sync_running_ = bcm_.startTX(boost::chrono::milliseconds(sync_ms_),sync_frames_.front(), sync_frames_.size(), &sync_frames_[0]);
75  }
76  }
77 protected:
78  virtual void handleRead(LayerStatus &status, const LayerState &current_state) {
79  if(current_state > Init){
80  }
81  }
82  virtual void handleWrite(LayerStatus &status, const LayerState &current_state) {
83  if(current_state > Init){
84  }
85  }
86  virtual void handleDiag(LayerReport &report){
87  boost::mutex::scoped_lock lock(mutex_);
88  if(!sync_running_) report.warn("sync is not running");
89 
90  report.add("sync_running", sync_running_);
91  report.add("known_nodes", join(known_nodes_, ", "));
92  report.add("started_nodes", join(started_nodes_, ", "));
93  }
94 
95  virtual void handleInit(LayerStatus &status){
96  boost::mutex::scoped_lock lock(mutex_);
97  started_nodes_.clear();
98 
99  if(!bcm_.init(device_)){
100  status.error("BCM_init failed");
101  return;
102  }
103  int sc = driver_->getInternalSocket();
104 
105  struct can_filter filter[2];
106 
107  filter[0].can_id = NMT_ID;
108  filter[0].can_mask = CAN_SFF_MASK;
109  filter[1].can_id = HEARTBEAT_ID;
110  filter[1].can_mask = ~ALL_NODES_MASK;
111 
112  if(setsockopt(sc, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0){
113  status.warn("could not set filter");
114  return;
115  }
116 
117  handler_ = driver_->createMsgListenerM(this, &BCMsync::handleFrame);
118  }
119  virtual void handleShutdown(LayerStatus &status){
120  boost::mutex::scoped_lock lock(mutex_);
121  handler_.reset();
122  bcm_.shutdown();
123  }
124 
125  virtual void handleHalt(LayerStatus &status) {
126  boost::mutex::scoped_lock lock(mutex_);
127  if(sync_running_){
128  bcm_.stopTX(sync_frames_.front());
129  sync_running_ = false;
130  started_nodes_.clear();
131  }
132  }
133 
134  virtual void handleRecover(LayerStatus &status){
135  handleShutdown(status);
136  handleInit(status);
137  }
138 public:
139  static const uint32_t ALL_NODES_MASK = 127;
140  static const uint32_t HEARTBEAT_ID = 0x700;
141  static const uint32_t NMT_ID = 0x000;
142 
143  BCMsync(const std::string &device, can::SocketCANDriverSharedPtr driver, const SyncProperties &sync_properties)
144  : Layer(device + " SyncLayer"), device_(device), sync_running_(false), sync_ms_(sync_properties.period_ms_), driver_(driver) {
145  if(sync_properties.overflow_ == 0){
146  sync_frames_.resize(1);
147  sync_frames_[0] = can::Frame(sync_properties.header_,0);
148  }else{
149  sync_frames_.resize(sync_properties.overflow_);
150  for(int i = 0; i < sync_properties.overflow_; ++i){
151  sync_frames_[i] = can::Frame(sync_properties.header_,1);
152  sync_frames_[i].data[0] = i+1;
153  }
154  }
155  }
156  template <class T> void setMonitored(const T &other){
157  monitored_nodes_.clear();
158  monitored_nodes_.insert(other.begin(), other.end());
159  }
160  template <class T> void setIgnored(const T &other){
161  ignored_nodes_.clear();
162  ignored_nodes_.insert(other.begin(), other.end());
163  }
164 };
165 
166 }
167 #endif
std::array< value_type, 8 > data
const uint8_t overflow_
Definition: canopen.h:173
bool sync_running_
Definition: bcm_sync.h:31
uint16_t sync_ms_
Definition: bcm_sync.h:34
bool stopTX(Header header)
std::set< int > known_nodes_
Definition: bcm_sync.h:28
virtual void handleRecover(LayerStatus &status)
Definition: bcm_sync.h:134
string cmd
const void warn(const std::string &r)
Definition: layer.h:44
const can::Header header_
Definition: canopen.h:171
virtual void handleInit(LayerStatus &status)
Definition: bcm_sync.h:95
virtual void handleDiag(LayerReport &report)
Definition: bcm_sync.h:86
virtual void handleShutdown(LayerStatus &status)
Definition: bcm_sync.h:119
void handleFrame(const can::Frame &frame)
Definition: bcm_sync.h:46
void setMonitored(const T &other)
Definition: bcm_sync.h:156
void setIgnored(const T &other)
Definition: bcm_sync.h:160
static const uint32_t NMT_ID
Definition: bcm_sync.h:141
BCMsync(const std::string &device, can::SocketCANDriverSharedPtr driver, const SyncProperties &sync_properties)
Definition: bcm_sync.h:143
static const uint32_t ALL_NODES_MASK
Definition: bcm_sync.h:139
virtual void handleHalt(LayerStatus &status)
Definition: bcm_sync.h:125
std::set< int > started_nodes_
Definition: bcm_sync.h:29
CommInterface::FrameListenerConstSharedPtr FrameListenerConstSharedPtr
std::string device_
Definition: bcm_sync.h:24
std::vector< can::Frame > sync_frames_
Definition: bcm_sync.h:37
bool init(const std::string &device)
can::FrameListenerConstSharedPtr handler_
Definition: bcm_sync.h:35
std::shared_ptr< SocketCANDriver > SocketCANDriverSharedPtr
void add(const std::string &key, const T &value)
Definition: layer.h:52
std::set< int > ignored_nodes_
Definition: bcm_sync.h:26
const void error(const std::string &r)
Definition: layer.h:45
bool startTX(DurationType period, Header header, size_t num, Frame *frames)
virtual void handleWrite(LayerStatus &status, const LayerState &current_state)
Definition: bcm_sync.h:82
can::BCMsocket bcm_
Definition: bcm_sync.h:32
unsigned char dlc
bool skipNode(uint8_t id)
Definition: bcm_sync.h:39
unsigned int id
std::set< int > monitored_nodes_
Definition: bcm_sync.h:27
virtual void handleRead(LayerStatus &status, const LayerState &current_state)
Definition: bcm_sync.h:78
std::string join(const T &container, const std::string &delim)
Definition: bcm_sync.h:10
boost::mutex mutex_
Definition: bcm_sync.h:22
can::SocketCANDriverSharedPtr driver_
Definition: bcm_sync.h:33
static const uint32_t HEARTBEAT_ID
Definition: bcm_sync.h:140


canopen_master
Author(s): Mathias L├╝dtke
autogenerated on Tue May 4 2021 02:50:21