1 /*
2 * CANopen Process Data Object.
3 *
4 * @file CO_PDO.c
5 * @ingroup CO_PDO
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 "CO_NMT_Heartbeat.h"
31 #include "CO_SYNC.h"
32 #include "CO_PDO.h"
33
34 /*
35 * Read received message from CAN module.
36 *
37 * Function will be called (by CAN receive interrupt) every time, when CAN
38 * message with correct identifier will be received. For more information and
39 * description of parameters see file CO_driver.h.
40 * If new message arrives and previous message wasn't processed yet, then
41 * previous message will be lost and overwritten by new message. That's OK with PDOs.
42 */
CO_PDO_receive(void * object,const CO_CANrxMsg_t * msg)43 static void CO_PDO_receive(void *object, const CO_CANrxMsg_t *msg){
44 CO_RPDO_t *RPDO;
45
46 RPDO = (CO_RPDO_t*)object; /* this is the correct pointer type of the first argument */
47
48 if( (RPDO->valid) &&
49 (*RPDO->operatingState == CO_NMT_OPERATIONAL) &&
50 (msg->DLC >= RPDO->dataLength))
51 {
52 if(RPDO->SYNC && RPDO->synchronous && RPDO->SYNC->CANrxToggle) {
53 /* copy data into second buffer and set 'new message' flag */
54 RPDO->CANrxData[1][0] = msg->data[0];
55 RPDO->CANrxData[1][1] = msg->data[1];
56 RPDO->CANrxData[1][2] = msg->data[2];
57 RPDO->CANrxData[1][3] = msg->data[3];
58 RPDO->CANrxData[1][4] = msg->data[4];
59 RPDO->CANrxData[1][5] = msg->data[5];
60 RPDO->CANrxData[1][6] = msg->data[6];
61 RPDO->CANrxData[1][7] = msg->data[7];
62
63 SET_CANrxNew(RPDO->CANrxNew[1]);
64 }
65 else {
66 /* copy data into default buffer and set 'new message' flag */
67 RPDO->CANrxData[0][0] = msg->data[0];
68 RPDO->CANrxData[0][1] = msg->data[1];
69 RPDO->CANrxData[0][2] = msg->data[2];
70 RPDO->CANrxData[0][3] = msg->data[3];
71 RPDO->CANrxData[0][4] = msg->data[4];
72 RPDO->CANrxData[0][5] = msg->data[5];
73 RPDO->CANrxData[0][6] = msg->data[6];
74 RPDO->CANrxData[0][7] = msg->data[7];
75
76 SET_CANrxNew(RPDO->CANrxNew[0]);
77 }
78 }
79 }
80
81
82 /*
83 * Configure RPDO Communication parameter.
84 *
85 * Function is called from commuincation reset or when parameter changes.
86 *
87 * Function configures following variable from CO_RPDO_t: _valid_. It also
88 * configures CAN rx buffer. If configuration fails, emergency message is send
89 * and device is not able to enter NMT operational.
90 *
91 * @param RPDO RPDO object.
92 * @param COB_IDUsedByRPDO _RPDO communication parameter_, _COB-ID for PDO_ variable
93 * from Object dictionary (index 0x1400+, subindex 1).
94 */
CO_RPDOconfigCom(CO_RPDO_t * RPDO,uint32_t COB_IDUsedByRPDO)95 static void CO_RPDOconfigCom(CO_RPDO_t* RPDO, uint32_t COB_IDUsedByRPDO){
96 uint16_t ID;
97 CO_ReturnError_t r;
98
99 ID = (uint16_t)COB_IDUsedByRPDO;
100
101 /* is RPDO used? */
102 if((COB_IDUsedByRPDO & 0xBFFFF800L) == 0 && RPDO->dataLength && ID){
103 /* is used default COB-ID? */
104 if(ID == RPDO->defaultCOB_ID) ID += RPDO->nodeId;
105 RPDO->valid = true;
106 RPDO->synchronous = (RPDO->RPDOCommPar->transmissionType <= 240) ? true : false;
107 }
108 else{
109 ID = 0;
110 RPDO->valid = false;
111 CLEAR_CANrxNew(RPDO->CANrxNew[0]);
112 CLEAR_CANrxNew(RPDO->CANrxNew[1]);
113 }
114 r = CO_CANrxBufferInit(
115 RPDO->CANdevRx, /* CAN device */
116 RPDO->CANdevRxIdx, /* rx buffer index */
117 ID, /* CAN identifier */
118 0x7FF, /* mask */
119 0, /* rtr */
120 (void*)RPDO, /* object passed to receive function */
121 CO_PDO_receive); /* this function will process received message */
122 if(r != CO_ERROR_NO){
123 RPDO->valid = false;
124 CLEAR_CANrxNew(RPDO->CANrxNew[0]);
125 CLEAR_CANrxNew(RPDO->CANrxNew[1]);
126 }
127 }
128
129
130 /*
131 * Configure TPDO Communication parameter.
132 *
133 * Function is called from commuincation reset or when parameter changes.
134 *
135 * Function configures following variable from CO_TPDO_t: _valid_. It also
136 * configures CAN tx buffer. If configuration fails, emergency message is send
137 * and device is not able to enter NMT operational.
138 *
139 * @param TPDO TPDO object.
140 * @param COB_IDUsedByTPDO _TPDO communication parameter_, _COB-ID for PDO_ variable
141 * from Object dictionary (index 0x1400+, subindex 1).
142 * @param syncFlag Indicate, if TPDO is synchronous.
143 */
CO_TPDOconfigCom(CO_TPDO_t * TPDO,uint32_t COB_IDUsedByTPDO,uint8_t syncFlag)144 static void CO_TPDOconfigCom(CO_TPDO_t* TPDO, uint32_t COB_IDUsedByTPDO, uint8_t syncFlag){
145 uint16_t ID;
146
147 ID = (uint16_t)COB_IDUsedByTPDO;
148
149 /* is TPDO used? */
150 if((COB_IDUsedByTPDO & 0xBFFFF800L) == 0 && TPDO->dataLength && ID){
151 /* is used default COB-ID? */
152 if(ID == TPDO->defaultCOB_ID) ID += TPDO->nodeId;
153 TPDO->valid = true;
154 }
155 else{
156 ID = 0;
157 TPDO->valid = false;
158 }
159
160 TPDO->CANtxBuff = CO_CANtxBufferInit(
161 TPDO->CANdevTx, /* CAN device */
162 TPDO->CANdevTxIdx, /* index of specific buffer inside CAN module */
163 ID, /* CAN identifier */
164 0, /* rtr */
165 TPDO->dataLength, /* number of data bytes */
166 syncFlag); /* synchronous message flag bit */
167
168 if(TPDO->CANtxBuff == 0){
169 TPDO->valid = false;
170 }
171 }
172
173
174 /*
175 * Find mapped variable in Object Dictionary.
176 *
177 * Function is called from CO_R(T)PDOconfigMap or when mapping parameter changes.
178 *
179 * @param SDO SDO object.
180 * @param map PDO mapping parameter.
181 * @param R_T 0 for RPDO map, 1 for TPDO map.
182 * @param ppData Pointer to returning parameter: pointer to data of mapped variable.
183 * @param pLength Pointer to returning parameter: *add* length of mapped variable.
184 * @param pSendIfCOSFlags Pointer to returning parameter: sendIfCOSFlags variable.
185 * @param pIsMultibyteVar Pointer to returning parameter: true for multibyte variable.
186 *
187 * @return 0 on success, otherwise SDO abort code.
188 */
CO_PDOfindMap(CO_SDO_t * SDO,uint32_t map,uint8_t R_T,uint8_t ** ppData,uint8_t * pLength,uint8_t * pSendIfCOSFlags,uint8_t * pIsMultibyteVar)189 static uint32_t CO_PDOfindMap(
190 CO_SDO_t *SDO,
191 uint32_t map,
192 uint8_t R_T,
193 uint8_t **ppData,
194 uint8_t *pLength,
195 uint8_t *pSendIfCOSFlags,
196 uint8_t *pIsMultibyteVar)
197 {
198 uint16_t entryNo;
199 uint16_t index;
200 uint8_t subIndex;
201 uint8_t dataLen;
202 uint8_t objectLen;
203 uint8_t attr;
204
205 index = (uint16_t)(map>>16);
206 subIndex = (uint8_t)(map>>8);
207 dataLen = (uint8_t) map; /* data length in bits */
208
209 /* data length must be byte aligned */
210 if(dataLen&0x07) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
211
212 dataLen >>= 3; /* new data length is in bytes */
213 *pLength += dataLen;
214
215 /* total PDO length can not be more than 8 bytes */
216 if(*pLength > 8) return CO_SDO_AB_MAP_LEN; /* The number and length of the objects to be mapped would exceed PDO length. */
217
218 /* is there a reference to dummy entries */
219 if(index <=7 && subIndex == 0){
220 static uint32_t dummyTX = 0;
221 static uint32_t dummyRX;
222 uint8_t dummySize = 4;
223
224 if(index<2) dummySize = 0;
225 else if(index==2 || index==5) dummySize = 1;
226 else if(index==3 || index==6) dummySize = 2;
227
228 /* is size of variable big enough for map */
229 if(dummySize < dataLen) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
230
231 /* Data and ODE pointer */
232 if(R_T == 0) *ppData = (uint8_t*) &dummyRX;
233 else *ppData = (uint8_t*) &dummyTX;
234
235 return 0;
236 }
237
238 /* find object in Object Dictionary */
239 entryNo = CO_OD_find(SDO, index);
240
241 /* Does object exist in OD? */
242 if(entryNo == 0xFFFF || subIndex > SDO->OD[entryNo].maxSubIndex)
243 return CO_SDO_AB_NOT_EXIST; /* Object does not exist in the object dictionary. */
244
245 attr = CO_OD_getAttribute(SDO, entryNo, subIndex);
246 /* Is object Mappable for RPDO? */
247 if(R_T==0 && !((attr&CO_ODA_RPDO_MAPABLE) && (attr&CO_ODA_WRITEABLE))) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
248 /* Is object Mappable for TPDO? */
249 if(R_T!=0 && !((attr&CO_ODA_TPDO_MAPABLE) && (attr&CO_ODA_READABLE))) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
250
251 /* is size of variable big enough for map */
252 objectLen = CO_OD_getLength(SDO, entryNo, subIndex);
253 if(objectLen < dataLen) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
254
255 /* mark multibyte variable */
256 *pIsMultibyteVar = (attr&CO_ODA_MB_VALUE) ? 1 : 0;
257
258 /* pointer to data */
259 *ppData = (uint8_t*) CO_OD_getDataPointer(SDO, entryNo, subIndex);
260 #ifdef CO_BIG_ENDIAN
261 /* skip unused MSB bytes */
262 if(*pIsMultibyteVar){
263 *ppData += objectLen - dataLen;
264 }
265 #endif
266
267 /* setup change of state flags */
268 if(attr&CO_ODA_TPDO_DETECT_COS){
269 int16_t i;
270 for(i=*pLength-dataLen; i<*pLength; i++){
271 *pSendIfCOSFlags |= 1<<i;
272 }
273 }
274
275 return 0;
276 }
277
278
279 /*
280 * Configure RPDO Mapping parameter.
281 *
282 * Function is called from communication reset or when parameter changes.
283 *
284 * Function configures following variables from CO_RPDO_t: _dataLength_ and
285 * _mapPointer_.
286 *
287 * @param RPDO RPDO object.
288 * @param noOfMappedObjects Number of mapped object (from OD).
289 *
290 * @return 0 on success, otherwise SDO abort code.
291 */
CO_RPDOconfigMap(CO_RPDO_t * RPDO,uint8_t noOfMappedObjects)292 static uint32_t CO_RPDOconfigMap(CO_RPDO_t* RPDO, uint8_t noOfMappedObjects){
293 int16_t i;
294 uint8_t length = 0;
295 uint32_t ret = 0;
296 const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1;
297
298 for(i=noOfMappedObjects; i>0; i--){
299 int16_t j;
300 uint8_t* pData;
301 uint8_t dummy = 0;
302 uint8_t prevLength = length;
303 uint8_t MBvar;
304 uint32_t map = *(pMap++);
305
306 /* function do much checking of errors in map */
307 ret = CO_PDOfindMap(
308 RPDO->SDO,
309 map,
310 0,
311 &pData,
312 &length,
313 &dummy,
314 &MBvar);
315 if(ret){
316 length = 0;
317 CO_errorReport(RPDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map);
318 break;
319 }
320
321 /* write PDO data pointers */
322 #ifdef CO_BIG_ENDIAN
323 if(MBvar){
324 for(j=length-1; j>=prevLength; j--)
325 RPDO->mapPointer[j] = pData++;
326 }
327 else{
328 for(j=prevLength; j<length; j++)
329 RPDO->mapPointer[j] = pData++;
330 }
331 #else
332 for(j=prevLength; j<length; j++){
333 RPDO->mapPointer[j] = pData++;
334 }
335 #endif
336
337 }
338
339 RPDO->dataLength = length;
340
341 return ret;
342 }
343
344
345 /*
346 * Configure TPDO Mapping parameter.
347 *
348 * Function is called from communication reset or when parameter changes.
349 *
350 * Function configures following variables from CO_TPDO_t: _dataLength_,
351 * _mapPointer_ and _sendIfCOSFlags_.
352 *
353 * @param TPDO TPDO object.
354 * @param noOfMappedObjects Number of mapped object (from OD).
355 *
356 * @return 0 on success, otherwise SDO abort code.
357 */
CO_TPDOconfigMap(CO_TPDO_t * TPDO,uint8_t noOfMappedObjects)358 static uint32_t CO_TPDOconfigMap(CO_TPDO_t* TPDO, uint8_t noOfMappedObjects){
359 int16_t i;
360 uint8_t length = 0;
361 uint32_t ret = 0;
362 const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1;
363
364 TPDO->sendIfCOSFlags = 0;
365
366 for(i=noOfMappedObjects; i>0; i--){
367 int16_t j;
368 uint8_t* pData;
369 uint8_t prevLength = length;
370 uint8_t MBvar;
371 uint32_t map = *(pMap++);
372
373 /* function do much checking of errors in map */
374 ret = CO_PDOfindMap(
375 TPDO->SDO,
376 map,
377 1,
378 &pData,
379 &length,
380 &TPDO->sendIfCOSFlags,
381 &MBvar);
382 if(ret){
383 length = 0;
384 CO_errorReport(TPDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map);
385 break;
386 }
387
388 /* write PDO data pointers */
389 #ifdef CO_BIG_ENDIAN
390 if(MBvar){
391 for(j=length-1; j>=prevLength; j--)
392 TPDO->mapPointer[j] = pData++;
393 }
394 else{
395 for(j=prevLength; j<length; j++)
396 TPDO->mapPointer[j] = pData++;
397 }
398 #else
399 for(j=prevLength; j<length; j++){
400 TPDO->mapPointer[j] = pData++;
401 }
402 #endif
403
404 }
405
406 TPDO->dataLength = length;
407
408 return ret;
409 }
410
411
412 /*
413 * Function for accessing _RPDO communication parameter_ (index 0x1400+) from SDO server.
414 *
415 * For more information see file CO_SDO.h.
416 */
CO_ODF_RPDOcom(CO_ODF_arg_t * ODF_arg)417 static CO_SDO_abortCode_t CO_ODF_RPDOcom(CO_ODF_arg_t *ODF_arg){
418 CO_RPDO_t *RPDO;
419
420 RPDO = (CO_RPDO_t*) ODF_arg->object;
421
422 /* Reading Object Dictionary variable */
423 if(ODF_arg->reading){
424 if(ODF_arg->subIndex == 1){
425 uint32_t value = CO_getUint32(ODF_arg->data);
426
427 /* if default COB ID is used, write default value here */
428 if(((value)&0xFFFF) == RPDO->defaultCOB_ID && RPDO->defaultCOB_ID)
429 value += RPDO->nodeId;
430
431 /* If PDO is not valid, set bit 31 */
432 if(!RPDO->valid) value |= 0x80000000L;
433
434 CO_setUint32(ODF_arg->data, value);
435 }
436 return CO_SDO_AB_NONE;
437 }
438
439 /* Writing Object Dictionary variable */
440 if(RPDO->restrictionFlags & 0x04)
441 return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */
442 if(*RPDO->operatingState == CO_NMT_OPERATIONAL && (RPDO->restrictionFlags & 0x01))
443 return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
444
445 if(ODF_arg->subIndex == 1){ /* COB_ID */
446 uint32_t value = CO_getUint32(ODF_arg->data);
447
448 /* bits 11...29 must be zero */
449 if(value & 0x3FFF8000L)
450 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
451
452 /* if default COB-ID is being written, write defaultCOB_ID without nodeId */
453 if(((value)&0xFFFF) == (RPDO->defaultCOB_ID + RPDO->nodeId)){
454 value &= 0xC0000000L;
455 value += RPDO->defaultCOB_ID;
456 CO_setUint32(ODF_arg->data, value);
457 }
458
459 /* if PDO is valid, bits 0..29 can not be changed */
460 if(RPDO->valid && ((value ^ RPDO->RPDOCommPar->COB_IDUsedByRPDO) & 0x3FFFFFFFL))
461 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
462
463 /* configure RPDO */
464 CO_RPDOconfigCom(RPDO, value);
465 }
466 else if(ODF_arg->subIndex == 2){ /* Transmission_type */
467 uint8_t *value = (uint8_t*) ODF_arg->data;
468 bool_t synchronousPrev = RPDO->synchronous;
469
470 /* values from 241...253 are not valid */
471 if(*value >= 241 && *value <= 253)
472 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
473
474 RPDO->synchronous = (*value <= 240) ? true : false;
475
476 /* Remove old message from second buffer. */
477 if(RPDO->synchronous != synchronousPrev) {
478 CLEAR_CANrxNew(RPDO->CANrxNew[1]);
479 }
480 }
481
482 return CO_SDO_AB_NONE;
483 }
484
485
486 /*
487 * Function for accessing _TPDO communication parameter_ (index 0x1800+) from SDO server.
488 *
489 * For more information see file CO_SDO.h.
490 */
CO_ODF_TPDOcom(CO_ODF_arg_t * ODF_arg)491 static CO_SDO_abortCode_t CO_ODF_TPDOcom(CO_ODF_arg_t *ODF_arg){
492 CO_TPDO_t *TPDO;
493
494 TPDO = (CO_TPDO_t*) ODF_arg->object;
495
496 if(ODF_arg->subIndex == 4) return CO_SDO_AB_SUB_UNKNOWN; /* Sub-index does not exist. */
497
498 /* Reading Object Dictionary variable */
499 if(ODF_arg->reading){
500 if(ODF_arg->subIndex == 1){ /* COB_ID */
501 uint32_t value = CO_getUint32(ODF_arg->data);
502
503 /* if default COB ID is used, write default value here */
504 if(((value)&0xFFFF) == TPDO->defaultCOB_ID && TPDO->defaultCOB_ID)
505 value += TPDO->nodeId;
506
507 /* If PDO is not valid, set bit 31 */
508 if(!TPDO->valid) value |= 0x80000000L;
509
510 CO_setUint32(ODF_arg->data, value);
511 }
512 return CO_SDO_AB_NONE;
513 }
514
515 /* Writing Object Dictionary variable */
516 if(TPDO->restrictionFlags & 0x04)
517 return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */
518 if(*TPDO->operatingState == CO_NMT_OPERATIONAL && (TPDO->restrictionFlags & 0x01))
519 return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
520
521 if(ODF_arg->subIndex == 1){ /* COB_ID */
522 uint32_t value = CO_getUint32(ODF_arg->data);
523
524 /* bits 11...29 must be zero */
525 if(value & 0x3FFF8000L)
526 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
527
528 /* if default COB-ID is being written, write defaultCOB_ID without nodeId */
529 if(((value)&0xFFFF) == (TPDO->defaultCOB_ID + TPDO->nodeId)){
530 value &= 0xC0000000L;
531 value += TPDO->defaultCOB_ID;
532
533 CO_setUint32(ODF_arg->data, value);
534 }
535
536 /* if PDO is valid, bits 0..29 can not be changed */
537 if(TPDO->valid && ((value ^ TPDO->TPDOCommPar->COB_IDUsedByTPDO) & 0x3FFFFFFFL))
538 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
539
540 /* configure TPDO */
541 CO_TPDOconfigCom(TPDO, value, TPDO->CANtxBuff->syncFlag);
542 TPDO->syncCounter = 255;
543 }
544 else if(ODF_arg->subIndex == 2){ /* Transmission_type */
545 uint8_t *value = (uint8_t*) ODF_arg->data;
546
547 /* values from 241...253 are not valid */
548 if(*value >= 241 && *value <= 253)
549 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
550 TPDO->CANtxBuff->syncFlag = (*value <= 240) ? 1 : 0;
551 TPDO->syncCounter = 255;
552 }
553 else if(ODF_arg->subIndex == 3){ /* Inhibit_Time */
554 /* if PDO is valid, value can not be changed */
555 if(TPDO->valid)
556 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
557
558 TPDO->inhibitTimer = 0;
559 }
560 else if(ODF_arg->subIndex == 5){ /* Event_Timer */
561 uint16_t value = CO_getUint16(ODF_arg->data);
562
563 TPDO->eventTimer = ((uint32_t) value) * 1000;
564 }
565 else if(ODF_arg->subIndex == 6){ /* SYNC start value */
566 uint8_t *value = (uint8_t*) ODF_arg->data;
567
568 /* if PDO is valid, value can not be changed */
569 if(TPDO->valid)
570 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
571
572 /* values from 240...255 are not valid */
573 if(*value > 240)
574 return CO_SDO_AB_INVALID_VALUE; /* Invalid value for parameter (download only). */
575 }
576
577 return CO_SDO_AB_NONE;
578 }
579
580
581 /*
582 * Function for accessing _RPDO mapping parameter_ (index 0x1600+) from SDO server.
583 *
584 * For more information see file CO_SDO.h.
585 */
CO_ODF_RPDOmap(CO_ODF_arg_t * ODF_arg)586 static CO_SDO_abortCode_t CO_ODF_RPDOmap(CO_ODF_arg_t *ODF_arg){
587 CO_RPDO_t *RPDO;
588
589 RPDO = (CO_RPDO_t*) ODF_arg->object;
590
591 /* Reading Object Dictionary variable */
592 if(ODF_arg->reading){
593 uint8_t *value = (uint8_t*) ODF_arg->data;
594
595 if(ODF_arg->subIndex == 0){
596 /* If there is error in mapping, dataLength is 0, so numberOfMappedObjects is 0. */
597 if(!RPDO->dataLength) *value = 0;
598 }
599 return CO_SDO_AB_NONE;
600 }
601
602 /* Writing Object Dictionary variable */
603 if(RPDO->restrictionFlags & 0x08)
604 return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */
605 if(*RPDO->operatingState == CO_NMT_OPERATIONAL && (RPDO->restrictionFlags & 0x02))
606 return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
607 if(RPDO->valid)
608 return CO_SDO_AB_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */
609
610 /* numberOfMappedObjects */
611 if(ODF_arg->subIndex == 0){
612 uint8_t *value = (uint8_t*) ODF_arg->data;
613
614 if(*value > 8)
615 return CO_SDO_AB_MAP_LEN; /* Number and length of object to be mapped exceeds PDO length. */
616
617 /* configure mapping */
618 return CO_RPDOconfigMap(RPDO, *value);
619 }
620
621 /* mappedObject */
622 else{
623 uint32_t value = CO_getUint32(ODF_arg->data);
624 uint8_t* pData;
625 uint8_t length = 0;
626 uint8_t dummy = 0;
627 uint8_t MBvar;
628
629 if(RPDO->dataLength)
630 return CO_SDO_AB_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */
631
632 /* verify if mapping is correct */
633 return CO_PDOfindMap(
634 RPDO->SDO,
635 value,
636 0,
637 &pData,
638 &length,
639 &dummy,
640 &MBvar);
641 }
642
643 return CO_SDO_AB_NONE;
644 }
645
646
647 /*
648 * Function for accessing _TPDO mapping parameter_ (index 0x1A00+) from SDO server.
649 *
650 * For more information see file CO_SDO.h.
651 */
CO_ODF_TPDOmap(CO_ODF_arg_t * ODF_arg)652 static CO_SDO_abortCode_t CO_ODF_TPDOmap(CO_ODF_arg_t *ODF_arg){
653 CO_TPDO_t *TPDO;
654
655 TPDO = (CO_TPDO_t*) ODF_arg->object;
656
657 /* Reading Object Dictionary variable */
658 if(ODF_arg->reading){
659 uint8_t *value = (uint8_t*) ODF_arg->data;
660
661 if(ODF_arg->subIndex == 0){
662 /* If there is error in mapping, dataLength is 0, so numberOfMappedObjects is 0. */
663 if(!TPDO->dataLength) *value = 0;
664 }
665 return CO_SDO_AB_NONE;
666 }
667
668 /* Writing Object Dictionary variable */
669 if(TPDO->restrictionFlags & 0x08)
670 return CO_SDO_AB_READONLY; /* Attempt to write a read only object. */
671 if(*TPDO->operatingState == CO_NMT_OPERATIONAL && (TPDO->restrictionFlags & 0x02))
672 return CO_SDO_AB_DATA_DEV_STATE; /* Data cannot be transferred or stored to the application because of the present device state. */
673 if(TPDO->valid)
674 return CO_SDO_AB_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */
675
676 /* numberOfMappedObjects */
677 if(ODF_arg->subIndex == 0){
678 uint8_t *value = (uint8_t*) ODF_arg->data;
679
680 if(*value > 8)
681 return CO_SDO_AB_MAP_LEN; /* Number and length of object to be mapped exceeds PDO length. */
682
683 /* configure mapping */
684 return CO_TPDOconfigMap(TPDO, *value);
685 }
686
687 /* mappedObject */
688 else{
689 uint32_t value = CO_getUint32(ODF_arg->data);
690 uint8_t* pData;
691 uint8_t length = 0;
692 uint8_t dummy = 0;
693 uint8_t MBvar;
694
695 if(TPDO->dataLength)
696 return CO_SDO_AB_UNSUPPORTED_ACCESS; /* Unsupported access to an object. */
697
698 /* verify if mapping is correct */
699 return CO_PDOfindMap(
700 TPDO->SDO,
701 value,
702 1,
703 &pData,
704 &length,
705 &dummy,
706 &MBvar);
707 }
708
709 return CO_SDO_AB_NONE;
710 }
711
712
713 /******************************************************************************/
CO_RPDO_init(CO_RPDO_t * RPDO,CO_EM_t * em,CO_SDO_t * SDO,CO_SYNC_t * SYNC,uint8_t * operatingState,uint8_t nodeId,uint16_t defaultCOB_ID,uint8_t restrictionFlags,const CO_RPDOCommPar_t * RPDOCommPar,const CO_RPDOMapPar_t * RPDOMapPar,uint16_t idx_RPDOCommPar,uint16_t idx_RPDOMapPar,CO_CANmodule_t * CANdevRx,uint16_t CANdevRxIdx)714 CO_ReturnError_t CO_RPDO_init(
715 CO_RPDO_t *RPDO,
716 CO_EM_t *em,
717 CO_SDO_t *SDO,
718 CO_SYNC_t *SYNC,
719 uint8_t *operatingState,
720 uint8_t nodeId,
721 uint16_t defaultCOB_ID,
722 uint8_t restrictionFlags,
723 const CO_RPDOCommPar_t *RPDOCommPar,
724 const CO_RPDOMapPar_t *RPDOMapPar,
725 uint16_t idx_RPDOCommPar,
726 uint16_t idx_RPDOMapPar,
727 CO_CANmodule_t *CANdevRx,
728 uint16_t CANdevRxIdx)
729 {
730 /* verify arguments */
731 if(RPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
732 RPDOCommPar==NULL || RPDOMapPar==NULL || CANdevRx==NULL){
733 return CO_ERROR_ILLEGAL_ARGUMENT;
734 }
735
736 /* Configure object variables */
737 RPDO->em = em;
738 RPDO->SDO = SDO;
739 RPDO->SYNC = SYNC;
740 RPDO->RPDOCommPar = RPDOCommPar;
741 RPDO->RPDOMapPar = RPDOMapPar;
742 RPDO->operatingState = operatingState;
743 RPDO->nodeId = nodeId;
744 RPDO->defaultCOB_ID = defaultCOB_ID;
745 RPDO->restrictionFlags = restrictionFlags;
746
747 /* Configure Object dictionary entry at index 0x1400+ and 0x1600+ */
748 CO_OD_configure(SDO, idx_RPDOCommPar, CO_ODF_RPDOcom, (void*)RPDO, 0, 0);
749 CO_OD_configure(SDO, idx_RPDOMapPar, CO_ODF_RPDOmap, (void*)RPDO, 0, 0);
750
751 /* configure communication and mapping */
752 CLEAR_CANrxNew(RPDO->CANrxNew[0]);
753 CLEAR_CANrxNew(RPDO->CANrxNew[1]);
754 RPDO->CANdevRx = CANdevRx;
755 RPDO->CANdevRxIdx = CANdevRxIdx;
756
757 CO_RPDOconfigMap(RPDO, RPDOMapPar->numberOfMappedObjects);
758 CO_RPDOconfigCom(RPDO, RPDOCommPar->COB_IDUsedByRPDO);
759
760 return CO_ERROR_NO;
761 }
762
763
764 /******************************************************************************/
CO_TPDO_init(CO_TPDO_t * TPDO,CO_EM_t * em,CO_SDO_t * SDO,CO_SYNC_t * SYNC,uint8_t * operatingState,uint8_t nodeId,uint16_t defaultCOB_ID,uint8_t restrictionFlags,const CO_TPDOCommPar_t * TPDOCommPar,const CO_TPDOMapPar_t * TPDOMapPar,uint16_t idx_TPDOCommPar,uint16_t idx_TPDOMapPar,CO_CANmodule_t * CANdevTx,uint16_t CANdevTxIdx)765 CO_ReturnError_t CO_TPDO_init(
766 CO_TPDO_t *TPDO,
767 CO_EM_t *em,
768 CO_SDO_t *SDO,
769 CO_SYNC_t *SYNC,
770 uint8_t *operatingState,
771 uint8_t nodeId,
772 uint16_t defaultCOB_ID,
773 uint8_t restrictionFlags,
774 const CO_TPDOCommPar_t *TPDOCommPar,
775 const CO_TPDOMapPar_t *TPDOMapPar,
776 uint16_t idx_TPDOCommPar,
777 uint16_t idx_TPDOMapPar,
778 CO_CANmodule_t *CANdevTx,
779 uint16_t CANdevTxIdx)
780 {
781 /* verify arguments */
782 if(TPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
783 TPDOCommPar==NULL || TPDOMapPar==NULL || CANdevTx==NULL){
784 return CO_ERROR_ILLEGAL_ARGUMENT;
785 }
786
787 /* Configure object variables */
788 TPDO->em = em;
789 TPDO->SDO = SDO;
790 TPDO->SYNC = SYNC;
791 TPDO->TPDOCommPar = TPDOCommPar;
792 TPDO->TPDOMapPar = TPDOMapPar;
793 TPDO->operatingState = operatingState;
794 TPDO->nodeId = nodeId;
795 TPDO->defaultCOB_ID = defaultCOB_ID;
796 TPDO->restrictionFlags = restrictionFlags;
797
798 /* Configure Object dictionary entry at index 0x1800+ and 0x1A00+ */
799 CO_OD_configure(SDO, idx_TPDOCommPar, CO_ODF_TPDOcom, (void*)TPDO, 0, 0);
800 CO_OD_configure(SDO, idx_TPDOMapPar, CO_ODF_TPDOmap, (void*)TPDO, 0, 0);
801
802 /* configure communication and mapping */
803 TPDO->CANdevTx = CANdevTx;
804 TPDO->CANdevTxIdx = CANdevTxIdx;
805 TPDO->syncCounter = 255;
806 TPDO->inhibitTimer = 0;
807 TPDO->eventTimer = ((uint32_t) TPDOCommPar->eventTimer) * 1000;
808 if(TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1;
809
810 CO_TPDOconfigMap(TPDO, TPDOMapPar->numberOfMappedObjects);
811 CO_TPDOconfigCom(TPDO, TPDOCommPar->COB_IDUsedByTPDO, ((TPDOCommPar->transmissionType<=240) ? 1 : 0));
812
813 if((TPDOCommPar->transmissionType>240 &&
814 TPDOCommPar->transmissionType<254) ||
815 TPDOCommPar->SYNCStartValue>240){
816 TPDO->valid = false;
817 }
818
819 return CO_ERROR_NO;
820 }
821
822
823 /******************************************************************************/
CO_TPDOisCOS(CO_TPDO_t * TPDO)824 uint8_t CO_TPDOisCOS(CO_TPDO_t *TPDO){
825
826 /* Prepare TPDO data automatically from Object Dictionary variables */
827 uint8_t* pPDOdataByte;
828 uint8_t** ppODdataByte;
829
830 pPDOdataByte = &TPDO->CANtxBuff->data[TPDO->dataLength];
831 ppODdataByte = &TPDO->mapPointer[TPDO->dataLength];
832
833 switch(TPDO->dataLength){
834 case 8: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x80)) return 1; // fallthrough
835 case 7: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x40)) return 1; // fallthrough
836 case 6: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x20)) return 1; // fallthrough
837 case 5: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x10)) return 1; // fallthrough
838 case 4: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x08)) return 1; // fallthrough
839 case 3: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x04)) return 1; // fallthrough
840 case 2: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x02)) return 1; // fallthrough
841 case 1: if(*(--pPDOdataByte) != **(--ppODdataByte) && (TPDO->sendIfCOSFlags&0x01)) return 1; // fallthrough
842 }
843
844 return 0;
845 }
846
847 //#define TPDO_CALLS_EXTENSION
848 /******************************************************************************/
CO_TPDOsend(CO_TPDO_t * TPDO)849 int16_t CO_TPDOsend(CO_TPDO_t *TPDO){
850 int16_t i;
851 uint8_t* pPDOdataByte;
852 uint8_t** ppODdataByte;
853
854 #ifdef TPDO_CALLS_EXTENSION
855 if(TPDO->SDO->ODExtensions){
856 /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
857 const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1;
858 CO_SDO_t *pSDO = TPDO->SDO;
859
860 for(i=TPDO->TPDOMapPar->numberOfMappedObjects; i>0; i--){
861 uint32_t map = *(pMap++);
862 uint16_t index = (uint16_t)(map>>16);
863 uint8_t subIndex = (uint8_t)(map>>8);
864 uint16_t entryNo = CO_OD_find(pSDO, index);
865 if ( entryNo == 0xFFFF ) continue;
866 CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
867 if( ext->pODFunc == NULL) continue;
868 CO_ODF_arg_t ODF_arg;
869 memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
870 ODF_arg.reading = true;
871 ODF_arg.index = index;
872 ODF_arg.subIndex = subIndex;
873 ODF_arg.object = ext->object;
874 ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
875 ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
876 ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); //https://github.com/CANopenNode/CANopenNode/issues/100
877 ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
878 ext->pODFunc(&ODF_arg);
879 }
880 }
881 #endif
882 i = TPDO->dataLength;
883 pPDOdataByte = &TPDO->CANtxBuff->data[0];
884 ppODdataByte = &TPDO->mapPointer[0];
885
886 /* Copy data from Object dictionary. */
887 for(; i>0; i--) {
888 *(pPDOdataByte++) = **(ppODdataByte++);
889 }
890
891 TPDO->sendRequest = 0;
892
893 return CO_CANsend(TPDO->CANdevTx, TPDO->CANtxBuff);
894 }
895
896 //#define RPDO_CALLS_EXTENSION
897 /******************************************************************************/
CO_RPDO_process(CO_RPDO_t * RPDO,bool_t syncWas)898 void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas){
899
900 if(!RPDO->valid || !(*RPDO->operatingState == CO_NMT_OPERATIONAL))
901 {
902 CLEAR_CANrxNew(RPDO->CANrxNew[0]);
903 CLEAR_CANrxNew(RPDO->CANrxNew[1]);
904 }
905 else if(!RPDO->synchronous || syncWas)
906 {
907 #if defined(RPDO_CALLS_EXTENSION)
908 bool_t update = false;
909 #endif /* defined(RPDO_CALLS_EXTENSION) */
910
911 uint8_t bufNo = 0;
912
913 /* Determine, which of the two rx buffers, contains relevant message. */
914 if(RPDO->SYNC && RPDO->synchronous && !RPDO->SYNC->CANrxToggle) {
915 bufNo = 1;
916 }
917
918 while(IS_CANrxNew(RPDO->CANrxNew[bufNo])){
919 int16_t i;
920 uint8_t* pPDOdataByte;
921 uint8_t** ppODdataByte;
922
923 i = RPDO->dataLength;
924 pPDOdataByte = &RPDO->CANrxData[bufNo][0];
925 ppODdataByte = &RPDO->mapPointer[0];
926
927 /* Copy data to Object dictionary. If between the copy operation CANrxNew
928 * is set to true by receive thread, then copy the latest data again. */
929 CLEAR_CANrxNew(RPDO->CANrxNew[bufNo]);
930 for(; i>0; i--) {
931 **(ppODdataByte++) = *(pPDOdataByte++);
932 }
933 #if defined(RPDO_CALLS_EXTENSION)
934 update = true;
935 #endif /* defined(RPDO_CALLS_EXTENSION) */
936 }
937 #ifdef RPDO_CALLS_EXTENSION
938 if(update && RPDO->SDO->ODExtensions){
939 int16_t i;
940 /* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
941 const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1;
942 CO_SDO_t *pSDO = RPDO->SDO;
943
944 for(i=RPDO->RPDOMapPar->numberOfMappedObjects; i>0; i--){
945 uint32_t map = *(pMap++);
946 uint16_t index = (uint16_t)(map>>16);
947 uint8_t subIndex = (uint8_t)(map>>8);
948 uint16_t entryNo = CO_OD_find(pSDO, index);
949 if ( entryNo == 0xFFFF ) continue;
950 CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
951 if( ext->pODFunc == NULL) continue;
952 CO_ODF_arg_t ODF_arg;
953 memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
954 ODF_arg.reading = false;
955 ODF_arg.index = index;
956 ODF_arg.subIndex = subIndex;
957 ODF_arg.object = ext->object;
958 ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
959 ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
960 ODF_arg.data = CO_OD_getDataPointer(pSDO, entryNo, subIndex); //https://github.com/CANopenNode/CANopenNode/issues/100
961 ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
962 ext->pODFunc(&ODF_arg);
963 }
964 }
965 #endif
966 }
967 }
968
969
970 /******************************************************************************/
CO_TPDO_process(CO_TPDO_t * TPDO,bool_t syncWas,uint32_t timeDifference_us)971 void CO_TPDO_process(
972 CO_TPDO_t *TPDO,
973 bool_t syncWas,
974 uint32_t timeDifference_us)
975 {
976 if(TPDO->valid && *TPDO->operatingState == CO_NMT_OPERATIONAL){
977
978 /* Send PDO by application request or by Event timer */
979 if(TPDO->TPDOCommPar->transmissionType >= 253){
980 if(TPDO->inhibitTimer == 0 && (TPDO->sendRequest || (TPDO->TPDOCommPar->eventTimer && TPDO->eventTimer == 0))){
981 if(CO_TPDOsend(TPDO) == CO_ERROR_NO){
982 /* successfully sent */
983 TPDO->inhibitTimer = ((uint32_t) TPDO->TPDOCommPar->inhibitTime) * 100;
984 TPDO->eventTimer = ((uint32_t) TPDO->TPDOCommPar->eventTimer) * 1000;
985 }
986 }
987 }
988
989 /* Synchronous PDOs */
990 else if(TPDO->SYNC && syncWas){
991 /* send synchronous acyclic PDO */
992 if(TPDO->TPDOCommPar->transmissionType == 0){
993 if(TPDO->sendRequest) CO_TPDOsend(TPDO);
994 }
995 /* send synchronous cyclic PDO */
996 else{
997 /* is the start of synchronous TPDO transmission */
998 if(TPDO->syncCounter == 255){
999 if(TPDO->SYNC->counterOverflowValue && TPDO->TPDOCommPar->SYNCStartValue)
1000 TPDO->syncCounter = 254; /* SYNCStartValue is in use */
1001 else
1002 TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType;
1003 }
1004 /* if the SYNCStartValue is in use, start first TPDO after SYNC with matched SYNCStartValue. */
1005 if(TPDO->syncCounter == 254){
1006 if(TPDO->SYNC->counter == TPDO->TPDOCommPar->SYNCStartValue){
1007 TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType;
1008 CO_TPDOsend(TPDO);
1009 }
1010 }
1011 /* Send PDO after every N-th Sync */
1012 else if(--TPDO->syncCounter == 0){
1013 TPDO->syncCounter = TPDO->TPDOCommPar->transmissionType;
1014 CO_TPDOsend(TPDO);
1015 }
1016 }
1017 }
1018
1019 }
1020 else{
1021 /* Not operational or valid. Force TPDO first send after operational or valid. */
1022 if(TPDO->TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1;
1023 else TPDO->sendRequest = 0;
1024 }
1025
1026 /* update timers */
1027 TPDO->inhibitTimer = (TPDO->inhibitTimer > timeDifference_us) ? (TPDO->inhibitTimer - timeDifference_us) : 0;
1028 TPDO->eventTimer = (TPDO->eventTimer > timeDifference_us) ? (TPDO->eventTimer - timeDifference_us) : 0;
1029 }
1030
