1 /*
2 * CANopen Emergency object.
3 *
4 * @file CO_Emergency.c
5 * @ingroup CO_Emergency
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
27 #include "CO_driver.h"
28 #include "CO_SDO.h"
29 #include "CO_Emergency.h"
30 #include "CANopen.h"
31
32
33 /*
34 * Read received message from CAN module.
35 *
36 * Function will be called (by CAN receive interrupt) every time, when CAN
37 * message with correct identifier will be received. For more information and
38 * description of parameters see file CO_driver.h.
39 */
CO_EM_receive(void * object,const CO_CANrxMsg_t * msg)40 static void CO_EM_receive(void *object, const CO_CANrxMsg_t *msg){
41 CO_EM_t *em;
42 uint16_t errorCode;
43 uint32_t infoCode;
44
45 em = (CO_EM_t*)object;
46
47 if(em!=NULL && em->pFunctSignalRx!=NULL){
48 CO_memcpySwap2(&errorCode, &msg->data[0]);
49 CO_memcpySwap4(&infoCode, &msg->data[4]);
50 em->pFunctSignalRx(CO_CANrxMsg_readIdent(msg),
51 errorCode,
52 msg->data[2],
53 msg->data[3],
54 infoCode);
55 }
56 }
57
58 /*
59 * Function for accessing _Pre-Defined Error Field_ (index 0x1003) from SDO server.
60 *
61 * For more information see file CO_SDO.h.
62 */
63 static CO_SDO_abortCode_t CO_ODF_1003(CO_ODF_arg_t *ODF_arg);
CO_ODF_1003(CO_ODF_arg_t * ODF_arg)64 static CO_SDO_abortCode_t CO_ODF_1003(CO_ODF_arg_t *ODF_arg){
65 CO_EMpr_t *emPr;
66 uint8_t value;
67 CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
68
69 emPr = (CO_EMpr_t*) ODF_arg->object;
70 value = ODF_arg->data[0];
71
72 if(ODF_arg->reading){
73 uint8_t noOfErrors;
74 noOfErrors = emPr->preDefErrNoOfErrors;
75
76 if(ODF_arg->subIndex == 0U){
77 ODF_arg->data[0] = noOfErrors;
78 }
79 else if(ODF_arg->subIndex > noOfErrors){
80 ret = CO_SDO_AB_NO_DATA;
81 }
82 else{
83 ret = CO_SDO_AB_NONE;
84 }
85 }
86 else{
87 /* only '0' may be written to subIndex 0 */
88 if(ODF_arg->subIndex == 0U){
89 if(value == 0U){
90 emPr->preDefErrNoOfErrors = 0U;
91 }
92 else{
93 ret = CO_SDO_AB_INVALID_VALUE;
94 }
95 }
96 else{
97 ret = CO_SDO_AB_READONLY;
98 }
99 }
100
101 return ret;
102 }
103
104
105 /*
106 * Function for accessing _COB ID EMCY_ (index 0x1014) from SDO server.
107 *
108 * For more information see file CO_SDO.h.
109 */
110 static CO_SDO_abortCode_t CO_ODF_1014(CO_ODF_arg_t *ODF_arg);
CO_ODF_1014(CO_ODF_arg_t * ODF_arg)111 static CO_SDO_abortCode_t CO_ODF_1014(CO_ODF_arg_t *ODF_arg){
112 uint8_t *nodeId;
113 uint32_t value;
114 CO_SDO_abortCode_t ret = CO_SDO_AB_NONE;
115
116 nodeId = (uint8_t*) ODF_arg->object;
117 value = CO_getUint32(ODF_arg->data);
118
119 /* add nodeId to the value */
120 if(ODF_arg->reading){
121 CO_setUint32(ODF_arg->data, value + *nodeId);
122 }
123
124 return ret;
125 }
126
127
128 /******************************************************************************/
CO_EM_init(CO_EM_t * em,CO_EMpr_t * emPr,CO_SDO_t * SDO,uint8_t * errorStatusBits,uint8_t errorStatusBitsSize,uint8_t * errorRegister,uint32_t * preDefErr,uint8_t preDefErrSize,CO_CANmodule_t * CANdevRx,uint16_t CANdevRxIdx,CO_CANmodule_t * CANdevTx,uint16_t CANdevTxIdx,uint16_t CANidTxEM)129 CO_ReturnError_t CO_EM_init(
130 CO_EM_t *em,
131 CO_EMpr_t *emPr,
132 CO_SDO_t *SDO,
133 uint8_t *errorStatusBits,
134 uint8_t errorStatusBitsSize,
135 uint8_t *errorRegister,
136 uint32_t *preDefErr,
137 uint8_t preDefErrSize,
138 CO_CANmodule_t *CANdevRx,
139 uint16_t CANdevRxIdx,
140 CO_CANmodule_t *CANdevTx,
141 uint16_t CANdevTxIdx,
142 uint16_t CANidTxEM)
143 {
144 uint8_t i;
145
146 /* verify arguments */
147 if(em==NULL || emPr==NULL || SDO==NULL || errorStatusBits==NULL || errorStatusBitsSize<6U ||
148 errorRegister==NULL || preDefErr==NULL || CANdevTx==NULL || CANdevRx==NULL){
149 return CO_ERROR_ILLEGAL_ARGUMENT;
150 }
151
152 /* Configure object variables */
153 em->errorStatusBits = errorStatusBits;
154 em->errorStatusBitsSize = errorStatusBitsSize;
155 em->bufEnd = em->buf + (CO_EM_INTERNAL_BUFFER_SIZE * 8);
156 em->bufWritePtr = em->buf;
157 em->bufReadPtr = em->buf;
158 em->bufFull = 0U;
159 em->wrongErrorReport = 0U;
160 em->pFunctSignal = NULL;
161 em->pFunctSignalRx = NULL;
162 emPr->em = em;
163 emPr->errorRegister = errorRegister;
164 emPr->preDefErr = preDefErr;
165 emPr->preDefErrSize = preDefErrSize;
166 emPr->preDefErrNoOfErrors = 0U;
167 emPr->inhibitEmTimer = 0U;
168
169 /* clear error status bits */
170 for(i=0U; i<errorStatusBitsSize; i++){
171 em->errorStatusBits[i] = 0U;
172 }
173
174 /* Configure Object dictionary entry at index 0x1003 and 0x1014 */
175 CO_OD_configure(SDO, OD_H1003_PREDEF_ERR_FIELD, CO_ODF_1003, (void*)emPr, 0, 0U);
176 CO_OD_configure(SDO, OD_H1014_COBID_EMERGENCY, CO_ODF_1014, (void*)&SDO->nodeId, 0, 0U);
177
178 /* configure SDO server CAN reception */
179 CO_CANrxBufferInit(
180 CANdevRx, /* CAN device */
181 CANdevRxIdx, /* rx buffer index */
182 CO_CAN_ID_EMERGENCY, /* CAN identifier */
183 0x780, /* mask */
184 0, /* rtr */
185 (void*)em, /* object passed to receive function */
186 CO_EM_receive); /* this function will process received message */
187
188 /* configure emergency message CAN transmission */
189 emPr->CANdev = CANdevTx;
190 emPr->CANdev->em = (void*)em; /* update pointer inside CAN device. */
191 emPr->CANtxBuff = CO_CANtxBufferInit(
192 CANdevTx, /* CAN device */
193 CANdevTxIdx, /* index of specific buffer inside CAN module */
194 CANidTxEM, /* CAN identifier */
195 0, /* rtr */
196 8U, /* number of data bytes */
197 0); /* synchronous message flag bit */
198
199 return CO_ERROR_NO;
200 }
201
202
203 /******************************************************************************/
CO_EM_initCallback(CO_EM_t * em,void (* pFunctSignal)(void))204 void CO_EM_initCallback(
205 CO_EM_t *em,
206 void (*pFunctSignal)(void))
207 {
208 if(em != NULL){
209 em->pFunctSignal = pFunctSignal;
210 }
211 }
212
213
214 /******************************************************************************/
CO_EM_initCallbackRx(CO_EM_t * em,void (* pFunctSignalRx)(const uint16_t ident,const uint16_t errorCode,const uint8_t errorRegister,const uint8_t errorBit,const uint32_t infoCode))215 void CO_EM_initCallbackRx(
216 CO_EM_t *em,
217 void (*pFunctSignalRx)(const uint16_t ident,
218 const uint16_t errorCode,
219 const uint8_t errorRegister,
220 const uint8_t errorBit,
221 const uint32_t infoCode))
222 {
223 if(em != NULL){
224 em->pFunctSignalRx = pFunctSignalRx;
225 }
226 }
227
228
229 /******************************************************************************/
CO_EM_process(CO_EMpr_t * emPr,bool_t NMTisPreOrOperational,uint16_t timeDifference_100us,uint16_t emInhTime,uint16_t * timerNext_ms)230 void CO_EM_process(
231 CO_EMpr_t *emPr,
232 bool_t NMTisPreOrOperational,
233 uint16_t timeDifference_100us,
234 uint16_t emInhTime,
235 uint16_t *timerNext_ms)
236 {
237
238 CO_EM_t *em = emPr->em;
239 uint8_t errorRegister;
240 uint8_t errorMask;
241 uint8_t i;
242
243 /* verify errors from driver and other */
244 CO_CANverifyErrors(emPr->CANdev);
245 if(em->wrongErrorReport != 0U){
246 CO_errorReport(em, CO_EM_WRONG_ERROR_REPORT, CO_EMC_SOFTWARE_INTERNAL, (uint32_t)em->wrongErrorReport);
247 em->wrongErrorReport = 0U;
248 }
249
250
251 /* calculate Error register */
252 errorRegister = 0U;
253 errorMask = (uint8_t)~(CO_ERR_REG_GENERIC_ERR | CO_ERR_REG_COMM_ERR | CO_ERR_REG_MANUFACTURER);
254 /* generic error */
255 if(em->errorStatusBits[5]){
256 errorRegister |= CO_ERR_REG_GENERIC_ERR;
257 }
258 /* communication error (overrun, error state) */
259 if(em->errorStatusBits[2] || em->errorStatusBits[3]){
260 errorRegister |= CO_ERR_REG_COMM_ERR;
261 }
262 /* Manufacturer */
263 for(i=6; i<em->errorStatusBitsSize; i++) {
264 if (em->errorStatusBits[i]) {
265 errorRegister |= CO_ERR_REG_MANUFACTURER;
266 }
267 }
268 *emPr->errorRegister = (*emPr->errorRegister & errorMask) | errorRegister;
269
270 /* inhibit time */
271 if(emPr->inhibitEmTimer < emInhTime){
272 emPr->inhibitEmTimer += timeDifference_100us;
273 }
274
275 /* send Emergency message. */
276 if( NMTisPreOrOperational &&
277 !emPr->CANtxBuff->bufferFull &&
278 (em->bufReadPtr != em->bufWritePtr || em->bufFull))
279 {
280 uint32_t preDEF; /* preDefinedErrorField */
281 uint16_t diff;
282
283 if (emPr->inhibitEmTimer >= emInhTime) {
284 /* inhibit time elapsed, send message */
285
286 /* add error register */
287 em->bufReadPtr[2] = *emPr->errorRegister;
288
289 /* copy data to CAN emergency message */
290 CO_memcpy(emPr->CANtxBuff->data, em->bufReadPtr, 8U);
291 CO_memcpy((uint8_t*)&preDEF, em->bufReadPtr, 4U);
292 em->bufReadPtr += 8;
293
294 /* Update read buffer pointer and reset inhibit timer */
295 if(em->bufReadPtr == em->bufEnd){
296 em->bufReadPtr = em->buf;
297 }
298 emPr->inhibitEmTimer = 0U;
299
300 /* verify message buffer overflow, then clear full flag */
301 if(em->bufFull == 2U){
302 em->bufFull = 0U; /* will be updated below */
303 CO_errorReport(em, CO_EM_EMERGENCY_BUFFER_FULL, CO_EMC_GENERIC, 0U);
304 }
305 else{
306 em->bufFull = 0;
307 CO_errorReset(em, CO_EM_EMERGENCY_BUFFER_FULL, 0);
308 }
309
310 /* write to 'pre-defined error field' (object dictionary, index 0x1003) */
311 if(emPr->preDefErr){
312 uint8_t j;
313
314 if(emPr->preDefErrNoOfErrors < emPr->preDefErrSize)
315 emPr->preDefErrNoOfErrors++;
316 for(j=emPr->preDefErrNoOfErrors-1; j>0; j--)
317 emPr->preDefErr[j] = emPr->preDefErr[j-1];
318 emPr->preDefErr[0] = preDEF;
319 }
320
321 /* send CAN message */
322 CO_CANsend(emPr->CANdev, emPr->CANtxBuff);
323 }
324
325 /* check again after inhibit time elapsed */
326 diff = (emInhTime + 9) / 10; /* time difference in ms, always round up */
327 if (timerNext_ms != NULL && *timerNext_ms > diff) {
328 *timerNext_ms = diff;
329 }
330 }
331
332 return;
333 }
334
335
336 /******************************************************************************/
CO_errorReport(CO_EM_t * em,const uint8_t errorBit,const uint16_t errorCode,const uint32_t infoCode)337 void CO_errorReport(CO_EM_t *em, const uint8_t errorBit, const uint16_t errorCode, const uint32_t infoCode){
338 uint8_t index = errorBit >> 3;
339 uint8_t bitmask = 1 << (errorBit & 0x7);
340 uint8_t *errorStatusBits = 0;
341 bool_t sendEmergency = true;
342
343 if(em == NULL){
344 sendEmergency = false;
345 }
346 else if(index >= em->errorStatusBitsSize){
347 /* if errorBit value not supported, send emergency 'CO_EM_WRONG_ERROR_REPORT' */
348 em->wrongErrorReport = errorBit;
349 sendEmergency = false;
350 }
351 else{
352 errorStatusBits = &em->errorStatusBits[index];
353 /* if error was already reported, do nothing */
354 if((*errorStatusBits & bitmask) != 0){
355 sendEmergency = false;
356 }
357 }
358
359 if(sendEmergency){
360 /* set error bit */
361 if(errorBit){
362 /* any error except NO_ERROR */
363 *errorStatusBits |= bitmask;
364 }
365
366 /* verify buffer full, set overflow */
367 if(em->bufFull){
368 em->bufFull = 2;
369 }
370 else{
371 uint8_t bufCopy[8];
372
373 /* prepare data for emergency message */
374 CO_memcpySwap2(&bufCopy[0], &errorCode);
375 bufCopy[2] = 0; /* error register will be set later */
376 bufCopy[3] = errorBit;
377 CO_memcpySwap4(&bufCopy[4], &infoCode);
378
379 /* copy data to the buffer, increment writePtr and verify buffer full */
380 CO_LOCK_EMCY();
381 CO_memcpy(em->bufWritePtr, &bufCopy[0], 8);
382 em->bufWritePtr += 8;
383
384 if(em->bufWritePtr == em->bufEnd) em->bufWritePtr = em->buf;
385 if(em->bufWritePtr == em->bufReadPtr) em->bufFull = 1;
386 CO_UNLOCK_EMCY();
387
388 /* Optional signal to RTOS, which can resume task, which handles CO_EM_process */
389 if(em->pFunctSignal != NULL) {
390 em->pFunctSignal();
391 }
392 }
393 }
394 }
395
396
397 /******************************************************************************/
CO_errorReset(CO_EM_t * em,const uint8_t errorBit,const uint32_t infoCode)398 void CO_errorReset(CO_EM_t *em, const uint8_t errorBit, const uint32_t infoCode){
399 uint8_t index = errorBit >> 3;
400 uint8_t bitmask = 1 << (errorBit & 0x7);
401 uint8_t *errorStatusBits = 0;
402 bool_t sendEmergency = true;
403
404 if(em == NULL){
405 sendEmergency = false;
406 }
407 else if(index >= em->errorStatusBitsSize){
408 /* if errorBit value not supported, send emergency 'CO_EM_WRONG_ERROR_REPORT' */
409 em->wrongErrorReport = errorBit;
410 sendEmergency = false;
411 }
412 else{
413 errorStatusBits = &em->errorStatusBits[index];
414 /* if error was allready cleared, do nothing */
415 if((*errorStatusBits & bitmask) == 0){
416 sendEmergency = false;
417 }
418 }
419
420 if(sendEmergency){
421 /* erase error bit */
422 *errorStatusBits &= ~bitmask;
423
424 /* verify buffer full */
425 if(em->bufFull){
426 em->bufFull = 2;
427 }
428 else{
429 uint8_t bufCopy[8];
430
431 /* prepare data for emergency message */
432 bufCopy[0] = 0;
433 bufCopy[1] = 0;
434 bufCopy[2] = 0; /* error register will be set later */
435 bufCopy[3] = errorBit;
436 CO_memcpySwap4(&bufCopy[4], &infoCode);
437
438 /* copy data to the buffer, increment writePtr and verify buffer full */
439 CO_LOCK_EMCY();
440 CO_memcpy(em->bufWritePtr, &bufCopy[0], 8);
441 em->bufWritePtr += 8;
442
443 if(em->bufWritePtr == em->bufEnd) em->bufWritePtr = em->buf;
444 if(em->bufWritePtr == em->bufReadPtr) em->bufFull = 1;
445 CO_UNLOCK_EMCY();
446
447 /* Optional signal to RTOS, which can resume task, which handles CO_EM_process */
448 if(em->pFunctSignal != NULL) {
449 em->pFunctSignal();
450 }
451 }
452 }
453 }
454
455
456 /******************************************************************************/
CO_isError(CO_EM_t * em,const uint8_t errorBit)457 bool_t CO_isError(CO_EM_t *em, const uint8_t errorBit){
458 uint8_t index = errorBit >> 3;
459 uint8_t bitmask = 1 << (errorBit & 0x7);
460 bool_t ret = false;
461
462 if(em != NULL && index < em->errorStatusBitsSize){
463 if((em->errorStatusBits[index] & bitmask) != 0){
464 ret = true;
465 }
466 }
467
468 return ret;
469 }
470
