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
46 
50 #define CO_EMCY_CAN_BUF_SIZE 16
51 #endif
52 #ifndef CO_EMCY_MAX_NMSG
53 
57 #define CO_EMCY_MAX_NMSG 8
58 #endif
59 #endif // LELY_NO_MALLOC
60 
62 struct co_emcy_msg {
64  co_unsigned16_t eec;
66  co_unsigned8_t er;
67 };
68 
70 struct co_emcy_node {
72  co_unsigned8_t id;
75 };
76 
82 static int co_emcy_node_recv(const struct can_msg *msg, void *data);
83 
85 struct __co_emcy {
91  int stopped;
97  size_t nmsg;
99 #if LELY_NO_MALLOC
100  struct co_emcy_msg msgs[CO_EMCY_MAX_NMSG];
101 #else
102  struct co_emcy_msg *msgs;
103 #endif
104  struct can_buf buf;
106 #if LELY_NO_MALLOC
107 
111  struct can_msg begin[CO_EMCY_CAN_BUF_SIZE];
112 #endif
116  struct timespec inhibit;
122  void *data;
123 };
124 
126 struct co_1003 {
128  co_unsigned8_t n;
130  co_unsigned32_t ef[0xfe];
131 };
132 
139 static int co_emcy_set_1003(co_emcy_t *emcy);
140 
147 static co_unsigned32_t co_1003_dn_ind(
148  co_sub_t *sub, struct co_sdo_req *req, void *data);
149 
156 static co_unsigned32_t co_1014_dn_ind(
157  co_sub_t *sub, struct co_sdo_req *req, void *data);
158 
166 static void co_emcy_set_1028(
167  co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid);
168 
175 static co_unsigned32_t co_1028_dn_ind(
176  co_sub_t *sub, struct co_sdo_req *req, void *data);
177 
183 static int co_emcy_timer(const struct timespec *tp, void *data);
184 
195 static int co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
196  const co_unsigned8_t msef[5]);
197 
202 static void co_emcy_flush(co_emcy_t *emcy);
203 
204 void *
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 
215 void
216 __co_emcy_free(void *ptr)
217 {
218  free(ptr);
219 }
220 
221 struct __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  }
263  can_timer_set_func(emcy->timer, &co_emcy_timer, emcy);
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);
301 error_start:
302 error_create_recv:
303  for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++)
304  can_recv_destroy(emcy->nodes[id - 1].recv);
305  can_timer_destroy(emcy->timer);
306 error_create_timer:
307  can_buf_fini(&emcy->buf);
308 error_sub_1001_00:
309  set_errc(errc);
310  return NULL;
311 }
312 
313 void
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 
323  can_timer_destroy(emcy->timer);
324 
325  can_buf_fini(&emcy->buf);
326 
327 #if !LELY_NO_MALLOC
328  free(emcy->msgs);
329 #endif
330 }
331 
332 co_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 
352 error_init_emcy:
353  __co_emcy_free(emcy);
354 error_alloc_emcy:
355  set_errc(errc);
356  return NULL;
357 }
358 
359 void
361 {
362  if (emcy) {
363  trace("destroying EMCY producer service");
364  __co_emcy_fini(emcy);
365  __co_emcy_free(emcy);
366  }
367 }
368 
369 int
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),
395  CO_NUM_NODES);
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 
409 void
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 
445 int
447 {
448  assert(emcy);
449 
450  return emcy->stopped;
451 }
452 
453 can_net_t *
455 {
456  assert(emcy);
457 
458  return emcy->net;
459 }
460 
461 co_dev_t *
463 {
464  assert(emcy);
465 
466  return emcy->dev;
467 }
468 
469 int
470 co_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 
525 int
526 co_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 
535 void
536 co_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 }
550 int
551 co_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 
581 ssize_t
582 co_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 
594 int
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 
613 void
614 co_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 
624 void
625 co_emcy_set_ind(co_emcy_t *emcy, co_emcy_ind_t *ind, void *data)
626 {
627  assert(emcy);
628 
629  emcy->ind = ind;
630  emcy->data = data;
631 }
632 
633 static int
634 co_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 
671 static 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 
692 static co_unsigned32_t
693 co_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 
725 static co_unsigned32_t
726 co_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 
768 static void
769 co_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 
792 static co_unsigned32_t
793 co_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),
813  CO_NUM_NODES);
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 
843 static int
844 co_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 
855 static int
856 co_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 
899 static 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
emcy.h
__co_emcy::nmsg
size_t nmsg
The number of messages in msgs.
Definition: emcy.c:97
can_recv_destroy
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:558
__co_emcy::sub_1001_00
co_sub_t * sub_1001_00
A pointer to the error register object.
Definition: emcy.c:93
co_sub_dn
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
can_msg::flags
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
co_emcy_flush
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
__co_emcy::stopped
int stopped
A flag specifying whether the EMCY service is stopped.
Definition: emcy.c:91
co_emcy_stop
void co_emcy_stop(co_emcy_t *emcy)
Stops an EMCY service.
Definition: emcy.c:410
can_buf_read
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
can_buf
A CAN frame buffer.
Definition: buf.h:42
co_emcy_node::id
co_unsigned8_t id
The node-ID.
Definition: emcy.c:72
can_msg::data
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition: msg.h:102
co_emcy_clear
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_MASK_EID
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
Definition: msg.h:34
dev.h
__co_emcy::net
can_net_t * net
A pointer to a CAN network interface.
Definition: emcy.c:87
can_net_get_time
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
val.h
CO_DEFTYPE_UNSIGNED32
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
Definition: type.h:50
CAN_MASK_BID
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition: msg.h:31
can_buf_reserve
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
string.h
DIAG_INFO
@ DIAG_INFO
An informational message.
Definition: diag.h:53
can_msg
A CAN or CAN FD format frame.
Definition: msg.h:87
CO_NUM_NODES
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
Definition: dev.h:56
__co_emcy::obj_1003
co_obj_t * obj_1003
A pointer to the pre-defined error field object.
Definition: emcy.c:95
can_msg::len
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
diag.h
co_1014_dn_ind
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_1028_dn_ind
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
can_timer_set_func
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
MIN
#define MIN(a, b)
Returns the minimum of a and b.
Definition: util.h:57
can_timer_stop
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:462
co_emcy_set_1028
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
co_emcy_find
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
__can_recv
A CAN frame receiver.
Definition: net.c:86
__co_emcy::nodes
struct co_emcy_node nodes[CO_NUM_NODES]
An array of pointers to remote nodes.
Definition: emcy.c:118
CO_EMCY_COBID_FRAME
#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
can_recv_set_func
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
get_errc
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition: errnum.c:932
__co_emcy::dev
co_dev_t * dev
A pointer to a CANopen device.
Definition: emcy.c:89
timespec_cmp
int timespec_cmp(const void *p1, const void *p2)
Compares two times.
Definition: time.h:251
co_emcy_get_ind
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
ERRNUM_NOMEM
@ ERRNUM_NOMEM
Not enough space.
Definition: errnum.h:172
errno2c
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:46
CO_EMCY_COBID_VALID
#define CO_EMCY_COBID_VALID
The bit in the EMCY COB-ID specifying whether the EMCY exists and is valid.
Definition: emcy.h:29
CO_DEFTYPE_UNSIGNED8
#define CO_DEFTYPE_UNSIGNED8
The data type (and object index) of an 8-bit unsigned integer.
Definition: type.h:44
co.h
co_obj_addressof_val
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_emcy::data
void * data
A pointer to user-specified data for ind.
Definition: emcy.c:122
sdo.h
can_timer_start
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
__co_emcy::msgs
struct co_emcy_msg * msgs
An array of EMCY messages. The first element is the most recent.
Definition: emcy.c:102
CAN_MSG_INIT
#define CAN_MSG_INIT
The static initializer for can_msg.
Definition: msg.h:113
__co_emcy::buf
struct can_buf buf
The CAN frame buffer.
Definition: emcy.c:105
co_emcy_ind_t
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
co_emcy_is_stopped
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
co_obj_set_dn_ind
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
buf.h
CAN_MAX_LEN
#define CAN_MAX_LEN
The maximum number of bytes in the payload of a CAN format frame.
Definition: msg.h:72
__can_timer
A CAN timer.
Definition: net.c:63
co_obj_get_subidx
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
set_errnum
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition: errnum.h:424
set_errc
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:944
co_emcy_pop
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
can_timer_destroy
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:401
co_emcy_timer
static int co_emcy_timer(const struct timespec *tp, void *data)
The CAN timer callback function for an EMCY service.
Definition: emcy.c:844
co_sub_get_obj
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
MAX
#define MAX(a, b)
Returns the maximum of a and b.
Definition: util.h:65
CO_SDO_AC_NO_SUB
#define CO_SDO_AC_NO_SUB
SDO abort code: Sub-index does not exist.
Definition: sdo.h:132
co_emcy_send
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
co_emcy_node_recv
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
errnum2c
int errnum2c(errnum_t errnum)
Transforms a platform-independent error number to a native error code.
Definition: errnum.c:810
co_sdo_req
A CANopen SDO upload/download request.
Definition: sdo.h:181
co_emcy_msg::er
co_unsigned8_t er
The error register.
Definition: emcy.c:66
co_emcy_set_1003
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
__co_emcy
A CANopen EMCY producer/consumer service.
Definition: emcy.c:85
co_1003::n
co_unsigned8_t n
Number of errors.
Definition: emcy.c:128
co_emcy_peek
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
co_obj_find_sub
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
co_1003_dn_ind
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
ERRNUM_INVAL
@ ERRNUM_INVAL
Invalid argument.
Definition: errnum.h:132
can_buf_init
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
co_emcy_node
A remote CANopen EMCY producer node.
Definition: emcy.c:70
__co_emcy::inhibit
struct timespec inhibit
The time at which the next EMCY message may be sent.
Definition: emcy.c:116
co_emcy_get_net
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
co_emcy_set_ind
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
CO_SDO_AC_NO_WRITE
#define CO_SDO_AC_NO_WRITE
SDO abort code: Attempt to write a read only object.
Definition: sdo.h:90
stle_u32
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
co_emcy_get_dev
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
can_buf_write
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
time.h
co_1003::ef
co_unsigned32_t ef[0xfe]
An array of standard error fields.
Definition: emcy.c:130
co_emcy_node::recv
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: emcy.c:74
co_val
A union of the CANopen static data types.
Definition: val.h:273
diag
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:171
can_recv_start
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
co_sdo_req_dn_val
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
__co_emcy::timer
can_timer_t * timer
A pointer to the CAN timer.
Definition: emcy.c:114
__co_obj
A CANopen object.
Definition: obj.h:31
co_emcy_destroy
void co_emcy_destroy(co_emcy_t *emcy)
Destroys a CANopen EMCY producer/consumer service.
Definition: emcy.c:360
co_sub_get_type
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
CO_SDO_AC_PARAM_VAL
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
Definition: sdo.h:135
CAN_FLAG_RTR
@ CAN_FLAG_RTR
The Remote Transmission Request (RTR) flag (unavailable in CAN FD format frames).
Definition: msg.h:48
__co_dev
A CANopen device.
Definition: dev.h:30
can_net_send
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
co_emcy_create
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
co_emcy_start
int co_emcy_start(co_emcy_t *emcy)
Starts an EMCY service.
Definition: emcy.c:370
structof
#define structof(ptr, type, member)
Obtains the address of a structure from the address of one of its members.
Definition: util.h:93
co_sub_get_subidx
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
can_buf_size
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
obj.h
stle_u16
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
co_emcy_msg
An EMCY message.
Definition: emcy.c:62
ldle_u16
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
co_emcy_remove
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
can_buf_fini
void can_buf_fini(struct can_buf *buf)
Finalizes a CAN frame buffer.
Definition: buf.c:41
stdlib.h
co_emcy_msg::eec
co_unsigned16_t eec
The emergency error code.
Definition: emcy.c:64
__co_sub
A CANopen sub-object.
Definition: obj.h:53
co_dev_find_sub
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
co_emcy_push
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_1003
The pre-defined error field.
Definition: emcy.c:126
CAN_FLAG_IDE
@ CAN_FLAG_IDE
The Identifier Extension (IDE) flag.
Definition: msg.h:43
can_recv_stop
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
timespec_add_usec
void timespec_add_usec(struct timespec *tp, uint_least64_t usec)
Adds usec microseconds to the time at tp.
Definition: time.h:149
__co_emcy::ind
co_emcy_ind_t * ind
A pointer to the indication function.
Definition: emcy.c:120
__can_net
A CAN network interface.
Definition: net.c:37
co_obj_get_idx
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:164
can_recv_create
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:533
can_timer_create
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:376
ERRNUM_NOSYS
@ ERRNUM_NOSYS
Function not supported.
Definition: errnum.h:184
can_msg::id
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
Definition: msg.h:89
co_type_is_array
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
co_dev_find_obj
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
endian.h