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 */
}
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);
}
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;
}
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;
}
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);
}
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_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;
}
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;
}
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;
}
