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 }
can_recv_destroy
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:558
__co_nmt_hb::id
co_unsigned8_t id
The node-ID.
Definition: nmt_hb.c:43
can_msg::data
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition: msg.h:102
co_nmt_hb_ind
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
dev.h
__co_nmt_hb::state
int state
Indicates whether a heartbeat error occurred (CO_NMT_EC_OCCURRED or CO_NMT_EC_RESOLVED).
Definition: nmt_hb.c:52
DIAG_INFO
@ DIAG_INFO
An informational message.
Definition: diag.h:53
can_msg
A CAN or CAN FD format frame.
Definition: msg.h:87
CO_NUM_NODES
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
Definition: dev.h:56
can_msg::len
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
diag.h
co_nmt_hb_set_st
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
__co_nmt_hb::net
can_net_t * net
A pointer to a CAN network interface.
Definition: nmt_hb.c:35
can_timer_set_func
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_timer_stop
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:462
__can_recv
A CAN frame receiver.
Definition: net.c:86
can_recv_set_func
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
get_errc
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition: errnum.c:932
__co_nmt_hb::nmt
co_nmt_t * nmt
A pointer to an NMT master/slave service.
Definition: nmt_hb.c:37
errno2c
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:46
co_nmt_hb_create
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
co.h
CO_NMT_EC_CANID
#define CO_NMT_EC_CANID(id)
The CAN identifier used for both node guarding and heartbeat monitoring.
Definition: nmt.h:76
CO_NMT_EC_TIMEOUT
@ CO_NMT_EC_TIMEOUT
An NMT error control timeout event.
Definition: nmt.h:87
__co_nmt_hb::st
co_unsigned8_t st
The state of the node (excluding the toggle bit).
Definition: nmt_hb.c:45
__can_timer
A CAN timer.
Definition: net.c:63
CO_NMT_EC_RESOLVED
@ CO_NMT_EC_RESOLVED
An NMT error control event was resolved.
Definition: nmt.h:82
set_errc
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:944
can_timer_destroy
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:401
nmt_hb.h
CO_NMT_ST_TOGGLE
#define CO_NMT_ST_TOGGLE
The mask to get/set the toggle bit from an NMT state.
Definition: nmt.h:73
__co_nmt
A CANopen NMT master/slave service.
Definition: nmt.c:148
__co_nmt_hb
A CANopen NMT heartbeat consumer.
Definition: nmt_hb.c:33
CO_NMT_EC_OCCURRED
@ CO_NMT_EC_OCCURRED
An NMT error control event occurred.
Definition: nmt.h:80
__co_nmt_hb::timer
can_timer_t * timer
A pointer to the CAN timer.
Definition: nmt_hb.c:41
diag
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:171
can_recv_start
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
__co_nmt_hb::recv
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: nmt_hb.c:39
stdlib.h
co_nmt_hb_set_1016
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
co_nmt_hb_recv
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_EC_STATE
@ CO_NMT_EC_STATE
An NMT error control state change event.
Definition: nmt.h:89
__co_nmt_hb::ms
co_unsigned16_t ms
The consumer heartbeat time (in milliseconds).
Definition: nmt_hb.c:47
can_recv_stop
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
co_nmt_hb_destroy
void co_nmt_hb_destroy(co_nmt_hb_t *hb)
Destroys a CANopen NMT heartbeat consumer service.
Definition: nmt_hb.c:162
co_nmt_hb_timer
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
__can_net
A CAN network interface.
Definition: net.c:37
can_recv_create
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:533
can_timer_create
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:376
can_timer_timeout
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
can_msg::id
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
Definition: msg.h:89