Lely core libraries 2.3.4
node.cpp
Go to the documentation of this file.
1
24#include "coapp.hpp"
25#include <lely/co/dev.h>
26#if !LELY_NO_CO_EMCY
27#include <lely/co/emcy.h>
28#endif
29#if !LELY_NO_CO_LSS
30#include <lely/co/lss.h>
31#endif
32#include <lely/co/nmt.h>
33#if !LELY_NO_CO_RPDO
34#include <lely/co/rpdo.h>
35#endif
36#if !LELY_NO_CO_SYNC
37#include <lely/co/sync.h>
38#endif
39#if !LELY_NO_CO_TIME
40#include <lely/co/time.h>
41#endif
42#if !LELY_NO_CO_TPDO
43#include <lely/co/tpdo.h>
44#if !LELY_NO_CO_MPDO
45#include <lely/co/val.h>
46#endif
47#endif // !LELY_NO_CO_TPDO
48#include <lely/coapp/node.hpp>
49#if !LELY_NO_CO_RPDO && !LELY_NO_CO_MPDO
50#include <lely/util/endian.h>
51#endif
52
53#include <memory>
54#include <string>
55
56#include <cassert>
57
58namespace lely {
59
60namespace canopen {
61
64 struct NmtDeleter {
65 void
66 operator()(co_nmt_t* nmt) const noexcept {
67 co_nmt_destroy(nmt);
68 }
69 };
70
72
73 void OnCsInd(co_nmt_t* nmt, uint8_t cs) noexcept;
74 void OnHbInd(co_nmt_t* nmt, uint8_t id, int state, int reason) noexcept;
75 void OnStInd(co_nmt_t* nmt, uint8_t id, uint8_t st) noexcept;
76
77#if !LELY_NO_CO_RPDO
78 void OnRpdoInd(co_rpdo_t* pdo, uint32_t ac, const void* ptr,
79 size_t n) noexcept;
80 void OnRpdoErr(co_rpdo_t* pdo, uint16_t eec, uint8_t er) noexcept;
81#endif
82
83#if !LELY_NO_CO_TPDO
84 void OnTpdoInd(co_tpdo_t* pdo, uint32_t ac, const void* ptr,
85 size_t n) noexcept;
86#endif
87
88#if !LELY_NO_CO_SYNC
89 void OnSyncInd(co_nmt_t* nmt, uint8_t cnt) noexcept;
90 void OnSyncErr(co_sync_t* sync, uint16_t eec, uint8_t er) noexcept;
91#endif
92
93#if !LELY_NO_CO_TIME
94 void OnTimeInd(co_time_t* time, const timespec* tp) noexcept;
95#endif
96
97#if !LELY_NO_CO_EMCY
98 void OnEmcyInd(co_emcy_t* emcy, uint8_t id, uint16_t ec, uint8_t er,
99 uint8_t msef[5]) noexcept;
100#endif
101
102#if !LELY_NO_CO_LSS
103 void OnRateInd(co_lss_t*, uint16_t rate, int delay) noexcept;
104 int OnStoreInd(co_lss_t*, uint8_t id, uint16_t rate) noexcept;
105#endif
106
107#if !LELY_NO_CO_RPDO
108 void RpdoRtr(int num) noexcept;
109#endif
110
111 Node* self{nullptr};
112
113 ::std::function<void(io::CanState, io::CanState)> on_can_state;
114 ::std::function<void(io::CanError)> on_can_error;
115
116 ::std::unique_ptr<co_nmt_t, NmtDeleter> nmt;
117
118 ::std::function<void(NmtCommand)> on_command;
119 ::std::function<void(uint8_t, bool)> on_heartbeat;
120 ::std::function<void(uint8_t, NmtState)> on_state;
121#if !LELY_NO_CO_RPDO
122 ::std::function<void(int, ::std::error_code, const void*, ::std::size_t)>
123 on_rpdo;
124 ::std::function<void(int, uint16_t, uint8_t)> on_rpdo_error;
125#endif
126#if !LELY_NO_CO_TPDO
127 ::std::function<void(int, ::std::error_code, const void*, ::std::size_t)>
128 on_tpdo;
129#endif
130#if !LELY_NO_CO_SYNC
131 ::std::function<void(uint8_t, const time_point&)> on_sync;
132 ::std::function<void(uint16_t, uint8_t)> on_sync_error;
133#endif
134#if !LELY_NO_CO_TIME
135 ::std::function<void(const ::std::chrono::system_clock::time_point&)> on_time;
136#endif
137#if !LELY_NO_CO_EMCY
138 ::std::function<void(uint8_t, uint16_t, uint8_t, uint8_t[5])> on_emcy;
139#endif
140#if !LELY_NO_CO_LSS
141 ::std::function<void(int, ::std::chrono::milliseconds)> on_switch_bitrate;
142#endif
143};
144
146 const ::std::string& dcf_txt, const ::std::string& dcf_bin,
147 uint8_t id)
148 : io::CanNet(exec, timer, chan, 0, 0),
149 Device(dcf_txt, dcf_bin, id, this),
150 tpdo_event_mutex(*this),
151 impl_(new Impl_(this, net(), Device::dev())) {
152 // Start processing CAN frames.
153 start();
154}
155
156Node::~Node() = default;
157
162
165 return get_ctx();
166}
167
170 return get_clock();
171}
172
173void
174Node::SubmitWait(const time_point& t, io_tqueue_wait& wait) {
175 wait.value = util::to_timespec(t);
176 io_tqueue_submit_wait(*this, &wait);
177}
178
179void
180Node::SubmitWait(const duration& d, io_tqueue_wait& wait) {
181 SubmitWait(GetClock().gettime() + d, wait);
182}
183
185Node::AsyncWait(ev_exec_t* exec, const time_point& t, io_tqueue_wait** pwait) {
186 if (!exec) exec = GetExecutor();
187 auto value = util::to_timespec(t);
188 ev::Future<void, int> f{io_tqueue_async_wait(*this, exec, &value, pwait)};
189 if (!f) util::throw_errc("AsyncWait");
190 return f.then(exec, [](ev::Future<void, int> f) {
191 // Convert the error code into an exception pointer.
192 int errc = f.get().error();
193 if (errc) util::throw_errc("AsyncWait", errc);
194 });
195}
196
198Node::AsyncWait(ev_exec_t* exec, const duration& d, io_tqueue_wait** pwait) {
199 return AsyncWait(exec, GetClock().gettime() + d, pwait);
200}
201
202bool
204 return io_tqueue_cancel_wait(*this, &wait) != 0;
205}
206
207bool
209 return io_tqueue_abort_wait(*this, &wait) != 0;
210}
211
214 ::std::chrono::milliseconds delay) {
215 // Stop transmitting CAN frames.
216 ctrl.stop();
217 return AsyncWait(GetExecutor(), delay)
218 .then(GetExecutor(),
220 // Propagate the exception, if any.
221 f.get().value();
222 // Wait for the delay period before switching the bitrate.
223 return AsyncWait(GetExecutor(), delay);
224 })
225 .then(GetExecutor(),
226 [this, &ctrl, bitrate,
228 // Propagate the exception, if any.
229 f.get().value();
230 // Activate the new bitrate.
231 ctrl.set_bitrate(bitrate);
232 // Wait for the delay period before resuming CAN frame
233 // transmission.
234 return AsyncWait(GetExecutor(), delay);
235 })
237 // Propagate the exception, if any.
238 f.get().value();
239 // Resume CAN frame transmission.
240 ctrl.restart();
241 });
242}
243
244void
246 ::std::function<void(io::CanState, io::CanState)> on_can_state) {
247 ::std::lock_guard<util::BasicLockable> lock(*this);
248 impl_->on_can_state = on_can_state;
249}
250
251void
252Node::OnCanError(::std::function<void(io::CanError)> on_can_error) {
253 ::std::lock_guard<util::BasicLockable> lock(*this);
254 impl_->on_can_error = on_can_error;
255}
256
257void
259 ::std::lock_guard<util::BasicLockable> lock(*this);
260
261 // Update the CAN network time before resetting the node. In the case of a
262 // master, this ensures that SDO timeouts do not occur too soon.
263 SetTime();
264
266 util::throw_errc("Reset");
267}
268
269void
270Node::ConfigHeartbeat(uint8_t id, const ::std::chrono::milliseconds& ms,
271 ::std::error_code& ec) {
272 ::std::lock_guard<util::BasicLockable> lock(*this);
273
274 auto ac = co_dev_cfg_hb(dev(), id, ms.count());
275
276 if (ac)
277 ec = static_cast<SdoErrc>(ac);
278 else
279 ec.clear();
280}
281
282void
283Node::ConfigHeartbeat(uint8_t id, const ::std::chrono::milliseconds& ms) {
284 ::std::error_code ec;
285 ConfigHeartbeat(id, ms, ec);
286 if (ec) throw SdoError(Device::id(), 0x1016, 0, ec, "ConfigHeartbeat");
287}
288
289void
290Node::OnCommand(::std::function<void(NmtCommand)> on_command) {
291 ::std::lock_guard<util::BasicLockable> lock(*this);
292 impl_->on_command = on_command;
293}
294
295void
296Node::OnHeartbeat(::std::function<void(uint8_t, bool)> on_heartbeat) {
297 ::std::lock_guard<util::BasicLockable> lock(*this);
298 impl_->on_heartbeat = on_heartbeat;
299}
300
301void
302Node::OnState(::std::function<void(uint8_t, NmtState)> on_state) {
303 ::std::lock_guard<util::BasicLockable> lock(*this);
304 impl_->on_state = on_state;
305}
306
307void
309 ::std::function<void(int, ::std::error_code, const void*, ::std::size_t)>
310 on_rpdo) {
311#if LELY_NO_CO_RPDO
312 (void)on_rpdo;
313#else
314 ::std::lock_guard<util::BasicLockable> lock(*this);
315 impl_->on_rpdo = on_rpdo;
316#endif
317}
318
319void
320Node::OnRpdoError(::std::function<void(int, uint16_t, uint8_t)> on_rpdo_error) {
321#if LELY_NO_CO_RPDO
322 (void)on_rpdo_error;
323#else
324 ::std::lock_guard<util::BasicLockable> lock(*this);
325 impl_->on_rpdo_error = on_rpdo_error;
326#endif
327}
328
329void
331 ::std::function<void(int, ::std::error_code, const void*, ::std::size_t)>
332 on_tpdo) {
333#if LELY_NO_CO_TPDO
334 (void)on_tpdo;
335#else
336 ::std::lock_guard<util::BasicLockable> lock(*this);
337 impl_->on_tpdo = on_tpdo;
338#endif
339}
340
341void
342Node::OnSync(::std::function<void(uint8_t, const time_point&)> on_sync) {
343#if LELY_NO_CO_SYNC
344 (void)on_sync;
345#else
346 ::std::lock_guard<util::BasicLockable> lock(*this);
347 impl_->on_sync = on_sync;
348#endif
349}
350
351void
352Node::OnSyncError(::std::function<void(uint16_t, uint8_t)> on_sync_error) {
353#if LELY_NO_CO_SYNC
354 (void)on_sync_error;
355#else
356 ::std::lock_guard<util::BasicLockable> lock(*this);
357 impl_->on_sync_error = on_sync_error;
358#endif
359}
360
361void
363 ::std::function<void(const ::std::chrono::system_clock::time_point&)>
364 on_time) {
365#if LELY_NO_CO_TIME
366 (void)on_time;
367#else
368 ::std::lock_guard<util::BasicLockable> lock(*this);
369 impl_->on_time = on_time;
370#endif
371}
372
373void
375 ::std::function<void(uint8_t, uint16_t, uint8_t, uint8_t[5])> on_emcy) {
376#if LELY_NO_CO_EMCY
377 (void)on_emcy;
378#else
379 ::std::lock_guard<util::BasicLockable> lock(*this);
380 impl_->on_emcy = on_emcy;
381#endif
382}
383
384void
386 ::std::function<void(int, ::std::chrono::milliseconds)> on_switch_bitrate) {
387#if LELY_NO_CO_LSS
388 (void)on_switch_bitrate;
389#else
390 ::std::lock_guard<util::BasicLockable> lock(*this);
391 impl_->on_switch_bitrate = on_switch_bitrate;
392#endif
393}
394
395void
397#if !LELY_NO_CO_TPDO
399#endif
400}
401
402void
404#if !LELY_NO_CO_TPDO
405 co_nmt_on_tpdo_event_unlock(node->nmt());
406#endif
407}
408
411 return *this;
412}
413
414void
416 set_time();
417}
418
419void
422
423 // TODO(jseldenthuis@lely.com): Clear EMCY in error active mode.
424 if (new_state == io::CanState::PASSIVE) {
425 // CAN in error passive mode.
426 Error(0x8120, 0x10);
427 } else if (old_state == io::CanState::BUSOFF) {
428 // Recovered from bus off.
429 Error(0x8140, 0x10);
430 }
431}
432
435 return impl_->nmt.get();
436}
437
438void
439Node::Error(uint16_t eec, uint8_t er, const uint8_t msef[5]) noexcept {
440 co_nmt_on_err(nmt(), eec, er, msef);
441}
442
443void
444Node::RpdoRtr(int num) noexcept {
445#if LELY_NO_CO_RPDO
446 (void)num;
447#else
448 if (num) {
449 impl_->RpdoRtr(num);
450 } else {
451 for (num = 1; num <= 512; num++) impl_->RpdoRtr(num);
452 }
453#endif
454}
455
456void
457Node::TpdoEvent(int num) noexcept {
458#if LELY_NO_CO_TPDO
459 (void)num;
460#else
462#endif
463}
464
465template <class T>
466typename ::std::enable_if<is_canopen_basic<T>::value && sizeof(T) <= 4,
467 void>::type
468Node::DamMpdoEvent(int num, uint8_t id, uint16_t idx, uint8_t subidx, T value) {
469#if LELY_NO_CO_MPDO
470 (void)num;
471 (void)id;
472 (void)idx;
473 (void)subidx;
474 (void)value;
475#else
476 auto pdo = co_nmt_get_tpdo(nmt(), num);
477 if (pdo) {
478 uint8_t buf[4] = {0};
479 co_val_write(canopen_traits<T>::index, &value, buf, buf + 4);
480 co_dam_mpdo_event(pdo, id, idx, subidx, buf);
481 }
482#endif
483}
484
485#ifndef DOXYGEN_SHOULD_SKIP_THIS
486
487// BOOLEAN
488template void Node::DamMpdoEvent<bool>(int, uint8_t, uint16_t, uint8_t, bool);
489
490// INTEGER8
491template void Node::DamMpdoEvent<int8_t>(int, uint8_t, uint16_t, uint8_t,
492 int8_t);
493
494// INTEGER16
495template void Node::DamMpdoEvent<int16_t>(int, uint8_t, uint16_t, uint8_t,
496 int16_t);
497
498// INTEGER32
499template void Node::DamMpdoEvent<int32_t>(int, uint8_t, uint16_t, uint8_t,
500 int32_t);
501
502// UNSIGNED8
503template void Node::DamMpdoEvent<uint8_t>(int, uint8_t, uint16_t, uint8_t,
504 uint8_t);
505
506// UNSIGNED16
507template void Node::DamMpdoEvent<uint16_t>(int, uint8_t, uint16_t, uint8_t,
508 uint16_t);
509
510// UNSIGNED32
511template void Node::DamMpdoEvent<uint32_t>(int, uint8_t, uint16_t, uint8_t,
512 uint32_t);
513
514// REAL32
515template void Node::DamMpdoEvent<float>(int, uint8_t, uint16_t, uint8_t, float);
516
517// VISIBLE_STRING
518// OCTET_STRING
519// UNICODE_STRING
520// TIME_OF_DAY
521// TIME_DIFFERENCE
522// DOMAIN
523// INTEGER24
524// REAL64
525// INTEGER40
526// INTEGER48
527// INTEGER56
528// INTEGER64
529// UNSIGNED24
530// UNSIGNED40
531// UNSIGNED48
532// UNSIGNED56
533// UNSIGNED64
534
535#endif // !DOXYGEN_SHOULD_SKIP_THIS
536
537void
538Node::on_can_state(io::CanState new_state, io::CanState old_state) noexcept {
539 OnCanState(new_state, old_state);
540 if (impl_->on_can_state) {
541 auto f = impl_->on_can_state;
542 util::UnlockGuard<util::BasicLockable> unlock(*this);
543 f(new_state, old_state);
544 }
545}
546
547void
548Node::on_can_error(io::CanError error) noexcept {
549 OnCanError(error);
550 if (impl_->on_can_error) {
551 auto f = impl_->on_can_error;
552 util::UnlockGuard<util::BasicLockable> unlock(*this);
553 f(error);
554 }
555}
556
557void
558Node::OnStore(uint8_t, int) {
559 util::throw_error_code("OnStore", ::std::errc::operation_not_supported);
560}
561
562Node::Impl_::Impl_(Node* self_, can_net_t* net, co_dev_t* dev)
563 : self(self_), nmt(co_nmt_create(net, dev)) {
565 nmt.get(),
566 [](co_nmt_t* nmt, uint8_t cs, void* data) noexcept {
567 static_cast<Impl_*>(data)->OnCsInd(nmt, cs);
568 },
569 this);
570
572 nmt.get(),
573 [](co_nmt_t* nmt, uint8_t id, int state, int reason,
574 void* data) noexcept {
575 static_cast<Impl_*>(data)->OnHbInd(nmt, id, state, reason);
576 },
577 this);
578
580 nmt.get(),
581 [](co_nmt_t* nmt, uint8_t id, uint8_t st, void* data) noexcept {
582 static_cast<Impl_*>(data)->OnStInd(nmt, id, st);
583 },
584 this);
585
586#if !LELY_NO_CO_SYNC
588 nmt.get(),
589 [](co_nmt_t* nmt, uint8_t cnt, void* data) noexcept {
590 static_cast<Impl_*>(data)->OnSyncInd(nmt, cnt);
591 },
592 this);
593#endif
594}
595
596void
597Node::Impl_::OnCsInd(co_nmt_t* nmt, uint8_t cs) noexcept {
598 (void)nmt;
599
600 if (cs == CO_NMT_CS_RESET_COMM) {
601#if !LELY_NO_CO_LSS
602 auto lss = co_nmt_get_lss(nmt);
603 if (lss) {
605 lss,
606 [](co_lss_t* lss, uint16_t rate, int delay, void* data) noexcept {
607 static_cast<Impl_*>(data)->OnRateInd(lss, rate, delay);
608 },
609 this);
610
612 lss,
613 [](co_lss_t* lss, uint8_t id, uint16_t rate, void* data) noexcept {
614 return static_cast<Impl_*>(data)->OnStoreInd(lss, id, rate);
615 },
616 this);
617 }
618#endif
619 }
620
621#if !LELY_NO_CO_SYNC
622 if (cs == CO_NMT_CS_START || cs == CO_NMT_CS_ENTER_PREOP) {
623 auto sync = co_nmt_get_sync(nmt);
624 if (sync) {
626 sync,
627 [](co_sync_t* sync, uint16_t eec, uint8_t er, void* data) noexcept {
628 static_cast<Impl_*>(data)->OnSyncErr(sync, eec, er);
629 },
630 this);
631 }
632#endif
633
634#if !LELY_NO_CO_TIME
635 auto time = co_nmt_get_time(nmt);
636 if (time) {
638 time,
639 [](co_time_t* time, const timespec* tp, void* data) noexcept {
640 static_cast<Impl_*>(data)->OnTimeInd(time, tp);
641 },
642 this);
643 }
644#endif
645
646#if !LELY_NO_CO_EMCY
647 auto emcy = co_nmt_get_emcy(nmt);
648 if (emcy) {
650 emcy,
651 [](co_emcy_t* emcy, uint8_t id, uint16_t eec, uint8_t er,
652 uint8_t msef[5], void* data) noexcept {
653 static_cast<Impl_*>(data)->OnEmcyInd(emcy, id, eec, er, msef);
654 },
655 this);
656 }
657 }
658#endif
659
660 if (cs == CO_NMT_CS_START) {
661#if !LELY_NO_CO_RPDO
662 for (int i = 1; i <= 512; i++) {
663 auto pdo = co_nmt_get_rpdo(nmt, i);
664 if (pdo) {
666 pdo,
667 [](co_rpdo_t* pdo, uint32_t ac, const void* ptr, size_t n,
668 void* data) noexcept {
669 static_cast<Impl_*>(data)->OnRpdoInd(pdo, ac, ptr, n);
670 },
671 this);
672
674 pdo,
675 [](co_rpdo_t* pdo, uint16_t eec, uint8_t er, void* data) noexcept {
676 static_cast<Impl_*>(data)->OnRpdoErr(pdo, eec, er);
677 },
678 this);
679 }
680 }
681#endif
682#if !LELY_NO_CO_TPDO
683 for (int i = 1; i <= 512; i++) {
684 auto pdo = co_nmt_get_tpdo(nmt, i);
685 if (pdo) {
687 pdo,
688 [](co_tpdo_t* pdo, uint32_t ac, const void* ptr, size_t n,
689 void* data) noexcept {
690 static_cast<Impl_*>(data)->OnTpdoInd(pdo, ac, ptr, n);
691 },
692 this);
693 }
694 }
695#endif
696 }
697
698 if (cs != CO_NMT_CS_RESET_NODE && cs != CO_NMT_CS_RESET_COMM) {
699#if !LELY_NO_CO_RPDO
700 self->UpdateRpdoMapping();
701#endif
702#if !LELY_NO_CO_TPDO
703 self->UpdateTpdoMapping();
704#endif
705 }
706
707 self->OnCommand(static_cast<NmtCommand>(cs));
708
709 if (on_command) {
710 auto f = on_command;
711 util::UnlockGuard<util::BasicLockable> unlock(*self);
712 f(static_cast<NmtCommand>(cs));
713 }
714}
715
716void
717Node::Impl_::OnHbInd(co_nmt_t* nmt, uint8_t id, int state,
718 int reason) noexcept {
719 // Invoke the default behavior before notifying the implementation.
720 co_nmt_on_hb(nmt, id, state, reason);
721 // Only handle heartbeat timeout events. State changes are handled by OnSt().
722 if (reason != CO_NMT_EC_TIMEOUT) return;
723 // Notify the implementation.
724 bool occurred = state == CO_NMT_EC_OCCURRED;
725 self->OnHeartbeat(id, occurred);
726
727 if (on_heartbeat) {
728 auto f = on_heartbeat;
729 util::UnlockGuard<util::BasicLockable> unlock(*self);
730 f(id, occurred);
731 }
732}
733
734void
735Node::Impl_::OnStInd(co_nmt_t* nmt, uint8_t id, uint8_t st) noexcept {
736 // Invoke the default behavior before notifying the implementation.
737 co_nmt_on_st(nmt, id, st);
738 // Ignore local state changes.
739 if (id == co_dev_get_id(co_nmt_get_dev(nmt))) return;
740 // Notify the implementation.
741 self->OnState(id, static_cast<NmtState>(st));
742
743 if (on_state) {
744 auto f = on_state;
745 util::UnlockGuard<util::BasicLockable> unlock(*self);
746 f(id, static_cast<NmtState>(st));
747 }
748}
749
750#if !LELY_NO_CO_RPDO
751
752void
753Node::Impl_::OnRpdoInd(co_rpdo_t* pdo, uint32_t ac, const void* ptr,
754 size_t n) noexcept {
755#if !LELY_NO_CO_MPDO
756 assert(ptr || !n);
757
758 if (!ac) {
759 // Check if this is a SAM-MPDO.
760 auto par = co_rpdo_get_map_par(pdo);
761 assert(par);
762 auto buf = static_cast<const uint8_t*>(ptr);
763 if (par->n == CO_PDO_MAP_SAM_MPDO && n == CAN_MAX_LEN && !(buf[0] & 0x80))
764 self->RpdoWrite(buf[0], ldle_u16(buf + 1), buf[3]);
765 }
766#endif
767
768 int num = co_rpdo_get_num(pdo);
769 self->OnRpdo(num, static_cast<SdoErrc>(ac), ptr, n);
770
771 if (on_rpdo) {
772 auto f = on_rpdo;
773 util::UnlockGuard<util::BasicLockable> unlock(*self);
774 f(num, static_cast<SdoErrc>(ac), ptr, n);
775 }
776}
777
778void
779Node::Impl_::OnRpdoErr(co_rpdo_t* pdo, uint16_t eec, uint8_t er) noexcept {
780 int num = co_rpdo_get_num(pdo);
781 self->OnRpdoError(num, eec, er);
782
783 if (on_rpdo_error) {
784 auto f = on_rpdo_error;
785 util::UnlockGuard<util::BasicLockable> unlock(*self);
786 f(num, eec, er);
787 }
788}
789
790#endif // !LELY_NO_CO_RPDO
791
792#if !LELY_NO_CO_TPDO
793void
794Node::Impl_::OnTpdoInd(co_tpdo_t* pdo, uint32_t ac, const void* ptr,
795 size_t n) noexcept {
796 int num = co_tpdo_get_num(pdo);
797 self->OnTpdo(num, static_cast<SdoErrc>(ac), ptr, n);
798
799 if (on_tpdo) {
800 auto f = on_tpdo;
801 util::UnlockGuard<util::BasicLockable> unlock(*self);
802 f(num, static_cast<SdoErrc>(ac), ptr, n);
803 }
804}
805#endif
806
807#if !LELY_NO_CO_SYNC
808
809void
810Node::Impl_::OnSyncInd(co_nmt_t*, uint8_t cnt) noexcept {
811 auto t = self->GetClock().gettime();
812 self->OnSync(cnt, t);
813
814 if (on_sync) {
815 auto f = on_sync;
816 util::UnlockGuard<util::BasicLockable> unlock(*self);
817 f(cnt, t);
818 }
819}
820
821void
822Node::Impl_::OnSyncErr(co_sync_t*, uint16_t eec, uint8_t er) noexcept {
823 self->OnSyncError(eec, er);
824
825 if (on_sync_error) {
826 auto f = on_sync_error;
827 util::UnlockGuard<util::BasicLockable> unlock(*self);
828 f(eec, er);
829 }
830}
831
832#endif // !LELY_NO_CO_SYNC
833
834#if !LELY_NO_CO_TIME
835void
836Node::Impl_::OnTimeInd(co_time_t*, const timespec* tp) noexcept {
837 assert(tp);
838 ::std::chrono::system_clock::time_point abs_time(util::from_timespec(*tp));
839 self->OnTime(abs_time);
840
841 if (on_time) {
842 auto f = on_time;
843 util::UnlockGuard<util::BasicLockable> unlock(*self);
844 f(abs_time);
845 }
846}
847#endif
848
849#if !LELY_NO_CO_EMCY
850void
851Node::Impl_::OnEmcyInd(co_emcy_t*, uint8_t id, uint16_t ec, uint8_t er,
852 uint8_t msef[5]) noexcept {
853 self->OnEmcy(id, ec, er, msef);
854
855 if (on_emcy) {
856 auto f = on_emcy;
857 util::UnlockGuard<util::BasicLockable> unlock(*self);
858 f(id, ec, er, msef);
859 }
860}
861#endif
862
863#if !LELY_NO_CO_LSS
864
865void
866Node::Impl_::OnRateInd(co_lss_t*, uint16_t rate, int delay) noexcept {
867 self->OnSwitchBitrate(rate * 1000, ::std::chrono::milliseconds(delay));
868
869 if (on_switch_bitrate) {
870 auto f = on_switch_bitrate;
871 util::UnlockGuard<util::BasicLockable> unlock(*self);
872 f(rate * 1000, ::std::chrono::milliseconds(delay));
873 }
874}
875
876int
877Node::Impl_::OnStoreInd(co_lss_t*, uint8_t id, uint16_t rate) noexcept {
878 try {
879 self->OnStore(id, rate * 1000);
880 return 0;
881 } catch (...) {
882 return -1;
883 }
884}
885
886#endif // !LELY_NO_CO_LSS
887
888#if !LELY_NO_CO_RPDO
889void
890Node::Impl_::RpdoRtr(int num) noexcept {
891 auto pdo = co_nmt_get_rpdo(nmt.get(), num);
892 if (pdo) {
893 int errsv = get_errc();
894 co_rpdo_rtr(pdo);
895 set_errc(errsv);
896 }
897}
898#endif
899
900} // namespace canopen
901
902} // namespace lely
#define CAN_MAX_LEN
The maximum number of bytes in the payload of a CAN format frame.
Definition msg.h:72
A CANopen value.
Definition val.hpp:42
The CANopen device description.
Definition device.hpp:45
__co_dev * dev() const noexcept
Returns a pointer to the internal CANopen device from <lely/co/dev.hpp>.
Definition device.cpp:828
uint8_t id() const noexcept
Returns the node-ID.
Definition device.cpp:193
void lock() override
Blocks until a lock can be obtained for the current execution agent (thread, process,...
Definition node.cpp:396
void unlock() override
Releases the lock held by the execution agent. Throws no exceptions.
Definition node.cpp:403
The base class for CANopen nodes.
Definition node.hpp:116
ev::Future< void, ::std::exception_ptr > AsyncWait(ev_exec_t *exec, const time_point &t, io_tqueue_wait **pwait=nullptr)
Submits an asynchronous wait operation and creates a future which becomes ready once the wait operati...
Definition node.cpp:185
void Reset()
(Re)starts the node.
Definition node.cpp:258
void OnSyncError(::std::function< void(uint16_t, uint8_t)> on_sync_error)
Registers the function to be invoked when the data length of a received SYNC message does not match.
Definition node.cpp:352
void OnSync(::std::function< void(uint8_t, const time_point &)> on_sync)
Registers the function to be invoked when a SYNC message is sent/received.
Definition node.cpp:342
void OnState(::std::function< void(uint8_t, NmtState)> on_state)
Registers the function to be invoked when an NMT state change or boot-up event is detected for a remo...
Definition node.cpp:302
void ConfigHeartbeat(uint8_t id, const ::std::chrono::milliseconds &ms, ::std::error_code &ec)
Configures heartbeat consumption for the specified node by updating CANopen object 1016 (Consumer hea...
Definition node.cpp:270
void SetTime()
Updates the CAN network time.
Definition node.cpp:415
void OnCanError(::std::function< void(io::CanError)> on_can_error)
Registers the function to be invoked when an error is detected on the CAN bus.
Definition node.cpp:252
void OnCommand(::std::function< void(NmtCommand)> on_command)
Registers the function to be invoked when an NMT command is received from the master.
Definition node.cpp:290
void OnSwitchBitrate(::std::function< void(int, ::std::chrono::milliseconds)> on_switch_bitrate)
Registers the function to be invoked when the LSS master activates the bit rate of all CANopen device...
Definition node.cpp:385
Node(ev_exec_t *exec, io::TimerBase &timer, io::CanChannelBase &chan, const ::std::string &dcf_txt, const ::std::string &dcf_bin="", uint8_t id=0xff)
Creates a new CANopen node.
Definition node.cpp:145
__co_nmt * nmt() const noexcept
Returns a pointer to the internal CANopen NMT master/slave service from <lely/co/nmt....
Definition node.cpp:434
io::ContextBase GetContext() const noexcept
Returns the underlying I/O context with which this context is registered.
Definition node.cpp:164
void TpdoEvent(int num=0) noexcept
Triggers the transmission of an acyclic or event-driven PDO.
Definition node.cpp:457
void OnHeartbeat(::std::function< void(uint8_t, bool)> on_heartbeat)
Registers the function to be invoked when a heartbeat timeout event occurs or is resolved.
Definition node.cpp:296
ev::Future< void, ::std::exception_ptr > AsyncSwitchBitrate(io::CanControllerBase &ctrl, int bitrate, ::std::chrono::milliseconds delay)
Stops the specified CAN controller and submits asynchronous operations to wait for the delay period,...
Definition node.cpp:213
__can_net * net() const noexcept
Returns a pointer to the internal CAN network interface from <lely/can/net.h>.
Definition node.cpp:410
ev::Executor GetExecutor() const noexcept
Returns the executor used to process I/O and CANopen events.
Definition node.cpp:159
void OnTime(::std::function< void(const ::std::chrono::system_clock::time_point &)> on_time)
Registers the function to be invoked when a TIME message is received.
Definition node.cpp:362
void OnRpdo(::std::function< void(int, ::std::error_code, const void *, ::std::size_t)> on_rpdo)
Registers the function to be invoked when a Receive-PDO is processed.
Definition node.cpp:308
void OnRpdoError(::std::function< void(int, uint16_t, uint8_t)> on_rpdo_error)
Registers the function to be invoked when a Receive-PDO length mismatch or timeout error occurs.
Definition node.cpp:320
void OnTpdo(::std::function< void(int, ::std::error_code, const void *, ::std::size_t)> on_tpdo)
Registers the function to be invoked after a Transmit-PDO is sent or an error occurs.
Definition node.cpp:330
io::Clock GetClock() const noexcept
Returns the clock used by the timer.
Definition node.cpp:169
bool AbortWait(io_tqueue_wait &wait) noexcept
Aborts the specified wait operation if it is pending.
Definition node.cpp:208
void OnEmcy(::std::function< void(uint8_t, uint16_t, uint8_t, uint8_t[5])> on_emcy)
Registers the function to be invoked when an EMCY message is received.
Definition node.cpp:374
void OnCanState(::std::function< void(io::CanState, io::CanState)> on_can_state)
Registers the function to be invoked when a CAN bus state change is detected.
Definition node.cpp:245
void SubmitWait(const time_point &t, io_tqueue_wait &wait)
Submits a wait operation.
Definition node.cpp:174
void RpdoRtr(int num=0) noexcept
Requests the transmission of a PDO by sending a CAN frame with the RTR (Remote Transmission Request) ...
Definition node.cpp:444
bool CancelWait(io_tqueue_wait &wait) noexcept
Cancels the specified wait operation if it is pending.
Definition node.cpp:203
The type of exception thrown when an SDO abort code is received.
The type of objects thrown as exceptions to report a system error with an associated error code.
Definition exception.hpp:54
An abstract task executor. This class is a wrapper around #ev_exec_t*.
Definition exec.hpp:38
A future.
Definition future.hpp:384
A reference to an abstract CAN channel.
Definition can.hpp:430
A reference to an abstract CAN controller.
Definition can.hpp:286
void stop(::std::error_code &ec) noexcept
Definition can.hpp:294
void set_bitrate(int nominal, int data, ::std::error_code &ec) noexcept
Definition can.hpp:379
void restart(::std::error_code &ec) noexcept
Definition can.hpp:339
Clock get_clock() const noexcept
Definition can_net.hpp:98
void unlock() final
Releases the lock held by the execution agent. Throws no exceptions.
Definition can_net.hpp:109
void lock() final
Blocks until a lock can be obtained for the current execution agent (thread, process,...
Definition can_net.hpp:104
void set_time()
Updates the CAN network time.
Definition can_net.hpp:124
virtual void on_can_error(CanError error) noexcept
The function invoked when an error is detected on the CAN bus.
Definition can_net.hpp:208
void start() noexcept
Definition can_net.hpp:80
ev::Executor get_executor() const noexcept
Definition can_net.hpp:92
virtual void on_can_state(CanState new_state, CanState old_state) noexcept
The function invoked when a CAN bus state change is detected.
Definition can_net.hpp:191
ContextBase get_ctx() const noexcept
Definition can_net.hpp:86
An abstract clock. This class is a wrapper around #io_clock_t*.
Definition clock.hpp:35
A refence to an I/O context. This class is a wrapper around #io_ctx_t*.
Definition ctx.hpp:49
A reference to an abstract timer.
Definition timer.hpp:130
This header file is part of the CANopen library; it contains the device description declarations.
co_unsigned8_t co_dev_get_id(const co_dev_t *dev)
Returns the node-ID of a CANopen device.
Definition dev.c:197
This header file is part of the CANopen library; it contains the time stamp (TIME) object declaration...
void co_time_set_ind(co_time_t *time, co_time_ind_t *ind, void *data)
Sets the indication function invoked when a CANopen time stamp is received.
Definition time.c:355
This is the internal header file of the C++ CANopen application library.
This header file is part of the CANopen library; it contains the emergency (EMCY) object declarations...
void co_emcy_set_ind(co_emcy_t *emcy, co_emcy_ind_t *ind, void *data)
Sets the indication function invoked when a CANopen EMCY message is received.
Definition emcy.c:625
This header file is part of the utilities library; it contains the byte order (endianness) function d...
uint_least16_t ldle_u16(const uint_least8_t src[2])
Loads a 16-bit unsigned integer in little-endian byte order.
Definition endian.h:516
CanError
The error flags of a CAN bus, which are not mutually exclusive.
Definition err.hpp:47
CanState
The states of a CAN node, depending on the TX/RX error count.
Definition err.hpp:33
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition errnum.c:932
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition errnum.c:944
const struct ev_exec_vtbl *const ev_exec_t
An abstract task executor.
Definition ev.h:29
This header file is part of the CANopen library; it contains the Layer Setting Services (LSS) and pro...
void co_lss_set_store_ind(co_lss_t *lss, co_lss_store_ind_t *ind, void *data)
Sets the indication function invoked when an LSS 'store configuration' request is received.
Definition lss.c:897
void co_lss_set_rate_ind(co_lss_t *lss, co_lss_rate_ind_t *ind, void *data)
Sets the indication function invoked when an LSS 'activate bit timing' request is received.
Definition lss.c:876
NmtState
The NMT states.
Definition node.hpp:56
SdoErrc
The SDO abort codes.
Definition sdo_error.hpp:42
NmtCommand
The NMT command specifiers.
Definition node.hpp:42
This header file is part of the CANopen library; it contains the network management (NMT) declaration...
void co_nmt_destroy(co_nmt_t *nmt)
Destroys a CANopen NMT master/slave service.
Definition nmt.c:1258
void co_nmt_set_sync_ind(co_nmt_t *nmt, co_nmt_sync_ind_t *ind, void *data)
Sets the indication function invoked by co_nmt_on_sync() after all PDOs have been transmitted/process...
Definition nmt.c:1601
int co_nmt_cs_ind(co_nmt_t *nmt, co_unsigned8_t cs)
Processes an NMT command from the master or the application.
Definition nmt.c:2098
#define CO_NMT_CS_START
The NMT command specifier 'start'.
Definition nmt.h:40
void co_nmt_on_st(co_nmt_t *nmt, co_unsigned8_t id, co_unsigned8_t st)
Implements the default behavior when a state change is detected by the node guarding or heartbeat pro...
Definition nmt.c:1454
void co_nmt_set_hb_ind(co_nmt_t *nmt, co_nmt_hb_ind_t *ind, void *data)
Sets the indication function invoked when a heartbeat event occurs.
Definition nmt.c:1392
co_rpdo_t * co_nmt_get_rpdo(const co_nmt_t *nmt, co_unsigned16_t n)
Returns a pointer to a Receive-PDO service.
Definition nmt.c:2175
#define CO_NMT_CS_ENTER_PREOP
The NMT command specifier 'enter pre-operational'.
Definition nmt.h:46
#define CO_NMT_CS_RESET_NODE
The NMT command specifier 'reset node'.
Definition nmt.h:49
void co_nmt_on_tpdo_event(co_nmt_t *nmt, co_unsigned16_t n)
Implements the default behavior when an event is indicated for an event-driven (asynchronous) Transmi...
Definition nmt.c:1659
co_nmt_t * co_nmt_create(can_net_t *net, co_dev_t *dev)
Creates a new CANopen NMT master/slave service.
Definition nmt.c:1233
void co_nmt_on_tpdo_event_lock(co_nmt_t *nmt)
Postpones the transmission of PDOs triggered by co_nmt_on_tpdo_event() until a matching call to co_nm...
Definition nmt.c:1690
void co_nmt_on_err(co_nmt_t *nmt, co_unsigned16_t eec, co_unsigned8_t er, const co_unsigned8_t msef[5])
Implements the default error handling behavior by generating an EMCY message with co_emcy_push() and ...
Definition nmt.c:1636
co_lss_t * co_nmt_get_lss(const co_nmt_t *nmt)
Returns a pointer to the LSS master/slave service.
Definition nmt.c:2282
void co_nmt_on_tpdo_event_unlock(co_nmt_t *nmt)
Undoes the effect of a single call to co_nmt_on_tpdo_event_lock() and possibly triggers the transmiss...
Definition nmt.c:1698
void co_nmt_on_hb(co_nmt_t *nmt, co_unsigned8_t id, int state, int reason)
Implements the default behavior when a heartbeat event occurs (see sections 7.2.8....
Definition nmt.c:1401
co_dev_t * co_nmt_get_dev(const co_nmt_t *nmt)
Returns a pointer to the CANopen device of an NMT master/slave service.
Definition nmt.c:1275
@ CO_NMT_EC_TIMEOUT
An NMT error control timeout event.
Definition nmt.h:87
co_emcy_t * co_nmt_get_emcy(const co_nmt_t *nmt)
Returns a pointer to the EMCY producer/consumer service.
Definition nmt.c:2268
co_unsigned32_t co_dev_cfg_hb(co_dev_t *dev, co_unsigned8_t id, co_unsigned16_t ms)
Configures heartbeat consumption for the specified node by updating CANopen object 1016 (Consumer hea...
Definition nmt.c:744
void co_nmt_set_cs_ind(co_nmt_t *nmt, co_nmt_cs_ind_t *ind, void *data)
Sets the indication function invoked when an NMT command is received.
Definition nmt.c:1294
co_time_t * co_nmt_get_time(const co_nmt_t *nmt)
Returns a pointer to the TIME producer/consumer service.
Definition nmt.c:2254
co_tpdo_t * co_nmt_get_tpdo(const co_nmt_t *nmt, co_unsigned16_t n)
Returns a pointer to a Transmit-PDO service.
Definition nmt.c:2193
co_sync_t * co_nmt_get_sync(const co_nmt_t *nmt)
Returns a pointer to the SYNC producer/consumer service.
Definition nmt.c:2240
@ CO_NMT_EC_OCCURRED
An NMT error control event occurred.
Definition nmt.h:80
void co_nmt_set_st_ind(co_nmt_t *nmt, co_nmt_st_ind_t *ind, void *data)
Sets the indication function invoked when a state change is detected.
Definition nmt.c:1445
#define CO_NMT_CS_RESET_COMM
The NMT command specifier 'reset communication'.
Definition nmt.h:52
This header file is part of the C++ CANopen application library; it contains the CANopen node declara...
#define CO_PDO_MAP_SAM_MPDO
The value of sub-index 0 of the PDO mapping parameter record indicating a a source address mode multi...
Definition pdo.h:52
This header file is part of the CANopen library; it contains the Receive-PDO declarations.
int co_rpdo_rtr(co_rpdo_t *pdo)
Requests the transmission of a PDO.
Definition rpdo.c:472
void co_rpdo_set_err(co_rpdo_t *pdo, co_rpdo_err_t *err, void *data)
Sets the error handling function of a Receive-PDO service.
Definition rpdo.c:463
co_unsigned16_t co_rpdo_get_num(const co_rpdo_t *pdo)
Returns the PDO number of a Receive-PDO.
Definition rpdo.c:408
void co_rpdo_set_ind(co_rpdo_t *pdo, co_rpdo_ind_t *ind, void *data)
Sets the indication function invoked when a Receive-PDO error occurs.
Definition rpdo.c:443
const struct co_pdo_map_par * co_rpdo_get_map_par(const co_rpdo_t *pdo)
Returns a pointer to the PDO mapping parameter record of a Receive-PDO.
Definition rpdo.c:424
A CAN network interface.
Definition net.c:37
A CANopen device.
Definition dev.h:30
A CANopen EMCY producer/consumer service.
Definition emcy.c:85
A CANopen LSS master/slave service.
Definition lss.c:44
A CANopen NMT master/slave service.
Definition nmt.c:148
A CANopen Receive-PDO.
Definition rpdo.c:44
A CANopen SYNC producer/consumer service.
Definition sync.c:40
A CANopen TIME producer/consumer service.
Definition time.c:41
A CANopen Transmit-PDO.
Definition tpdo.c:53
A wait operation suitable for use with a timer queue.
Definition tqueue.h:36
struct timespec value
The absolute expiration time.
Definition tqueue.h:38
The internal implementation of the CANopen node.
Definition node.cpp:63
A time type with nanosecond resolution.
Definition time.h:88
This header file is part of the CANopen library; it contains the synchronization (SYNC) object declar...
void co_sync_set_err(co_sync_t *sync, co_sync_err_t *err, void *data)
Sets the error handling function of a SYNC consumer service.
Definition sync.c:371
This header file is part of the CANopen library; it contains the Transmit-PDO declarations.
int co_dam_mpdo_event(co_tpdo_t *pdo, co_unsigned8_t id, co_unsigned16_t idx, co_unsigned8_t subidx, const co_unsigned8_t data[4])
Triggers the transmission of a DAM-MPDO.
Definition tpdo.c:666
void co_tpdo_set_ind(co_tpdo_t *pdo, co_tpdo_ind_t *ind, void *data)
Sets the indication function invoked when a Transmit-PDO is sent or an error occurs.
Definition tpdo.c:479
co_unsigned16_t co_tpdo_get_num(const co_tpdo_t *pdo)
Returns the PDO number of a Transmit-PDO.
Definition tpdo.c:444
size_t io_tqueue_abort_wait(io_tqueue_t *tq, struct io_tqueue_wait *wait)
Aborts the specified timer queue wait operation if it is pending.
Definition tqueue.c:313
size_t io_tqueue_cancel_wait(io_tqueue_t *tq, struct io_tqueue_wait *wait)
Cancels the specified timer queue wait operation if it is pending.
Definition tqueue.c:294
void io_tqueue_submit_wait(io_tqueue_t *tq, struct io_tqueue_wait *wait)
Submits a wait operation to a timer queue.
Definition tqueue.c:236
This header file is part of the CANopen library; it contains the CANopen value declarations.
size_t co_val_write(co_unsigned16_t type, const void *val, uint_least8_t *begin, uint_least8_t *end)
Writes a value of the specified data type to a memory buffer.
Definition val.c:791