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
A time type with nanosecond resolution.
Definition time.h:88