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);
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 }
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:45
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:156
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition: errnum.c:947
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:43
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_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
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
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
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:562
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:382
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
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
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:407
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
#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:160
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
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
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
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
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
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:99
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:104
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