Lely core libraries  2.2.5
emcy.c
Go to the documentation of this file.
1 
24 #include "co.h"
25 
26 #ifndef 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 
42 struct co_emcy_msg {
44  co_unsigned16_t eec;
46  co_unsigned8_t er;
47 };
48 
50 struct co_emcy_node {
54  co_unsigned8_t id;
57 };
58 
69 static struct co_emcy_node *co_emcy_node_create(
70  co_emcy_t *emcy, co_unsigned8_t id);
71 
73 static void co_emcy_node_destroy(struct co_emcy_node *node);
74 
80 static int co_emcy_node_recv(const struct can_msg *msg, void *data);
81 
83 struct __co_emcy {
93  size_t nmsg;
95  struct co_emcy_msg *msgs;
97  struct can_buf buf;
101  struct timespec inhibit;
103  struct co_emcy_node *nodes[CO_NUM_NODES];
107  void *data;
108 };
109 
111 struct co_1003 {
113  co_unsigned8_t n;
115  co_unsigned32_t ef[0xfe];
116 };
117 
124 static int co_emcy_set_1003(co_emcy_t *emcy);
125 
132 static co_unsigned32_t co_1003_dn_ind(
133  co_sub_t *sub, struct co_sdo_req *req, void *data);
134 
141 static co_unsigned32_t co_1014_dn_ind(
142  co_sub_t *sub, struct co_sdo_req *req, void *data);
143 
153 static int co_emcy_set_1028(
154  co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid);
155 
162 static co_unsigned32_t co_1028_dn_ind(
163  co_sub_t *sub, struct co_sdo_req *req, void *data);
164 
170 static int co_emcy_timer(const struct timespec *tp, void *data);
171 
182 static int co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
183  const co_unsigned8_t msef[5]);
184 
189 static void co_emcy_flush(co_emcy_t *emcy);
190 
191 void *
192 __co_emcy_alloc(void)
193 {
194  void *ptr = malloc(sizeof(struct __co_emcy));
195  if (!ptr)
196  set_errc(errno2c(errno));
197  return ptr;
198 }
199 
200 void
201 __co_emcy_free(void *ptr)
202 {
203  free(ptr);
204 }
205 
206 struct __co_emcy *
207 __co_emcy_init(struct __co_emcy *emcy, can_net_t *net, co_dev_t *dev)
208 {
209  assert(emcy);
210  assert(net);
211  assert(dev);
212 
213  int errc = 0;
214 
215  emcy->net = net;
216  emcy->dev = dev;
217 
218  emcy->sub_1001_00 = co_dev_find_sub(emcy->dev, 0x1001, 0x00);
219  if (!emcy->sub_1001_00) {
220  errc = errnum2c(ERRNUM_NOSYS);
221  goto error_sub_1001_00;
222  }
223  emcy->obj_1003 = co_dev_find_obj(emcy->dev, 0x1003);
224 
225  emcy->nmsg = 0;
226  emcy->msgs = NULL;
227 
228  if (can_buf_init(&emcy->buf, 0) == -1) {
229  errc = get_errc();
230  goto error_init_buf;
231  }
232 
233  emcy->timer = can_timer_create();
234  if (!emcy->timer) {
235  errc = get_errc();
236  goto error_create_timer;
237  }
238  can_timer_set_func(emcy->timer, &co_emcy_timer, emcy);
239 
240  can_net_get_time(emcy->net, &emcy->inhibit);
241 
242  memset(emcy->nodes, 0, CO_NUM_NODES * sizeof(struct co_emcy_node *));
243 
244  emcy->ind = NULL;
245  emcy->data = NULL;
246 
247  // Set the download indication function for the pre-defined error field.
248  if (emcy->obj_1003)
250 
251  // Set the download indication function for the EMCY COB-ID object.
252  co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
253  if (obj_1014)
254  co_obj_set_dn_ind(obj_1014, &co_1014_dn_ind, emcy);
255 
256  co_obj_t *obj_1028 = co_dev_find_obj(emcy->dev, 0x1028);
257  if (obj_1028) {
258  // Set the download indication function for the emergency
259  // consumer object.
260  co_obj_set_dn_ind(obj_1028, &co_1028_dn_ind, emcy);
261  // Initialize the nodes.
262  co_unsigned8_t maxid = MIN(co_obj_get_val_u8(obj_1028, 0x00),
263  CO_NUM_NODES);
264  for (co_unsigned8_t id = 1; id <= maxid; id++) {
265  co_unsigned32_t cobid =
266  co_obj_get_val_u32(obj_1028, id);
267  if (co_emcy_set_1028(emcy, id, cobid) == -1) {
268  errc = get_errc();
269  goto error_set_1028;
270  }
271  }
272  }
273 
274  return emcy;
275 
276 error_set_1028:
277  if (obj_1028)
278  co_obj_set_dn_ind(obj_1028, NULL, NULL);
279  if (obj_1014)
280  co_obj_set_dn_ind(obj_1014, NULL, NULL);
281  if (emcy->obj_1003)
282  co_obj_set_dn_ind(emcy->obj_1003, NULL, NULL);
283  can_timer_destroy(emcy->timer);
284 error_create_timer:
285  can_buf_fini(&emcy->buf);
286 error_init_buf:
287 error_sub_1001_00:
288  set_errc(errc);
289  return NULL;
290 }
291 
292 void
293 __co_emcy_fini(struct __co_emcy *emcy)
294 {
295  assert(emcy);
296 
297  // Remove the download indication function for the emergency consumer
298  // object.
299  co_obj_t *obj_1028 = co_dev_find_obj(emcy->dev, 0x1028);
300  if (obj_1028)
301  co_obj_set_dn_ind(obj_1028, NULL, NULL);
302 
303  // Remove the download indication function for the EMCY COB-ID object.
304  co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
305  if (obj_1014)
306  co_obj_set_dn_ind(obj_1014, NULL, NULL);
307 
308  // Remove the download indication function for the pre-defined error
309  // field.
310  if (emcy->obj_1003)
311  co_obj_set_dn_ind(emcy->obj_1003, NULL, NULL);
312 
313  for (co_unsigned8_t id = 1; id <= CO_NUM_NODES; id++)
314  co_emcy_node_destroy(emcy->nodes[id - 1]);
315 
316  can_timer_destroy(emcy->timer);
317 
318  can_buf_fini(&emcy->buf);
319 
320  free(emcy->msgs);
321 }
322 
323 co_emcy_t *
325 {
326  trace("creating EMCY producer service");
327 
328  int errc = 0;
329 
330  co_emcy_t *emcy = __co_emcy_alloc();
331  if (!emcy) {
332  errc = get_errc();
333  goto error_alloc_emcy;
334  }
335 
336  if (!__co_emcy_init(emcy, net, dev)) {
337  errc = get_errc();
338  goto error_init_emcy;
339  }
340 
341  return emcy;
342 
343 error_init_emcy:
344  __co_emcy_free(emcy);
345 error_alloc_emcy:
346  set_errc(errc);
347  return NULL;
348 }
349 
350 void
352 {
353  if (emcy) {
354  trace("destroying EMCY producer service");
355  __co_emcy_fini(emcy);
356  __co_emcy_free(emcy);
357  }
358 }
359 
360 can_net_t *
362 {
363  assert(emcy);
364 
365  return emcy->net;
366 }
367 
368 co_dev_t *
370 {
371  assert(emcy);
372 
373  return emcy->dev;
374 }
375 
376 int
377 co_emcy_push(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
378  const co_unsigned8_t msef[5])
379 {
380  assert(emcy);
381 
382  if (!eec) {
384  return -1;
385  }
386  // Bit 0 (generic error) shall be signaled at any error situation.
387  er |= 0x01;
388 
389  if (msef)
390  diag(DIAG_INFO, 0, "EMCY: %04X %02X %u %u %u %u %u", eec, er,
391  msef[0], msef[1], msef[2], msef[3], msef[4]);
392  else
393  diag(DIAG_INFO, 0, "EMCY: %04X %02X", eec, er);
394 
395  // Make room on the stack.
396  struct co_emcy_msg *msgs = realloc(emcy->msgs,
397  (emcy->nmsg + 1) * sizeof(struct co_emcy_msg));
398  if (!msgs) {
399  set_errc(errno2c(errno));
400  return -1;
401  }
402  emcy->msgs = msgs;
403  if (emcy->nmsg) {
404  // Copy the current error register.
405  er |= emcy->msgs[0].er;
406  // Move the older messages.
407  memmove(emcy->msgs + 1, emcy->msgs,
408  emcy->nmsg * sizeof(struct co_emcy_msg));
409  }
410  emcy->nmsg++;
411  // Push the error to the stack.
412  emcy->msgs[0].eec = eec;
413  emcy->msgs[0].er = er;
414 
415  // Update the pre-defined error field.
416  if (emcy->obj_1003)
417  co_emcy_set_1003(emcy);
418 
419  return co_emcy_send(emcy, eec, er, msef);
420 }
421 
422 int
423 co_emcy_pop(co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
424 {
425  assert(emcy);
426 
427  co_emcy_peek(emcy, peec, per);
428 
429  if (!emcy->nmsg)
430  return 0;
431 
432  // Move the older messages.
433  if (--emcy->nmsg)
434  memmove(emcy->msgs, emcy->msgs + 1,
435  emcy->nmsg * sizeof(struct co_emcy_msg));
436 
437  // Update the pre-defined error field.
438  if (emcy->obj_1003)
439  co_emcy_set_1003(emcy);
440 
441  if (emcy->nmsg) {
442  // Store the next error in the error register and the
443  // manufacturer-specific field.
444  co_unsigned8_t msef[5] = { 0 };
445  stle_u16(msef, emcy->msgs[0].eec);
446  return co_emcy_send(emcy, 0, emcy->msgs[0].er, msef);
447  } else {
448  return co_emcy_send(emcy, 0, 0, NULL);
449  }
450 }
451 
452 void
453 co_emcy_peek(const co_emcy_t *emcy, co_unsigned16_t *peec, co_unsigned8_t *per)
454 {
455  assert(emcy);
456 
457  if (peec)
458  *peec = emcy->nmsg ? emcy->msgs[0].eec : 0;
459  if (per)
460  *per = emcy->nmsg ? emcy->msgs[0].er : 0;
461 }
462 
463 int
465 {
466  assert(emcy);
467 
468  if (!emcy->nmsg)
469  return 0;
470 
471  // Clear the stack.
472  emcy->nmsg = 0;
473 
474  // Clear the pre-defined error field.
475  if (emcy->obj_1003)
476  co_emcy_set_1003(emcy);
477 
478  // Send the 'error reset/no error' message.
479  return co_emcy_send(emcy, 0, 0, NULL);
480 }
481 
482 void
483 co_emcy_get_ind(const co_emcy_t *emcy, co_emcy_ind_t **pind, void **pdata)
484 {
485  assert(emcy);
486 
487  if (pind)
488  *pind = emcy->ind;
489  if (pdata)
490  *pdata = emcy->data;
491 }
492 
493 void
494 co_emcy_set_ind(co_emcy_t *emcy, co_emcy_ind_t *ind, void *data)
495 {
496  assert(emcy);
497 
498  emcy->ind = ind;
499  emcy->data = data;
500 }
501 
502 struct co_emcy_node *
503 co_emcy_node_create(co_emcy_t *emcy, co_unsigned8_t id)
504 {
505  assert(emcy);
506  assert(id && id <= CO_NUM_NODES);
507 
508  trace("creating EMCY consumer service for node %d", id);
509 
510  int errc = 0;
511 
512  struct co_emcy_node *node = malloc(sizeof(*node));
513  if (!node) {
514  errc = errno2c(errno);
515  goto error_alloc_node;
516  }
517 
518  node->emcy = emcy;
519  node->id = id;
520 
521  node->recv = can_recv_create();
522  if (!node->recv) {
523  errc = get_errc();
524  goto error_create_recv;
525  }
527 
528  return node;
529 
530 error_create_recv:
531  free(node);
532 error_alloc_node:
533  set_errc(errc);
534  return NULL;
535 }
536 
537 void
539 {
540  if (node) {
541  trace("destroying EMCY consumer service for node %d", node->id);
542  can_recv_destroy(node->recv);
543  free(node);
544  }
545 }
546 
547 static int
548 co_emcy_node_recv(const struct can_msg *msg, void *data)
549 {
550  assert(msg);
551  struct co_emcy_node *node = data;
552  assert(node);
553  co_emcy_t *emcy = node->emcy;
554  assert(emcy);
555 
556  // Ignore remote frames.
557  if (msg->flags & CAN_FLAG_RTR)
558  return 0;
559 
560 #ifndef LELY_NO_CANFD
561  // Ignore CAN FD format frames.
562  if (msg->flags & CAN_FLAG_EDL)
563  return 0;
564 #endif
565 
566  // Extract the parameters from the frame.
567  co_unsigned16_t eec = 0;
568  if (msg->len >= 2)
569  eec = ldle_u16(msg->data);
570  co_unsigned8_t er = msg->len >= 3 ? msg->data[2] : 0;
571  co_unsigned8_t msef[5] = { 0 };
572  if (msg->len >= 4)
573  memcpy(msef, msg->data + 3,
574  MAX((uint_least8_t)(msg->len - 3), 5));
575 
576  // Notify the user.
577  trace("EMCY: received %04X %02X", eec, er);
578  if (emcy->ind)
579  emcy->ind(emcy, node->id, eec, er, msef, emcy->data);
580 
581  return 0;
582 }
583 
584 static int
586 {
587  assert(emcy);
588  assert(emcy->obj_1003);
589 
590  struct co_1003 *val_1003 = co_obj_addressof_val(emcy->obj_1003);
591  co_unsigned8_t nsubidx = co_obj_get_subidx(emcy->obj_1003, 0, NULL);
592  if (!nsubidx)
593  return 0;
594 
595  // Copy the emergency error codes.
596  val_1003->n = MIN((co_unsigned8_t)emcy->nmsg, nsubidx - 1);
597  for (int i = 0; i < val_1003->n; i++)
598  val_1003->ef[i] = emcy->msgs[i].eec;
599  for (int i = val_1003->n; i < nsubidx - 1; i++)
600  val_1003->ef[i] = 0;
601 
602  return 0;
603 }
604 
605 static co_unsigned32_t
606 co_1003_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
607 {
608  assert(sub);
609  assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1003);
610  assert(req);
611  co_emcy_t *emcy = data;
612  assert(emcy);
613 
614  co_unsigned32_t ac = 0;
615 
616  co_unsigned16_t type = co_sub_get_type(sub);
617  union co_val val;
618  if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
619  return ac;
620 
621  if (co_sub_get_subidx(sub)) {
622  ac = CO_SDO_AC_NO_WRITE;
623  goto error;
624  }
625 
626  // Only the value 0 is allowed.
627  assert(type == CO_DEFTYPE_UNSIGNED8);
628  if (val.u8) {
629  ac = CO_SDO_AC_PARAM_VAL;
630  goto error;
631  }
632 
633  emcy->nmsg = 0;
634 
635  co_sub_dn(sub, &val);
636  co_val_fini(type, &val);
637 
638  co_emcy_set_1003(emcy);
639  return 0;
640 
641 error:
642  co_val_fini(type, &val);
643  return ac;
644 }
645 
646 static co_unsigned32_t
647 co_1014_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
648 {
649  assert(sub);
650  assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1014);
651  assert(req);
652  (void)data;
653 
654  co_unsigned32_t ac = 0;
655 
656  co_unsigned16_t type = co_sub_get_type(sub);
657  union co_val val;
658  if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
659  return ac;
660 
661  if (co_sub_get_subidx(sub)) {
662  ac = CO_SDO_AC_NO_SUB;
663  goto error;
664  }
665 
666  assert(type == CO_DEFTYPE_UNSIGNED32);
667  co_unsigned32_t cobid = val.u32;
668  co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
669  if (cobid == cobid_old)
670  goto error;
671 
672  // The CAN-ID cannot be changed when the EMCY is and remains valid.
673  int valid = !(cobid & CO_EMCY_COBID_VALID);
674  int valid_old = !(cobid_old & CO_EMCY_COBID_VALID);
675  uint_least32_t canid = cobid & CAN_MASK_EID;
676  uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
677  if (valid && valid_old && canid != canid_old) {
678  ac = CO_SDO_AC_PARAM_VAL;
679  goto error;
680  }
681 
682  // A 29-bit CAN-ID is only valid if the frame bit is set.
683  if (!(cobid & CO_EMCY_COBID_FRAME)
684  && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID))) {
685  ac = CO_SDO_AC_PARAM_VAL;
686  goto error;
687  }
688 
689  co_sub_dn(sub, &val);
690 error:
691  co_val_fini(type, &val);
692  return ac;
693 }
694 
695 static int
696 co_emcy_set_1028(co_emcy_t *emcy, co_unsigned8_t id, co_unsigned32_t cobid)
697 {
698  assert(emcy);
699  assert(id && id <= CO_NUM_NODES);
700  struct co_emcy_node *node = emcy->nodes[id - 1];
701 
702  if (!(cobid & CO_EMCY_COBID_VALID)) {
703  if (!node) {
704  node = co_emcy_node_create(emcy, id);
705  if (!node)
706  return -1;
707  emcy->nodes[id - 1] = node;
708  }
709  // Register the receiver under the specified CAN-ID.
710  uint_least32_t id = cobid;
711  uint_least8_t flags = 0;
712  if (id & CO_EMCY_COBID_FRAME) {
713  id &= CAN_MASK_EID;
714  flags |= CAN_FLAG_IDE;
715  } else {
716  id &= CAN_MASK_BID;
717  }
718  can_recv_start(node->recv, emcy->net, id, flags);
719  } else if (node) {
720  co_emcy_node_destroy(node);
721  emcy->nodes[id - 1] = NULL;
722  }
723 
724  return 0;
725 }
726 
727 static co_unsigned32_t
728 co_1028_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
729 {
730  assert(sub);
731  assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1028);
732  assert(req);
733  co_emcy_t *emcy = data;
734  assert(emcy);
735 
736  co_unsigned32_t ac = 0;
737 
738  co_unsigned16_t type = co_sub_get_type(sub);
739  union co_val val;
740  if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
741  return ac;
742 
743  co_unsigned8_t id = co_sub_get_subidx(sub);
744  if (!id) {
745  ac = CO_SDO_AC_NO_WRITE;
746  goto error;
747  }
748  co_unsigned8_t maxid = MIN(co_obj_get_val_u8(co_sub_get_obj(sub), 0),
749  CO_NUM_NODES);
750  if (id > maxid) {
751  ac = CO_SDO_AC_NO_SUB;
752  goto error;
753  }
754 
755  assert(type == CO_DEFTYPE_UNSIGNED32);
756  co_unsigned32_t cobid = val.u32;
757  co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
758  if (cobid == cobid_old)
759  goto error;
760 
761  // The CAN-ID cannot be changed when the EMCY is and remains valid.
762  int valid = !(cobid & CO_EMCY_COBID_VALID);
763  int valid_old = !(cobid_old & CO_EMCY_COBID_VALID);
764  uint_least32_t canid = cobid & CAN_MASK_EID;
765  uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
766  if (valid && valid_old && canid != canid_old) {
767  ac = CO_SDO_AC_PARAM_VAL;
768  goto error;
769  }
770 
771  // A 29-bit CAN-ID is only valid if the frame bit is set.
772  if (!(cobid & CO_EMCY_COBID_FRAME)
773  && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID))) {
774  ac = CO_SDO_AC_PARAM_VAL;
775  goto error;
776  }
777 
778  if (co_emcy_set_1028(emcy, id, cobid) == -1) {
779  ac = CO_SDO_AC_ERROR;
780  goto error;
781  }
782 
783  co_sub_dn(sub, &val);
784 error:
785  co_val_fini(type, &val);
786  return ac;
787 }
788 
789 static int
790 co_emcy_timer(const struct timespec *tp, void *data)
791 {
792  (void)tp;
793  co_emcy_t *emcy = data;
794  assert(emcy);
795 
796  co_emcy_flush(emcy);
797 
798  return 0;
799 }
800 
801 static int
802 co_emcy_send(co_emcy_t *emcy, co_unsigned16_t eec, co_unsigned8_t er,
803  const co_unsigned8_t msef[5])
804 {
805  assert(emcy);
806 
807  // Update the error register in the object dictionary.
808  co_sub_set_val_u8(emcy->sub_1001_00, er);
809 
810  // Check whether the EMCY COB-ID exists and is valid.
811  co_obj_t *obj_1014 = co_dev_find_obj(emcy->dev, 0x1014);
812  if (!obj_1014)
813  return 0;
814  co_unsigned32_t cobid = co_obj_get_val_u32(obj_1014, 0x00);
815  if (cobid & CO_EMCY_COBID_VALID)
816  return 0;
817 
818  // Create the frame.
819  struct can_msg msg = CAN_MSG_INIT;
820  msg.id = cobid;
821  if (cobid & CO_EMCY_COBID_FRAME) {
822  msg.id &= CAN_MASK_EID;
823  msg.flags |= CAN_FLAG_IDE;
824  } else {
825  msg.id &= CAN_MASK_BID;
826  }
827  msg.len = CAN_MAX_LEN;
828  stle_u32(msg.data, eec);
829  msg.data[2] = er;
830  if (msef)
831  memcpy(msg.data + 3, msef, 5);
832 
833  // Add the frame to the buffer.
834  if (!can_buf_write(&emcy->buf, &msg, 1)) {
835  if (!can_buf_reserve(&emcy->buf, 1))
836  return -1;
837  can_buf_write(&emcy->buf, &msg, 1);
838  }
839 
840  co_emcy_flush(emcy);
841  return 0;
842 }
843 
844 static void
846 {
847  assert(emcy);
848 
849  co_unsigned16_t inhibit = co_dev_get_val_u16(emcy->dev, 0x1015, 0x00);
850 
851  struct timespec now = { 0, 0 };
852  can_net_get_time(emcy->net, &now);
853 
854  while (can_buf_size(&emcy->buf)) {
855  if (timespec_cmp(&now, &emcy->inhibit) < 0) {
856  can_timer_start(emcy->timer, emcy->net, &emcy->inhibit,
857  NULL);
858  return;
859  }
860  // Update the inhibit time.
861  emcy->inhibit = now;
862  timespec_add_usec(&emcy->inhibit, inhibit * 100);
863  // Send the frame.
864  struct can_msg msg;
865  if (can_buf_read(&emcy->buf, &msg, 1))
866  can_net_send(emcy->net, &msg);
867  }
868 }
869 
870 #endif // !LELY_NO_CO_EMCY
A CANopen SDO upload/download request.
Definition: sdo.h:178
A CAN or CAN FD format frame.
Definition: msg.h:87
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
Definition: dev.h:56
void can_buf_fini(struct can_buf *buf)
Finalizes a CAN frame buffer.
Definition: buf.c:67
#define CO_EMCY_COBID_VALID
The bit in the EMCY COB-ID specifying whether the EMCY exists and is valid.
Definition: emcy.h:29
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:210
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:647
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:606
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:372
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
Definition: msg.h:89
A CAN network interface.
Definition: net.c:37
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
A CANopen sub-object.
Definition: obj.h:54
co_emcy_t * emcy
A pointer to the EMCY service.
Definition: emcy.c:52
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, in which case it sets the CAN timer.
Definition: emcy.c:845
A CAN frame buffer.
Definition: buf.h:47
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:506
int errnum2c(errnum_t errnum)
Transforms a platform-independent error number to a native error code.
Definition: errnum.c:825
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition: errnum.h:375
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:156
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:494
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
Definition: sdo.h:135
A union of the CANopen static data types.
Definition: val.h:163
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:204
can_timer_t * timer
A pointer to the CAN timer.
Definition: emcy.c:99
#define CO_SDO_AC_ERROR
SDO abort code: General error.
Definition: sdo.h:150
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:548
static int co_emcy_set_1003(co_emcy_t *emcy)
Sets the value of CANopen object 1003 (Pre-defined error field).
Definition: emcy.c:585
#define MIN(a, b)
Returns the minimum of a and b.
Definition: util.h:57
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:437
int can_buf_init(struct can_buf *buf, size_t size)
Initializes a CAN frame buffer.
Definition: buf.c:39
An EMCY message.
Definition: emcy.c:42
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition: msg.h:31
This header file is part of the CANopen library; it contains the Service Data Object (SDO) declaratio...
co_unsigned8_t er
The error register.
Definition: emcy.c:46
struct co_emcy_node * nodes[CO_NUM_NODES]
An array of pointers to remote nodes.
Definition: emcy.c:103
This header file is part of the CANopen library; it contains the CANopen value declarations.
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
struct timespec inhibit
The time at which the next EMCY message may be sent.
Definition: emcy.c:101
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:428
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:453
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:537
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
Definition: type.h:50
void timespec_add_usec(struct timespec *tp, uint_least64_t usec)
Adds usec microseconds to the time at tp.
Definition: time.h:143
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition: msg.h:102
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:494
static void co_emcy_node_destroy(struct co_emcy_node *node)
Destroys a remote CANopen EMCY producer node.
Definition: emcy.c:538
co_unsigned8_t n
Number of errors.
Definition: emcy.c:113
This header file is part of the utilities library; it contains the time function declarations.
This header file is part of the CANopen library; it contains the emergency (EMCY) object declarations...
co_emcy_ind_t * ind
A pointer to the indication function.
Definition: emcy.c:105
co_unsigned16_t eec
The emergency error code.
Definition: emcy.c:44
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
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:562
void * data
A pointer to user-specified data for ind.
Definition: emcy.c:107
This header file is part of the utilities library; it contains the byte order (endianness) function d...
The Remote Transmission Request (RTR) flag (unavailable in CAN FD format frames). ...
Definition: msg.h:48
This is the internal header file of the CANopen library.
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:111
This header file is part of the CAN library; it contains the CAN frame buffer declarations.
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:324
#define CAN_MAX_LEN
The maximum number of bytes in the payload of a CAN format frame.
Definition: msg.h:72
void co_emcy_destroy(co_emcy_t *emcy)
Destroys a CANopen EMCY producer/consumer service.
Definition: emcy.c:351
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function...
Definition: errnum.c:947
A CAN timer.
Definition: net.c:63
#define MAX(a, b)
Returns the maximum of a and b.
Definition: util.h:65
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:407
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:954
#define CO_DEFTYPE_UNSIGNED8
The data type (and object index) of an 8-bit unsigned integer.
Definition: type.h:44
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: emcy.c:56
#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
Invalid argument.
Definition: errnum.h:129
size_t nmsg
The number of messages in msgs.
Definition: emcy.c:93
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:162
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:43
This header file is part of the utilities library; it contains the diagnostic declarations.
#define CO_SDO_AC_NO_WRITE
SDO abort code: Attempt to write a read only object.
Definition: sdo.h:90
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:572
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:483
int timespec_cmp(const void *p1, const void *p2)
Compares two times.
Definition: time.h:228
A CANopen device.
Definition: dev.c:41
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:268
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:582
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_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:520
The Identifier Extension (IDE) flag.
Definition: msg.h:43
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:382
A remote CANopen EMCY producer node.
Definition: emcy.c:50
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:299
struct co_emcy_msg * msgs
An array of EMCY messages. The first element is the most recent.
Definition: emcy.c:95
void co_val_fini(co_unsigned16_t type, void *val)
Finalizes a value of the specified data type.
Definition: val.c:275
int can_net_send(can_net_t *net, const struct can_msg *msg)
Sends a CAN frame from a network interface.
Definition: net.c:308
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:802
co_unsigned8_t id
The node-ID.
Definition: emcy.c:54
#define CO_SDO_AC_NO_SUB
SDO abort code: Sub-index does not exist.
Definition: sdo.h:132
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:300
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:165
struct can_buf buf
The CAN frame buffer.
Definition: emcy.c:97
A CANopen EMCY producer/consumer service.
Definition: emcy.c:83
int co_emcy_clear(co_emcy_t *emcy)
Clears the CANopen EMCY message stack and broadcasts the &#39;error reset/no error&#39; message if the EMCY p...
Definition: emcy.c:464
This header file is part of the CANopen library; it contains the device description declarations...
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:728
co_unsigned8_t co_sub_get_subidx(const co_sub_t *sub)
Returns the sub-index of a CANopen sub-object.
Definition: obj.c:528
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib.h> and defines any missing functionality.
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:377
A CAN frame receiver.
Definition: net.c:99
co_unsigned16_t co_sub_get_type(const co_sub_t *sub)
Returns the data type of a CANopen sub-object.
Definition: obj.c:570
can_net_t * net
A pointer to a CAN network interface.
Definition: emcy.c:85
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:361
co_sub_t * sub_1001_00
A pointer to the error register object.
Definition: emcy.c:89
static struct co_emcy_node * co_emcy_node_create(co_emcy_t *emcy, co_unsigned8_t id)
Creates a new remote CANopen EMCY producer node.
Definition: emcy.c:503
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:154
A CANopen object.
Definition: obj.h:32
This header file is part of the CANopen library; it contains the object dictionary declarations...
#define CAN_MSG_INIT
The static initializer for can_msg.
Definition: msg.h:113
co_obj_t * obj_1003
A pointer to the pre-defined error field object.
Definition: emcy.c:91
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:591
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:288
Function not supported.
Definition: errnum.h:181
co_unsigned32_t ef[0xfe]
An array of standard error fields.
Definition: emcy.c:115
static int 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:696
co_dev_t * dev
A pointer to a CANopen device.
Definition: emcy.c:87
void * co_obj_addressof_val(const co_obj_t *obj)
Returns the address of the value of a CANopen object.
Definition: obj.c:311
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 &#39;error reset&#39; message if t...
Definition: emcy.c:423
static int co_emcy_timer(const struct timespec *tp, void *data)
The CAN timer callback function for an EMCY service.
Definition: emcy.c:790
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
Definition: msg.h:34
The pre-defined error field.
Definition: emcy.c:111
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:369
An informational message.
Definition: diag.h:45