Lely core libraries  1.9.2
slave.cpp
Go to the documentation of this file.
1 
24 #include "coapp.hpp"
25 
26 #if !LELY_NO_COAPP_SLAVE
27 
28 #include <lely/coapp/slave.hpp>
29 
30 #include <map>
31 
32 #include <cassert>
33 
34 #include <lely/co/dev.hpp>
35 #include <lely/co/nmt.hpp>
36 #include <lely/co/obj.hpp>
37 
38 namespace lely {
39 
40 namespace canopen {
41 
43 struct BasicSlave::Impl_ {
44  Impl_(BasicSlave* self, CONMT* nmt);
45 
46  uint8_t
47  netid() const noexcept {
48  return self->dev()->getNetid();
49  }
50  uint8_t
51  id() const noexcept {
52  return self->dev()->getId();
53  }
54 
55  void OnLgInd(CONMT* nmt, int state) noexcept;
56 
57  template <uint16_t N>
58  uint32_t OnDnInd(COSub* sub, COVal<N>& val) noexcept;
59 
60  template <uint16_t N>
61  uint32_t OnUpInd(const COSub* sub, COVal<N>& val) noexcept;
62 
63  static constexpr uint32_t Key(uint16_t idx, uint8_t subidx) noexcept;
64  static uint32_t Key(const COSub* sub) noexcept;
65 
66  BasicSlave* self;
67 
68  ::std::map<uint32_t, ::std::function<uint32_t(COSub*, void*)>> dn_ind;
69  ::std::map<uint32_t, ::std::function<uint32_t(const COSub*, void*)>> up_ind;
70 };
71 
72 BasicSlave::BasicSlave(aio::TimerBase& timer, aio::CanBusBase& bus,
73  const ::std::string& dcf_txt,
74  const ::std::string& dcf_bin, uint8_t id)
75  : Node(timer, bus, dcf_txt, dcf_bin, id),
76  impl_(new Impl_(this, Node::nmt())) {}
77 
78 BasicSlave::~BasicSlave() = default;
79 
80 template <class T>
81 typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
82 BasicSlave::OnRead(uint16_t idx, uint8_t subidx,
83  ::std::function<OnReadSignature<T>> ind) {
84  ::std::error_code ec;
85  OnRead<T>(idx, subidx, ::std::move(ind), ec);
86  if (ec) throw SdoError(impl_->netid(), impl_->id(), idx, subidx, ec);
87 }
88 
89 namespace {
90 
91 template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
92 typename ::std::enable_if<detail::IsCanopenBasic<T>::value,
93  ::std::error_code>::type
94 OnUpInd(const COSub* sub, COVal<N>& val, const F& ind) {
95  assert(sub);
96 
97  try {
98  return ind(sub->getObj()->getIdx(), sub->getSubidx(), val);
99  } catch (...) {
100  return SdoErrc::ERROR;
101  }
102 }
103 
104 template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
105 typename ::std::enable_if<detail::IsCanopenArray<T>::value,
106  ::std::error_code>::type
107 OnUpInd(const COSub* sub, COVal<N>& val, const F& ind) {
108  assert(sub);
109 
110  try {
111  T value = val;
112  auto ec = ind(sub->getObj()->getIdx(), sub->getSubidx(), value);
113  if (!ec) val = ::std::move(value);
114  return ec;
115  } catch (...) {
116  return SdoErrc::ERROR;
117  }
118 }
119 
120 } // namespace
121 
122 template <class T>
123 typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
124 BasicSlave::OnRead(uint16_t idx, uint8_t subidx,
125  ::std::function<OnReadSignature<T>> ind,
126  ::std::error_code& ec) {
127  constexpr auto N = co_type_traits_T<T>::index;
128 
129  auto obj = dev()->find(idx);
130  if (!obj) {
131  ec = SdoErrc::NO_OBJ;
132  return;
133  }
134 
135  auto sub = obj->find(subidx);
136  if (!sub) {
137  ec = SdoErrc::NO_SUB;
138  return;
139  }
140 
141  if (!detail::IsCanopenSame(N, sub->getType())) {
142  ec = SdoErrc::TYPE_LEN;
143  return;
144  }
145 
146  auto key = Impl_::Key(sub);
147  if (ind) {
148  impl_->up_ind[key] = [=](const COSub* sub, void* p) -> uint32_t {
149  auto ec = OnUpInd<T>(sub, *static_cast<COVal<N>*>(p), ind);
150  return ec.value();
151  };
152  sub->setUpInd<N, Impl_, &Impl_::OnUpInd>(impl_.get());
153  } else {
154  if (impl_->up_ind.erase(key)) sub->setUpInd(nullptr, nullptr);
155  }
156 }
157 
158 template <class T>
159 typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
160 BasicSlave::OnWrite(uint16_t idx, uint8_t subidx,
161  ::std::function<OnWriteSignature<T>> ind) {
162  ::std::error_code ec;
163  OnWrite<T>(idx, subidx, ::std::move(ind), ec);
164  if (ec) throw SdoError(impl_->netid(), impl_->id(), idx, subidx, ec);
165 }
166 
167 namespace {
168 
169 template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
170 typename ::std::enable_if<detail::IsCanopenBasic<T>::value,
171  ::std::error_code>::type
172 OnDnInd(COSub* sub, COVal<N>& val, const F& ind) {
173  assert(sub);
174 
175  try {
176  return ind(sub->getObj()->getIdx(), sub->getSubidx(), val,
177  sub->getVal<N>());
178  } catch (...) {
179  return SdoErrc::ERROR;
180  }
181 }
182 
183 template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
184 typename ::std::enable_if<detail::IsCanopenArray<T>::value,
185  ::std::error_code>::type
186 OnDnInd(COSub* sub, COVal<N>& val, const F& ind) {
187  assert(sub);
188 
189  try {
190  T value = val;
191  auto ec = ind(sub->getObj()->getIdx(), sub->getSubidx(), value);
192  if (!ec) val = ::std::move(value);
193  return ec;
194  } catch (...) {
195  return SdoErrc::ERROR;
196  }
197 }
198 
199 } // namespace
200 
201 template <class T>
202 typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
203 BasicSlave::OnWrite(uint16_t idx, uint8_t subidx,
204  ::std::function<OnWriteSignature<T>> ind,
205  ::std::error_code& ec) {
206  constexpr auto N = co_type_traits_T<T>::index;
207 
208  auto obj = dev()->find(idx);
209  if (!obj) {
210  ec = SdoErrc::NO_OBJ;
211  return;
212  }
213 
214  auto sub = obj->find(subidx);
215  if (!sub) {
216  ec = SdoErrc::NO_SUB;
217  return;
218  }
219 
220  if (!detail::IsCanopenSame(N, sub->getType())) {
221  ec = SdoErrc::TYPE_LEN;
222  return;
223  }
224 
225  auto key = Impl_::Key(sub);
226  if (ind) {
227  impl_->dn_ind[key] = [=](COSub* sub, void* p) -> uint32_t {
228  auto ec = OnDnInd<T>(sub, *static_cast<COVal<N>*>(p), ind);
229  return ec.value();
230  };
231  sub->setDnInd<N, Impl_, &Impl_::OnDnInd>(impl_.get());
232  } else {
233  if (impl_->dn_ind.erase(key)) sub->setDnInd(nullptr, nullptr);
234  }
235 }
236 
237 #ifndef DOXYGEN_SHOULD_SKIP_THIS
238 
239 // BOOLEAN
240 template void BasicSlave::OnRead<bool>(uint16_t, uint8_t,
241  ::std::function<OnReadSignature<bool>>);
242 template void BasicSlave::OnRead<bool>(uint16_t, uint8_t,
243  ::std::function<OnReadSignature<bool>>,
244  ::std::error_code&);
245 template void BasicSlave::OnWrite<bool>(
246  uint16_t, uint8_t, ::std::function<OnWriteSignature<bool>>);
247 template void BasicSlave::OnWrite<bool>(uint16_t, uint8_t,
248  ::std::function<OnWriteSignature<bool>>,
249  ::std::error_code&);
250 
251 // INTEGER8
252 template void BasicSlave::OnRead<int8_t>(
253  uint16_t, uint8_t, ::std::function<OnReadSignature<int8_t>>);
254 template void BasicSlave::OnRead<int8_t>(
255  uint16_t, uint8_t, ::std::function<OnReadSignature<int8_t>>,
256  ::std::error_code&);
257 template void BasicSlave::OnWrite<int8_t>(
258  uint16_t, uint8_t, ::std::function<OnWriteSignature<int8_t>>);
259 template void BasicSlave::OnWrite<int8_t>(
260  uint16_t, uint8_t, ::std::function<OnWriteSignature<int8_t>>,
261  ::std::error_code&);
262 
263 // INTEGER16
264 template void BasicSlave::OnRead<int16_t>(
265  uint16_t, uint8_t, ::std::function<OnReadSignature<int16_t>>);
266 template void BasicSlave::OnRead<int16_t>(
267  uint16_t, uint8_t, ::std::function<OnReadSignature<int16_t>>,
268  ::std::error_code&);
269 template void BasicSlave::OnWrite<int16_t>(
270  uint16_t, uint8_t, ::std::function<OnWriteSignature<int16_t>>);
271 template void BasicSlave::OnWrite<int16_t>(
272  uint16_t, uint8_t, ::std::function<OnWriteSignature<int16_t>>,
273  ::std::error_code&);
274 
275 // INTEGER32
276 template void BasicSlave::OnRead<int32_t>(
277  uint16_t, uint8_t, ::std::function<OnReadSignature<int32_t>>);
278 template void BasicSlave::OnRead<int32_t>(
279  uint16_t, uint8_t, ::std::function<OnReadSignature<int32_t>>,
280  ::std::error_code&);
281 template void BasicSlave::OnWrite<int32_t>(
282  uint16_t, uint8_t, ::std::function<OnWriteSignature<int32_t>>);
283 template void BasicSlave::OnWrite<int32_t>(
284  uint16_t, uint8_t, ::std::function<OnWriteSignature<int32_t>>,
285  ::std::error_code&);
286 
287 // UNSIGNED8
288 template void BasicSlave::OnRead<uint8_t>(
289  uint16_t, uint8_t, ::std::function<OnReadSignature<uint8_t>>);
290 template void BasicSlave::OnRead<uint8_t>(
291  uint16_t, uint8_t, ::std::function<OnReadSignature<uint8_t>>,
292  ::std::error_code&);
293 template void BasicSlave::OnWrite<uint8_t>(
294  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint8_t>>);
295 template void BasicSlave::OnWrite<uint8_t>(
296  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint8_t>>,
297  ::std::error_code&);
298 
299 // UNSIGNED16
300 template void BasicSlave::OnRead<uint16_t>(
301  uint16_t, uint8_t, ::std::function<OnReadSignature<uint16_t>>);
302 template void BasicSlave::OnRead<uint16_t>(
303  uint16_t, uint8_t, ::std::function<OnReadSignature<uint16_t>>,
304  ::std::error_code&);
305 template void BasicSlave::OnWrite<uint16_t>(
306  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint16_t>>);
307 template void BasicSlave::OnWrite<uint16_t>(
308  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint16_t>>,
309  ::std::error_code&);
310 
311 // UNSIGNED32
312 template void BasicSlave::OnRead<uint32_t>(
313  uint16_t, uint8_t, ::std::function<OnReadSignature<uint32_t>>);
314 template void BasicSlave::OnRead<uint32_t>(
315  uint16_t, uint8_t, ::std::function<OnReadSignature<uint32_t>>,
316  ::std::error_code&);
317 template void BasicSlave::OnWrite<uint32_t>(
318  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint32_t>>);
319 template void BasicSlave::OnWrite<uint32_t>(
320  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint32_t>>,
321  ::std::error_code&);
322 
323 // REAL32
324 template void BasicSlave::OnRead<float>(
325  uint16_t, uint8_t, ::std::function<OnReadSignature<float>>);
326 template void BasicSlave::OnRead<float>(uint16_t, uint8_t,
327  ::std::function<OnReadSignature<float>>,
328  ::std::error_code&);
329 template void BasicSlave::OnWrite<float>(
330  uint16_t, uint8_t, ::std::function<OnWriteSignature<float>>);
331 template void BasicSlave::OnWrite<float>(
332  uint16_t, uint8_t, ::std::function<OnWriteSignature<float>>,
333  ::std::error_code&);
334 
335 // VISIBLE_STRING
336 template void BasicSlave::OnRead<::std::string>(
337  uint16_t, uint8_t, ::std::function<OnReadSignature<::std::string>>);
338 template void BasicSlave::OnRead<::std::string>(
339  uint16_t, uint8_t, ::std::function<OnReadSignature<::std::string>>,
340  ::std::error_code&);
341 template void BasicSlave::OnWrite<::std::string>(
342  uint16_t, uint8_t, ::std::function<OnWriteSignature<::std::string>>);
343 template void BasicSlave::OnWrite<::std::string>(
344  uint16_t, uint8_t, ::std::function<OnWriteSignature<::std::string>>,
345  ::std::error_code&);
346 
347 // OCTET_STRING
348 template void BasicSlave::OnRead<::std::vector<uint8_t>>(
349  uint16_t, uint8_t,
350  ::std::function<OnReadSignature<::std::vector<uint8_t>>>);
351 template void BasicSlave::OnRead<::std::vector<uint8_t>>(
352  uint16_t, uint8_t, ::std::function<OnReadSignature<::std::vector<uint8_t>>>,
353  ::std::error_code&);
354 template void BasicSlave::OnWrite<::std::vector<uint8_t>>(
355  uint16_t, uint8_t,
356  ::std::function<OnWriteSignature<::std::vector<uint8_t>>>);
357 template void BasicSlave::OnWrite<::std::vector<uint8_t>>(
358  uint16_t, uint8_t,
359  ::std::function<OnWriteSignature<::std::vector<uint8_t>>>,
360  ::std::error_code&);
361 
362 // UNICODE_STRING
363 template void BasicSlave::OnRead<::std::basic_string<char16_t>>(
364  uint16_t, uint8_t,
365  ::std::function<OnReadSignature<::std::basic_string<char16_t>>>);
366 template void BasicSlave::OnRead<::std::basic_string<char16_t>>(
367  uint16_t, uint8_t,
368  ::std::function<OnReadSignature<::std::basic_string<char16_t>>>,
369  ::std::error_code&);
370 template void BasicSlave::OnWrite<::std::basic_string<char16_t>>(
371  uint16_t, uint8_t,
372  ::std::function<OnWriteSignature<::std::basic_string<char16_t>>>);
373 template void BasicSlave::OnWrite<::std::basic_string<char16_t>>(
374  uint16_t, uint8_t,
375  ::std::function<OnWriteSignature<::std::basic_string<char16_t>>>,
376  ::std::error_code&);
377 
378 // TIME_OF_DAY
379 // TIME_DIFFERENCE
380 // DOMAIN
381 // INTEGER24
382 
383 // REAL64
384 template void BasicSlave::OnRead<double>(
385  uint16_t, uint8_t, ::std::function<OnReadSignature<double>>);
386 template void BasicSlave::OnRead<double>(
387  uint16_t, uint8_t, ::std::function<OnReadSignature<double>>,
388  ::std::error_code&);
389 template void BasicSlave::OnWrite<double>(
390  uint16_t, uint8_t, ::std::function<OnWriteSignature<double>>);
391 template void BasicSlave::OnWrite<double>(
392  uint16_t, uint8_t, ::std::function<OnWriteSignature<double>>,
393  ::std::error_code&);
394 
395 // INTEGER40
396 // INTEGER48
397 // INTEGER56
398 
399 // INTEGER64
400 template void BasicSlave::OnRead<int64_t>(
401  uint16_t, uint8_t, ::std::function<OnReadSignature<int64_t>>);
402 template void BasicSlave::OnRead<int64_t>(
403  uint16_t, uint8_t, ::std::function<OnReadSignature<int64_t>>,
404  ::std::error_code&);
405 template void BasicSlave::OnWrite<int64_t>(
406  uint16_t, uint8_t, ::std::function<OnWriteSignature<int64_t>>);
407 template void BasicSlave::OnWrite<int64_t>(
408  uint16_t, uint8_t, ::std::function<OnWriteSignature<int64_t>>,
409  ::std::error_code&);
410 
411 // UNSIGNED24
412 // UNSIGNED40
413 // UNSIGNED48
414 // UNSIGNED56
415 
416 // UNSIGNED64
417 template void BasicSlave::OnRead<uint64_t>(
418  uint16_t, uint8_t, ::std::function<OnReadSignature<uint64_t>>);
419 template void BasicSlave::OnRead<uint64_t>(
420  uint16_t, uint8_t, ::std::function<OnReadSignature<uint64_t>>,
421  ::std::error_code&);
422 template void BasicSlave::OnWrite<uint64_t>(
423  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint64_t>>);
424 template void BasicSlave::OnWrite<uint64_t>(
425  uint16_t, uint8_t, ::std::function<OnWriteSignature<uint64_t>>,
426  ::std::error_code&);
427 
428 #endif // DOXYGEN_SHOULD_SKIP_THIS
429 
430 BasicSlave::Impl_::Impl_(BasicSlave* self_, CONMT* nmt) : self(self_) {
431  nmt->setLgInd<Impl_, &Impl_::OnLgInd>(this);
432 }
433 
434 void
435 BasicSlave::Impl_::OnLgInd(CONMT* nmt, int state) noexcept {
436  // Invoke the default behavior before notifying the implementation.
437  nmt->onLg(state);
438  // Notify the implementation.
439  self->OnLifeGuarding(state == CO_NMT_EC_OCCURRED);
440 }
441 
442 template <uint16_t N>
443 uint32_t
444 BasicSlave::Impl_::OnDnInd(COSub* sub, COVal<N>& val) noexcept {
445  auto it = dn_ind.find(Key(sub));
446  if (it == dn_ind.end()) return 0;
447  return it->second(sub, static_cast<void*>(&val));
448 }
449 
450 template <uint16_t N>
451 uint32_t
452 BasicSlave::Impl_::OnUpInd(const COSub* sub, COVal<N>& val) noexcept {
453  auto it = up_ind.find(Key(sub));
454  if (it == up_ind.end()) return 0;
455  return it->second(sub, static_cast<void*>(&val));
456 }
457 
458 constexpr uint32_t
459 BasicSlave::Impl_::Key(uint16_t idx, uint8_t subidx) noexcept {
460  return (uint32_t(idx) << 8) | subidx;
461 }
462 
463 uint32_t
464 BasicSlave::Impl_::Key(const COSub* sub) noexcept {
465  assert(sub);
466 
467  return Key(sub->getObj()->getIdx(), sub->getSubidx());
468 }
469 
470 } // namespace canopen
471 
472 } // namespace lely
473 
474 #endif // !LELY_NO_COAPP_SLAVE
lely::canopen::BasicSlave::BasicSlave
BasicSlave(aio::TimerBase &timer, aio::CanBusBase &bus, const ::std::string &dcf_txt, const ::std::string &dcf_bin="", uint8_t id=0xff)
Creates a new CANopen slave.
Definition: slave.cpp:72
CO_NMT_EC_OCCURRED
An NMT error control event occurred.
Definition: nmt.h:80
lely::canopen::Node::nmt
CONMT * nmt() const noexcept
Returns a pointer to the internal CANopen NMT master/slave service from <lely/co/nmt.hpp>.
Definition: node.cpp:137
slave.hpp
This header file is part of the C++ CANopen application library; it contains the CANopen slave declar...
nmt.hpp
This header file is part of the CANopen library; it contains the C++ interface of the network managem...
dev.hpp
This header file is part of the CANopen library; it contains the C++ interface of the device descript...
lely::canopen::SdoErrc::NO_SUB
Sub-index does not exist.
lely::COSub
An opaque CANopen sub-object type.
Definition: obj.hpp:229
obj.hpp
This header file is part of the CANopen library; it contains the C++ interface of the object dictiona...
lely::canopen::BasicSlave
The base class for CANopen slaves.
Definition: slave.hpp:32
lely::canopen::SdoErrc::TYPE_LEN
Data type does not match, length of service parameter does not match.
lely::canopen::BasicSlave::OnWrite
typename ::std::enable_if< detail::IsCanopenType< T >::value >::type OnWrite(uint16_t idx, uint8_t subidx, ::std::function< OnWriteSignature< T >> ind)
Registers a callback function to be invoked on write (SDO download) access to the specified CANopen s...
coapp.hpp
This is the internal header file of the C++ CANopen application library.
lely::canopen::SdoErrc::NO_OBJ
Object does not exist in the object dictionary.
lely::canopen::BasicSlave::Impl_
The internal implementation of the CANopen slave.
Definition: slave.cpp:43
lely::canopen::BasicSlave::OnRead
typename ::std::enable_if< detail::IsCanopenType< T >::value >::type OnRead(uint16_t idx, uint8_t subidx, ::std::function< OnReadSignature< T >> ind)
Registers a callback function to be invoked on read (SDO upload) access to the specified CANopen sub-...
lely::CONMT
An opaque CANopen NMT master/slave service type.
Definition: nmt.hpp:70
lely::canopen::Device::dev
CODev * dev() const noexcept
Returns a pointer to the internal CANopen device from <lely/co/dev.hpp>.
Definition: device.cpp:387
lely
Global namespace for the Lely Industries N.V. libraries.
Definition: buf.hpp:32
lely::canopen::Node
The base class for CANopen nodes.
Definition: node.hpp:109
lely::COVal
A CANopen value.
Definition: val.hpp:42
lely::canopen::SdoError
The type of exception thrown when an SDO abort code is received.
Definition: sdo_error.hpp:116
lely::canopen::detail::IsCanopenSame
bool IsCanopenSame(uint16_t t1, uint16_t t2)
Returns true if the CANopen data types t1 and t2 map to the same C++ type, and false if not...
Definition: type_traits.hpp:178