pdo.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 class PDOid{
9  const uint32_t value_;
10 public:
11  static const unsigned int ID_MASK = (1u << 29)-1;
12  static const unsigned int EXTENDED_MASK = (1u << 29);
13  static const unsigned int NO_RTR_MASK = (1u << 30);
14  static const unsigned int INVALID_MASK = (1u << 31);
15 
16  PDOid(const uint32_t &val)
17  : value_(val)
18  {}
19  can::Header header(bool fill_rtr = false) const {
20  return can::Header(value_ & ID_MASK, value_ & EXTENDED_MASK, fill_rtr && !(value_ & NO_RTR_MASK), false);
21  }
22  bool isInvalid() const { return value_ & INVALID_MASK; }
23 };
24 
25 struct PDOmap{
26  uint8_t length;
27  uint8_t sub_index;
28  uint16_t index;
29  PDOmap(uint32_t val)
30  : length(val & 0xFF),
31  sub_index((val>>8) & 0xFF),
32  index(val>>16)
33  {}
34 };
35 
36 #pragma pack(pop) /* pop previous alignment from stack */
37 
38 
39 const uint8_t SUB_COM_NUM = 0;
40 const uint8_t SUB_COM_COB_ID = 1;
41 const uint8_t SUB_COM_TRANSMISSION_TYPE = 2;
42 const uint8_t SUB_COM_RESERVED = 4;
43 
44 const uint8_t SUB_MAP_NUM = 0;
45 
46 const uint16_t RPDO_COM_BASE =0x1400;
47 const uint16_t RPDO_MAP_BASE =0x1600;
48 const uint16_t TPDO_COM_BASE =0x1800;
49 const uint16_t TPDO_MAP_BASE =0x1A00;
50 
51 bool check_com_changed(const ObjectDict &dict, const uint16_t com_id){
52  bool com_changed = false;
53 
54  // check if com parameter has to be set
55  for(uint8_t sub = 0; sub <=6 ; ++sub){
56  try{
57  if(!dict(com_id,sub).init_val.is_empty()){
58  com_changed = true;
59  break;
60  }
61  }
62  catch (std::out_of_range) {}
63  }
64  return com_changed;
65 }
66 
67 bool check_map_changed(const uint8_t &num, const ObjectDict &dict, const uint16_t &map_index){
68  bool map_changed = false;
69 
70  // check if mapping has to be set
71  if(num <= 0x40){
72  for(uint8_t sub = 1; sub <=num ; ++sub){
73  try{
74  if(!dict(map_index,sub).init_val.is_empty()){
75  map_changed = true;
76  break;
77  }
78  }
79  catch (std::out_of_range) {}
80  }
81  }else{
82  map_changed = dict( map_index ,0 ).init_val.is_empty();
83  }
84  return map_changed;
85 }
86 void PDOMapper::PDO::parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write){
87 
88  const canopen::ObjectDict & dict = *storage->dict_;
89 
91  storage->entry(num_entry, map_index, SUB_MAP_NUM);
92 
93  uint8_t map_num;
94 
95  try{
96  map_num = num_entry.desc().value().get<uint8_t>();
97  }catch(...){
98  map_num = 0;
99  }
100 
101  bool map_changed = check_map_changed(map_num, dict, map_index);
102 
103  // disable PDO if needed
105  storage->entry(cob_id, com_index, SUB_COM_COB_ID);
106 
107  bool com_changed = check_com_changed(dict, map_index);
108  if((map_changed || com_changed) && cob_id.desc().writable){
109  cob_id.set(cob_id.get() | PDOid::INVALID_MASK);
110  }
111  if(map_num > 0 && map_num <= 0x40){ // actual mapping
112  if(map_changed){
113  num_entry.set(0);
114  }
115 
116  frame.dlc = 0;
117  for(uint8_t sub = 1; sub <=map_num; ++sub){
119  storage->entry(mapentry, map_index, sub);
120  const HoldAny init = dict(map_index ,sub).init_val;
121  if(!init.is_empty()) mapentry.set(init.get<uint32_t>());
122 
123  PDOmap param(mapentry.get_cached());
124  BufferSharedPtr b = boost::make_shared<Buffer>(param.length/8);
125  if(param.index < 0x1000){
126  // TODO: check DummyUsage
127  }else{
130  if(read) rd = ObjectStorage::ReadDelegate(b.get(), &Buffer::read);
131  if(read || write) wd = ObjectStorage::WriteDelegate(b.get(), &Buffer::write); // set writer for buffer setup or as write delegate
132  size_t l = storage->map(param.index, param.sub_index, rd, wd);
133  assert(l == param.length/8);
134  }
135 
136  frame.dlc += b->size;
137  assert( frame.dlc <= 8 );
138  b->clean();
139  buffers.push_back(b);
140  }
141  }
142  if(com_changed){
143  uint8_t subs = dict(com_index, SUB_COM_NUM).value().get<uint8_t>();
144  for(uint8_t i = SUB_COM_NUM+1; i <= subs; ++i){
145  if(i == SUB_COM_COB_ID || i == SUB_COM_RESERVED) continue;
146  try{
147  storage->init(ObjectDict::Key(com_index, i));
148  }
149  catch (const std::out_of_range &){
150  // entry was not provided, so skip it
151  }
152  }
153  }
154  if(map_changed){
155  num_entry.set(map_num);
156  }
157  if((com_changed || map_changed) && cob_id.desc().writable){
158  storage->init(ObjectDict::Key(com_index, SUB_COM_COB_ID));
159 
160  cob_id.set(NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_));
161  }
162 
163 
164 }
166 :interface_(interface)
167 {
168 }
170  boost::mutex::scoped_lock lock(mutex_);
171 
172  try{
173  rpdos_.clear();
174 
175  const canopen::ObjectDict & dict = *storage->dict_;
176  for(uint16_t i=0; i < 512 && rpdos_.size() < dict.device_info.nr_of_tx_pdo;++i){ // TPDOs of device
177  if(!dict.has(TPDO_COM_BASE + i,0) && !dict.has(TPDO_MAP_BASE + i,0)) continue;
178 
180  if(rpdo){
181  rpdos_.insert(rpdo);
182  }
183  }
184  // LOG("RPDOs: " << rpdos_.size());
185 
186  tpdos_.clear();
187  for(uint16_t i=0; i < 512 && tpdos_.size() < dict.device_info.nr_of_rx_pdo;++i){ // RPDOs of device
188  if(!dict.has(RPDO_COM_BASE + i,0) && !dict.has(RPDO_MAP_BASE + i,0)) continue;
189 
191  if(tpdo){
192  tpdos_.insert(tpdo);
193  }
194  }
195  // LOG("TPDOs: " << tpdos_.size());
196 
197  return true;
198  }
199  catch(const std::out_of_range &e){
200  status.error(std::string("PDO error: ") + e.what());
201  return false;
202  }
203 }
204 
205 
206 bool PDOMapper::RPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
207  boost::mutex::scoped_lock lock(mutex);
208  listener_.reset();
209  const canopen::ObjectDict & dict = *storage->dict_;
210  parse_and_set_mapping(storage, com_index, map_index, true, false);
211 
212  PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
213 
214  if(buffers.empty() || pdoid.isInvalid()){
215  return false;
216  }
217 
218  frame = pdoid.header(true);
219 
220  transmission_type = dict(com_index, SUB_COM_TRANSMISSION_TYPE).value().get<uint8_t>();
221 
222  listener_ = interface_->createMsgListener(pdoid.header() ,can::CommInterface::FrameDelegate(this, &RPDO::handleFrame));
223 
224  return true;
225 }
226 
227 bool PDOMapper::TPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
228  boost::mutex::scoped_lock lock(mutex);
229  const canopen::ObjectDict & dict = *storage->dict_;
230 
231 
232  PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
233  frame = pdoid.header();
234 
235  parse_and_set_mapping(storage, com_index, map_index, false, true);
236  if(buffers.empty() || pdoid.isInvalid()){
237  return false;
238  }
240  storage->entry(tt, com_index, SUB_COM_TRANSMISSION_TYPE);
241  transmission_type = tt.desc().value().get<uint8_t>();
242 
243  if(transmission_type != 1 && transmission_type <=240){
244  tt.set(1); // enforce 1 for compatibility
245  }
246  return true;
247 }
248 
250  boost::mutex::scoped_lock lock(mutex);
251 
252  bool updated = false;
253  size_t len = frame.dlc;
254  can::Frame::value_type * dest = frame.c_array();
255  for(std::vector< BufferSharedPtr >::iterator b_it = buffers.begin(); b_it != buffers.end(); ++b_it){
256  Buffer &b = **b_it;
257  if(len >= b.size){
258  updated = b.read(dest, len) || updated;
259  len -= b.size;
260  dest += b.size;
261  }else{
262  // ERROR
263  }
264  }
265 
266  if( len != 0){
267  // ERROR
268  }
269  if(updated){
270  interface_->send( frame );
271  }else{
272  // TODO: Notify
273  }
274 }
275 
277  boost::mutex::scoped_lock lock(mutex);
278  if((transmission_type >= 1 && transmission_type <= 240) || transmission_type == 0xFC){ // cyclic
279  if(timeout > 0){
280  --timeout;
281  }else if(timeout == 0) {
282  status.warn("RPDO timeout");
283  }
284  }
285  if(transmission_type == 0xFC || transmission_type == 0xFD){
286  if(frame.is_rtr){
287  interface_->send(frame);
288  }
289  }
290 }
291 
293  size_t offset = 0;
294  const uint8_t * src = msg.data.data();
295  for(std::vector<BufferSharedPtr >::iterator it = buffers.begin(); it != buffers.end(); ++it){
296  Buffer &b = **it;
297 
298  if( offset + b.size <= msg.dlc ){
299  b.write(src+offset, b.size);
300  offset += b.size;
301  }else{
302  // ERROR
303  }
304  }
305  if( offset != msg.dlc ){
306  // ERROR
307  }
308  {
309  boost::mutex::scoped_lock lock(mutex);
310  if(transmission_type >= 1 && transmission_type <= 240){
311  timeout = transmission_type + 2;
312  }else if(transmission_type == 0xFC || transmission_type == 0xFD){
313  if(frame.is_rtr){
314  timeout = 1+2;
315  }
316  }
317  }
318 }
319 
321  boost::mutex::scoped_lock lock(mutex_);
322  for(boost::unordered_set<RPDO::RPDOSharedPtr >::iterator it = rpdos_.begin(); it != rpdos_.end(); ++it){
323  (*it)->sync(status);
324  }
325 }
327  boost::mutex::scoped_lock lock(mutex_);
328  for(boost::unordered_set<TPDO::TPDOSharedPtr >::iterator it = tpdos_.begin(); it != tpdos_.end(); ++it){
329  (*it)->sync();
330  }
331  return true; // TODO: check for errors
332 }
333 
334 bool PDOMapper::Buffer::read(uint8_t* b, const size_t len){
335  boost::mutex::scoped_lock lock(mutex);
336  if(size > len){
337  BOOST_THROW_EXCEPTION( std::bad_cast() );
338  }
339  if(empty) return false;
340 
341  memcpy(b,&buffer[0], size);
342  bool was_dirty = dirty;
343  dirty = false;
344  return was_dirty;
345 }
346 void PDOMapper::Buffer::write(const uint8_t* b, const size_t len){
347  boost::mutex::scoped_lock lock(mutex);
348  if(size > len){
349  BOOST_THROW_EXCEPTION( std::bad_cast() );
350  }
351  empty = false;
352  dirty = true;
353  memcpy(&buffer[0], b, size);
354 }
356  boost::mutex::scoped_lock lock(mutex);
357  time_point abs_time = get_abs_time(boost::chrono::seconds(1));
358  if(size != data.size()){
359  THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
360  }
361  if(empty){
362  THROW_WITH_KEY(TimeoutException("PDO data empty"), ObjectDict::Key(entry));
363  }
364  if(dirty){
365  data.assign(buffer.begin(), buffer.end());
366  dirty = false;
367  }
368 }
370  boost::mutex::scoped_lock lock(mutex);
371  if(size != data.size()){
372  THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
373  }
374  empty = false;
375  dirty = true;
376  buffer.assign(data.begin(),data.end());
377 }
static RPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index)
Definition: canopen.h:114
bool init(const ObjectStorageSharedPtr storage, LayerStatus &status)
Definition: pdo.cpp:169
bool check_map_changed(const uint8_t &num, const ObjectDict &dict, const uint16_t &map_index)
Definition: pdo.cpp:67
bool read(uint8_t *b, const size_t len)
Definition: pdo.cpp:334
bool check_com_changed(const ObjectDict &dict, const uint16_t com_id)
Definition: pdo.cpp:51
boost::shared_ptr< Buffer > BufferSharedPtr
Definition: canopen.h:85
bool write()
Definition: pdo.cpp:326
const uint16_t RPDO_COM_BASE
Definition: pdo.cpp:46
boost::unordered_map< Key, EntryConstSharedPtr > dict_
Definition: objdict.h:228
bool isInvalid() const
Definition: pdo.cpp:22
const uint8_t SUB_COM_NUM
Definition: pdo.cpp:39
const ObjectDict::Entry & desc() const
Definition: objdict.h:438
PDOid(const uint32_t &val)
Definition: pdo.cpp:16
const void warn(const std::string &r)
Definition: layer.h:43
uint8_t sub_index
Definition: pdo.cpp:27
const uint8_t SUB_MAP_NUM
Definition: pdo.cpp:44
const uint16_t TPDO_MAP_BASE
Definition: pdo.cpp:49
Definition: pdo.cpp:25
bool is_empty() const
Definition: objdict.h:74
boost::array< value_type, 8 > data
PDOmap(uint32_t val)
Definition: pdo.cpp:29
boost::shared_ptr< CommInterface > CommInterfaceSharedPtr
boost::shared_ptr< TPDO > TPDOSharedPtr
Definition: canopen.h:96
const uint16_t RPDO_MAP_BASE
Definition: pdo.cpp:47
fastdelegate::FastDelegate2< const ObjectDict::Entry &, const String & > WriteDelegate
Definition: objdict.h:275
const uint32_t value_
Definition: pdo.cpp:9
PDOMapper(const can::CommInterfaceSharedPtr interface)
Definition: pdo.cpp:165
boost::chrono::high_resolution_clock::time_point time_point
Definition: canopen.h:18
uint16_t index
Definition: pdo.cpp:28
bool has(uint16_t i, uint8_t s) const
Definition: objdict.h:197
void read(LayerStatus &status)
Definition: pdo.cpp:320
uint16_t nr_of_rx_pdo
Definition: objdict.h:108
const T & get() const
Definition: objdict.h:83
void sync(LayerStatus &status)
Definition: pdo.cpp:276
fastdelegate::FastDelegate1< const Frame & > FrameDelegate
void write(const uint8_t *b, const size_t len)
Definition: pdo.cpp:346
bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index)
Definition: pdo.cpp:206
static const unsigned int INVALID_MASK
Definition: pdo.cpp:14
const uint8_t SUB_COM_TRANSMISSION_TYPE
Definition: pdo.cpp:41
uint16_t nr_of_tx_pdo
Definition: objdict.h:109
Definition: pdo.cpp:8
const can::CommInterfaceSharedPtr interface_
Definition: canopen.h:134
static TPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index)
Definition: canopen.h:98
const uint16_t TPDO_COM_BASE
Definition: pdo.cpp:48
const DeviceInfo device_info
Definition: objdict.h:214
boost::mutex mutex_
Definition: canopen.h:67
fastdelegate::FastDelegate2< const ObjectDict::Entry &, String & > ReadDelegate
Definition: objdict.h:274
boost::unordered_set< TPDO::TPDOSharedPtr > tpdos_
Definition: canopen.h:132
const void error(const std::string &r)
Definition: layer.h:44
const EntryConstSharedPtr & get(const Key &k) const
Definition: objdict.h:194
uint8_t length
Definition: pdo.cpp:26
unsigned char value_type
void set(const T &val)
Definition: objdict.h:407
bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index)
Definition: pdo.cpp:227
unsigned char dlc
#define THROW_WITH_KEY(e, k)
Definition: objdict.h:114
time_point get_abs_time(const time_duration &timeout)
Definition: canopen.h:20
can::Header header(bool fill_rtr=false) const
Definition: pdo.cpp:19
void parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write)
Definition: pdo.cpp:86
boost::shared_ptr< RPDO > RPDOSharedPtr
Definition: canopen.h:113
ObjectStorage::ObjectStorageSharedPtr ObjectStorageSharedPtr
Definition: objdict.h:523
const HoldAny & value() const
Definition: objdict.h:183
const uint8_t SUB_COM_RESERVED
Definition: pdo.cpp:42
void handleFrame(const can::Frame &msg)
Definition: pdo.cpp:292
boost::unordered_set< RPDO::RPDOSharedPtr > rpdos_
Definition: canopen.h:131
const uint8_t SUB_COM_COB_ID
Definition: pdo.cpp:40


canopen_master
Author(s): Mathias Lüdtke
autogenerated on Sat May 4 2019 02:40:43