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
38namespace lely {
39
40namespace canopen {
41
43struct 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
72BasicSlave::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
78BasicSlave::~BasicSlave() = default;
79
80template <class T>
81typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
82BasicSlave::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
89namespace {
90
91template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
92typename ::std::enable_if<detail::IsCanopenBasic<T>::value,
93 ::std::error_code>::type
94OnUpInd(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
104template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
105typename ::std::enable_if<detail::IsCanopenArray<T>::value,
106 ::std::error_code>::type
107OnUpInd(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
122template <class T>
123typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
124BasicSlave::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
158template <class T>
159typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
160BasicSlave::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
167namespace {
168
169template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
170typename ::std::enable_if<detail::IsCanopenBasic<T>::value,
171 ::std::error_code>::type
172OnDnInd(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
183template <class T, class F, uint16_t N = co_type_traits_T<T>::index>
184typename ::std::enable_if<detail::IsCanopenArray<T>::value,
185 ::std::error_code>::type
186OnDnInd(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
201template <class T>
202typename ::std::enable_if<detail::IsCanopenType<T>::value>::type
203BasicSlave::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
240template void BasicSlave::OnRead<bool>(uint16_t, uint8_t,
241 ::std::function<OnReadSignature<bool>>);
242template void BasicSlave::OnRead<bool>(uint16_t, uint8_t,
243 ::std::function<OnReadSignature<bool>>,
244 ::std::error_code&);
245template void BasicSlave::OnWrite<bool>(
246 uint16_t, uint8_t, ::std::function<OnWriteSignature<bool>>);
247template void BasicSlave::OnWrite<bool>(uint16_t, uint8_t,
248 ::std::function<OnWriteSignature<bool>>,
249 ::std::error_code&);
250
251// INTEGER8
252template void BasicSlave::OnRead<int8_t>(
253 uint16_t, uint8_t, ::std::function<OnReadSignature<int8_t>>);
254template void BasicSlave::OnRead<int8_t>(
255 uint16_t, uint8_t, ::std::function<OnReadSignature<int8_t>>,
256 ::std::error_code&);
257template void BasicSlave::OnWrite<int8_t>(
258 uint16_t, uint8_t, ::std::function<OnWriteSignature<int8_t>>);
259template void BasicSlave::OnWrite<int8_t>(
260 uint16_t, uint8_t, ::std::function<OnWriteSignature<int8_t>>,
261 ::std::error_code&);
262
263// INTEGER16
264template void BasicSlave::OnRead<int16_t>(
265 uint16_t, uint8_t, ::std::function<OnReadSignature<int16_t>>);
266template void BasicSlave::OnRead<int16_t>(
267 uint16_t, uint8_t, ::std::function<OnReadSignature<int16_t>>,
268 ::std::error_code&);
269template void BasicSlave::OnWrite<int16_t>(
270 uint16_t, uint8_t, ::std::function<OnWriteSignature<int16_t>>);
271template void BasicSlave::OnWrite<int16_t>(
272 uint16_t, uint8_t, ::std::function<OnWriteSignature<int16_t>>,
273 ::std::error_code&);
274
275// INTEGER32
276template void BasicSlave::OnRead<int32_t>(
277 uint16_t, uint8_t, ::std::function<OnReadSignature<int32_t>>);
278template void BasicSlave::OnRead<int32_t>(
279 uint16_t, uint8_t, ::std::function<OnReadSignature<int32_t>>,
280 ::std::error_code&);
281template void BasicSlave::OnWrite<int32_t>(
282 uint16_t, uint8_t, ::std::function<OnWriteSignature<int32_t>>);
283template void BasicSlave::OnWrite<int32_t>(
284 uint16_t, uint8_t, ::std::function<OnWriteSignature<int32_t>>,
285 ::std::error_code&);
286
287// UNSIGNED8
288template void BasicSlave::OnRead<uint8_t>(
289 uint16_t, uint8_t, ::std::function<OnReadSignature<uint8_t>>);
290template void BasicSlave::OnRead<uint8_t>(
291 uint16_t, uint8_t, ::std::function<OnReadSignature<uint8_t>>,
292 ::std::error_code&);
293template void BasicSlave::OnWrite<uint8_t>(
294 uint16_t, uint8_t, ::std::function<OnWriteSignature<uint8_t>>);
295template void BasicSlave::OnWrite<uint8_t>(
296 uint16_t, uint8_t, ::std::function<OnWriteSignature<uint8_t>>,
297 ::std::error_code&);
298
299// UNSIGNED16
300template void BasicSlave::OnRead<uint16_t>(
301 uint16_t, uint8_t, ::std::function<OnReadSignature<uint16_t>>);
302template void BasicSlave::OnRead<uint16_t>(
303 uint16_t, uint8_t, ::std::function<OnReadSignature<uint16_t>>,
304 ::std::error_code&);
305template void BasicSlave::OnWrite<uint16_t>(
306 uint16_t, uint8_t, ::std::function<OnWriteSignature<uint16_t>>);
307template void BasicSlave::OnWrite<uint16_t>(
308 uint16_t, uint8_t, ::std::function<OnWriteSignature<uint16_t>>,
309 ::std::error_code&);
310
311// UNSIGNED32
312template void BasicSlave::OnRead<uint32_t>(
313 uint16_t, uint8_t, ::std::function<OnReadSignature<uint32_t>>);
314template void BasicSlave::OnRead<uint32_t>(
315 uint16_t, uint8_t, ::std::function<OnReadSignature<uint32_t>>,
316 ::std::error_code&);
317template void BasicSlave::OnWrite<uint32_t>(
318 uint16_t, uint8_t, ::std::function<OnWriteSignature<uint32_t>>);
319template void BasicSlave::OnWrite<uint32_t>(
320 uint16_t, uint8_t, ::std::function<OnWriteSignature<uint32_t>>,
321 ::std::error_code&);
322
323// REAL32
324template void BasicSlave::OnRead<float>(
325 uint16_t, uint8_t, ::std::function<OnReadSignature<float>>);
326template void BasicSlave::OnRead<float>(uint16_t, uint8_t,
327 ::std::function<OnReadSignature<float>>,
328 ::std::error_code&);
329template void BasicSlave::OnWrite<float>(
330 uint16_t, uint8_t, ::std::function<OnWriteSignature<float>>);
331template void BasicSlave::OnWrite<float>(
332 uint16_t, uint8_t, ::std::function<OnWriteSignature<float>>,
333 ::std::error_code&);
334
335// VISIBLE_STRING
336template void BasicSlave::OnRead<::std::string>(
337 uint16_t, uint8_t, ::std::function<OnReadSignature<::std::string>>);
338template void BasicSlave::OnRead<::std::string>(
339 uint16_t, uint8_t, ::std::function<OnReadSignature<::std::string>>,
340 ::std::error_code&);
341template void BasicSlave::OnWrite<::std::string>(
342 uint16_t, uint8_t, ::std::function<OnWriteSignature<::std::string>>);
343template void BasicSlave::OnWrite<::std::string>(
344 uint16_t, uint8_t, ::std::function<OnWriteSignature<::std::string>>,
345 ::std::error_code&);
346
347// OCTET_STRING
348template void BasicSlave::OnRead<::std::vector<uint8_t>>(
349 uint16_t, uint8_t,
350 ::std::function<OnReadSignature<::std::vector<uint8_t>>>);
351template void BasicSlave::OnRead<::std::vector<uint8_t>>(
352 uint16_t, uint8_t, ::std::function<OnReadSignature<::std::vector<uint8_t>>>,
353 ::std::error_code&);
354template void BasicSlave::OnWrite<::std::vector<uint8_t>>(
355 uint16_t, uint8_t,
356 ::std::function<OnWriteSignature<::std::vector<uint8_t>>>);
357template 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
363template void BasicSlave::OnRead<::std::basic_string<char16_t>>(
364 uint16_t, uint8_t,
365 ::std::function<OnReadSignature<::std::basic_string<char16_t>>>);
366template 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&);
370template void BasicSlave::OnWrite<::std::basic_string<char16_t>>(
371 uint16_t, uint8_t,
372 ::std::function<OnWriteSignature<::std::basic_string<char16_t>>>);
373template 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
384template void BasicSlave::OnRead<double>(
385 uint16_t, uint8_t, ::std::function<OnReadSignature<double>>);
386template void BasicSlave::OnRead<double>(
387 uint16_t, uint8_t, ::std::function<OnReadSignature<double>>,
388 ::std::error_code&);
389template void BasicSlave::OnWrite<double>(
390 uint16_t, uint8_t, ::std::function<OnWriteSignature<double>>);
391template 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
400template void BasicSlave::OnRead<int64_t>(
401 uint16_t, uint8_t, ::std::function<OnReadSignature<int64_t>>);
402template void BasicSlave::OnRead<int64_t>(
403 uint16_t, uint8_t, ::std::function<OnReadSignature<int64_t>>,
404 ::std::error_code&);
405template void BasicSlave::OnWrite<int64_t>(
406 uint16_t, uint8_t, ::std::function<OnWriteSignature<int64_t>>);
407template 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
417template void BasicSlave::OnRead<uint64_t>(
418 uint16_t, uint8_t, ::std::function<OnReadSignature<uint64_t>>);
419template void BasicSlave::OnRead<uint64_t>(
420 uint16_t, uint8_t, ::std::function<OnReadSignature<uint64_t>>,
421 ::std::error_code&);
422template void BasicSlave::OnWrite<uint64_t>(
423 uint16_t, uint8_t, ::std::function<OnWriteSignature<uint64_t>>);
424template 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
430BasicSlave::Impl_::Impl_(BasicSlave* self_, CONMT* nmt) : self(self_) {
431 nmt->setLgInd<Impl_, &Impl_::OnLgInd>(this);
432}
433
434void
435BasicSlave::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
442template <uint16_t N>
443uint32_t
444BasicSlave::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
450template <uint16_t N>
451uint32_t
452BasicSlave::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
458constexpr uint32_t
459BasicSlave::Impl_::Key(uint16_t idx, uint8_t subidx) noexcept {
460 return (uint32_t(idx) << 8) | subidx;
461}
462
463uint32_t
464BasicSlave::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::CONMT
An opaque CANopen NMT master/slave service type.
Definition: nmt.hpp:70
lely::COSub
An opaque CANopen sub-object type.
Definition: obj.hpp:229
lely::COVal
A CANopen value.
Definition: val.hpp:42
lely::canopen::BasicSlave
The base class for CANopen slaves.
Definition: slave.hpp:32
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
lely::canopen::BasicSlave::OnWriteSignature
typename ::std::conditional< detail::IsCanopenBasic< T >::value, ::std::error_code(uint16_t idx, uint8_t subidx, T &new_val, T old_val), ::std::error_code(uint16_t idx, uint8_t subidx, T &new_val)>::type OnWriteSignature
The signature of the callback function invoked on write (SDO download) access to the local object dic...
Definition: slave.hpp:456
lely::canopen::BasicSlave::OnReadSignature
::std::error_code(uint16_t idx, uint8_t subdx, T &value) OnReadSignature
The signature of the callback function invoked on read (SDO upload) access to the local object dictio...
Definition: slave.hpp:433
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...
Definition: slave.cpp:160
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-...
Definition: slave.cpp:82
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::canopen::Node
The base class for CANopen nodes.
Definition: node.hpp:109
lely::canopen::Node::nmt
CONMT * nmt() const noexcept
Returns a pointer to the internal CANopen NMT master/slave service from <lely/co/nmt....
Definition: node.cpp:137
lely::canopen::SdoError
The type of exception thrown when an SDO abort code is received.
Definition: sdo_error.hpp:116
dev.hpp
This header file is part of the CANopen library; it contains the C++ interface of the device descript...
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
lely::canopen::SdoErrc::NO_SUB
@ NO_SUB
Sub-index does not exist.
lely::canopen::SdoErrc::TYPE_LEN
@ TYPE_LEN
Data type does not match, length of service parameter does not match.
lely::canopen::SdoErrc::NO_OBJ
@ NO_OBJ
Object does not exist in the object dictionary.
lely::canopen::SdoErrc::ERROR
@ ERROR
General error.
lely
Global namespace for the Lely Industries N.V. libraries.
Definition: buf.hpp:32
CO_NMT_EC_OCCURRED
@ CO_NMT_EC_OCCURRED
An NMT error control event occurred.
Definition: nmt.h:80
nmt.hpp
This header file is part of the CANopen library; it contains the C++ interface of the network managem...
obj.hpp
This header file is part of the CANopen library; it contains the C++ interface of the object dictiona...
slave.hpp
This header file is part of the C++ CANopen application library; it contains the CANopen slave declar...
coapp.hpp
This is the internal header file of the C++ CANopen application library.
lely::canopen::BasicSlave::Impl_
The internal implementation of the CANopen slave.
Definition: slave.cpp:43
lely::co_type_traits_T
A class template mapping CANopen types to C++ types.
Definition: type.hpp:59