Пример #1
0
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;
        }
    }
}
Пример #2
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{
Пример #3
0
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);
}
Пример #4
0
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;
}
Пример #5
0
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
    }
Пример #6
0
/*
 * 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;
}
Пример #7
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;
        }