void CO_OD_configure(
CO_SDO_t *SDO,
uint16_t index,
CO_SDO_abortCode_t (*pODFunc)(CO_ODF_arg_t *ODF_arg),
void *object,
uint8_t *flags,
uint8_t flagsSize)
{
uint16_t entryNo;
entryNo = CO_OD_find(SDO, index);
if(entryNo < 0xFFFFU){
CO_OD_extension_t *ext = &SDO->ODExtensions[entryNo];
uint8_t maxSubIndex = SDO->OD[entryNo].maxSubIndex;
ext->pODFunc = pODFunc;
ext->object = object;
if((flags != NULL) && (flagsSize != 0U) && (flagsSize == maxSubIndex)){
uint16_t i;
ext->flags = flags;
for(i=0U; i<=maxSubIndex; i++){
ext->flags[i] = 0U;
}
}
else{
ext->flags = NULL;
}
}
}
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);
}
uint32_t CO_SDO_initTransfer(CO_SDO_t *SDO, uint16_t index, uint8_t subIndex){
SDO->ODF_arg.index = index;
SDO->ODF_arg.subIndex = subIndex;
/* find object in Object Dictionary */
SDO->entryNo = CO_OD_find(SDO, index);
if(SDO->entryNo == 0xFFFFU){
return CO_SDO_AB_NOT_EXIST ; /* object does not exist in OD */
}
/* verify existance of subIndex */
if(subIndex > SDO->OD[SDO->entryNo].maxSubIndex){
return CO_SDO_AB_SUB_UNKNOWN; /* Sub-index does not exist. */
}
/* pointer to data in Object dictionary */
SDO->ODF_arg.ODdataStorage = CO_OD_getDataPointer(SDO, SDO->entryNo, subIndex);
/* fill ODF_arg */
SDO->ODF_arg.object = NULL;
if(SDO->ODExtensions){
CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo];
SDO->ODF_arg.object = ext->object;
}
SDO->ODF_arg.data = SDO->databuffer;
SDO->ODF_arg.dataLength = CO_OD_getLength(SDO, SDO->entryNo, subIndex);
SDO->ODF_arg.attribute = CO_OD_getAttribute(SDO, SDO->entryNo, subIndex);
SDO->ODF_arg.pFlags = CO_OD_getFlagsPointer(SDO, SDO->entryNo, subIndex);
SDO->ODF_arg.firstSegment = CO_true;
SDO->ODF_arg.lastSegment = CO_true;
/* indicate total data length, if not domain */
SDO->ODF_arg.dataLengthTotal = (SDO->ODF_arg.ODdataStorage) ? SDO->ODF_arg.dataLength : 0U;
/* verify length */
if(SDO->ODF_arg.dataLength > CO_SDO_BUFFER_SIZE){
return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
}
return 0U;
}
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
}
/*
* Find mapped variable in Object Dictionary.
*
* Function is called from CO_R(T)PDOconfigMap or when mapping parameter changes.
*
* @param SDO SDO object.
* @param map PDO mapping parameter.
* @param R_T 0 for RPDO map, 1 for TPDO map.
* @param ppData Pointer to returning parameter: pointer to data of mapped variable.
* @param pLength Pointer to returning parameter: *add* length of mapped variable.
* @param pSendIfCOSFlags Pointer to returning parameter: sendIfCOSFlags variable.
* @param pIsMultibyteVar Pointer to returning parameter: true for multibyte variable.
*
* @return 0 on success, otherwise SDO abort code.
*/
static uint32_t 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)
{
uint16_t entryNo;
uint16_t index;
uint8_t subIndex;
uint8_t dataLen;
uint8_t objectLen;
uint8_t attr;
index = (uint16_t)(map>>16);
subIndex = (uint8_t)(map>>8);
dataLen = (uint8_t) map; /* data length in bits */
/* data length must be byte aligned */
if(dataLen&0x07) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
dataLen >>= 3; /* new data length is in bytes */
*pLength += dataLen;
/* total PDO length can not be more than 8 bytes */
if(*pLength > 8) return CO_SDO_AB_MAP_LEN; /* The number and length of the objects to be mapped would exceed PDO length. */
/* is there a reference to dummy entries */
if(index <=7 && subIndex == 0){
static uint32_t dummyTX = 0;
static uint32_t dummyRX;
uint8_t dummySize = 4;
if(index<2) dummySize = 0;
else if(index==2 || index==5) dummySize = 1;
else if(index==3 || index==6) dummySize = 2;
/* is size of variable big enough for map */
if(dummySize < dataLen) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
/* Data and ODE pointer */
if(R_T == 0) *ppData = (uint8_t*) &dummyRX;
else *ppData = (uint8_t*) &dummyTX;
return 0;
}
/* find object in Object Dictionary */
entryNo = CO_OD_find(SDO, index);
/* Does object exist in OD? */
if(entryNo == 0xFFFF || subIndex > SDO->OD[entryNo].maxSubIndex)
return CO_SDO_AB_NOT_EXIST; /* Object does not exist in the object dictionary. */
attr = CO_OD_getAttribute(SDO, entryNo, subIndex);
/* Is object Mappable for RPDO? */
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. */
/* Is object Mappable for TPDO? */
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. */
/* is size of variable big enough for map */
objectLen = CO_OD_getLength(SDO, entryNo, subIndex);
if(objectLen < dataLen) return CO_SDO_AB_NO_MAP; /* Object cannot be mapped to the PDO. */
/* mark multibyte variable */
*pIsMultibyteVar = (attr&CO_ODA_MB_VALUE) ? 1 : 0;
/* pointer to data */
*ppData = (uint8_t*) CO_OD_getDataPointer(SDO, entryNo, subIndex);
#ifdef CO_BIG_ENDIAN
/* skip unused MSB bytes */
if(*pIsMultibyteVar){
*ppData += objectLen - dataLen;
}
#endif
/* setup change of state flags */
if(attr&CO_ODA_TPDO_DETECT_COS){
int16_t i;
for(i=*pLength-dataLen; i<*pLength; i++){
*pSendIfCOSFlags |= 1<<i;
}
}
return 0;
}
INTEGER8 CO_SDOclientDownload( CO_SDOclient_t *SDO_C,
UNSIGNED16 timeDifference_ms,
UNSIGNED16 SDOtimeoutTime,
UNSIGNED32 *pSDOabortCode)
{
//clear abort code
*pSDOabortCode = 0;
//verify parameters
if(!(SDO_C->state&0x22)) {
//communication was not properly initiated
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return -3;
}
//if nodeIDOfTheSDOServer == node-ID of this node, then exchange data with this node
if(SDO_C->SDO && SDO_C->ObjDict_SDOClientParameter->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId) {
const sCO_OD_object* pODE;
UNSIGNED8 length, attr;
UNSIGNED16 indexOfFoundObject;
UNSIGNED16 index = (UNSIGNED16)SDO_C->CANtxBuff->data[2]<<8 | SDO_C->CANtxBuff->data[1];
UNSIGNED8 subIndex = SDO_C->CANtxBuff->data[3];
pODE = CO_OD_find(SDO_C->SDO, index, &indexOfFoundObject);
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
if(pODE == 0) {
*pSDOabortCode = 0x06020000L; //object does not exist in OD
return -10;
}
if(subIndex > pODE->maxSubIndex) {
*pSDOabortCode = 0x06090011L; //Sub-index does not exist.
return -10;
}
length = CO_OD_getLength(pODE, subIndex);
if(SDO_C->bufferSize != length) {
*pSDOabortCode = 0x06070010L; //Length of service parameter does not match
return -10;
}
attr = CO_OD_getAttribute(pODE, subIndex);
if(!(attr&CO_ODA_WRITEABLE)) {
*pSDOabortCode = 0x06010002L; //attempt to write a read-only object
return -10;
}
//write to OD variable
*pSDOabortCode = pODE->pFunct(SDO_C->SDO->ObjectDictionaryPointers[indexOfFoundObject],
index,
subIndex,
length,
attr,
1,
(void*)SDO_C->buffer,
CO_OD_getDataPointer(pODE, subIndex));
if(*pSDOabortCode) {
return -10;
}
return 0;
}
//check if new SDO object received
if(SDO_C->CANrxNew) {
SDO_C->timeoutTimer = 0;
if(SDO_C->state & 0x20) {
//download initiated
switch(SDO_C->CANrxData[0]>>5) { //Switch Server Command Specifier
case 3: //response OK
if(SDO_C->bufferSize <= 4) {
//expedited transfer
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return 0;
}
else {
//segmented transfer - prepare the first segment
SDO_C->bufferOffset = 0;
SDO_C->state = 0x22;
//continue with segmented download
}
break;
case 4: //abort by server
memcpySwap4((UNSIGNED8*)pSDOabortCode , &SDO_C->CANrxData[4]);
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return -10;
default:
*pSDOabortCode = 0x05040001L; //Client/server command specifier not valid or unknown
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return -10;
}
}
if(SDO_C->state & 0x02) {
UNSIGNED8 i,j;
//segmented download in progress
if(SDO_C->state & 0x20) { //is the first segment?
SDO_C->state = 0x02;
}
else {
//verify response from previous segment sent
switch(SDO_C->CANrxData[0]>>5) { //Switch Server Command Specifier
case 1: //response OK
//verify toggle bit
if((SDO_C->CANrxData[0]&0x10) != (SDO_C->state&0x10)) {
*pSDOabortCode = 0x05030000L; //toggle bit not alternated
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return -10;
}
//is end of transfer?
if(SDO_C->bufferOffset == SDO_C->bufferSize) {
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return 0;
}
//alternate toggle bit
if(SDO_C->state&0x10) SDO_C->state &= 0xEF;
else SDO_C->state |= 0x10;
break;
case 4: //abort by server
memcpySwap4((UNSIGNED8*)pSDOabortCode , &SDO_C->CANrxData[4]);
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return -10;
default:
*pSDOabortCode = 0x05040001L; //Client/server command specifier not valid or unknown
SDO_C->state = 0;
SDO_C->CANrxNew = 0;
return -10;
}
}
//calculate length to be sent
j = SDO_C->bufferSize - SDO_C->bufferOffset;
if(j > 7) j = 7;
//fill data bytes
for(i=0; i<j; i++)
SDO_C->CANtxBuff->data[i+1] = SDO_C->buffer[SDO_C->bufferOffset + i];
for(; i<7; i++)
SDO_C->CANtxBuff->data[i+1] = 0;
SDO_C->bufferOffset += j;
//SDO command specifier
SDO_C->CANtxBuff->data[0] = 0x00 | (SDO_C->state&0x10) | ((7-j)<<1);
//is end of transfer?
if(SDO_C->bufferOffset == SDO_C->bufferSize) {
SDO_C->CANtxBuff->data[0] |= 1;
}
//Send next SDO message
CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
SDO_C->CANrxNew = 0;
return 2;
}
