45 #ifndef CO_EMCY_CAN_BUF_SIZE
50 #define CO_EMCY_CAN_BUF_SIZE 16
52 #ifndef CO_EMCY_MAX_NMSG
57 #define CO_EMCY_MAX_NMSG 8
111 struct can_msg begin[CO_EMCY_CAN_BUF_SIZE];
130 co_unsigned32_t
ef[0xfe];
167 co_emcy_t *emcy, co_unsigned8_t
id, co_unsigned32_t cobid);
183 static int co_emcy_timer(
const struct timespec *tp,
void *data);
196 const co_unsigned8_t msef[5]);
205 __co_emcy_alloc(
void)
207 void *ptr = malloc(
sizeof(
struct __co_emcy));
216 __co_emcy_free(
void *ptr)
238 goto error_sub_1001_00;
244 memset(emcy->
msgs, 0, CO_EMCY_MAX_NMSG *
sizeof(*emcy->
msgs));
253 memset(emcy->begin, 0, CO_EMCY_CAN_BUF_SIZE *
sizeof(*emcy->begin));
261 goto error_create_timer;
265 emcy->
inhibit = (
struct timespec){ 0, 0 };
287 goto error_create_recv;
335 trace(
"creating EMCY producer service");
342 goto error_alloc_emcy;
345 if (!__co_emcy_init(emcy, net, dev)) {
347 goto error_init_emcy;
353 __co_emcy_free(emcy);
363 trace(
"destroying EMCY producer service");
364 __co_emcy_fini(emcy);
365 __co_emcy_free(emcy);
394 co_unsigned8_t maxid =
MIN(co_obj_get_val_u8(obj_1028, 0x00),
396 for (co_unsigned8_t
id = 1;
id <= maxid;
id++) {
400 co_sub_get_val_u32(sub));
471 const co_unsigned8_t msef[5])
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]);
489 if (emcy->
nmsg > CO_EMCY_MAX_NMSG - 1) {
543 co_unsigned8_t
er = 0;
545 for (
size_t i = 0; i < emcy->
nmsg; i++)
560 memmove(emcy->
msgs + n, emcy->
msgs + n + 1,
570 co_unsigned16_t
eec = 0;
571 co_unsigned8_t
er = 0;
573 co_unsigned8_t msef[5] = { 0 };
586 for (
size_t i = 0; i < emcy->
nmsg; i++) {
649 if (msg->
flags & CAN_FLAG_EDL)
654 co_unsigned16_t eec = 0;
657 co_unsigned8_t er = msg->
len >= 3 ? msg->
data[2] : 0;
658 co_unsigned8_t msef[5] = { 0 };
660 memcpy(msef, msg->
data + 3,
661 MAX((uint_least8_t)(msg->
len - 3), 5));
664 trace(
"EMCY: received %04X %02X", eec, er);
666 emcy->
ind(emcy, node->
id, eec, er, msef, emcy->
data);
683 val_1003->
n =
MIN((co_unsigned8_t)emcy->
nmsg, nsubidx - 1);
684 for (
int i = 0; i < val_1003->
n; i++)
686 for (
int i = val_1003->
n; i < nsubidx - 1; i++)
692 static co_unsigned32_t
705 co_unsigned32_t ac = 0;
725 static co_unsigned32_t
737 co_unsigned32_t ac = 0;
745 co_unsigned32_t cobid = val.u32;
746 co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
747 if (cobid == cobid_old)
755 if (valid && valid_old && canid != canid_old)
777 uint_least32_t
id = cobid;
778 uint_least8_t flags = 0;
792 static co_unsigned32_t
805 co_unsigned32_t ac = 0;
818 co_unsigned32_t cobid = val.u32;
819 co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
820 if (cobid == cobid_old)
828 if (valid && valid_old && canid != canid_old)
857 const co_unsigned8_t msef[5])
868 co_unsigned32_t cobid = co_obj_get_val_u32(obj_1014, 0x00);
885 memcpy(msg.
data + 3, msef, 5);
904 co_unsigned16_t inhibit = co_dev_get_val_u16(emcy->
dev, 0x1015, 0x00);
906 struct timespec now = { 0, 0 };
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.
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.
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.
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.
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.
void can_buf_fini(struct can_buf *buf)
Finalizes a CAN frame buffer.
@ CAN_FLAG_IDE
The Identifier Extension (IDE) flag.
@ CAN_FLAG_RTR
The Remote Transmission Request (RTR) flag (unavailable in CAN FD format frames).
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
#define CAN_MAX_LEN
The maximum number of bytes in the payload of a CAN format frame.
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
#define CAN_MSG_INIT
The static initializer for can_msg.
This header file is part of the CANopen library; it contains the device description declarations.
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.
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
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.
This header file is part of the utilities library; it contains the diagnostic declarations.
@ DIAG_INFO
An informational message.
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
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.
void co_emcy_destroy(co_emcy_t *emcy)
Destroys a CANopen EMCY producer/consumer service.
co_emcy_t * co_emcy_create(can_net_t *net, co_dev_t *dev)
Creates a new CANopen EMCY producer/consumer service.
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.
static int co_emcy_timer(const struct timespec *tp, void *data)
The CAN timer callback function for an EMCY service.
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.
int co_emcy_is_stopped(const co_emcy_t *emcy)
Retuns 1 if the specified EMCY service is stopped, and 0 if not.
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,...
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.
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.
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...
int co_emcy_start(co_emcy_t *emcy)
Starts an EMCY service.
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...
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...
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.
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...
static int co_emcy_set_1003(co_emcy_t *emcy)
Sets the value of CANopen object 1003 (Pre-defined error field).
ssize_t co_emcy_find(const co_emcy_t *emcy, co_unsigned16_t eec)
Finds a CANopen EMCY message in the stack.
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.
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...
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).
void co_emcy_stop(co_emcy_t *emcy)
Stops an EMCY service.
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.
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,...
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.
#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.
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.
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.
void stle_u16(uint_least8_t dst[2], uint_least16_t x)
Stores a 16-bit unsigned integer in little-endian byte order.
void stle_u32(uint_least8_t dst[4], uint_least32_t x)
Stores a 32-bit unsigned integer in little-endian byte order.
int errnum2c(errnum_t errnum)
Transforms a platform-independent error number to a native error code.
@ ERRNUM_NOSYS
Function not supported.
@ ERRNUM_NOMEM
Not enough space.
@ ERRNUM_INVAL
Invalid argument.
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
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,...
#define CO_SDO_AC_NO_SUB
SDO abort code: Sub-index does not exist.
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
#define CO_SDO_AC_NO_WRITE
SDO abort code: Attempt to write a read only object.
#define structof(ptr, type, member)
Obtains the address of a structure from the address of one of its members.
#define MIN(a, b)
Returns the minimum of a and b.
#define MAX(a, b)
Returns the maximum of a and b.
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
int can_net_send(can_net_t *net, const struct can_msg *msg)
Sends a CAN frame from a network interface.
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.
void can_net_get_time(const can_net_t *net, struct timespec *tp)
Retrieves the current time of a CAN network interface.
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.
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
void can_recv_stop(can_recv_t *recv)
Stops a CAN frame receiver from processing frames and unregisters it with the network interface.
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.
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
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.
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
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.
co_unsigned8_t co_sub_get_subidx(const co_sub_t *sub)
Returns the sub-index of a CANopen sub-object.
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
void * co_obj_addressof_val(const co_obj_t *obj)
Returns the address of the value of a CANopen object.
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...
co_sub_t * co_obj_find_sub(const co_obj_t *obj, co_unsigned8_t subidx)
Finds a sub-object in a CANopen object.
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.
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.
co_unsigned16_t co_sub_get_type(const co_sub_t *sub)
Returns the data type of a CANopen sub-object.
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 CANopen EMCY producer/consumer service.
struct can_buf buf
The CAN frame buffer.
co_obj_t * obj_1003
A pointer to the pre-defined error field object.
co_dev_t * dev
A pointer to a CANopen device.
void * data
A pointer to user-specified data for ind.
can_timer_t * timer
A pointer to the CAN timer.
int stopped
A flag specifying whether the EMCY service is stopped.
co_sub_t * sub_1001_00
A pointer to the error register object.
size_t nmsg
The number of messages in msgs.
struct co_emcy_node nodes[CO_NUM_NODES]
An array of pointers to remote nodes.
struct timespec inhibit
The time at which the next EMCY message may be sent.
co_emcy_ind_t * ind
A pointer to the indication function.
can_net_t * net
A pointer to a CAN network interface.
struct co_emcy_msg * msgs
An array of EMCY messages. The first element is the most recent.
A CAN or CAN FD format frame.
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
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...
uint_least8_t len
The number of bytes in data (or the requested number of bytes in case of a remote frame).
The pre-defined error field.
co_unsigned32_t ef[0xfe]
An array of standard error fields.
co_unsigned8_t n
Number of errors.
co_unsigned8_t er
The error register.
co_unsigned16_t eec
The emergency error code.
A remote CANopen EMCY producer node.
can_recv_t * recv
A pointer to the CAN frame receiver.
co_unsigned8_t id
The node-ID.
A CANopen SDO upload/download request.
int co_type_is_array(co_unsigned16_t type)
Returns 1 if the specified (static) data type is an array, and 0 if not.
#define CO_DEFTYPE_UNSIGNED8
The data type (and object index) of an 8-bit unsigned integer.
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
A union of the CANopen static data types.
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.
void timespec_add_usec(struct timespec *tp, uint_least64_t usec)
Adds usec microseconds to the time at tp.
This header file is part of the CANopen library; it contains the CANopen value declarations.