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 if(monitoredNode->time > 0){/* is node monitored */
279 /* Verify if received message is heartbeat or bootup */
280 if(IS_CANrxNew(monitoredNode->CANrxNew)){
281 if(monitoredNode->NMTstate == CO_NMT_INITIALIZING){
282 /* bootup message, call callback */
283 if (monitoredNode->pFunctSignalRemoteReset != NULL) {
284 monitoredNode->pFunctSignalRemoteReset(monitoredNode->nodeId, i,
285 monitoredNode->functSignalObjectRemoteReset);
286 }
287 }
288 else {
289 /* heartbeat message */
290 if (monitoredNode->HBstate!=CO_HBconsumer_ACTIVE &&
291 monitoredNode->pFunctSignalHbStarted!=NULL) {
292 monitoredNode->pFunctSignalHbStarted(monitoredNode->nodeId, i,
293 monitoredNode->functSignalObjectHbStarted);
294 }
295 monitoredNode->HBstate = CO_HBconsumer_ACTIVE;
296 monitoredNode->timeoutTimer = 0; /* reset timer */
297 timeDifference_ms = 0;
298 }
299 CLEAR_CANrxNew(monitoredNode->CANrxNew);
300 }
301
302 /* Verify timeout */
303 if(monitoredNode->timeoutTimer < monitoredNode->time) {
304 monitoredNode->timeoutTimer += timeDifference_ms;
305 }
306 if(monitoredNode->HBstate!=CO_HBconsumer_UNCONFIGURED &&
307 monitoredNode->HBstate!=CO_HBconsumer_UNKNOWN) {
308 if(monitoredNode->timeoutTimer >= monitoredNode->time){
309 /* timeout expired */
310 CO_errorReport(HBcons->em, CO_EM_HEARTBEAT_CONSUMER, CO_EMC_HEARTBEAT, i);
311 emcyHeartbeatTimeoutActive = 1;
312
313 monitoredNode->NMTstate = CO_NMT_INITIALIZING;
314 if (monitoredNode->HBstate!=CO_HBconsumer_TIMEOUT &&
315 monitoredNode->pFunctSignalTimeout!=NULL) {
316 monitoredNode->pFunctSignalTimeout(monitoredNode->nodeId, i,
317 monitoredNode->functSignalObjectTimeout);
318 }
319 monitoredNode->HBstate = CO_HBconsumer_TIMEOUT;
320 }
321 else if(monitoredNode->NMTstate == CO_NMT_INITIALIZING){
322 /* there was a bootup message */
323 CO_errorReport(HBcons->em, CO_EM_HB_CONSUMER_REMOTE_RESET, CO_EMC_HEARTBEAT, i);
324 emcyRemoteResetActive = 1;
325
326 monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
327 }
328 }
329 if(monitoredNode->NMTstate != CO_NMT_OPERATIONAL) {
330 AllMonitoredOperationalCopy = 0;
331 }
332 }
333 monitoredNode++;
334 }
335 }
336 else{ /* not in (pre)operational state */
337 for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
338 monitoredNode->NMTstate = CO_NMT_INITIALIZING;
339 CLEAR_CANrxNew(monitoredNode->CANrxNew);
340 if(monitoredNode->HBstate != CO_HBconsumer_UNCONFIGURED){
341 monitoredNode->HBstate = CO_HBconsumer_UNKNOWN;
342 }
343 monitoredNode++;
344 }
345 AllMonitoredOperationalCopy = 0;
346 }
347 /* clear emergencies. We only have one emergency index for all
348 * monitored nodes! */
349 if ( ! emcyHeartbeatTimeoutActive) {
350 CO_errorReset(HBcons->em, CO_EM_HEARTBEAT_CONSUMER, 0);
351 }
352 if ( ! emcyRemoteResetActive) {
353 CO_errorReset(HBcons->em, CO_EM_HB_CONSUMER_REMOTE_RESET, 0);
354 }
355
356 HBcons->allMonitoredOperational = AllMonitoredOperationalCopy;
357 }
358
359
360 /******************************************************************************/
CO_HBconsumer_getIdxByNodeId(CO_HBconsumer_t * HBcons,uint8_t nodeId)361 int8_t CO_HBconsumer_getIdxByNodeId(
362 CO_HBconsumer_t *HBcons,
363 uint8_t nodeId)
364 {
365 uint8_t i;
366 CO_HBconsNode_t *monitoredNode;
367
368 if (HBcons == NULL) {
369 return -1;
370 }
371
372 /* linear search for the node */
373 monitoredNode = &HBcons->monitoredNodes[0];
374 for(i=0; i<HBcons->numberOfMonitoredNodes; i++){
375 if (monitoredNode->nodeId == nodeId) {
376 return i;
377 }
378 monitoredNode ++;
379 }
380 /* not found */
381 return -1;
382 }
383
384
385 /******************************************************************************/
CO_HBconsumer_getState(CO_HBconsumer_t * HBcons,uint8_t idx)386 CO_HBconsumer_state_t CO_HBconsumer_getState(
387 CO_HBconsumer_t *HBcons,
388 uint8_t idx)
389 {
390 CO_HBconsNode_t *monitoredNode;
391
392 if (HBcons==NULL || idx>HBcons->numberOfMonitoredNodes) {
393 return CO_HBconsumer_UNCONFIGURED;
394 }
395
396 monitoredNode = &HBcons->monitoredNodes[idx];
397 return monitoredNode->HBstate;
398 }
399
400 /******************************************************************************/
CO_HBconsumer_getNmtState(CO_HBconsumer_t * HBcons,uint8_t idx,CO_NMT_internalState_t * nmtState)401 int8_t CO_HBconsumer_getNmtState(
402 CO_HBconsumer_t *HBcons,
403 uint8_t idx,
404 CO_NMT_internalState_t *nmtState)
405 {
406 CO_HBconsNode_t *monitoredNode;
407
408 if (HBcons==NULL || nmtState==NULL || idx>HBcons->numberOfMonitoredNodes) {
409 return -1;
410 }
411 *nmtState = CO_NMT_INITIALIZING;
412
413 monitoredNode = &HBcons->monitoredNodes[idx];
414
415 if (monitoredNode->HBstate == CO_HBconsumer_ACTIVE) {
416 *nmtState = monitoredNode->NMTstate;
417 return 0;
418 }
419 return -1;
420 }
421
