node.cpp
Go to the documentation of this file.
2 
3 using namespace canopen;
4 
5 #pragma pack(push) /* push current alignment to stack */
6 #pragma pack(1) /* set alignment to 1 byte boundary */
7 
8 
9 struct NMTcommand{
10  enum Command{
11  Start = 1,
12  Stop = 2,
13  Prepare = 128,
14  Reset = 129,
15  Reset_Com = 130
16  };
17  uint8_t command;
18  uint8_t node_id;
19 
20  struct Frame: public FrameOverlay<NMTcommand>{
21  Frame(uint8_t node_id, const Command &c) : FrameOverlay(can::Header()) {
22  data.command = c;
23  data.node_id = node_id;
24  }
25  };
26 };
27 
28 #pragma pack(pop) /* pop previous alignment from stack */
29 
30 Node::Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync)
31 : Layer("Node 301"), node_id_(node_id), interface_(interface), sync_(sync) , state_(Unknown), sdo_(interface, dict, node_id), pdo_(interface){
32  try{
33  getStorage()->entry(heartbeat_, 0x1017);
34  }
35  catch(const std::out_of_range){
36  }
37 }
38 
40  boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
41  return state_;
42 }
43 
45  boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
46  getStorage()->reset();
48  if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
49  return false;
50  }
53  return true;
54 }
55 bool Node::reset(){
56  boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
57  getStorage()->reset();
58 
60  if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
61  return false;
62  }
65  return true;
66 }
67 
69  boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
70  if(state_ == BootUp){
71  // ERROR
72  }
74  return 0 != wait_for(PreOperational, boost::chrono::seconds(2));
75 }
76 bool Node::start(){
77  boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
78  if(state_ == BootUp){
79  // ERROR
80  }
82  return 0 != wait_for(Operational, boost::chrono::seconds(2));
83 }
84 bool Node::stop(){
85  boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
86  if(sync_) sync_->removeNode(this);
87  if(state_ == BootUp){
88  // ERROR
89  }
91  return true;
92 }
93 
94 void Node::switchState(const uint8_t &s){
95  bool changed = state_ != s;
96  switch(s){
97  case Operational:
98  if(changed && sync_) sync_->addNode(this);
99  break;
100  case BootUp:
101  case PreOperational:
102  case Stopped:
103  if(changed && sync_) sync_->removeNode(this);
104  break;
105  default:
106  //error
107  ;
108  }
109  if(changed){
110  state_ = (State) s;
111  state_dispatcher_.dispatch(state_);
112  cond.notify_one();
113  }
114 }
115 void Node::handleNMT(const can::Frame & msg){
116  boost::mutex::scoped_lock cond_lock(cond_mutex);
117  uint16_t interval = getHeartbeatInterval();
118  if(interval) heartbeat_timeout_ = get_abs_time(boost::chrono::milliseconds(3*interval));
119  assert(msg.dlc == 1);
120  switchState(msg.data[0]);
121 }
122 template<typename T> int Node::wait_for(const State &s, const T &timeout){
123  boost::mutex::scoped_lock cond_lock(cond_mutex);
124  time_point abs_time = get_abs_time(timeout);
125 
126  while(s != state_) {
127  if(cond.wait_until(cond_lock,abs_time) == boost::cv_status::timeout)
128  {
129  break;
130  }
131  }
132  if( s!= state_){
133  if(getHeartbeatInterval() == 0){
134  switchState(s);
135  return -1;
136  }
137  return 0;
138  }
139  return 1;
140 }
142  if(getHeartbeatInterval() == 0) return true; //disabled
143  boost::mutex::scoped_lock cond_lock(cond_mutex);
144  return heartbeat_timeout_ >= boost::chrono::high_resolution_clock::now();
145 }
146 
147 
148 void Node::handleRead(LayerStatus &status, const LayerState &current_state) {
149  if(current_state > Init){
150  if(!checkHeartbeat()){
151  status.error("heartbeat problem");
152  } else if(getState() != Operational){
153  status.error("not operational");
154  } else{
155  pdo_.read(status);
156  }
157  }
158 }
159 void Node::handleWrite(LayerStatus &status, const LayerState &current_state) {
160  if(current_state > Init){
161  if(getState() != Operational) status.error("not operational");
162  else if(! pdo_.write()) status.error("PDO write problem");
163  }
164 }
165 
166 
168  State state = getState();
169  if(state != Operational){
170  report.error("Mode not operational");
171  report.add("Node state", (int)state);
172  }else if(!checkHeartbeat()){
173  report.error("Heartbeat timeout");
174  }
175 }
178 
179  sdo_.init();
180  try{
181  if(!reset_com()) BOOST_THROW_EXCEPTION( TimeoutException("reset_timeout") );
182  }
183  catch(const TimeoutException&){
184  status.error(boost::str(boost::format("could not reset node '%1%'") % (int)node_id_));
185  return;
186  }
187 
188  if(!pdo_.init(getStorage(), status)){
189  return;
190  }
191  getStorage()->init_all();
192  sdo_.init(); // reread SDO paramters;
193  // TODO: set SYNC data
194 
195  try{
196  if(!start()) BOOST_THROW_EXCEPTION( TimeoutException("start timeout") );
197  }
198  catch(const TimeoutException&){
199  status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_));
200  }
201 }
203  try{
204  start();
205  }
206  catch(const TimeoutException&){
207  status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_));
208  }
209 }
211  if(getHeartbeatInterval()> 0) heartbeat_.set(0);
212  stop();
213  nmt_listener_.reset();
215 }
217  // do nothing
218 }
bool init(const ObjectStorageSharedPtr storage, LayerStatus &status)
Definition: pdo.cpp:169
boost::shared_ptr< SyncCounter > SyncCounterSharedPtr
Definition: canopen.h:182
bool write()
Definition: pdo.cpp:326
ObjectStorage::Entry< ObjectStorage::DataType< ObjectDict::DEFTYPE_UNSIGNED16 >::type > heartbeat_
Definition: canopen.h:235
void switchState(const uint8_t &s)
Definition: node.cpp:94
Frame(uint8_t node_id, const Command &c)
Definition: node.cpp:21
void handleNMT(const can::Frame &msg)
Definition: node.cpp:115
const can::CommInterfaceSharedPtr interface_
Definition: canopen.h:231
const uint8_t node_id_
Definition: canopen.h:189
virtual void handleInit(LayerStatus &status)
Definition: node.cpp:176
virtual void handleDiag(LayerReport &report)
Definition: node.cpp:167
boost::atomic< LayerState > state
Definition: layer.h:143
boost::array< value_type, 8 > data
uint8_t node_id
Definition: node.cpp:18
boost::shared_ptr< CommInterface > CommInterfaceSharedPtr
int wait_for(const State &s, const T &timeout)
Definition: node.cpp:122
boost::chrono::high_resolution_clock::time_point time_point
Definition: canopen.h:18
const SyncCounterSharedPtr sync_
Definition: canopen.h:232
virtual void handleShutdown(LayerStatus &status)
Definition: node.cpp:210
void read(LayerStatus &status)
Definition: pdo.cpp:320
bool reset()
Definition: node.cpp:55
bool reset_com()
Definition: node.cpp:44
ObjectDict::ObjectDictSharedPtr ObjectDictSharedPtr
Definition: objdict.h:230
const ObjectStorageSharedPtr getStorage()
Definition: canopen.h:195
bool start()
Definition: node.cpp:76
boost::mutex cond_mutex
Definition: canopen.h:228
virtual void handleWrite(LayerStatus &status, const LayerState &current_state)
Definition: node.cpp:159
boost::chrono::high_resolution_clock::time_point heartbeat_timeout_
Definition: canopen.h:246
uint16_t getHeartbeatInterval()
Definition: canopen.h:247
SDOClient sdo_
Definition: canopen.h:243
const State getState()
Definition: node.cpp:39
bool prepare()
Definition: node.cpp:68
can::FrameListenerConstSharedPtr nmt_listener_
Definition: canopen.h:233
Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync=SyncCounterSharedPtr())
Definition: node.cpp:30
virtual void handleRecover(LayerStatus &status)
Definition: node.cpp:202
void setHeartbeatInterval()
Definition: canopen.h:248
void add(const std::string &key, const T &value)
Definition: layer.h:51
uint8_t command
Definition: node.cpp:17
PDOMapper pdo_
Definition: canopen.h:244
virtual void handleRead(LayerStatus &status, const LayerState &current_state)
Definition: node.cpp:148
boost::condition_variable cond
Definition: canopen.h:229
const void error(const std::string &r)
Definition: layer.h:44
unsigned char dlc
bool stop()
Definition: node.cpp:84
time_point get_abs_time(const time_duration &timeout)
Definition: canopen.h:20
virtual void handleHalt(LayerStatus &status)
Definition: node.cpp:216
State state_
Definition: canopen.h:242
bool checkHeartbeat()
Definition: node.cpp:141
can::SimpleDispatcher< StateListener > state_dispatcher_
Definition: canopen.h:237
boost::timed_mutex mutex
Definition: canopen.h:227


canopen_master
Author(s): Mathias Lüdtke
autogenerated on Fri May 14 2021 02:59:40