void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas){
if(RPDO->valid && (*RPDO->operatingState == CO_NMT_OPERATIONAL) &&
((RPDO->synchronous && syncWas) || !RPDO->synchronous)){
while(RPDO->CANrxNew){
int16_t i;
uint8_t* pPDOdataByte;
uint8_t** ppODdataByte;
i = RPDO->dataLength;
pPDOdataByte = &RPDO->CANrxData[0];
ppODdataByte = &RPDO->mapPointer[0];
/* Copy data to Object dictionary. If between the copy operation CANrxNew
* is set to true by receive thread, then copy the latest data again. */
RPDO->CANrxNew = false;
for(; i>0; i--) {
**(ppODdataByte++) = *(pPDOdataByte++);
}
#ifdef RPDO_CALLS_EXTENSION
if(RPDO->SDO->ODExtensions){
/* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1;
CO_SDO_t *pSDO = RPDO->SDO;
for(i=RPDO->RPDOMapPar->numberOfMappedObjects; i>0; i--){
uint32_t map = *(pMap++);
uint16_t index = (uint16_t)(map>>16);
uint8_t subIndex = (uint8_t)(map>>8);
uint16_t entryNo = CO_OD_find(pSDO, index);
CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
if( ext->pODFunc == NULL) continue;
CO_ODF_arg_t ODF_arg;
memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
ODF_arg.reading = false;
ODF_arg.index = index;
ODF_arg.subIndex = subIndex;
ODF_arg.object = ext->object;
ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
ODF_arg.data = pSDO->OD[entryNo].pData;
ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
ext->pODFunc(&ODF_arg);
}
}
#endif
}
}
else{
int16_t CO_TPDOsend(CO_TPDO_t *TPDO){
int16_t i;
uint8_t* pPDOdataByte;
uint8_t** ppODdataByte;
#ifdef TPDO_CALLS_EXTENSION
if(TPDO->SDO->ODExtensions){
/* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1;
CO_SDO_t *pSDO = TPDO->SDO;
for(i=TPDO->TPDOMapPar->numberOfMappedObjects; i>0; i--){
uint32_t map = *(pMap++);
uint16_t index = (uint16_t)(map>>16);
uint8_t subIndex = (uint8_t)(map>>8);
uint16_t entryNo = CO_OD_find(pSDO, index);
CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
if( ext->pODFunc == NULL) continue;
CO_ODF_arg_t ODF_arg;
memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
ODF_arg.reading = true;
ODF_arg.index = index;
ODF_arg.subIndex = subIndex;
ODF_arg.object = ext->object;
ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
ODF_arg.data = pSDO->OD[entryNo].pData;
ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
ext->pODFunc(&ODF_arg);
}
}
#endif
i = TPDO->dataLength;
pPDOdataByte = &TPDO->CANtxBuff->data[0];
ppODdataByte = &TPDO->mapPointer[0];
/* Copy data from Object dictionary. */
for(; i>0; i--) {
*(pPDOdataByte++) = **(ppODdataByte++);
}
TPDO->sendRequest = 0;
return CO_CANsend(TPDO->CANdevTx, TPDO->CANtxBuff);
}
void CO_RPDO_process(CO_RPDO_t *RPDO){
if(RPDO->CANrxNew && RPDO->valid && *RPDO->operatingState == CO_NMT_OPERATIONAL){
int16_t i;
uint8_t* pPDOdataByte;
uint8_t** ppODdataByte;
pPDOdataByte = &RPDO->CANrxData[0];
ppODdataByte = &RPDO->mapPointer[0];
for(i=RPDO->dataLength; i>0; i--)
**(ppODdataByte++) = *(pPDOdataByte++);
#ifdef RPDO_CALLS_EXTENSION
if(RPDO->SDO->ODExtensions){
/* for each mapped OD, check mapping to see if an OD extension is available, and call it if it is */
const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1;
CO_SDO_t *pSDO = RPDO->SDO;
for(i=RPDO->RPDOMapPar->numberOfMappedObjects; i>0; i--){
uint32_t map = *(pMap++);
uint16_t index = (uint16_t)(map>>16);
uint8_t subIndex = (uint8_t)(map>>8);
uint16_t entryNo = CO_OD_find(pSDO, index);
CO_OD_extension_t *ext = &pSDO->ODExtensions[entryNo];
if( ext->pODFunc == NULL) continue;
CO_ODF_arg_t ODF_arg;
memset((void*)&ODF_arg, 0, sizeof(CO_ODF_arg_t));
ODF_arg.reading = false;
ODF_arg.index = index;
ODF_arg.subIndex = subIndex;
ODF_arg.object = ext->object;
ODF_arg.attribute = CO_OD_getAttribute(pSDO, entryNo, subIndex);
ODF_arg.pFlags = CO_OD_getFlagsPointer(pSDO, entryNo, subIndex);
ODF_arg.data = pSDO->OD[entryNo].pData;
ODF_arg.dataLength = CO_OD_getLength(pSDO, entryNo, subIndex);
ext->pODFunc(&ODF_arg);
}
}
#endif
}
uint32_t CO_SDO_readOD(CO_SDO_t *SDO, uint16_t SDOBufferSize){
uint8_t *SDObuffer = SDO->ODF_arg.data;
uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage;
uint16_t length = SDO->ODF_arg.dataLength;
CO_OD_extension_t *ext = 0;
/* is object readable? */
if((SDO->ODF_arg.attribute & CO_ODA_READABLE) == 0)
return CO_SDO_AB_WRITEONLY; /* attempt to read a write-only object */
/* find extension */
if(SDO->ODExtensions != NULL){
ext = &SDO->ODExtensions[SDO->entryNo];
}
/* copy data from OD to SDO buffer if not domain */
if(ODdata != NULL){
CO_DISABLE_INTERRUPTS();
while(length--) *(SDObuffer++) = *(ODdata++);
CO_ENABLE_INTERRUPTS();
}
/* if domain, Object dictionary function MUST exist */
else{
if(ext->pODFunc == NULL){
return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
}
}
/* call Object dictionary function if registered */
SDO->ODF_arg.reading = CO_true;
if(ext->pODFunc != NULL){
uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg);
if(abortCode != 0U){
return abortCode;
}
/* dataLength (upadted by pODFunc) must be inside limits */
if((SDO->ODF_arg.dataLength == 0U) || (SDO->ODF_arg.dataLength > SDOBufferSize)){
return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
}
}
SDO->ODF_arg.firstSegment = CO_false;
/* swap data if processor is not little endian (CANopen is) */
#ifdef CO_BIG_ENDIAN
if((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){
uint16_t len = SDO->ODF_arg.dataLength;
uint8_t *buf1 = SDO->ODF_arg.data;
uint8_t *buf2 = buf1 + len - 1;
len /= 2;
while(len--){
uint8_t b = *buf1;
*(buf1++) = *buf2;
*(buf2--) = b;
}
}
#endif
return 0U;
}
