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 }
177  nmt_listener_ = interface_->createMsgListenerM(can::MsgHeader(0x700 + node_id_), this, &Node::handleNMT);
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 }
canopen::Node::stop
bool stop()
Definition: node.cpp:84
canopen::Node::wait_for
int wait_for(const State &s, const T &timeout)
Definition: node.cpp:122
canopen.h
can::Frame
canopen::Node::checkHeartbeat
bool checkHeartbeat()
Definition: node.cpp:141
canopen::Node::Unknown
@ Unknown
Definition: canopen.h:192
canopen::Node::Node
Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync=SyncCounterSharedPtr())
Definition: node.cpp:30
canopen::Node::handleRecover
virtual void handleRecover(LayerStatus &status)
Definition: node.cpp:202
canopen::Node::sdo_
SDOClient sdo_
Definition: canopen.h:250
canopen::Node::interface_
const can::CommInterfaceSharedPtr interface_
Definition: canopen.h:238
canopen::TimeoutException
Definition: exceptions.h:25
canopen::Node::reset_com
bool reset_com()
Definition: node.cpp:44
canopen::Node::setHeartbeatInterval
void setHeartbeatInterval()
Definition: canopen.h:255
canopen::LayerStatus::error
const void error(const std::string &r)
Definition: layer.h:45
canopen::Node::handleDiag
virtual void handleDiag(LayerReport &report)
Definition: node.cpp:167
NMTcommand::node_id
uint8_t node_id
Definition: node.cpp:18
canopen::LayerReport::add
void add(const std::string &key, const T &value)
Definition: layer.h:52
canopen::Node::heartbeat_timeout_
boost::chrono::high_resolution_clock::time_point heartbeat_timeout_
Definition: canopen.h:253
can::Frame::dlc
unsigned char dlc
canopen::Layer::Init
@ Init
Definition: layer.h:67
canopen::Node::handleNMT
void handleNMT(const can::Frame &msg)
Definition: node.cpp:115
canopen::Layer::LayerState
LayerState
Definition: layer.h:65
canopen::Node::BootUp
@ BootUp
Definition: canopen.h:192
can::MsgHeader
canopen::Node::switchState
void switchState(const uint8_t &s)
Definition: node.cpp:94
canopen::Node::start
bool start()
Definition: node.cpp:76
can::CommInterfaceSharedPtr
std::shared_ptr< CommInterface > CommInterfaceSharedPtr
canopen::Node::handleRead
virtual void handleRead(LayerStatus &status, const LayerState &current_state)
Definition: node.cpp:148
canopen::Node::cond
boost::condition_variable cond
Definition: canopen.h:236
canopen::time_point
boost::chrono::high_resolution_clock::time_point time_point
Definition: canopen.h:18
canopen::PDOMapper::init
bool init(const ObjectStorageSharedPtr storage, LayerStatus &status)
Definition: pdo.cpp:175
canopen::Node::cond_mutex
boost::mutex cond_mutex
Definition: canopen.h:235
canopen::Node::pdo_
PDOMapper pdo_
Definition: canopen.h:251
canopen::Node::node_id_
const uint8_t node_id_
Definition: canopen.h:194
canopen::SyncCounterSharedPtr
std::shared_ptr< SyncCounter > SyncCounterSharedPtr
Definition: canopen.h:187
canopen::Node::handleHalt
virtual void handleHalt(LayerStatus &status)
Definition: node.cpp:216
canopen::Node::PreOperational
@ PreOperational
Definition: canopen.h:192
canopen::Node::handleShutdown
virtual void handleShutdown(LayerStatus &status)
Definition: node.cpp:210
canopen::Node::state_dispatcher_
can::SimpleDispatcher< StateListener > state_dispatcher_
Definition: canopen.h:244
canopen::FrameOverlay
Definition: canopen.h:24
canopen::Node::prepare
bool prepare()
Definition: node.cpp:68
canopen::ObjectDictSharedPtr
ObjectDict::ObjectDictSharedPtr ObjectDictSharedPtr
Definition: objdict.h:235
canopen::Node::getHeartbeatInterval
uint16_t getHeartbeatInterval()
Definition: canopen.h:254
can::Frame::data
std::array< value_type, 8 > data
NMTcommand::Start
@ Start
Definition: node.cpp:11
canopen::Node::getStorage
const ObjectStorageSharedPtr getStorage()
Definition: canopen.h:200
canopen::Node::nmt_listener_
can::FrameListenerConstSharedPtr nmt_listener_
Definition: canopen.h:240
NMTcommand::Command
Command
Definition: node.cpp:10
canopen::Node::heartbeat_
ObjectStorage::Entry< ObjectStorage::DataType< ObjectDict::DEFTYPE_UNSIGNED16 >::type > heartbeat_
Definition: canopen.h:242
NMTcommand
Definition: node.cpp:9
can
canopen
Definition: bcm_sync.h:8
NMTcommand::command
uint8_t command
Definition: node.cpp:17
canopen::Node::handleWrite
virtual void handleWrite(LayerStatus &status, const LayerState &current_state)
Definition: node.cpp:159
canopen::Layer::state
std::atomic< LayerState > state
Definition: layer.h:144
canopen::PDOMapper::read
void read(LayerStatus &status)
Definition: pdo.cpp:326
canopen::Node::State
State
Definition: canopen.h:191
NMTcommand::Frame
Definition: node.cpp:20
canopen::PDOMapper::write
bool write()
Definition: pdo.cpp:332
canopen::Node::Stopped
@ Stopped
Definition: canopen.h:192
canopen::Node::handleInit
virtual void handleInit(LayerStatus &status)
Definition: node.cpp:176
NMTcommand::Prepare
@ Prepare
Definition: node.cpp:13
canopen::Node::sync_
const SyncCounterSharedPtr sync_
Definition: canopen.h:239
NMTcommand::Reset_Com
@ Reset_Com
Definition: node.cpp:15
canopen::Node::reset
bool reset()
Definition: node.cpp:55
canopen::get_abs_time
time_point get_abs_time(const time_duration &timeout)
Definition: canopen.h:20
canopen::Node::getState
const State getState()
Definition: node.cpp:39
canopen::Node::state_
State state_
Definition: canopen.h:249
NMTcommand::Reset
@ Reset
Definition: node.cpp:14
canopen::Node::mutex
boost::timed_mutex mutex
Definition: canopen.h:234
canopen::Layer
Definition: layer.h:63
canopen::Node::Operational
@ Operational
Definition: canopen.h:192
canopen::LayerStatus
Definition: layer.h:12
NMTcommand::Stop
@ Stop
Definition: node.cpp:12
NMTcommand::Frame::Frame
Frame(uint8_t node_id, const Command &c)
Definition: node.cpp:21
canopen::SDOClient::init
void init()
Definition: sdo.cpp:373
canopen::LayerReport
Definition: layer.h:48


canopen_master
Author(s): Mathias Lüdtke
autogenerated on Wed Mar 2 2022 00:52:26