Lely core libraries 2.3.4
emcy.c
Go to the documentation of this file.
1
24#include "co.h"
25
26#if !LELY_NO_CO_EMCY
27
28#include <lely/can/buf.h>
29#include <lely/co/dev.h>
30#include <lely/co/emcy.h>
31#include <lely/co/obj.h>
32#include <lely/co/sdo.h>
33#include <lely/co/val.h>
34#include <lely/util/diag.h>
35#include <lely/util/endian.h>
36#include <lely/util/time.h>
37
38#include <assert.h>
39#include <stdlib.h>
40#if LELY_NO_MALLOC
41#include <string.h>
42#endif
43
44#if LELY_NO_MALLOC
45#ifndef CO_EMCY_CAN_BUF_SIZE
50#define CO_EMCY_CAN_BUF_SIZE 16
51#endif
52#ifndef CO_EMCY_MAX_NMSG
57#define CO_EMCY_MAX_NMSG 8
58#endif
59#endif // LELY_NO_MALLOC
60
64 co_unsigned16_t eec;
66 co_unsigned8_t er;
67};
68
72 co_unsigned8_t id;
75};
76
82static int co_emcy_node_recv(const struct can_msg *msg, void *data);
83
85struct __co_emcy {
97 size_t nmsg;
99#if LELY_NO_MALLOC
100 struct co_emcy_msg msgs[CO_EMCY_MAX_NMSG];
101#else
103#endif
105 struct can_buf buf;
106#if LELY_NO_MALLOC
111 struct can_msg begin[CO_EMCY_CAN_BUF_SIZE];
112#endif
116 struct timespec inhibit;
122 void *data;
123};
124
126struct co_1003 {
128 co_unsigned8_t n;
130 co_unsigned32_t ef[0xfe];
131};
132
139static int co_emcy_set_1003(co_emcy_t *emcy);
140
147static co_unsigned32_t co_1003_dn_ind(
148 co_sub_t *sub, struct co_sdo_req *req, void *data);
149
156static co_unsigned32_t co_1014_dn_ind(
157 co_sub_t *sub, struct co_sdo_req *req, void *data);
158
166static void co_emcy_set_1028(
167 co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid);
168
175static co_unsigned32_t co_1028_dn_ind(
176 co_sub_t *sub, struct co_sdo_req *req, void *data);
177
183static int co_emcy_timer(const struct timespec *tp, void *data);
184
195static int co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
196 const co_unsigned8_t msef[5]);
197
202static void co_emcy_flush(co_emcy_t *emcy);
203
204void *
205__co_emcy_alloc(void)
206{
207 void *ptr = malloc(sizeof(struct __co_emcy));
208#if !LELY_NO_ERRNO
209 if (!ptr)
210 set_errc(errno2c(errno));
211#endif
212 return ptr;
213}
214
215void
216__co_emcy_free(void *ptr)
217{
218 free(ptr);
219}
220
221struct __co_emcy *
222__co_emcy_init(struct __co_emcy *emcy, can_net_t *net, co_dev_t *dev)
223{
224 assert(emcy);
225 assert(net);
226 assert(dev);
227
228 int errc = 0;
229
230 emcy->net = net;
231 emcy->dev = dev;
232
233 emcy->stopped = 1;
234
235 emcy->sub_1001_00 = co_dev_find_sub(emcy->dev, 0x1001, 0x00);
236 if (!emcy->sub_1001_00) {
237 errc = errnum2c(ERRNUM_NOSYS);
238 goto error_sub_1001_00;
239 }
240 emcy->obj_1003 = co_dev_find_obj(emcy->dev, 0x1003);
241
242 emcy->nmsg = 0;
243#if LELY_NO_MALLOC
244 memset(emcy->msgs, 0, CO_EMCY_MAX_NMSG * sizeof(*emcy->msgs));
245#else
246 emcy->msgs = NULL;
247#endif
248
249 // Create a CAN frame buffer for pending EMCY messages that will be send
250 // once the inhibit time has elapsed.
251#if LELY_NO_MALLOC
252 can_buf_init(&emcy->buf, emcy->begin, CO_EMCY_CAN_BUF_SIZE);
253 memset(emcy->begin, 0, CO_EMCY_CAN_BUF_SIZE * sizeof(*emcy->begin));
254#else
255 can_buf_init(&emcy->buf, NULL, 0);
256#endif
257
258 emcy->timer = can_timer_create();
259 if (!emcy->timer) {
260 errc = get_errc();
261 goto error_create_timer;
262 }
264
265 emcy->inhibit = (struct timespec){ 0, 0 };
266
267 emcy->ind = NULL;
268 emcy->data = NULL;
269
270 for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
271 struct co_emcy_node *node = &emcy->nodes[id - 1];
272 node->id = id;
273 node->recv = NULL;
274 }
275
276 co_obj_t *obj_1028 = co_dev_find_obj(emcy->dev, 0x1028);
277 if (obj_1028) {
278 // Initialize the nodes.
279 for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
280 co_sub_t *sub = co_obj_find_sub(obj_1028, id);
281 if (!sub)
282 continue;
283 struct co_emcy_node *node = &emcy->nodes[id - 1];
284 node->recv = can_recv_create();
285 if (!node->recv) {
286 errc = get_errc();
287 goto error_create_recv;
288 }
290 }
291 }
292
293 if (co_emcy_start(emcy) == -1) {
294 errc = get_errc();
295 goto error_start;
296 }
297
298 return emcy;
299
300 // co_emcy_stop(emcy);
301error_start:
302error_create_recv:
303 for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++)
304 can_recv_destroy(emcy->nodes[id - 1].recv);
306error_create_timer:
307 can_buf_fini(&emcy->buf);
308error_sub_1001_00:
309 set_errc(errc);
310 return NULL;
311}
312
313void
314__co_emcy_fini(struct __co_emcy *emcy)
315{
316 assert(emcy);
317
318 co_emcy_stop(emcy);
319
320 for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++)
321 can_recv_destroy(emcy->nodes[id - 1].recv);
322
324
325 can_buf_fini(&emcy->buf);
326
327#if !LELY_NO_MALLOC
328 free(emcy->msgs);
329#endif
330}
331
332co_emcy_t *
334{
335 trace("creating EMCY producer service");
336
337 int errc = 0;
338
339 co_emcy_t *emcy = __co_emcy_alloc();
340 if (!emcy) {
341 errc = get_errc();
342 goto error_alloc_emcy;
343 }
344
345 if (!__co_emcy_init(emcy, net, dev)) {
346 errc = get_errc();
347 goto error_init_emcy;
348 }
349
350 return emcy;
351
352error_init_emcy:
353 __co_emcy_free(emcy);
354error_alloc_emcy:
355 set_errc(errc);
356 return NULL;
357}
358
359void
361{
362 if (emcy) {
363 trace("destroying EMCY producer service");
364 __co_emcy_fini(emcy);
365 __co_emcy_free(emcy);
366 }
367}
368
369int
371{
372 assert(emcy);
373
374 if (!emcy->stopped)
375 return 0;
376
377 can_net_get_time(emcy->net, &emcy->inhibit);
378
379 // Set the download indication function for the pre-defined error field.
380 if (emcy->obj_1003)
382
383 // Set the download indication function for the EMCY COB-ID object.
384 co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
385 if (obj_1014)
386 co_obj_set_dn_ind(obj_1014, &co_1014_dn_ind, emcy);
387
388 co_obj_t *obj_1028 = co_dev_find_obj(emcy->dev, 0x1028);
389 if (obj_1028) {
390 // Set the download indication function for the emergency
391 // consumer object.
392 co_obj_set_dn_ind(obj_1028, &co_1028_dn_ind, emcy);
393 // Start the CAN frame receivers for the nodes.
394 co_unsigned8_t maxid = MIN(co_obj_get_val_u8(obj_1028, 0x00),
396 for (co_unsigned8_t id = 1; id <= maxid; id++) {
397 co_sub_t *sub = co_obj_find_sub(obj_1028, id);
398 if (sub)
399 co_emcy_set_1028(emcy, id,
400 co_sub_get_val_u32(sub));
401 }
402 }
403
404 emcy->stopped = 0;
405
406 return 0;
407}
408
409void
411{
412 assert(emcy);
413
414 if (emcy->stopped)
415 return;
416
417 can_timer_stop(emcy->timer);
418
419 co_obj_t *obj_1028 = co_dev_find_obj(emcy->dev, 0x1028);
420 if (obj_1028) {
421 // Stop all CAN frame receivers.
422 for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++) {
423 co_sub_t *sub = co_obj_find_sub(obj_1028, id);
424 if (sub)
425 can_recv_stop(emcy->nodes[id - 1].recv);
426 }
427 // Remove the download indication function for the emergency
428 // consumer object.
429 co_obj_set_dn_ind(obj_1028, NULL, NULL);
430 }
431
432 // Remove the download indication function for the EMCY COB-ID object.
433 co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
434 if (obj_1014)
435 co_obj_set_dn_ind(obj_1014, NULL, NULL);
436
437 // Remove the download indication function for the pre-defined error
438 // field.
439 if (emcy->obj_1003)
440 co_obj_set_dn_ind(emcy->obj_1003, NULL, NULL);
441
442 emcy->stopped = 1;
443}
444
445int
447{
448 assert(emcy);
449
450 return emcy->stopped;
451}
452
453can_net_t *
455{
456 assert(emcy);
457
458 return emcy->net;
459}
460
461co_dev_t *
463{
464 assert(emcy);
465
466 return emcy->dev;
467}
468
469int
470co_emcy_push(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
471 const co_unsigned8_t msef[5])
472{
473 assert(emcy);
474
475 if (!eec) {
477 return -1;
478 }
479 // Bit 0 (generic error) shall be signaled at any error situation.
480 er |= 0x01;
481
482 if (msef)
483 diag(DIAG_INFO, 0, "EMCY: %04X %02X %u %u %u %u %u", eec, er,
484 msef[0], msef[1], msef[2], msef[3], msef[4]);
485 else
486 diag(DIAG_INFO, 0, "EMCY: %04X %02X", eec, er);
487
488#if LELY_NO_MALLOC
489 if (emcy->nmsg > CO_EMCY_MAX_NMSG - 1) {
491 return -1;
492 }
493#else
494 // Make room on the stack.
495 struct co_emcy_msg *msgs = realloc(emcy->msgs,
496 (emcy->nmsg + 1) * sizeof(struct co_emcy_msg));
497 if (!msgs) {
498#if !LELY_NO_ERRNO
499 set_errc(errno2c(errno));
500#endif
501 return -1;
502 }
503 emcy->msgs = msgs;
504#endif
505
506 // Move the older messages.
507 if (emcy->nmsg)
508 memmove(emcy->msgs + 1, emcy->msgs,
509 emcy->nmsg * sizeof(struct co_emcy_msg));
510 emcy->nmsg++;
511 // Push the error to the stack.
512 emcy->msgs[0].eec = eec;
513 emcy->msgs[0].er = er;
514
515 // Update the pre-defined error field.
516 if (emcy->obj_1003)
517 co_emcy_set_1003(emcy);
518
519 // Obtain the new (combined) error register.
520 co_emcy_peek(emcy, NULL, &er);
521
522 return co_emcy_send(emcy, eec, er, msef);
523}
524
525int
526co_emcy_pop(co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
527{
528 assert(emcy);
529
530 co_emcy_peek(emcy, peec, per);
531
532 return co_emcy_remove(emcy, 0);
533}
534
535void
536co_emcy_peek(const co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
537{
538 assert(emcy);
539
540 if (peec)
541 *peec = emcy->nmsg ? emcy->msgs[0].eec : 0;
542 if (per) {
543 co_unsigned8_t er = 0;
544 // Compute the combined error register.
545 for (size_t i = 0; i < emcy->nmsg; i++)
546 er |= emcy->msgs[i].er;
547 *per = er;
548 }
549}
550int
551co_emcy_remove(co_emcy_t *emcy, size_t n)
552{
553 assert(emcy);
554
555 if (n >= emcy->nmsg)
556 return 0;
557
558 // Move the older messages.
559 if (--emcy->nmsg)
560 memmove(emcy->msgs + n, emcy->msgs + n + 1,
561 (emcy->nmsg - n) * sizeof(struct co_emcy_msg));
562
563 // Update the pre-defined error field.
564 if (emcy->obj_1003)
565 co_emcy_set_1003(emcy);
566
567 if (emcy->nmsg) {
568 // Store the most recent error in the error register and the
569 // manufacturer-specific field.
570 co_unsigned16_t eec = 0;
571 co_unsigned8_t er = 0;
572 co_emcy_peek(emcy, &eec, &er);
573 co_unsigned8_t msef[5] = { 0 };
574 stle_u16(msef, eec);
575 return co_emcy_send(emcy, 0, er, msef);
576 } else {
577 return co_emcy_send(emcy, 0, 0, NULL);
578 }
579}
580
581ssize_t
582co_emcy_find(const co_emcy_t *emcy, co_unsigned16_t eec)
583{
584 assert(emcy);
585
586 for (size_t i = 0; i < emcy->nmsg; i++) {
587 if (emcy->msgs[i].eec == eec)
588 return i;
589 }
590
591 return -1;
592}
593
594int
596{
597 assert(emcy);
598
599 if (!emcy->nmsg)
600 return 0;
601
602 // Clear the stack.
603 emcy->nmsg = 0;
604
605 // Clear the pre-defined error field.
606 if (emcy->obj_1003)
607 co_emcy_set_1003(emcy);
608
609 // Send the 'error reset/no error' message.
610 return co_emcy_send(emcy, 0, 0, NULL);
611}
612
613void
614co_emcy_get_ind(const co_emcy_t *emcy, co_emcy_ind_t **pind, void **pdata)
615{
616 assert(emcy);
617
618 if (pind)
619 *pind = emcy->ind;
620 if (pdata)
621 *pdata = emcy->data;
622}
623
624void
626{
627 assert(emcy);
628
629 emcy->ind = ind;
630 emcy->data = data;
631}
632
633static int
634co_emcy_node_recv(const struct can_msg *msg, void *data)
635{
636 assert(msg);
637 struct co_emcy_node *node = data;
638 assert(node);
639 assert(node->id > 0 && node->id <= CO_NUM_NODES);
640 co_emcy_t *emcy = structof(node - (node->id - 1), co_emcy_t, nodes[0]);
641 assert(emcy);
642
643 // Ignore remote frames.
644 if (msg->flags & CAN_FLAG_RTR)
645 return 0;
646
647#if !LELY_NO_CANFD
648 // Ignore CAN FD format frames.
649 if (msg->flags & CAN_FLAG_EDL)
650 return 0;
651#endif
652
653 // Extract the parameters from the frame.
654 co_unsigned16_t eec = 0;
655 if (msg->len >= 2)
656 eec = ldle_u16(msg->data);
657 co_unsigned8_t er = msg->len >= 3 ? msg->data[2] : 0;
658 co_unsigned8_t msef[5] = { 0 };
659 if (msg->len >= 4)
660 memcpy(msef, msg->data + 3,
661 MAX((uint_least8_t)(msg->len - 3), 5));
662
663 // Notify the user.
664 trace("EMCY: received %04X %02X", eec, er);
665 if (emcy->ind)
666 emcy->ind(emcy, node->id, eec, er, msef, emcy->data);
667
668 return 0;
669}
670
671static int
673{
674 assert(emcy);
675 assert(emcy->obj_1003);
676
677 struct co_1003 *val_1003 = co_obj_addressof_val(emcy->obj_1003);
678 co_unsigned8_t nsubidx = co_obj_get_subidx(emcy->obj_1003, 0, NULL);
679 if (!nsubidx)
680 return 0;
681
682 // Copy the emergency error codes.
683 val_1003->n = MIN((co_unsigned8_t)emcy->nmsg, nsubidx - 1);
684 for (int i = 0; i < val_1003->n; i++)
685 val_1003->ef[i] = emcy->msgs[i].eec;
686 for (int i = val_1003->n; i < nsubidx - 1; i++)
687 val_1003->ef[i] = 0;
688
689 return 0;
690}
691
692static co_unsigned32_t
693co_1003_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
694{
695 assert(sub);
696 assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1003);
697 assert(req);
698 co_emcy_t *emcy = data;
699 assert(emcy);
700
701 co_unsigned16_t type = co_sub_get_type(sub);
702 assert(!co_type_is_array(type));
703
704 union co_val val;
705 co_unsigned32_t ac = 0;
706 if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
707 return ac;
708
709 if (co_sub_get_subidx(sub))
710 return CO_SDO_AC_NO_WRITE;
711
712 // Only the value 0 is allowed.
713 assert(type == CO_DEFTYPE_UNSIGNED8);
714 if (val.u8)
715 return CO_SDO_AC_PARAM_VAL;
716
717 emcy->nmsg = 0;
718
719 co_sub_dn(sub, &val);
720
721 co_emcy_set_1003(emcy);
722 return 0;
723}
724
725static co_unsigned32_t
726co_1014_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
727{
728 assert(sub);
729 assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1014);
730 assert(req);
731 (void)data;
732
733 co_unsigned16_t type = co_sub_get_type(sub);
734 assert(!co_type_is_array(type));
735
736 union co_val val;
737 co_unsigned32_t ac = 0;
738 if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
739 return ac;
740
741 if (co_sub_get_subidx(sub))
742 return CO_SDO_AC_NO_SUB;
743
744 assert(type == CO_DEFTYPE_UNSIGNED32);
745 co_unsigned32_t cobid = val.u32;
746 co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
747 if (cobid == cobid_old)
748 return 0;
749
750 // The CAN-ID cannot be changed when the EMCY is and remains valid.
751 int valid = !(cobid & CO_EMCY_COBID_VALID);
752 int valid_old = !(cobid_old & CO_EMCY_COBID_VALID);
753 uint_least32_t canid = cobid & CAN_MASK_EID;
754 uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
755 if (valid && valid_old && canid != canid_old)
756 return CO_SDO_AC_PARAM_VAL;
757
758 // A 29-bit CAN-ID is only valid if the frame bit is set.
759 if (!(cobid & CO_EMCY_COBID_FRAME)
760 && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))
761 return CO_SDO_AC_PARAM_VAL;
762
763 co_sub_dn(sub, &val);
764
765 return 0;
766}
767
768static void
769co_emcy_set_1028(co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid)
770{
771 assert(emcy);
772 assert(id && id <= CO_NUM_NODES);
773 struct co_emcy_node *node = &emcy->nodes[id - 1];
774
775 if (!(cobid & CO_EMCY_COBID_VALID)) {
776 // Register the receiver under the specified CAN-ID.
777 uint_least32_t id = cobid;
778 uint_least8_t flags = 0;
779 if (id & CO_EMCY_COBID_FRAME) {
780 id &= CAN_MASK_EID;
781 flags |= CAN_FLAG_IDE;
782 } else {
783 id &= CAN_MASK_BID;
784 }
785 can_recv_start(node->recv, emcy->net, id, flags);
786 } else {
787 // Stop the receiver unless the EMCY COB-ID is valid.
788 can_recv_stop(node->recv);
789 }
790}
791
792static co_unsigned32_t
793co_1028_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
794{
795 assert(sub);
796 assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1028);
797 assert(req);
798 co_emcy_t *emcy = data;
799 assert(emcy);
800
801 co_unsigned16_t type = co_sub_get_type(sub);
802 assert(!co_type_is_array(type));
803
804 union co_val val;
805 co_unsigned32_t ac = 0;
806 if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
807 return ac;
808
809 co_unsigned8_t id = co_sub_get_subidx(sub);
810 if (!id)
811 return CO_SDO_AC_NO_WRITE;
812 co_unsigned8_t maxid = MIN(co_obj_get_val_u8(co_sub_get_obj(sub), 0),
814 if (id > maxid)
815 return CO_SDO_AC_NO_SUB;
816
817 assert(type == CO_DEFTYPE_UNSIGNED32);
818 co_unsigned32_t cobid = val.u32;
819 co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
820 if (cobid == cobid_old)
821 return 0;
822
823 // The CAN-ID cannot be changed when the EMCY is and remains valid.
824 int valid = !(cobid & CO_EMCY_COBID_VALID);
825 int valid_old = !(cobid_old & CO_EMCY_COBID_VALID);
826 uint_least32_t canid = cobid & CAN_MASK_EID;
827 uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
828 if (valid && valid_old && canid != canid_old)
829 return CO_SDO_AC_PARAM_VAL;
830
831 // A 29-bit CAN-ID is only valid if the frame bit is set.
832 if (!(cobid & CO_EMCY_COBID_FRAME)
833 && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))
834 return CO_SDO_AC_PARAM_VAL;
835
836 co_emcy_set_1028(emcy, id, cobid);
837
838 co_sub_dn(sub, &val);
839
840 return 0;
841}
842
843static int
844co_emcy_timer(const struct timespec *tp, void *data)
845{
846 (void)tp;
847 co_emcy_t *emcy = data;
848 assert(emcy);
849
850 co_emcy_flush(emcy);
851
852 return 0;
853}
854
855static int
856co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
857 const co_unsigned8_t msef[5])
858{
859 assert(emcy);
860
861 // Update the error register in the object dictionary.
862 co_sub_set_val_u8(emcy->sub_1001_00, er);
863
864 // Check whether the EMCY COB-ID exists and is valid.
865 co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
866 if (!obj_1014)
867 return 0;
868 co_unsigned32_t cobid = co_obj_get_val_u32(obj_1014, 0x00);
869 if (cobid & CO_EMCY_COBID_VALID)
870 return 0;
871
872 // Create the frame.
873 struct can_msg msg = CAN_MSG_INIT;
874 msg.id = cobid;
875 if (cobid & CO_EMCY_COBID_FRAME) {
876 msg.id &= CAN_MASK_EID;
877 msg.flags |= CAN_FLAG_IDE;
878 } else {
879 msg.id &= CAN_MASK_BID;
880 }
881 msg.len = CAN_MAX_LEN;
882 stle_u32(msg.data, eec);
883 msg.data[2] = er;
884 if (msef) {
885 memcpy(msg.data + 3, msef, 5);
886 }
887
888 // Add the frame to the buffer.
889 if (!can_buf_write(&emcy->buf, &msg, 1)) {
890 if (!can_buf_reserve(&emcy->buf, 1))
891 return -1;
892 can_buf_write(&emcy->buf, &msg, 1);
893 }
894
895 co_emcy_flush(emcy);
896 return 0;
897}
898
899static void
901{
902 assert(emcy);
903
904 co_unsigned16_t inhibit = co_dev_get_val_u16(emcy->dev, 0x1015, 0x00);
905
906 struct timespec now = { 0, 0 };
907 can_net_get_time(emcy->net, &now);
908
909 while (can_buf_size(&emcy->buf)) {
910 if (timespec_cmp(&now, &emcy->inhibit) < 0) {
911 can_timer_start(emcy->timer, emcy->net, &emcy->inhibit,
912 NULL);
913 return;
914 }
915 // Update the inhibit time.
916 emcy->inhibit = now;
917 timespec_add_usec(&emcy->inhibit, inhibit * 100);
918 // Send the frame.
919 struct can_msg msg;
920 if (can_buf_read(&emcy->buf, &msg, 1))
921 can_net_send(emcy->net, &msg);
922 }
923}
924
925#endif // !LELY_NO_CO_EMCY
This header file is part of the CAN library; it contains the CAN frame buffer declarations.
LELY_CAN_BUF_INLINE void can_buf_init(struct can_buf *buf, struct can_msg *ptr, size_t size)
Initializes a CAN frame buffer.
Definition: buf.h:165
LELY_CAN_BUF_INLINE size_t can_buf_write(struct can_buf *buf, const struct can_msg *ptr, size_t n)
Writes frames to a CAN frame buffer.
Definition: buf.h:246
size_t can_buf_reserve(struct can_buf *buf, size_t n)
Resizes a CAN frame buffer, if necessary, to make room for at least n additional frames.
Definition: buf.c:53
LELY_CAN_BUF_INLINE size_t can_buf_size(const struct can_buf *buf)
Returns the number of frames available for reading in a CAN buffer.
Definition: buf.h:186
LELY_CAN_BUF_INLINE size_t can_buf_read(struct can_buf *buf, struct can_msg *ptr, size_t n)
Reads, and removes, frames from a CAN frame buffer.
Definition: buf.h:224
void can_buf_fini(struct can_buf *buf)
Finalizes a CAN frame buffer.
Definition: buf.c:41
@ CAN_FLAG_IDE
The Identifier Extension (IDE) flag.
Definition: msg.h:43
@ CAN_FLAG_RTR
The Remote Transmission Request (RTR) flag (unavailable in CAN FD format frames).
Definition: msg.h:48
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
Definition: msg.h:34
#define CAN_MAX_LEN
The maximum number of bytes in the payload of a CAN format frame.
Definition: msg.h:72
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition: msg.h:31
#define CAN_MSG_INIT
The static initializer for can_msg.
Definition: msg.h:113
This header file is part of the CANopen library; it contains the device description declarations.
co_obj_t * co_dev_find_obj(const co_dev_t *dev, co_unsigned16_t idx)
Finds an object in the object dictionary of a CANopen device.
Definition: dev.c:279
co_sub_t * co_dev_find_sub(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Finds a sub-object in the object dictionary of a CANopen device.
Definition: dev.c:290
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
Definition: dev.h:56
This header file is part of the utilities library; it contains the diagnostic declarations.
@ DIAG_INFO
An informational message.
Definition: diag.h:53
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:171
void co_emcy_destroy(co_emcy_t *emcy)
Destroys a CANopen EMCY producer/consumer service.
Definition: emcy.c:360
static int co_emcy_timer(const struct timespec *tp, void *data)
The CAN timer callback function for an EMCY service.
Definition: emcy.c:844
void co_emcy_get_ind(const co_emcy_t *emcy, co_emcy_ind_t **pind, void **pdata)
Retrieves the indication function invoked when a CANopen EMCY message is received.
Definition: emcy.c:614
int co_emcy_is_stopped(const co_emcy_t *emcy)
Retuns 1 if the specified EMCY service is stopped, and 0 if not.
Definition: emcy.c:446
int co_emcy_remove(co_emcy_t *emcy, size_t n)
Pops a CANopen EMCY message from the stack, even if it is not the most recent message,...
Definition: emcy.c:551
int co_emcy_push(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er, const co_unsigned8_t msef[5])
Pushes a CANopen EMCY message to the stack and broadcasts it if the EMCY producer service is active.
Definition: emcy.c:470
co_emcy_t * co_emcy_create(can_net_t *net, co_dev_t *dev)
Creates a new CANopen EMCY producer/consumer service.
Definition: emcy.c:333
static int co_emcy_node_recv(const struct can_msg *msg, void *data)
The CAN receive callback function for a remote CANopen EMCY producer node.
Definition: emcy.c:634
static co_unsigned32_t co_1003_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
The download indication function for (all sub-objects of) CANopen object 1003 (Pre-defined error fiel...
Definition: emcy.c:693
int co_emcy_start(co_emcy_t *emcy)
Starts an EMCY service.
Definition: emcy.c:370
int co_emcy_clear(co_emcy_t *emcy)
Clears the CANopen EMCY message stack and broadcasts the 'error reset/no error' message if the EMCY p...
Definition: emcy.c:595
can_net_t * co_emcy_get_net(const co_emcy_t *emcy)
Returns a pointer to the CAN network of an EMCY producer/consumer service.
Definition: emcy.c:454
int co_emcy_pop(co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
Pops the most recent CANopen EMCY message from the stack and broadcasts an 'error reset' message if t...
Definition: emcy.c:526
void co_emcy_peek(const co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
Retrieves, but does not pop, the most recent CANopen EMCY message from the stack.
Definition: emcy.c:536
static co_unsigned32_t co_1028_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
The download indication function for (all sub-objects of) CANopen object 1028 (Emergency consumer obj...
Definition: emcy.c:793
static int co_emcy_set_1003(co_emcy_t *emcy)
Sets the value of CANopen object 1003 (Pre-defined error field).
Definition: emcy.c:672
ssize_t co_emcy_find(const co_emcy_t *emcy, co_unsigned16_t eec)
Finds a CANopen EMCY message in the stack.
Definition: emcy.c:582
static int co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er, const co_unsigned8_t msef[5])
Adds an EMCY message to the CAN frame buffer and sends it if possible.
Definition: emcy.c:856
static co_unsigned32_t co_1014_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
The download indication function for (all sub-objects of) CANopen object 1014 (COB-ID emergency messa...
Definition: emcy.c:726
co_dev_t * co_emcy_get_dev(const co_emcy_t *emcy)
Returns a pointer to the CANopen device of an EMCY producer/consumer service.
Definition: emcy.c:462
static void co_emcy_set_1028(co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid)
Sets the value of CANopen object 1028 (Emergency consumer object).
Definition: emcy.c:769
void co_emcy_stop(co_emcy_t *emcy)
Stops an EMCY service.
Definition: emcy.c:410
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
static void co_emcy_flush(co_emcy_t *emcy)
Sends any messages in the CAN frame buffer unless the inhibit time has not yet elapsed,...
Definition: emcy.c:900
This header file is part of the CANopen library; it contains the emergency (EMCY) object declarations...
#define CO_EMCY_COBID_VALID
The bit in the EMCY COB-ID specifying whether the EMCY exists and is valid.
Definition: emcy.h:29
#define CO_EMCY_COBID_FRAME
The bit in the EMCY COB-ID specifying whether to use an 11-bit (0) or 29-bit (1) CAN-ID.
Definition: emcy.h:35
void co_emcy_ind_t(co_emcy_t *emcy, co_unsigned8_t id, co_unsigned16_t eec, co_unsigned8_t er, co_unsigned8_t msef[5], void *data)
The type of a CANopen EMCY indication function, invoked when an EMCY message is received.
Definition: emcy.h:52
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
void stle_u16(uint_least8_t dst[2], uint_least16_t x)
Stores a 16-bit unsigned integer in little-endian byte order.
Definition: endian.h:504
void stle_u32(uint_least8_t dst[4], uint_least32_t x)
Stores a 32-bit unsigned integer in little-endian byte order.
Definition: endian.h:582
int errnum2c(errnum_t errnum)
Transforms a platform-independent error number to a native error code.
Definition: errnum.c:810
@ ERRNUM_NOSYS
Function not supported.
Definition: errnum.h:184
@ ERRNUM_NOMEM
Not enough space.
Definition: errnum.h:172
@ ERRNUM_INVAL
Invalid argument.
Definition: errnum.h:132
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
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:46
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition: errnum.h:424
This header file is part of the CANopen library; it contains the Service Data Object (SDO) declaratio...
int co_sdo_req_dn_val(struct co_sdo_req *req, co_unsigned16_t type, void *val, co_unsigned32_t *pac)
Copies the next segment of the specified CANopen SDO download request to the internal buffer and,...
Definition: sdo.c:170
#define CO_SDO_AC_NO_SUB
SDO abort code: Sub-index does not exist.
Definition: sdo.h:132
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
Definition: sdo.h:135
#define CO_SDO_AC_NO_WRITE
SDO abort code: Attempt to write a read only object.
Definition: sdo.h:90
#define structof(ptr, type, member)
Obtains the address of a structure from the address of one of its members.
Definition: util.h:93
#define MIN(a, b)
Returns the minimum of a and b.
Definition: util.h:57
#define MAX(a, b)
Returns the maximum of a and b.
Definition: util.h:65
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:462
int can_net_send(can_net_t *net, const struct can_msg *msg)
Sends a CAN frame from a network interface.
Definition: net.c:300
void can_timer_start(can_timer_t *timer, can_net_t *net, const struct timespec *start, const struct timespec *interval)
Starts a CAN timer and registers it with a network interface.
Definition: net.c:431
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:376
void can_net_get_time(const can_net_t *net, struct timespec *tp)
Retrieves the current time of a CAN network interface.
Definition: net.c:196
void can_timer_set_func(can_timer_t *timer, can_timer_func_t *func, void *data)
Sets the callback function invoked when a CAN timer is triggered.
Definition: net.c:422
void can_recv_stop(can_recv_t *recv)
Stops a CAN frame receiver from processing frames and unregisters it with the network interface.
Definition: net.c:609
void can_recv_set_func(can_recv_t *recv, can_recv_func_t *func, void *data)
Sets the callback function used to process CAN frames with a receiver.
Definition: net.c:578
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:558
void can_recv_start(can_recv_t *recv, can_net_t *net, uint_least32_t id, uint_least8_t flags)
Registers a CAN frame receiver with a network interface and starts processing frames.
Definition: net.c:587
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:533
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:401
This header file is part of the CANopen library; it contains the object dictionary declarations.
co_unsigned8_t co_obj_get_subidx(const co_obj_t *obj, co_unsigned8_t maxsubidx, co_unsigned8_t *subidx)
Retrieves a list of sub-indices in a CANopen object.
Definition: obj.c:172
co_unsigned8_t co_sub_get_subidx(const co_sub_t *sub)
Returns the sub-index of a CANopen sub-object.
Definition: obj.c:559
void * co_obj_addressof_val(const co_obj_t *obj)
Returns the address of the value of a CANopen object.
Definition: obj.c:328
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:164
co_sub_t * co_obj_find_sub(const co_obj_t *obj, co_unsigned8_t subidx)
Finds a sub-object in a CANopen object.
Definition: obj.c:240
int co_sub_dn(co_sub_t *sub, void *val)
Downloads (moves) a value into a CANopen sub-object if the refuse-write-on-download flag (CO_OBJ_FLAG...
Definition: obj.c:996
void co_obj_set_dn_ind(co_obj_t *obj, co_sub_dn_ind_t *ind, void *data)
Sets the download indication function for a CANopen object.
Definition: obj.c:389
co_obj_t * co_sub_get_obj(const co_sub_t *sub)
Returns the a pointer to the CANopen object containing the specified sub-object.
Definition: obj.c:551
co_unsigned16_t co_sub_get_type(const co_sub_t *sub)
Returns the data type of a CANopen sub-object.
Definition: obj.c:603
This is the internal header file of the CANopen library.
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib....
This header file is part of the C11 and POSIX compatibility library; it includes <string....
A CAN network interface.
Definition: net.c:37
A CAN frame receiver.
Definition: net.c:86
A CAN timer.
Definition: net.c:63
A CANopen device.
Definition: dev.h:30
A CANopen EMCY producer/consumer service.
Definition: emcy.c:85
struct can_buf buf
The CAN frame buffer.
Definition: emcy.c:105
co_obj_t * obj_1003
A pointer to the pre-defined error field object.
Definition: emcy.c:95
co_dev_t * dev
A pointer to a CANopen device.
Definition: emcy.c:89
void * data
A pointer to user-specified data for ind.
Definition: emcy.c:122
can_timer_t * timer
A pointer to the CAN timer.
Definition: emcy.c:114
int stopped
A flag specifying whether the EMCY service is stopped.
Definition: emcy.c:91
co_sub_t * sub_1001_00
A pointer to the error register object.
Definition: emcy.c:93
size_t nmsg
The number of messages in msgs.
Definition: emcy.c:97
struct co_emcy_node nodes[CO_NUM_NODES]
An array of pointers to remote nodes.
Definition: emcy.c:118
struct timespec inhibit
The time at which the next EMCY message may be sent.
Definition: emcy.c:116
co_emcy_ind_t * ind
A pointer to the indication function.
Definition: emcy.c:120
can_net_t * net
A pointer to a CAN network interface.
Definition: emcy.c:87
struct co_emcy_msg * msgs
An array of EMCY messages. The first element is the most recent.
Definition: emcy.c:102
A CANopen object.
Definition: obj.h:31
A CANopen sub-object.
Definition: obj.h:53
A CAN frame buffer.
Definition: buf.h:42
A CAN or CAN FD format frame.
Definition: msg.h:87
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition: msg.h:102
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
Definition: msg.h:89
uint_least8_t flags
The flags (any combination of CAN_FLAG_IDE, CAN_FLAG_RTR, CAN_FLAG_FDF, CAN_FLAG_BRS and CAN_FLAG_ESI...
Definition: msg.h:94
uint_least8_t len
The number of bytes in data (or the requested number of bytes in case of a remote frame).
Definition: msg.h:100
The pre-defined error field.
Definition: emcy.c:126
co_unsigned32_t ef[0xfe]
An array of standard error fields.
Definition: emcy.c:130
co_unsigned8_t n
Number of errors.
Definition: emcy.c:128
An EMCY message.
Definition: emcy.c:62
co_unsigned8_t er
The error register.
Definition: emcy.c:66
co_unsigned16_t eec
The emergency error code.
Definition: emcy.c:64
A remote CANopen EMCY producer node.
Definition: emcy.c:70
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: emcy.c:74
co_unsigned8_t id
The node-ID.
Definition: emcy.c:72
A CANopen SDO upload/download request.
Definition: sdo.h:181
int co_type_is_array(co_unsigned16_t type)
Returns 1 if the specified (static) data type is an array, and 0 if not.
Definition: type.c:40
#define CO_DEFTYPE_UNSIGNED8
The data type (and object index) of an 8-bit unsigned integer.
Definition: type.h:44
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
Definition: type.h:50
A union of the CANopen static data types.
Definition: val.h:273
This header file is part of the utilities library; it contains the time function declarations.
int timespec_cmp(const void *p1, const void *p2)
Compares two times.
Definition: time.h:251
void timespec_add_usec(struct timespec *tp, uint_least64_t usec)
Adds usec microseconds to the time at tp.
Definition: time.h:149
This header file is part of the CANopen library; it contains the CANopen value declarations.