1 /*
2 * CANopen Heartbeat consumer object.
3 *
4 * @file CO_HBconsumer.c
5 * @ingroup CO_HBconsumer
6 * @author Janez Paternoster
7 * @copyright 2004 - 2020 Janez Paternoster
8 *
9 * This file is part of CANopenNode, an opensource CANopen Stack.
10 * Project home page is <https://github.com/CANopenNode/CANopenNode>.
11 * For more information on CANopen see <http://www.can-cia.org/>.
12 *
13 * Licensed under the Apache License, Version 2.0 (the "License");
14 * you may not use this file except in compliance with the License.
15 * You may obtain a copy of the License at
16 *
17 * http://www.apache.org/licenses/LICENSE-2.0
18 *
19 * Unless required by applicable law or agreed to in writing, software
20 * distributed under the License is distributed on an "AS IS" BASIS,
21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 * See the License for the specific language governing permissions and
23 * limitations under the License.
24 */
25
26 #include "CANopen.h"
27 #include "CO_HBconsumer.h"
28
29 /*
30 * Read received message from CAN module.
31 *
32 * Function will be called (by CAN receive interrupt) every time, when CAN
33 * message with correct identifier will be received. For more information and
34 * description of parameters see file CO_driver.h.
35 */
CO_HBcons_receive(void * object,const CO_CANrxMsg_t * msg)36 static void CO_HBcons_receive(void *object, const CO_CANrxMsg_t *msg){
37 CO_HBconsNode_t *HBconsNode;
38
39 HBconsNode = (CO_HBconsNode_t*) object; /* this is the correct pointer type of the first argument */
40
41 /* verify message length */
42 if(msg->DLC == 1){
43 /* copy data and set 'new message' flag. */
44 HBconsNode->NMTstate = (CO_NMT_internalState_t)msg->data[0];
45 SET_CANrxNew(HBconsNode->CANrxNew);
46 }
47 }
48
49
50 /*
51 * Configure one monitored node.
52 */
CO_HBcons_monitoredNodeConfig(CO_HBconsumer_t * HBcons,uint8_t idx,uint8_t nodeId,uint16_t time)53 static void CO_HBcons_monitoredNodeConfig(
54 CO_HBconsumer_t *HBcons,
55 uint8_t idx,
56 uint8_t nodeId,
57 uint16_t time)
58 {
59 uint16_t COB_ID;
60 CO_HBconsNode_t *monitoredNode;
61
62 if(idx >= HBcons->numberOfMonitoredNodes) return;
63
64 monitoredNode = &HBcons->monitoredNodes[idx];
65 monitoredNode->nodeId = nodeId;
66 monitoredNode->time = time;
67 monitoredNode->NMTstate = CO_NMT_INITIALIZING;
68 monitoredNode->HBstate = CO_HBconsumer_UNCONFIGURED;
69
70 /* is channel used */
71 if(monitoredNode->nodeId && monitoredNode->time){
72 COB_ID = monitoredNode->nodeId + CO_CAN_ID_HEARTBEAT;
73 monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
74
75 }
76 else{
77 COB_ID = 0;
78 monitoredNode->time = 0;
79 }
80
81 /* configure Heartbeat consumer CAN reception */
82 if (monitoredNode->HBstate != CO_HBconsumer_UNCONFIGURED) {
83 CO_CANrxBufferInit(
84 HBcons->CANdevRx,
85 HBcons->CANdevRxIdxStart + idx,
86 COB_ID,
87 0x7FF,
88 0,
89 (void*)&HBcons->monitoredNodes[idx],
90 CO_HBcons_receive);
91 }
92 }
93
94
95 /*
96 * OD function for accessing _Consumer Heartbeat Time_ (index 0x1016) from SDO server.
97 *
98 * For more information see file CO_SDO.h.
99 */
CO_ODF_1016(CO_ODF_arg_t * ODF_arg)100 static CO_SDO_abortCode_t CO_ODF_1016(CO_ODF_arg_t *ODF_arg)
101 {
102 CO_HBconsumer_t *HBcons;
103 uint8_t NodeID;
104 uint16_t HBconsTime;
105 uint32_t value;
106 CO_ReturnError_t ret;
107
108 if(ODF_arg->reading){
109 return CO_SDO_AB_NONE;
110 }
111
112 HBcons = (CO_HBconsumer_t*) ODF_arg->object;
113 value = CO_getUint32(ODF_arg->data);
114 NodeID = (value >> 16U) & 0xFFU;
115 HBconsTime = value & 0xFFFFU;
116
117 if((value & 0xFF800000U) != 0){
118 return CO_SDO_AB_PRAM_INCOMPAT;
119 }
120
121 ret = CO_HBconsumer_initEntry(HBcons, ODF_arg->subIndex-1U, NodeID, HBconsTime);
122 if (ret != CO_ERROR_NO) {
123 return CO_SDO_AB_PRAM_INCOMPAT;
124 }
125 return CO_SDO_AB_NONE;
126 }
127
128
129 /******************************************************************************/
CO_HBconsumer_init(CO_HBconsumer_t * HBcons,CO_EM_t * em,CO_SDO_t * SDO,const uint32_t HBconsTime[],CO_HBconsNode_t monitoredNodes[],uint8_t numberOfMonitoredNodes,CO_CANmodule_t * CANdevRx,uint16_t CANdevRxIdxStart)130 CO_ReturnError_t CO_HBconsumer_init(
131 CO_HBconsumer_t *HBcons,
132 CO_EM_t *em,
133 CO_SDO_t *SDO,
134 const uint32_t HBconsTime[],
135 CO_HBconsNode_t monitoredNodes[],
136 uint8_t numberOfMonitoredNodes,
137 CO_CANmodule_t *CANdevRx,
138 uint16_t CANdevRxIdxStart)
139 {
140 uint8_t i;
141
142 /* verify arguments */
143 if(HBcons==NULL || em==NULL || SDO==NULL || HBconsTime==NULL ||
144 monitoredNodes==NULL || CANdevRx==NULL){
145 return CO_ERROR_ILLEGAL_ARGUMENT;
146 }
147
148 /* Configure object variables */
149 HBcons->em = em;
150 HBcons->HBconsTime = HBconsTime;
151 HBcons->monitoredNodes = monitoredNodes;
152 HBcons->numberOfMonitoredNodes = numberOfMonitoredNodes;
153 HBcons->allMonitoredOperational = 0;
154 HBcons->CANdevRx = CANdevRx;
155 HBcons->CANdevRxIdxStart = CANdevRxIdxStart;
156
157 for(i=0; i<HBcons->numberOfMonitoredNodes; i++) {
158 uint8_t nodeId = (HBcons->HBconsTime[i] >> 16U) & 0xFFU;
159 uint16_t time = HBcons->HBconsTime[i] & 0xFFFFU;
160 CO_HBconsumer_initEntry(HBcons, i, nodeId, time);
161 }
162
163 /* Configure Object dictionary entry at index 0x1016 */
164 CO_OD_configure(SDO, OD_H1016_CONSUMER_HB_TIME, CO_ODF_1016, (void*)HBcons, 0, 0);
165
166 return CO_ERROR_NO;
167 }
168
169
170 /******************************************************************************/
CO_HBconsumer_initEntry(CO_HBconsumer_t * HBcons,uint8_t idx,uint8_t nodeId,uint16_t consumerTime)171 CO_ReturnError_t CO_HBconsumer_initEntry(
172 CO_HBconsumer_t *HBcons,
173 uint8_t idx,
174 uint8_t nodeId,
175 uint16_t consumerTime)
176 {
177 CO_ReturnError_t ret = CO_ERROR_NO;
178
179 /* verify arguments */
180 if(HBcons==NULL){
181 return CO_ERROR_ILLEGAL_ARGUMENT;
182 }
183
184 if((consumerTime != 0) && (nodeId != 0)){
185 uint8_t i;
186 /* there must not be more entries with same index and time different than zero */
187 for(i = 0U; i<HBcons->numberOfMonitoredNodes; i++){
188 uint32_t objectCopy = HBcons->HBconsTime[i];
189 uint8_t NodeIDObj = (objectCopy >> 16U) & 0xFFU;
190 uint16_t HBconsTimeObj = objectCopy & 0xFFFFU;
191 if((idx != i) && (HBconsTimeObj != 0) && (nodeId == NodeIDObj)){
192 ret = CO_ERROR_ILLEGAL_ARGUMENT;
193 }
194 }
195 }
196
197 /* Configure */
198 if(ret == CO_ERROR_NO){
199 CO_HBcons_monitoredNodeConfig(HBcons, idx, nodeId, consumerTime);
200 }
201 return ret;
202 }
203
204
205 /******************************************************************************/
CO_HBconsumer_initCallbackHeartbeatStarted(CO_HBconsumer_t * HBcons,uint8_t idx,void * object,void (* pFunctSignal)(uint8_t nodeId,uint8_t idx,void * object))206 void CO_HBconsumer_initCallbackHeartbeatStarted(
207 CO_HBconsumer_t *HBcons,
208 uint8_t idx,
209 void *object,
210 void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
211 {
212 CO_HBconsNode_t *monitoredNode;
213
214 if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
215 return;
216 }
217
218 monitoredNode = &HBcons->monitoredNodes[idx];
219 monitoredNode->pFunctSignalHbStarted = pFunctSignal;
220 monitoredNode->functSignalObjectHbStarted = object;
221 }
222
223
224 /******************************************************************************/
CO_HBconsumer_initCallbackTimeout(CO_HBconsumer_t * HBcons,uint8_t idx,void * object,void (* pFunctSignal)(uint8_t nodeId,uint8_t idx,void * object))225 void CO_HBconsumer_initCallbackTimeout(
226 CO_HBconsumer_t *HBcons,
227 uint8_t idx,
228 void *object,
229 void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
230 {
231 CO_HBconsNode_t *monitoredNode;
232
233 if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
234 return;
235 }
236
237 monitoredNode = &HBcons->monitoredNodes[idx];
238 monitoredNode->pFunctSignalTimeout = pFunctSignal;
239 monitoredNode->functSignalObjectTimeout = object;
240 }
241
242
243 /******************************************************************************/
CO_HBconsumer_initCallbackRemoteReset(CO_HBconsumer_t * HBcons,uint8_t idx,void * object,void (* pFunctSignal)(uint8_t nodeId,uint8_t idx,void * object))244 void CO_HBconsumer_initCallbackRemoteReset(
245 CO_HBconsumer_t *HBcons,
246 uint8_t idx,
247 void *object,
248 void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
249 {
250 CO_HBconsNode_t *monitoredNode;
251
252 if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
253 return;
254 }
255
256 monitoredNode = &HBcons->monitoredNodes[idx];
257 monitoredNode->pFunctSignalRemoteReset = pFunctSignal;
258 monitoredNode->functSignalObjectRemoteReset = object;
259 }
260
261 /******************************************************************************/
CO_HBconsumer_process(CO_HBconsumer_t * HBcons,bool_t NMTisPreOrOperational,uint16_t timeDifference_ms)262 void CO_HBconsumer_process(
263 CO_HBconsumer_t *HBcons,
264 bool_t NMTisPreOrOperational,
265 uint16_t timeDifference_ms)
266 {
267 uint8_t i;
268 uint8_t emcyHeartbeatTimeoutActive = 0;
269 uint8_t emcyRemoteResetActive = 0;
270 uint8_t AllMonitoredOperationalCopy;
271 CO_HBconsNode_t *monitoredNode;
272
273 AllMonitoredOperationalCopy = 5;
274 monitoredNode = &HBcons->monitoredNodes[0];
275
276 if(NMTisPreOrOperational){
277 for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
278 uint16_t timeDifference_ms_copy = timeDifference_ms;
279 if(monitoredNode->time > 0){/* is node monitored */
280 /* Verify if received message is heartbeat or bootup */
281 if(IS_CANrxNew(monitoredNode->CANrxNew)){
282 if(monitoredNode->NMTstate == CO_NMT_INITIALIZING){
283 /* bootup message, call callback */
284 if (monitoredNode->pFunctSignalRemoteReset != NULL) {
285 monitoredNode->pFunctSignalRemoteReset(monitoredNode->nodeId, i,
286 monitoredNode->functSignalObjectRemoteReset);
287 }
288 }
289 else {
290 /* heartbeat message */
291 if (monitoredNode->HBstate!=CO_HBconsumer_ACTIVE &&
292 monitoredNode->pFunctSignalHbStarted!=NULL) {
293 monitoredNode->pFunctSignalHbStarted(monitoredNode->nodeId, i,
294 monitoredNode->functSignalObjectHbStarted);
295 }
296 monitoredNode->HBstate = CO_HBconsumer_ACTIVE;
297 monitoredNode->timeoutTimer = 0; /* reset timer */
298 timeDifference_ms_copy = 0;
299 }
300 CLEAR_CANrxNew(monitoredNode->CANrxNew);
301 }
302
303 /* Verify timeout */
304 if(monitoredNode->timeoutTimer < monitoredNode->time) {
305 monitoredNode->timeoutTimer += timeDifference_ms_copy;
306 }
307 if(monitoredNode->HBstate!=CO_HBconsumer_UNCONFIGURED &&
308 monitoredNode->HBstate!=CO_HBconsumer_UNKNOWN) {
309 if(monitoredNode->timeoutTimer >= monitoredNode->time){
310 /* timeout expired */
311 CO_errorReport(HBcons->em, CO_EM_HEARTBEAT_CONSUMER, CO_EMC_HEARTBEAT, i);
312 emcyHeartbeatTimeoutActive = 1;
313
314 monitoredNode->NMTstate = CO_NMT_INITIALIZING;
315 if (monitoredNode->HBstate!=CO_HBconsumer_TIMEOUT &&
316 monitoredNode->pFunctSignalTimeout!=NULL) {
317 monitoredNode->pFunctSignalTimeout(monitoredNode->nodeId, i,
318 monitoredNode->functSignalObjectTimeout);
319 }
320 monitoredNode->HBstate = CO_HBconsumer_TIMEOUT;
321 }
322 else if(monitoredNode->NMTstate == CO_NMT_INITIALIZING){
323 /* there was a bootup message */
324 CO_errorReport(HBcons->em, CO_EM_HB_CONSUMER_REMOTE_RESET, CO_EMC_HEARTBEAT, i);
325 emcyRemoteResetActive = 1;
326
327 monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
328 }
329 }
330 if(monitoredNode->NMTstate != CO_NMT_OPERATIONAL) {
331 AllMonitoredOperationalCopy = 0;
332 }
333 }
334 monitoredNode++;
335 }
336 }
337 else{ /* not in (pre)operational state */
338 for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
339 monitoredNode->NMTstate = CO_NMT_INITIALIZING;
340 CLEAR_CANrxNew(monitoredNode->CANrxNew);
341 if(monitoredNode->HBstate != CO_HBconsumer_UNCONFIGURED){
342 monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
343 }
344 monitoredNode++;
345 }
346 AllMonitoredOperationalCopy = 0;
347 }
348 /* clear emergencies. We only have one emergency index for all
349 * monitored nodes! */
350 if ( ! emcyHeartbeatTimeoutActive) {
351 CO_errorReset(HBcons->em, CO_EM_HEARTBEAT_CONSUMER, 0);
352 }
353 if ( ! emcyRemoteResetActive) {
354 CO_errorReset(HBcons->em, CO_EM_HB_CONSUMER_REMOTE_RESET, 0);
355 }
356
357 HBcons->allMonitoredOperational = AllMonitoredOperationalCopy;
358 }
359
360
361 /******************************************************************************/
CO_HBconsumer_getIdxByNodeId(CO_HBconsumer_t * HBcons,uint8_t nodeId)362 int8_t CO_HBconsumer_getIdxByNodeId(
363 CO_HBconsumer_t *HBcons,
364 uint8_t nodeId)
365 {
366 uint8_t i;
367 CO_HBconsNode_t *monitoredNode;
368
369 if (HBcons == NULL) {
370 return -1;
371 }
372
373 /* linear search for the node */
374 monitoredNode = &HBcons->monitoredNodes[0];
375 for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
376 if (monitoredNode->nodeId == nodeId) {
377 return i;
378 }
379 monitoredNode ++;
380 }
381 /* not found */
382 return -1;
383 }
384
385
386 /******************************************************************************/
CO_HBconsumer_getState(CO_HBconsumer_t * HBcons,uint8_t idx)387 CO_HBconsumer_state_t CO_HBconsumer_getState(
388 CO_HBconsumer_t *HBcons,
389 uint8_t idx)
390 {
391 CO_HBconsNode_t *monitoredNode;
392
393 if (HBcons==NULL || idx>=HBcons->numberOfMonitoredNodes) {
394 return CO_HBconsumer_UNCONFIGURED;
395 }
396
397 monitoredNode = &HBcons->monitoredNodes[idx];
398 return monitoredNode->HBstate;
399 }
400
401 /******************************************************************************/
CO_HBconsumer_getNmtState(CO_HBconsumer_t * HBcons,uint8_t idx,CO_NMT_internalState_t * nmtState)402 int8_t CO_HBconsumer_getNmtState(
403 CO_HBconsumer_t *HBcons,
404 uint8_t idx,
405 CO_NMT_internalState_t *nmtState)
406 {
407 CO_HBconsNode_t *monitoredNode;
408
409 if (HBcons==NULL || nmtState==NULL || idx>=HBcons->numberOfMonitoredNodes) {
410 return -1;
411 }
412 *nmtState = CO_NMT_INITIALIZING;
413
414 monitoredNode = &HBcons->monitoredNodes[idx];
415
416 if (monitoredNode->HBstate == CO_HBconsumer_ACTIVE) {
417 *nmtState = monitoredNode->NMTstate;
418 return 0;
419 }
420 return -1;
421 }
422
