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 
33 struct __co_nmt_hb {
43  co_unsigned8_t id;
45  co_unsigned8_t st;
47  co_unsigned16_t ms;
52  int state;
53 };
54 
60 static int co_nmt_hb_recv(const struct can_msg *msg, void *data);
61 
67 static int co_nmt_hb_timer(const struct timespec *tp, void *data);
68 
69 void *
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 
80 void
81 __co_nmt_hb_free(void *ptr)
82 {
83  free(ptr);
84 }
85 
86 struct __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);
120 error_create_timer:
121  can_recv_destroy(hb->recv);
122 error_create_recv:
123  set_errc(errc);
124  return NULL;
125 }
126 
127 void
128 __co_nmt_hb_fini(struct __co_nmt_hb *hb)
129 {
130  assert(hb);
131 
133  can_recv_destroy(hb->recv);
134 }
135 
136 co_nmt_hb_t *
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 
154 error_init_hb:
155  __co_nmt_hb_free(hb);
156 error_alloc_hb:
157  set_errc(errc);
158  return NULL;
159 }
160 
161 void
163 {
164  if (hb) {
165  __co_nmt_hb_fini(hb);
166  __co_nmt_hb_free(hb);
167  }
168 }
169 
170 void
171 co_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);
176  can_timer_stop(hb->timer);
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);
187  can_timer_stop(hb->timer);
188  }
189 }
190 
191 void
192 co_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 
204 static int
205 co_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 
253 static int
254 co_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
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
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:533
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
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:376
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
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
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
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
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