Lely core libraries 2.3.4
nmt_hb.c
Go to the documentation of this file.
1
24#include "nmt_hb.h"
25#include "co.h"
26#include <lely/co/dev.h>
27#include <lely/util/diag.h>
28
29#include <assert.h>
30#include <stdlib.h>
31
43 co_unsigned8_t id;
45 co_unsigned8_t st;
47 co_unsigned16_t ms;
52 int state;
53};
54
60static int co_nmt_hb_recv(const struct can_msg *msg, void *data);
61
67static int co_nmt_hb_timer(const struct timespec *tp, void *data);
68
69void *
70__co_nmt_hb_alloc(void)
71{
72 void *ptr = malloc(sizeof(struct __co_nmt_hb));
73#if !LELY_NO_ERRNO
74 if (!ptr)
75 set_errc(errno2c(errno));
76#endif
77 return ptr;
78}
79
80void
81__co_nmt_hb_free(void *ptr)
82{
83 free(ptr);
84}
85
86struct __co_nmt_hb *
87__co_nmt_hb_init(struct __co_nmt_hb *hb, can_net_t *net, co_nmt_t *nmt)
88{
89 assert(hb);
90 assert(net);
91 assert(nmt);
92
93 int errc = 0;
94
95 hb->net = net;
96 hb->nmt = nmt;
97
98 hb->recv = can_recv_create();
99 if (!hb->recv) {
100 errc = get_errc();
101 goto error_create_recv;
102 }
104
105 hb->timer = can_timer_create();
106 if (!hb->timer) {
107 errc = get_errc();
108 goto error_create_timer;
109 }
111
112 hb->id = 0;
113 hb->st = 0;
114 hb->ms = 0;
116
117 return hb;
118
119 // can_timer_destroy(hb->timer);
120error_create_timer:
122error_create_recv:
123 set_errc(errc);
124 return NULL;
125}
126
127void
128__co_nmt_hb_fini(struct __co_nmt_hb *hb)
129{
130 assert(hb);
131
134}
135
138{
139 int errc = 0;
140
141 co_nmt_hb_t *hb = __co_nmt_hb_alloc();
142 if (!hb) {
143 errc = get_errc();
144 goto error_alloc_hb;
145 }
146
147 if (!__co_nmt_hb_init(hb, net, nmt)) {
148 errc = get_errc();
149 goto error_init_hb;
150 }
151
152 return hb;
153
154error_init_hb:
155 __co_nmt_hb_free(hb);
156error_alloc_hb:
157 set_errc(errc);
158 return NULL;
159}
160
161void
163{
164 if (hb) {
165 __co_nmt_hb_fini(hb);
166 __co_nmt_hb_free(hb);
167 }
168}
169
170void
171co_nmt_hb_set_1016(co_nmt_hb_t *hb, co_unsigned8_t id, co_unsigned16_t ms)
172{
173 assert(hb);
174
175 can_recv_stop(hb->recv);
177
178 hb->id = id;
179 hb->st = 0;
180 hb->ms = ms;
182
183 if (hb->id && hb->id <= CO_NUM_NODES && hb->ms) {
184 can_recv_start(hb->recv, hb->net, CO_NMT_EC_CANID(hb->id), 0);
185 } else {
186 can_recv_stop(hb->recv);
188 }
189}
190
191void
192co_nmt_hb_set_st(co_nmt_hb_t *hb, co_unsigned8_t st)
193{
194 assert(hb);
195
196 if (hb->id && hb->id <= CO_NUM_NODES && hb->ms) {
197 hb->st = st;
199 // Reset the CAN timer for the heartbeat consumer.
200 can_timer_timeout(hb->timer, hb->net, hb->ms);
201 }
202}
203
204static int
205co_nmt_hb_recv(const struct can_msg *msg, void *data)
206{
207 assert(msg);
208 co_nmt_hb_t *hb = data;
209 assert(hb);
210 assert(hb->id && hb->id <= CO_NUM_NODES);
211 assert(msg->id == (uint_least32_t)CO_NMT_EC_CANID(hb->id));
212
213 // Obtain the node status from the CAN frame. Ignore if the toggle bit
214 // is set, since then it is not a heartbeat message.
215 if (msg->len < 1)
216 return 0;
217 co_unsigned8_t st = msg->data[0];
218 if (st & CO_NMT_ST_TOGGLE)
219 return 0;
220
221 // This might happen upon receipt of a boot-up message. The 'boot slave'
222 // process has disabled the heartbeat consumer, but the event has
223 // already been scheduled.
224 if (!hb->ms)
225 return 0;
226
227 // Update the state.
228 co_unsigned8_t old_st = hb->st;
229 int old_state = hb->state;
230 co_nmt_hb_set_st(hb, st);
231
232 if (old_state == CO_NMT_EC_OCCURRED) {
233 diag(DIAG_INFO, 0,
234 "NMT: heartbeat time out resolved for node %d",
235 hb->id);
236 // If a heartbeat timeout event occurred, notify the user that
237 // it has been resolved.
238 co_nmt_hb_ind(hb->nmt, hb->id, hb->state, CO_NMT_EC_TIMEOUT, 0);
239 }
240
241 // Notify the application of the occurrence of a state change.
242 if (st != old_st) {
243 diag(DIAG_INFO, 0,
244 "NMT: heartbeat state change occurred for node %d",
245 hb->id);
248 }
249
250 return 0;
251}
252
253static int
254co_nmt_hb_timer(const struct timespec *tp, void *data)
255{
256 (void)tp;
257 co_nmt_hb_t *hb = data;
258 assert(hb);
259
260 // Notify the application of the occurrence of a heartbeat timeout
261 // event.
262 diag(DIAG_INFO, 0, "NMT: heartbeat time out occurred for node %d",
263 hb->id);
265 co_nmt_hb_ind(hb->nmt, hb->id, hb->state, CO_NMT_EC_TIMEOUT, 0);
266
267 return 0;
268}
This header file is part of the CANopen library; it contains the device description declarations.
#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
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 can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:462
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:376
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_timer_timeout(can_timer_t *timer, can_net_t *net, int timeout)
Starts a CAN timer and registers it with a network interface.
Definition: net.c:478
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
void co_nmt_hb_ind(co_nmt_t *nmt, co_unsigned8_t id, int state, int reason, co_unsigned8_t st)
The CANopen NMT heartbeat indication function, invoked when a heartbeat event occurs.
Definition: nmt.c:2420
#define CO_NMT_EC_CANID(id)
The CAN identifier used for both node guarding and heartbeat monitoring.
Definition: nmt.h:76
@ CO_NMT_EC_STATE
An NMT error control state change event.
Definition: nmt.h:89
@ CO_NMT_EC_TIMEOUT
An NMT error control timeout event.
Definition: nmt.h:87
#define CO_NMT_ST_TOGGLE
The mask to get/set the toggle bit from an NMT state.
Definition: nmt.h:73
@ CO_NMT_EC_OCCURRED
An NMT error control event occurred.
Definition: nmt.h:80
@ CO_NMT_EC_RESOLVED
An NMT error control event was resolved.
Definition: nmt.h:82
void co_nmt_hb_destroy(co_nmt_hb_t *hb)
Destroys a CANopen NMT heartbeat consumer service.
Definition: nmt_hb.c:162
void co_nmt_hb_set_1016(co_nmt_hb_t *hb, co_unsigned8_t id, co_unsigned16_t ms)
Processes the value of CANopen object 1016 (Consumer heartbeat time) for the specified heartbeat cons...
Definition: nmt_hb.c:171
static int co_nmt_hb_timer(const struct timespec *tp, void *data)
The CAN timer callback function for a heartbeat consumer.
Definition: nmt_hb.c:254
static int co_nmt_hb_recv(const struct can_msg *msg, void *data)
The CAN receive callback function for a heartbeat consumer.
Definition: nmt_hb.c:205
co_nmt_hb_t * co_nmt_hb_create(can_net_t *net, co_nmt_t *nmt)
Creates a new CANopen NMT heartbeat consumer service.
Definition: nmt_hb.c:137
void co_nmt_hb_set_st(co_nmt_hb_t *hb, co_unsigned8_t st)
Sets the expected state of a remote NMT node.
Definition: nmt_hb.c:192
This is the internal header file of the NMT heartbeat consumer declarations.
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....
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 NMT heartbeat consumer.
Definition: nmt_hb.c:33
can_timer_t * timer
A pointer to the CAN timer.
Definition: nmt_hb.c:41
co_unsigned8_t st
The state of the node (excluding the toggle bit).
Definition: nmt_hb.c:45
can_net_t * net
A pointer to a CAN network interface.
Definition: nmt_hb.c:35
co_unsigned16_t ms
The consumer heartbeat time (in milliseconds).
Definition: nmt_hb.c:47
co_unsigned8_t id
The node-ID.
Definition: nmt_hb.c:43
co_nmt_t * nmt
A pointer to an NMT master/slave service.
Definition: nmt_hb.c:37
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: nmt_hb.c:39
int state
Indicates whether a heartbeat error occurred (CO_NMT_EC_OCCURRED or CO_NMT_EC_RESOLVED).
Definition: nmt_hb.c:52
A CANopen NMT master/slave service.
Definition: nmt.c:148
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 len
The number of bytes in data (or the requested number of bytes in case of a remote frame).
Definition: msg.h:100