emcy.cpp
Go to the documentation of this file.
3 
4 using namespace canopen;
5 
6 #pragma pack(push) /* push current alignment to stack */
7 #pragma pack(1) /* set alignment to 1 byte boundary */
8 
9 struct EMCYid{
10  uint32_t id:29;
11  uint32_t extended:1;
12  uint32_t :1;
13  uint32_t invalid:1;
14  EMCYid(uint32_t val){
15  *(uint32_t*) this = val;
16  }
18  return can::Header(id, extended, false, false);
19  }
20  const uint32_t get() const { return *(uint32_t*) this; }
21 };
22 
23 struct EMCYfield{
24  uint32_t error_code:16;
25  uint32_t addition_info:16;
26  EMCYfield(uint32_t val){
27  *(uint32_t*) this = val;
28  }
29 };
30 
31 struct EMCYmsg{
32  uint16_t error_code;
33  uint8_t error_register;
34  uint8_t manufacturer_specific_error_field[5];
35 
36  struct Frame: public FrameOverlay<EMCYmsg>{
37  Frame(const can::Frame &f) : FrameOverlay(f){ }
38  };
39 };
40 
41 #pragma pack(pop) /* pop previous alignment from stack */
42 
44  EMCYmsg::Frame em(msg);
45  ROSCANOPEN_ERROR("canopen_master", "EMCY received: " << msg);
46  has_error_ = (em.data.error_register & ~32) != 0;
47 }
48 
49 EMCYHandler::EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage): Layer("EMCY handler"), storage_(storage), has_error_(true){
50  storage_->entry(error_register_, 0x1001);
51  try{
52  storage_->entry(num_errors_, 0x1003,0);
53  }
54  catch(...){
55  // pass, 1003 is optional
56  }
57  try{
58  EMCYid emcy_id(storage_->entry<uint32_t>(0x1014).get_cached());
59  emcy_listener_ = interface->createMsgListenerM(emcy_id.header(), this, &EMCYHandler::handleEMCY);
60 
61 
62  }
63  catch(...){
64  // pass, EMCY is optional
65  }
66 }
67 
68 void EMCYHandler::handleRead(LayerStatus &status, const LayerState &current_state) {
69  if(current_state == Ready){
70  if(has_error_){
71  status.error("Node has emergency error");
72  }
73  }
74 }
75 void EMCYHandler::handleWrite(LayerStatus &status, const LayerState &current_state) {
76  // noithing to do
77 }
78 
80  uint8_t error_register = 0;
81  if(!error_register_.get(error_register)){
82  report.error("Could not read error error_register");
83  return;
84  }
85 
86  if(error_register){
87  if(error_register & 1){ // first bit should be set on all errors
88  report.error("Node has emergency error");
89  }else if(error_register & ~32){ // filter profile-specific bit
90  report.warn("Error register is not zero");
91  }
92  report.add("error_register", (uint32_t) error_register);
93 
94  uint8_t num = num_errors_.valid() ? num_errors_.get() : 0;
95  std::stringstream buf;
96  for(size_t i = 0; i < num; ++i) {
97  if( i!= 0){
98  buf << ", ";
99  }
100  try{
102  storage_->entry(error, 0x1003,i+1);
103  EMCYfield field(error.get());
104  buf << std::hex << field.error_code << "#" << field.addition_info;
105  }
106  catch (const std::out_of_range & e){
107  buf << "NOT_IN_DICT!";
108  }
109  catch (const TimeoutException & e){
110  buf << "LIST_UNDERFLOW!";
111  break;
112  }
113 
114  }
115  report.add("errors", buf.str());
116 
117  }
118 }
120  uint8_t error_register = 0;
121  if(!error_register_.get(error_register)){
122  status.error("Could not read error error_register");
123  return;
124  }else if(error_register & 1){
125  ROSCANOPEN_ERROR("canopen_master", "error register: " << int(error_register));
126  status.error("Node has emergency error");
127  return;
128  }
129 
130  resetErrors(status);
131 }
133  if(num_errors_.valid()) num_errors_.set(0);
134  has_error_ = false;
135 }
136 
138  handleInit(status);
139 }
141 }
143  // do nothing
144 }
EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage)
Definition: emcy.cpp:49
uint32_t error_code
Definition: emcy.cpp:24
std::atomic< bool > has_error_
Definition: canopen.h:149
Frame(const can::Frame &f)
Definition: emcy.cpp:37
virtual void handleHalt(LayerStatus &status)
Definition: emcy.cpp:142
Definition: emcy.cpp:9
#define ROSCANOPEN_ERROR(name, args)
uint32_t id
Definition: emcy.cpp:10
const void warn(const std::string &r)
Definition: layer.h:44
EMCYfield(uint32_t val)
Definition: emcy.cpp:26
ObjectStorage::Entry< uint8_t > num_errors_
Definition: canopen.h:151
uint32_t invalid
Definition: emcy.cpp:13
virtual void handleWrite(LayerStatus &status, const LayerState &current_state)
Definition: emcy.cpp:75
void handleEMCY(const can::Frame &msg)
Definition: emcy.cpp:43
can::FrameListenerConstSharedPtr emcy_listener_
Definition: canopen.h:152
virtual void handleRecover(LayerStatus &status)
Definition: emcy.cpp:137
std::shared_ptr< CommInterface > CommInterfaceSharedPtr
virtual void handleInit(LayerStatus &status)
Definition: emcy.cpp:119
can::Header header()
Definition: emcy.cpp:17
uint32_t extended
Definition: emcy.cpp:11
void resetErrors(LayerStatus &status)
Definition: emcy.cpp:132
virtual void handleDiag(LayerReport &report)
Definition: emcy.cpp:79
uint8_t error_register
Definition: emcy.cpp:33
const ObjectStorageSharedPtr storage_
Definition: canopen.h:154
uint16_t error_code
Definition: emcy.cpp:32
ObjectStorage::Entry< uint8_t > error_register_
Definition: canopen.h:150
Definition: emcy.cpp:31
void add(const std::string &key, const T &value)
Definition: layer.h:52
virtual void handleShutdown(LayerStatus &status)
Definition: emcy.cpp:140
const void error(const std::string &r)
Definition: layer.h:45
uint32_t addition_info
Definition: emcy.cpp:25
void set(const T &val)
Definition: objdict.h:420
EMCYid(uint32_t val)
Definition: emcy.cpp:14
ObjectStorage::ObjectStorageSharedPtr ObjectStorageSharedPtr
Definition: objdict.h:537
virtual void handleRead(LayerStatus &status, const LayerState &current_state)
Definition: emcy.cpp:68


canopen_master
Author(s): Mathias Lüdtke
autogenerated on Mon Feb 28 2022 23:28:03