Lely core libraries  2.2.5
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 (!ptr)
74  set_errc(errno2c(errno));
75  return ptr;
76 }
77 
78 void
79 __co_nmt_hb_free(void *ptr)
80 {
81  free(ptr);
82 }
83 
84 struct __co_nmt_hb *
85 __co_nmt_hb_init(struct __co_nmt_hb *hb, can_net_t *net, co_nmt_t *nmt)
86 {
87  assert(hb);
88  assert(net);
89  assert(nmt);
90 
91  int errc = 0;
92 
93  hb->net = net;
94  hb->nmt = nmt;
95 
96  hb->recv = can_recv_create();
97  if (!hb->recv) {
98  errc = get_errc();
99  goto error_create_recv;
100  }
102 
103  hb->timer = can_timer_create();
104  if (!hb->timer) {
105  errc = get_errc();
106  goto error_create_timer;
107  }
109 
110  hb->id = 0;
111  hb->st = 0;
112  hb->ms = 0;
114 
115  return hb;
116 
117  // can_timer_destroy(hb->timer);
118 error_create_timer:
119  can_recv_destroy(hb->recv);
120 error_create_recv:
121  set_errc(errc);
122  return NULL;
123 }
124 
125 void
126 __co_nmt_hb_fini(struct __co_nmt_hb *hb)
127 {
128  assert(hb);
129 
131  can_recv_destroy(hb->recv);
132 }
133 
134 co_nmt_hb_t *
136 {
137  int errc = 0;
138 
139  co_nmt_hb_t *hb = __co_nmt_hb_alloc();
140  if (!hb) {
141  errc = get_errc();
142  goto error_alloc_hb;
143  }
144 
145  if (!__co_nmt_hb_init(hb, net, nmt)) {
146  errc = get_errc();
147  goto error_init_hb;
148  }
149 
150  return hb;
151 
152 error_init_hb:
153  __co_nmt_hb_free(hb);
154 error_alloc_hb:
155  set_errc(errc);
156  return NULL;
157 }
158 
159 void
161 {
162  if (hb) {
163  __co_nmt_hb_fini(hb);
164  __co_nmt_hb_free(hb);
165  }
166 }
167 
168 void
169 co_nmt_hb_set_1016(co_nmt_hb_t *hb, co_unsigned8_t id, co_unsigned16_t ms)
170 {
171  assert(hb);
172 
173  can_recv_stop(hb->recv);
174  can_timer_stop(hb->timer);
175 
176  hb->id = id;
177  hb->st = 0;
178  hb->ms = ms;
180 
181  if (hb->id && hb->id <= CO_NUM_NODES && hb->ms) {
182  can_recv_start(hb->recv, hb->net, CO_NMT_EC_CANID(hb->id), 0);
183  } else {
184  can_recv_stop(hb->recv);
185  can_timer_stop(hb->timer);
186  }
187 }
188 
189 void
190 co_nmt_hb_set_st(co_nmt_hb_t *hb, co_unsigned8_t st)
191 {
192  assert(hb);
193 
194  if (hb->id && hb->id <= CO_NUM_NODES && hb->ms) {
195  hb->st = st;
197  // Reset the CAN timer for the heartbeat consumer.
198  can_timer_timeout(hb->timer, hb->net, hb->ms);
199  }
200 }
201 
202 static int
203 co_nmt_hb_recv(const struct can_msg *msg, void *data)
204 {
205  assert(msg);
206  co_nmt_hb_t *hb = data;
207  assert(hb);
208  assert(hb->id && hb->id <= CO_NUM_NODES);
209  assert(msg->id == (uint_least32_t)CO_NMT_EC_CANID(hb->id));
210 
211  // Obtain the node status from the CAN frame. Ignore if the toggle bit
212  // is set, since then it is not a heartbeat message.
213  if (msg->len < 1)
214  return 0;
215  co_unsigned8_t st = msg->data[0];
216  if (st & CO_NMT_ST_TOGGLE)
217  return 0;
218 
219  // This might happen upon receipt of a boot-up message. The 'boot slave'
220  // process has disabled the heartbeat consumer, but the event has
221  // already been scheduled.
222  if (!hb->ms)
223  return 0;
224 
225  // Update the state.
226  co_unsigned8_t old_st = hb->st;
227  int old_state = hb->state;
228  co_nmt_hb_set_st(hb, st);
229 
230  if (old_state == CO_NMT_EC_OCCURRED) {
231  diag(DIAG_INFO, 0,
232  "NMT: heartbeat time out resolved for node %d",
233  hb->id);
234  // If a heartbeat timeout event occurred, notify the user that
235  // it has been resolved.
236  co_nmt_hb_ind(hb->nmt, hb->id, hb->state, CO_NMT_EC_TIMEOUT, 0);
237  }
238 
239  // Notify the application of the occurrence of a state change.
240  if (st != old_st) {
241  diag(DIAG_INFO, 0,
242  "NMT: heartbeat state change occurred for node %d",
243  hb->id);
245  CO_NMT_EC_STATE, st);
246  }
247 
248  return 0;
249 }
250 
251 static int
252 co_nmt_hb_timer(const struct timespec *tp, void *data)
253 {
254  (void)tp;
255  co_nmt_hb_t *hb = data;
256  assert(hb);
257 
258  // Notify the application of the occurrence of a heartbeat timeout
259  // event.
260  diag(DIAG_INFO, 0, "NMT: heartbeat time out occurred for node %d",
261  hb->id);
263  co_nmt_hb_ind(hb->nmt, hb->id, hb->state, CO_NMT_EC_TIMEOUT, 0);
264 
265  return 0;
266 }
An NMT error control timeout event.
Definition: nmt.h:87
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
can_timer_t * timer
A pointer to the CAN timer.
Definition: nmt_hb.c:41
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:203
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
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:252
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:484
#define CO_NMT_ST_TOGGLE
The mask to get/set the toggle bit from an NMT state.
Definition: nmt.h:73
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: nmt_hb.c:39
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:156
An NMT error control event occurred.
Definition: nmt.h:80
An NMT error control event was resolved.
Definition: nmt.h:82
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:190
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:135
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
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
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:537
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition: msg.h:102
co_unsigned16_t ms
The consumer heartbeat time (in milliseconds).
Definition: nmt_hb.c:47
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:468
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:562
co_unsigned8_t id
The node-ID.
Definition: nmt_hb.c:43
This is the internal header file of the CANopen library.
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 CO_NMT_EC_CANID(id)
The CAN identifier used for both node guarding and heartbeat monitoring.
Definition: nmt.h:76
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:407
A CANopen NMT master/slave service.
Definition: nmt.c:104
An NMT error control state change event.
Definition: nmt.h:89
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.
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 heartbeat consumer.
Definition: nmt_hb.c:33
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
can_net_t * net
A pointer to a CAN network interface.
Definition: nmt_hb.c:35
co_nmt_t * nmt
A pointer to an NMT master/slave service.
Definition: nmt_hb.c:37
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:382
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:613
co_unsigned8_t st
The state of the node (excluding the toggle bit).
Definition: nmt_hb.c:45
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:2089
This header file is part of the CANopen library; it contains the device description declarations...
void co_nmt_hb_destroy(co_nmt_hb_t *hb)
Destroys a CANopen NMT heartbeat consumer service.
Definition: nmt_hb.c:160
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib.h> and defines any missing functionality.
A CAN frame receiver.
Definition: net.c:99
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:169
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
This is the internal header file of the NMT heartbeat consumer declarations.
An informational message.
Definition: diag.h:45