uint32_t CO_SDO_writeOD(CO_SDO_t *SDO, uint16_t length){ uint8_t *SDObuffer = SDO->ODF_arg.data; uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage; /* is object writeable? */ if((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0){ return CO_SDO_AB_READONLY; /* attempt to write a read-only object */ } /* length of domain data is application specific and not verified */ if(ODdata == 0){ SDO->ODF_arg.dataLength = length; } /* verify length except for domain data type */ else if(SDO->ODF_arg.dataLength != length){ return CO_SDO_AB_TYPE_MISMATCH; /* Length of service parameter does not match */ } /* 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 /* call Object dictionary function if registered */ SDO->ODF_arg.reading = CO_false; if(SDO->ODExtensions != NULL){ CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo]; if(ext->pODFunc != NULL){ uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg); if(abortCode != 0U){ return abortCode; } } } SDO->ODF_arg.firstSegment = CO_false; /* copy data from SDO buffer to OD if not domain */ if(ODdata != NULL){ CO_DISABLE_INTERRUPTS(); while(length--){ *(ODdata++) = *(SDObuffer++); } CO_ENABLE_INTERRUPTS(); } return 0; }
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; }