예제 #1
0
    uint8_t CO_sendNMTcommand(CO_t *CO, uint8_t command, uint8_t nodeID){
        if(NMTM_txBuff == 0){
            /* error, CO_CANtxBufferInit() was not called for this buffer. */
            return CO_ERROR_TX_UNCONFIGURED; /* -11 */
        }
        NMTM_txBuff->data[0] = command;
        NMTM_txBuff->data[1] = nodeID;

        /* Apply NMT command also to this node, if set so. */
        if(nodeID == 0 || nodeID == CO->NMT->nodeId){
            switch(command){
                case CO_NMT_ENTER_OPERATIONAL:
                    if((*CO->NMT->emPr->errorRegister) == 0) {
                        CO->NMT->operatingState = CO_NMT_OPERATIONAL;
                    }
                    break;
                case CO_NMT_ENTER_STOPPED:
                    CO->NMT->operatingState = CO_NMT_STOPPED;
                    break;
                case CO_NMT_ENTER_PRE_OPERATIONAL:
                    CO->NMT->operatingState = CO_NMT_PRE_OPERATIONAL;
                    break;
                case CO_NMT_RESET_NODE:
                    CO->NMT->resetCommand = CO_RESET_APP;
                    break;
                case CO_NMT_RESET_COMMUNICATION:
                    CO->NMT->resetCommand = CO_RESET_COMM;
                    break;
            }
        }

        return CO_CANsend(CO->CANmodule[0], NMTM_txBuff); /* 0 = success */
    }
예제 #2
0
static void CO_SDO_abort(CO_SDO_t *SDO, uint32_t code){
    SDO->CANtxBuff->data[0] = 0x80;
    SDO->CANtxBuff->data[1] = SDO->ODF_arg.index & 0xFF;
    SDO->CANtxBuff->data[2] = (SDO->ODF_arg.index>>8) & 0xFF;
    SDO->CANtxBuff->data[3] = SDO->ODF_arg.subIndex;
    CO_memcpySwap4(&SDO->CANtxBuff->data[4], (uint8_t*)&code);
    SDO->state = CO_SDO_ST_IDLE;
    SDO->CANrxNew = CO_false;
    CO_CANsend(SDO->CANdevTx, SDO->CANtxBuff);
}
예제 #3
0
UNSIGNED8 CO_SYNC_process( CO_SYNC_t           *SYNC,
                           UNSIGNED32           timeDifference_us,
                           UNSIGNED32           ObjDict_synchronousWindowLength)
{
   UNSIGNED8 ret = 0;
   UNSIGNED32 timerNew;

   if((*SYNC->operatingState == CO_NMT_OPERATIONAL || *SYNC->operatingState == CO_NMT_PRE_OPERATIONAL)){
      //was SYNC just received
      if(SYNC->running && SYNC->timer == 0)
         ret = 1;

      //update sync timer, no overflow
      timerNew = SYNC->timer + timeDifference_us;
      if(timerNew > SYNC->timer) SYNC->timer = timerNew;

      //SYNC producer
      if(SYNC->isProducer && SYNC->periodTime){
         if(SYNC->timer >= SYNC->periodTime){
            if(++SYNC->counter > SYNC->counterOverflowValue) SYNC->counter = 1;
            SYNC->running = 1;
            SYNC->timer = 0;
            SYNC->CANtxBuff->data[0] = SYNC->counter;
            CO_CANsend(SYNC->CANdevTx, SYNC->CANtxBuff);
            ret = 1;
         }
      }

      //Synchronous PDOs are allowed only inside time window
      if(ObjDict_synchronousWindowLength){
         if(SYNC->timer > ObjDict_synchronousWindowLength){
            if(SYNC->curentSyncTimeIsInsideWindow == 1){
               ret = 2;
            }
            SYNC->curentSyncTimeIsInsideWindow = 0;
         }
         else{
            SYNC->curentSyncTimeIsInsideWindow = 1;
         }
      }
      else{
         SYNC->curentSyncTimeIsInsideWindow = 1;
      }

      //Verify timeout of SYNC
      if(SYNC->periodTime && SYNC->timer > SYNC->periodTimeoutTime && *SYNC->operatingState == CO_NMT_OPERATIONAL)
         CO_errorReport(SYNC->EM, ERROR_SYNC_TIME_OUT, SYNC->timer);
   }
   else{
      SYNC->running = 0;
   }
   return ret;
}
예제 #4
0
INTEGER8 CO_SDOclientDownloadInitiate( CO_SDOclient_t   *SDO_C,
                                       UNSIGNED16        index,
                                       UNSIGNED8         subIndex,
                                       UNSIGNED8        *dataTx,
                                       UNSIGNED8         dataSize)
{
    //verify parameters
    if(dataTx == 0 || dataSize == 0) return -2;

    //save parameters
    SDO_C->buffer = dataTx;
    SDO_C->bufferSize = dataSize;
    SDO_C->state = 0x20;

    //prepare CAN tx message
    SDO_C->CANtxBuff->data[1] = index & 0xFF;
    SDO_C->CANtxBuff->data[2] = index >> 8;
    SDO_C->CANtxBuff->data[3] = subIndex;

    //if nodeIDOfTheSDOServer == node-ID of this node, then exchange data with this node
    if(SDO_C->ObjDict_SDOClientParameter->nodeIDOfTheSDOServer == SDO_C->SDO->nodeId) {
        return 0;
    }

    //continue: prepare dataBuff for CAN message
    if(dataSize <= 4) {
        UNSIGNED8 i;
        //expedited transfer
        SDO_C->CANtxBuff->data[0] = 0x23 | ((4-dataSize) << 2);
        //clear unused data bytes
        SDO_C->CANtxBuff->data[4] = 0;
        SDO_C->CANtxBuff->data[5] = 0;
        SDO_C->CANtxBuff->data[6] = 0;
        SDO_C->CANtxBuff->data[7] = 0;
        //copy data
        for(i=dataSize+3; i>=4; i--) SDO_C->CANtxBuff->data[i] = dataTx[i-4];
    }
    else {
        UNSIGNED32 len;
        //segmented transfer
        SDO_C->CANtxBuff->data[0] = 0x21;
        len = dataSize;
        memcpySwap4(&SDO_C->CANtxBuff->data[4], (UNSIGNED8*)&len);
    }

    //empty receive buffer, reset timeout timer and send message
    SDO_C->CANrxNew = 0;
    SDO_C->timeoutTimer = 0;
    CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);

    return 0;
}
예제 #5
0
int16_t CO_TPDOsend(CO_TPDO_t *TPDO){
    int16_t i;
    uint8_t* pPDOdataByte;
    uint8_t** ppODdataByte;

    pPDOdataByte = &TPDO->CANtxBuff->data[0];
    ppODdataByte = &TPDO->mapPointer[0];

    for(i=TPDO->dataLength; i>0; i--)
        *(pPDOdataByte++) = **(ppODdataByte++);

    TPDO->sendRequest = 0;

    return CO_CANsend(TPDO->CANdevTx, TPDO->CANtxBuff);
}
예제 #6
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);
}
예제 #7
0
void CO_EM_process(
        CO_EMpr_t              *emPr,
        CO_bool_t               NMTisPreOrOperational,
        uint16_t                timeDifference_100us,
        uint16_t                emInhTime)
{

    CO_EM_t *em = emPr->em;
    uint8_t errorRegister;

    /* verify errors from driver and other */
    CO_CANverifyErrors(emPr->CANdev);
    if(em->wrongErrorReport != 0U){
        CO_errorReport(em, CO_EM_WRONG_ERROR_REPORT, CO_EMC_SOFTWARE_INTERNAL, (uint32_t)em->wrongErrorReport);
        em->wrongErrorReport = 0U;
    }


    /* calculate Error register */
    errorRegister = 0U;
    /* generic error */
    if(em->errorStatusBits[5]){
        errorRegister |= CO_ERR_REG_GENERIC_ERR;
    }
    /* communication error (overrun, error state) */
    if(em->errorStatusBits[2] || em->errorStatusBits[3]){
        errorRegister |= CO_ERR_REG_COMM_ERR;
    }
    *emPr->errorRegister = (*emPr->errorRegister & 0xEEU) | errorRegister;

    /* inhibit time */
    if(emPr->inhibitEmTimer < emInhTime){
        emPr->inhibitEmTimer += timeDifference_100us;
    }

    /* send Emergency message. */
    if(     NMTisPreOrOperational &&
            !emPr->CANtxBuff->bufferFull &&
            emPr->inhibitEmTimer >= emInhTime &&
            (em->bufReadPtr != em->bufWritePtr || em->bufFull))
    {
        uint32_t preDEF;    /* preDefinedErrorField */
        
        /* add error register */
        em->bufReadPtr[2] = *emPr->errorRegister;

        /* copy data to CAN emergency message */
        CO_memcpy(emPr->CANtxBuff->data, em->bufReadPtr, 8U);
        CO_memcpy((uint8_t*)&preDEF, em->bufReadPtr, 4U);
        em->bufReadPtr += 8;

        /* Update read buffer pointer and reset inhibit timer */
        if(em->bufReadPtr == em->bufEnd){
            em->bufReadPtr = em->buf;
        }
        emPr->inhibitEmTimer = 0U;

        /* verify message buffer overflow, then clear full flag */
        if(em->bufFull == 2U){
            em->bufFull = 0U;    /* will be updated below */
            CO_errorReport(em, CO_EM_EMERGENCY_BUFFER_FULL, CO_EMC_GENERIC, 0U);
        }
        else{
            em->bufFull = 0;
        }

        /* write to 'pre-defined error field' (object dictionary, index 0x1003) */
        if(emPr->preDefErr){
            uint8_t i;

            if(emPr->preDefErrNoOfErrors < emPr->preDefErrSize)
                emPr->preDefErrNoOfErrors++;
            for(i=emPr->preDefErrNoOfErrors-1; i>0; i--)
                emPr->preDefErr[i] = emPr->preDefErr[i-1];
            emPr->preDefErr[0] = preDEF;
        }

        /* send CAN message */
        CO_CANsend(emPr->CANdev, emPr->CANtxBuff);
    }

    return;
}
예제 #8
0
uint8_t CO_SYNC_process(
        CO_SYNC_t              *SYNC,
        uint32_t                timeDifference_us,
        uint32_t                ObjDict_synchronousWindowLength)
{
    uint8_t ret = 0;
    uint32_t timerNew;

    if(*SYNC->operatingState == CO_NMT_OPERATIONAL || *SYNC->operatingState == CO_NMT_PRE_OPERATIONAL){
        /* update sync timer, no overflow */
        timerNew = SYNC->timer + timeDifference_us;
        if(timerNew > SYNC->timer) SYNC->timer = timerNew;

        /* was SYNC just received */
        if(SYNC->CANrxNew){
            SYNC->timer = 0;
            ret = 1;
        }

        /* SYNC producer */
        if(SYNC->isProducer && SYNC->periodTime){
            if(SYNC->timer >= SYNC->periodTime){
                if(++SYNC->counter > SYNC->counterOverflowValue) SYNC->counter = 1;
                SYNC->timer = 0;
                ret = 1;
                if(SYNC->cbSync != NULL){
                    SYNC->cbSync(false); //callback
                }
                SYNC->CANtxBuff->data[0] = SYNC->counter;
                CO_CANsend(SYNC->CANdevTx, SYNC->CANtxBuff);
            }
        }

        /* Synchronous PDOs are allowed only inside time window */
        if(ObjDict_synchronousWindowLength){
            if(SYNC->timer > ObjDict_synchronousWindowLength){
                if(SYNC->curentSyncTimeIsInsideWindow){
                    ret = 2;
                }
                SYNC->curentSyncTimeIsInsideWindow = false;
            }
            else{
                SYNC->curentSyncTimeIsInsideWindow = true;
            }
        }
        else{
            SYNC->curentSyncTimeIsInsideWindow = true;
        }

        /* Verify timeout of SYNC */
        if(SYNC->periodTime && SYNC->timer > SYNC->periodTimeoutTime && *SYNC->operatingState == CO_NMT_OPERATIONAL)
            CO_errorReport(SYNC->em, CO_EM_SYNC_TIME_OUT, CO_EMC_COMMUNICATION, SYNC->timer);
    }

    /* verify error from receive function */
    if(SYNC->receiveError != 0U){
        CO_errorReport(SYNC->em, CO_EM_SYNC_LENGTH, CO_EMC_SYNC_DATA_LENGTH, (uint32_t)SYNC->receiveError);
        SYNC->receiveError = 0U;
    }

    SYNC->CANrxNew = false;

    return ret;
}
void CO_EM_process(
        CO_EMpr_t              *EMpr,
        uint8_t                 NMTisPreOrOperational,
        uint16_t                timeDifference_100us,
        uint16_t                EMinhTime)
{

    CO_EM_t *EM = EMpr->EM;
    uint8_t errorRegister;

    /* verify errors from driver and other */
    CO_CANverifyErrors(EMpr->CANdev);
    if(EM->errorReportBusyError){
        CO_errorReport(EM, ERROR_ERROR_REPORT_BUSY, EM->errorReportBusyError);
        EM->errorReportBusyError = 0;
    }
    if(EM->wrongErrorReport){
        CO_errorReport(EM, ERROR_WRONG_ERROR_REPORT, EM->wrongErrorReport);
        EM->wrongErrorReport = 0;
    }


    /* calculate Error register */
    errorRegister = 0;
    /* generic error */
    if(EM->errorStatusBits[5])
        errorRegister |= 0x01;
        
    /* communication error (overrun, error state) */
    if(EM->errorStatusBits[2] || EM->errorStatusBits[3]){
        printf("EM->errorStatusBits[2] || EM->errorStatusBits[3]\n\r");
        errorRegister |= 0x10;
    }
    *EMpr->errorRegister = (*EMpr->errorRegister & 0xEE) | errorRegister;

    /* inhibit time */
    if(EMpr->inhibitEmTimer < EMinhTime) EMpr->inhibitEmTimer += timeDifference_100us;

    /* send Emergency message. */
    if(     NMTisPreOrOperational &&
            !EMpr->CANtxBuff->bufferFull &&
            EMpr->inhibitEmTimer >= EMinhTime &&
            (EM->bufReadPtr != EM->bufWritePtr || EM->bufFull))
    {
        /* copy data from emergency buffer into CAN buffer and preDefinedErrorField buffer */
        uint8_t* EMdataPtr = EM->bufReadPtr;
        uint8_t* CANtxData = EMpr->CANtxBuff->data;
        uint32_t preDEF;
        uint8_t* ppreDEF = (uint8_t*) &preDEF;
        *(CANtxData++) = *EMdataPtr; *(ppreDEF++) = *(EMdataPtr++);
        *(CANtxData++) = *EMdataPtr; *(ppreDEF++) = *(EMdataPtr++);
        *(CANtxData++) = *EMpr->errorRegister; *(ppreDEF++) = *EMpr->errorRegister; EMdataPtr++;
        *(CANtxData++) = *EMdataPtr; *(ppreDEF++) = *(EMdataPtr++);
        *(CANtxData++) = *(EMdataPtr++);
        *(CANtxData++) = *(EMdataPtr++);
        *(CANtxData++) = *(EMdataPtr++);
        *(CANtxData++) = *(EMdataPtr++);

        /* Update read buffer pointer and reset inhibit timer */
        if(EMdataPtr == EM->bufEnd) EM->bufReadPtr = EM->buf;
        else                        EM->bufReadPtr = EMdataPtr;
        EMpr->inhibitEmTimer = 0;

        /* verify message buffer overflow, then clear full flag */
        if(EM->bufFull == 2){
            EM->bufFull = 0;
            CO_errorReport(EM, ERROR_EMERGENCY_BUFFER_FULL, 0);
        }
        else
            EM->bufFull = 0;

        /* write to 'pre-defined error field' (object dictionary, index 0x1003) */
        if(EMpr->preDefErr){
            uint8_t i;

            if(EMpr->preDefErrNoOfErrors < EMpr->preDefErrSize)
                EMpr->preDefErrNoOfErrors++;
            for(i=EMpr->preDefErrNoOfErrors-1; i>0; i--)
                EMpr->preDefErr[i] = EMpr->preDefErr[i-1];
            EMpr->preDefErr[0] = preDEF;
        }

        CO_CANsend(EMpr->CANdev, EMpr->CANtxBuff);
    }

    return;
}
예제 #10
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;
        }