/* * Configure TPDO Communication parameter. * * Function is called from commuincation reset or when parameter changes. * * Function configures following variable from CO_TPDO_t: _valid_. It also * configures CAN tx buffer. If configuration fails, emergency message is send * and device is not able to enter NMT operational. * * @param TPDO TPDO object. * @param COB_IDUsedByTPDO _TPDO communication parameter_, _COB-ID for PDO_ variable * from Object dictionary (index 0x1400+, subindex 1). * @param syncFlag Indicate, if TPDO is synchronous. */ static void CO_TPDOconfigCom(CO_TPDO_t* TPDO, uint32_t COB_IDUsedByTPDO, uint8_t syncFlag){ uint16_t ID; ID = (uint16_t)COB_IDUsedByTPDO; /* is TPDO used? */ if((COB_IDUsedByTPDO & 0xBFFFF800L) == 0 && TPDO->dataLength && ID){ /* is used default COB-ID? */ if(ID == TPDO->defaultCOB_ID) ID += TPDO->nodeId; TPDO->valid = true; } else{ ID = 0; TPDO->valid = false; } TPDO->CANtxBuff = CO_CANtxBufferInit( TPDO->CANdevTx, /* CAN device */ TPDO->CANdevTxIdx, /* index of specific buffer inside CAN module */ ID, /* CAN identifier */ 0, /* rtr */ TPDO->dataLength, /* number of data bytes */ syncFlag); /* synchronous message flag bit */ if(TPDO->CANtxBuff == 0){ TPDO->valid = false; } }
/** * Function for accessing _Synchronous counter overflow value_ (index 0x1019) from SDO server. * * For more information see file CO_SDO.h. */ static CO_SDO_abortCode_t CO_ODF_1019(CO_ODF_arg_t *ODF_arg){ CO_SYNC_t *SYNC; uint8_t value; CO_SDO_abortCode_t ret = CO_SDO_AB_NONE; SYNC = (CO_SYNC_t*) ODF_arg->object; value = ODF_arg->data[0]; if(!ODF_arg->reading){ uint8_t len = 0U; if(SYNC->periodTime){ ret = CO_SDO_AB_DATA_DEV_STATE; } else{ SYNC->counterOverflowValue = value; if(value != 0){ len = 1U; } SYNC->CANtxBuff = CO_CANtxBufferInit( SYNC->CANdevTx, /* CAN device */ SYNC->CANdevTxIdx, /* index of specific buffer inside CAN module */ SYNC->COB_ID, /* CAN identifier */ 0, /* rtr */ len, /* number of data bytes */ 0); /* synchronous message flag bit */ } } return ret; }
UNSIGNED32 CO_ODF_1019( void *object, UNSIGNED16 index, UNSIGNED8 subIndex, UNSIGNED8 length, UNSIGNED16 attribute, UNSIGNED8 dir, void *dataBuff, const void *pData) { UNSIGNED32 abortCode; CO_SYNC_t *SYNC; SYNC = (CO_SYNC_t*) object; //this is the correct pointer type of the first argument if(dir == 1){ //Writing Object Dictionary variable if(SYNC->periodTime) return 0x08000022L; //Data cannot be transferred or stored to the application because of the present device state. } abortCode = CO_ODF(object, index, subIndex, length, attribute, dir, dataBuff, pData); if(abortCode == 0 && dir == 1){ UNSIGNED8 len = 0; if(SYNC->counterOverflowValue) len = 1; SYNC->counterOverflowValue = *((UNSIGNED8*)dataBuff); SYNC->CANtxBuff = CO_CANtxBufferInit( SYNC->CANdevTx, //CAN device SYNC->CANdevTxIdx, //index of specific buffer inside CAN module SYNC->COB_ID, //CAN identifier 0, //rtr len, //number of data bytes 0); //synchronous message flag bit } return abortCode; }
CO_ReturnError_t CO_EM_init( CO_EM_t *em, CO_EMpr_t *emPr, CO_SDO_t *SDO, uint8_t *errorStatusBits, uint8_t errorStatusBitsSize, uint8_t *errorRegister, uint32_t *preDefErr, uint8_t preDefErrSize, CO_CANmodule_t *CANdev, uint16_t CANdevTxIdx, uint16_t CANidTxEM) { uint8_t i; /* verify arguments */ if(em==NULL || emPr==NULL || SDO==NULL || errorStatusBits==NULL || errorStatusBitsSize<6U || errorRegister==NULL || preDefErr==NULL || CANdev==NULL){ return CO_ERROR_ILLEGAL_ARGUMENT; } /* Configure object variables */ em->errorStatusBits = errorStatusBits; em->errorStatusBitsSize = errorStatusBitsSize; em->bufEnd = em->buf + (CO_EM_INTERNAL_BUFFER_SIZE * 8); em->bufWritePtr = em->buf; em->bufReadPtr = em->buf; em->bufFull = 0U; em->wrongErrorReport = 0U; em->pFunctSignal = NULL; emPr->em = em; emPr->errorRegister = errorRegister; emPr->preDefErr = preDefErr; emPr->preDefErrSize = preDefErrSize; emPr->preDefErrNoOfErrors = 0U; emPr->inhibitEmTimer = 0U; /* clear error status bits */ for(i=0U; i<errorStatusBitsSize; i++){ em->errorStatusBits[i] = 0U; } /* Configure Object dictionary entry at index 0x1003 and 0x1014 */ CO_OD_configure(SDO, OD_H1003_PREDEF_ERR_FIELD, CO_ODF_1003, (void*)emPr, 0, 0U); CO_OD_configure(SDO, OD_H1014_COBID_EMERGENCY, CO_ODF_1014, (void*)&SDO->nodeId, 0, 0U); /* configure emergency message CAN transmission */ emPr->CANdev = CANdev; emPr->CANdev->em = (void*)em; /* update pointer inside CAN device. */ emPr->CANtxBuff = CO_CANtxBufferInit( CANdev, /* CAN device */ CANdevTxIdx, /* index of specific buffer inside CAN module */ CANidTxEM, /* CAN identifier */ 0, /* rtr */ 8U, /* number of data bytes */ 0); /* synchronous message flag bit */ return CO_ERROR_NO; }
int16_t CO_EM_init( CO_EM_t *EM, CO_EMpr_t *EMpr, CO_SDO_t *SDO, uint8_t *errorStatusBits, uint8_t errorStatusBitsSize, uint8_t *errorRegister, uint32_t *preDefErr, uint8_t preDefErrSize, CO_CANmodule_t *CANdev, uint16_t CANdevTxIdx, uint16_t CANidTxEM) { uint8_t i; /* Configure object variables */ EM->errorStatusBits = errorStatusBits; EM->errorStatusBitsSize = errorStatusBitsSize; if(errorStatusBitsSize < 6) return CO_ERROR_ILLEGAL_ARGUMENT; EM->bufEnd = EM->buf + CO_EM_INTERNAL_BUFFER_SIZE * 8; EM->bufWritePtr = EM->buf; EM->bufReadPtr = EM->buf; EM->bufFull = 0; EM->wrongErrorReport = 0; EM->errorReportBusy = 0; EM->errorReportBusyError = 0; EMpr->EM = EM; EMpr->errorRegister = errorRegister; EMpr->preDefErr = preDefErr; EMpr->preDefErrSize = preDefErrSize; EMpr->preDefErrNoOfErrors = 0; EMpr->inhibitEmTimer = 0; /* clear error status bits */ for(i=0; i<errorStatusBitsSize; i++) EM->errorStatusBits[i] = 0; /* Configure Object dictionary entry at index 0x1003 and 0x1014 */ CO_OD_configure(SDO, 0x1003, CO_ODF_1003, (void*)EMpr, 0, 0); CO_OD_configure(SDO, 0x1014, CO_ODF_1014, (void*)&SDO->nodeId, 0, 0); /* configure emergency message CAN transmission */ EMpr->CANdev = CANdev; EMpr->CANdev->EM = (void*)EM; /* update pointer inside CAN device. */ EMpr->CANtxBuff = CO_CANtxBufferInit( CANdev, /* CAN device */ CANdevTxIdx, /* index of specific buffer inside CAN module */ CANidTxEM, /* CAN identifier */ 0, /* rtr */ 8, /* number of data bytes */ 0); /* synchronous message flag bit */ return CO_ERROR_NO; }
INTEGER8 CO_SDOclient_setup( CO_SDOclient_t *SDO_C, UNSIGNED32 COB_IDClientToServer, UNSIGNED32 COB_IDServerToClient, UNSIGNED8 nodeIDOfTheSDOServer) { //verify parameters if((COB_IDClientToServer&0x7FFFF800L) || (COB_IDServerToClient&0x7FFFF800L) || nodeIDOfTheSDOServer > 127) return -2; //Configure object variables SDO_C->state = 0; //setup Object Dictionary variables if((COB_IDClientToServer & 0x80000000L) || (COB_IDServerToClient & 0x80000000L) || nodeIDOfTheSDOServer == 0) { //SDO is NOT used SDO_C->ObjDict_SDOClientParameter->COB_IDClientToServer = 0x80000000L; SDO_C->ObjDict_SDOClientParameter->COB_IDServerToClient = 0x80000000L; SDO_C->ObjDict_SDOClientParameter->nodeIDOfTheSDOServer = 0; } else { if(COB_IDClientToServer == 0 || COB_IDServerToClient == 0) { SDO_C->ObjDict_SDOClientParameter->COB_IDClientToServer = 0x600 + nodeIDOfTheSDOServer; SDO_C->ObjDict_SDOClientParameter->COB_IDServerToClient = 0x580 + nodeIDOfTheSDOServer; } else { SDO_C->ObjDict_SDOClientParameter->COB_IDClientToServer = COB_IDClientToServer; SDO_C->ObjDict_SDOClientParameter->COB_IDServerToClient = COB_IDServerToClient; } SDO_C->ObjDict_SDOClientParameter->nodeIDOfTheSDOServer = nodeIDOfTheSDOServer; } //configure SDO client CAN reception CO_CANrxBufferInit( SDO_C->CANdevRx, //CAN device SDO_C->CANdevRxIdx, //rx buffer index (UNSIGNED16)SDO_C->ObjDict_SDOClientParameter->COB_IDServerToClient,//CAN identifier 0x7FF, //mask 0, //rtr (void*)SDO_C, //object passed to receive function CO_SDOclient_receive); //this function will process received message //configure SDO client CAN transmission SDO_C->CANtxBuff = CO_CANtxBufferInit( SDO_C->CANdevTx, //CAN device SDO_C->CANdevTxIdx, //index of specific buffer inside CAN module (UNSIGNED16)SDO_C->ObjDict_SDOClientParameter->COB_IDClientToServer,//CAN identifier 0, //rtr 8, //number of data bytes 0); //synchronous message flag bit return 0; }
/* * Function for accessing _COB ID SYNC Message_ (index 0x1005) from SDO server. * * For more information see file CO_SDO.h. */ static CO_SDO_abortCode_t CO_ODF_1005(CO_ODF_arg_t *ODF_arg){ CO_SYNC_t *SYNC; uint32_t value; CO_SDO_abortCode_t ret = CO_SDO_AB_NONE; SYNC = (CO_SYNC_t*) ODF_arg->object; value = CO_getUint32(ODF_arg->data); if(!ODF_arg->reading){ uint8_t configureSyncProducer = 0; /* only 11-bit CAN identifier is supported */ if(value & 0x20000000UL){ ret = CO_SDO_AB_INVALID_VALUE; } else{ /* is 'generate Sync messge' bit set? */ if(value & 0x40000000UL){ /* if bit was set before, value can not be changed */ if(SYNC->isProducer){ ret = CO_SDO_AB_DATA_DEV_STATE; } else{ configureSyncProducer = 1; } } } /* configure sync producer and consumer */ if(ret == CO_SDO_AB_NONE){ SYNC->COB_ID = (uint16_t)(value & 0x7FFU); if(configureSyncProducer){ uint8_t len = 0U; if(SYNC->counterOverflowValue != 0U){ len = 1U; SYNC->counter = 0U; SYNC->timer = 0U; } SYNC->CANtxBuff = CO_CANtxBufferInit( SYNC->CANdevTx, /* CAN device */ SYNC->CANdevTxIdx, /* index of specific buffer inside CAN module */ SYNC->COB_ID, /* CAN identifier */ 0, /* rtr */ len, /* number of data bytes */ 0); /* synchronous message flag bit */ SYNC->isProducer = true; } else{ SYNC->isProducer = false; } CO_CANrxBufferInit( SYNC->CANdevRx, /* CAN device */ SYNC->CANdevRxIdx, /* rx buffer index */ SYNC->COB_ID, /* CAN identifier */ 0x7FF, /* mask */ 0, /* rtr */ (void*)SYNC, /* object passed to receive function */ CO_SYNC_receive); /* this function will process received message */ } } return ret; }
CO_ReturnError_t CO_SYNC_init( CO_SYNC_t *SYNC, CO_EM_t *em, CO_SDO_t *SDO, uint8_t *operatingState, uint32_t COB_ID_SYNCMessage, uint32_t communicationCyclePeriod, uint8_t synchronousCounterOverflowValue, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx) { uint8_t len = 0; /* verify arguments */ if(SYNC==NULL || em==NULL || SDO==NULL || operatingState==NULL || CANdevRx==NULL || CANdevTx==NULL){ return CO_ERROR_ILLEGAL_ARGUMENT; } /* Configure object variables */ SYNC->isProducer = (COB_ID_SYNCMessage&0x40000000L) ? true : false; SYNC->COB_ID = COB_ID_SYNCMessage&0x7FF; SYNC->periodTime = communicationCyclePeriod; SYNC->periodTimeoutTime = communicationCyclePeriod / 2 * 3; /* overflow? */ if(SYNC->periodTimeoutTime < communicationCyclePeriod) SYNC->periodTimeoutTime = 0xFFFFFFFFL; SYNC->counterOverflowValue = synchronousCounterOverflowValue; if(synchronousCounterOverflowValue) len = 1; SYNC->curentSyncTimeIsInsideWindow = true; SYNC->CANrxNew = false; SYNC->timer = 0; SYNC->counter = 0; SYNC->receiveError = 0U; SYNC->em = em; SYNC->operatingState = operatingState; SYNC->cbSync = NULL; SYNC->CANdevRx = CANdevRx; SYNC->CANdevRxIdx = CANdevRxIdx; /* Configure Object dictionary entry at index 0x1005, 0x1006 and 0x1019 */ CO_OD_configure(SDO, OD_H1005_COBID_SYNC, CO_ODF_1005, (void*)SYNC, 0, 0); CO_OD_configure(SDO, OD_H1006_COMM_CYCL_PERIOD, CO_ODF_1006, (void*)SYNC, 0, 0); CO_OD_configure(SDO, OD_H1019_SYNC_CNT_OVERFLOW, CO_ODF_1019, (void*)SYNC, 0, 0); /* configure SYNC CAN reception */ CO_CANrxBufferInit( CANdevRx, /* CAN device */ CANdevRxIdx, /* rx buffer index */ SYNC->COB_ID, /* CAN identifier */ 0x7FF, /* mask */ 0, /* rtr */ (void*)SYNC, /* object passed to receive function */ CO_SYNC_receive); /* this function will process received message */ /* configure SYNC CAN transmission */ SYNC->CANdevTx = CANdevTx; SYNC->CANdevTxIdx = CANdevTxIdx; SYNC->CANtxBuff = CO_CANtxBufferInit( CANdevTx, /* CAN device */ CANdevTxIdx, /* index of specific buffer inside CAN module */ SYNC->COB_ID, /* CAN identifier */ 0, /* rtr */ len, /* number of data bytes */ 0); /* synchronous message flag bit */ return CO_ERROR_NO; }
UNSIGNED32 CO_ODF_1005( void *object, UNSIGNED16 index, UNSIGNED8 subIndex, UNSIGNED8 length, UNSIGNED16 attribute, UNSIGNED8 dir, void *dataBuff, const void *pData) { UNSIGNED8 configureSyncProducer = 0; UNSIGNED32 abortCode; CO_SYNC_t *SYNC; UNSIGNED32 COB_ID; SYNC = (CO_SYNC_t*) object; //this is the correct pointer type of the first argument memcpySwap4((UNSIGNED8*)&COB_ID, (UNSIGNED8*)dataBuff); if(dir == 1){ //Writing Object Dictionary variable //only 11-bit CAN identifier is supported if(COB_ID & 0x20000000L) return 0x06090030L; //Invalid value for parameter (download only). //is 'generate Sync messge' bit set? if(COB_ID&0x40000000L){ //if bit was set before, value can not be changed if(SYNC->isProducer) return 0x08000022L; //Data cannot be transferred or stored to the application because of the present device state. configureSyncProducer = 1; } } abortCode = CO_ODF(object, index, subIndex, length, attribute, dir, dataBuff, pData); if(abortCode == 0 && dir == 1){ SYNC->COB_ID = COB_ID&0x7FF; if(configureSyncProducer){ UNSIGNED8 len = 0; if(SYNC->counterOverflowValue){ len = 1; SYNC->counter = 0; SYNC->running = 0; SYNC->timer = 0; } SYNC->CANtxBuff = CO_CANtxBufferInit( SYNC->CANdevTx, //CAN device SYNC->CANdevTxIdx, //index of specific buffer inside CAN module SYNC->COB_ID, //CAN identifier 0, //rtr len, //number of data bytes 0); //synchronous message flag bit SYNC->isProducer = 1; } else{ SYNC->isProducer = 0; } CO_CANrxBufferInit(SYNC->CANdevRx, //CAN device SYNC->CANdevRxIdx, //rx buffer index SYNC->COB_ID, //CAN identifier 0x7FF, //mask 0, //rtr (void*)SYNC, //object passed to receive function CO_SYNC_receive); //this function will process received message } return abortCode; }
INTEGER16 CO_SYNC_init( CO_SYNC_t **ppSYNC, CO_emergencyReport_t *EM, CO_SDO_t *SDO, UNSIGNED8 *operatingState, UNSIGNED32 ObjDict_COB_ID_SYNCMessage, UNSIGNED32 ObjDict_communicationCyclePeriod, UNSIGNED8 ObjDict_synchronousCounterOverflowValue, CO_CANmodule_t *CANdevRx, UNSIGNED16 CANdevRxIdx, CO_CANmodule_t *CANdevTx, UNSIGNED16 CANdevTxIdx) { UNSIGNED8 len = 0; CO_SYNC_t *SYNC; //allocate memory if not already allocated if((*ppSYNC) == NULL){ if(((*ppSYNC) = (CO_SYNC_t*) malloc(sizeof(CO_SYNC_t))) == NULL){ return CO_ERROR_OUT_OF_MEMORY;} } SYNC = *ppSYNC; //pointer to (newly created) object //Configure object variables SYNC->isProducer = (ObjDict_COB_ID_SYNCMessage&0x40000000L) ? 1 : 0; SYNC->COB_ID = ObjDict_COB_ID_SYNCMessage&0x7FF; SYNC->periodTime = ObjDict_communicationCyclePeriod; SYNC->periodTimeoutTime = ObjDict_communicationCyclePeriod / 2 * 3; //overflow? if(SYNC->periodTimeoutTime < ObjDict_communicationCyclePeriod) SYNC->periodTimeoutTime = 0xFFFFFFFFL; SYNC->counterOverflowValue = ObjDict_synchronousCounterOverflowValue; if(ObjDict_synchronousCounterOverflowValue) len = 1; SYNC->curentSyncTimeIsInsideWindow = 1; CANdevTx->curentSyncTimeIsInsideWindow = &SYNC->curentSyncTimeIsInsideWindow; //parameter inside CAN module. SYNC->running = 0; SYNC->timer = 0; SYNC->counter = 0; SYNC->EM = EM; SYNC->operatingState = operatingState; SYNC->CANdevRx = CANdevRx; SYNC->CANdevRxIdx = CANdevRxIdx; //Configure SDO server for first argument of CO_ODF_1005, CO_ODF_1006 and CO_ODF_1019. CO_OD_configureArgumentForODF(SDO, 0x1005, (void*)SYNC); CO_OD_configureArgumentForODF(SDO, 0x1006, (void*)SYNC); CO_OD_configureArgumentForODF(SDO, 0x1019, (void*)SYNC); //configure SYNC CAN reception CO_CANrxBufferInit( CANdevRx, //CAN device CANdevRxIdx, //rx buffer index SYNC->COB_ID, //CAN identifier 0x7FF, //mask 0, //rtr (void*)SYNC, //object passed to receive function CO_SYNC_receive); //this function will process received message //configure SYNC CAN transmission SYNC->CANdevTx = CANdevTx; SYNC->CANdevTxIdx = CANdevTxIdx; SYNC->CANtxBuff = CO_CANtxBufferInit( CANdevTx, //CAN device CANdevTxIdx, //index of specific buffer inside CAN module SYNC->COB_ID, //CAN identifier 0, //rtr len, //number of data bytes 0); //synchronous message flag bit return CO_ERROR_NO; }
CO_ReturnError_t CO_init( int32_t CANbaseAddress, uint8_t nodeId, uint16_t bitRate) { int16_t i; CO_ReturnError_t err; #ifndef CO_USE_GLOBALS uint16_t errCnt; #endif #if CO_NO_TRACE > 0 uint32_t CO_traceBufferSize[CO_NO_TRACE]; #endif /* Verify parameters from CO_OD */ if( sizeof(OD_TPDOCommunicationParameter_t) != sizeof(CO_TPDOCommPar_t) || sizeof(OD_TPDOMappingParameter_t) != sizeof(CO_TPDOMapPar_t) || sizeof(OD_RPDOCommunicationParameter_t) != sizeof(CO_RPDOCommPar_t) || sizeof(OD_RPDOMappingParameter_t) != sizeof(CO_RPDOMapPar_t)) { return CO_ERROR_PARAMETERS; } #if CO_NO_SDO_CLIENT == 1 if(sizeof(OD_SDOClientParameter_t) != sizeof(CO_SDOclientPar_t)){ return CO_ERROR_PARAMETERS; } #endif /* Initialize CANopen object */ #ifdef CO_USE_GLOBALS CO = &COO; CO->CANmodule[0] = &COO_CANmodule; CO_CANmodule_rxArray0 = &COO_CANmodule_rxArray0[0]; CO_CANmodule_txArray0 = &COO_CANmodule_txArray0[0]; for(i=0; i<CO_NO_SDO_SERVER; i++) CO->SDO[i] = &COO_SDO[i]; CO_SDO_ODExtensions = &COO_SDO_ODExtensions[0]; CO->em = &COO_EM; CO->emPr = &COO_EMpr; CO->NMT = &COO_NMT; CO->SYNC = &COO_SYNC; for(i=0; i<CO_NO_RPDO; i++) CO->RPDO[i] = &COO_RPDO[i]; for(i=0; i<CO_NO_TPDO; i++) CO->TPDO[i] = &COO_TPDO[i]; CO->HBcons = &COO_HBcons; CO_HBcons_monitoredNodes = &COO_HBcons_monitoredNodes[0]; #if CO_NO_SDO_CLIENT == 1 CO->SDOclient = &COO_SDOclient; #endif #if CO_NO_TRACE > 0 for(i=0; i<CO_NO_TRACE; i++) { CO->trace[i] = &COO_trace[i]; CO_traceTimeBuffers[i] = &COO_traceTimeBuffers[i][0]; CO_traceValueBuffers[i] = &COO_traceValueBuffers[i][0]; CO_traceBufferSize[i] = CO_TRACE_BUFFER_SIZE_FIXED; } #endif #else if(CO == NULL){ /* Use malloc only once */ CO = &COO; CO->CANmodule[0] = (CO_CANmodule_t *) calloc(1, sizeof(CO_CANmodule_t)); CO_CANmodule_rxArray0 = (CO_CANrx_t *) calloc(CO_RXCAN_NO_MSGS, sizeof(CO_CANrx_t)); CO_CANmodule_txArray0 = (CO_CANtx_t *) calloc(CO_TXCAN_NO_MSGS, sizeof(CO_CANtx_t)); for(i=0; i<CO_NO_SDO_SERVER; i++){ CO->SDO[i] = (CO_SDO_t *) calloc(1, sizeof(CO_SDO_t)); } CO_SDO_ODExtensions = (CO_OD_extension_t*) calloc(CO_OD_NoOfElements, sizeof(CO_OD_extension_t)); CO->em = (CO_EM_t *) calloc(1, sizeof(CO_EM_t)); CO->emPr = (CO_EMpr_t *) calloc(1, sizeof(CO_EMpr_t)); CO->NMT = (CO_NMT_t *) calloc(1, sizeof(CO_NMT_t)); CO->SYNC = (CO_SYNC_t *) calloc(1, sizeof(CO_SYNC_t)); for(i=0; i<CO_NO_RPDO; i++){ CO->RPDO[i] = (CO_RPDO_t *) calloc(1, sizeof(CO_RPDO_t)); } for(i=0; i<CO_NO_TPDO; i++){ CO->TPDO[i] = (CO_TPDO_t *) calloc(1, sizeof(CO_TPDO_t)); } CO->HBcons = (CO_HBconsumer_t *) calloc(1, sizeof(CO_HBconsumer_t)); CO_HBcons_monitoredNodes = (CO_HBconsNode_t *) calloc(CO_NO_HB_CONS, sizeof(CO_HBconsNode_t)); #if CO_NO_SDO_CLIENT == 1 CO->SDOclient = (CO_SDOclient_t *) calloc(1, sizeof(CO_SDOclient_t)); #endif #if CO_NO_TRACE > 0 for(i=0; i<CO_NO_TRACE; i++) { CO->trace[i] = (CO_trace_t *) calloc(1, sizeof(CO_trace_t)); CO_traceTimeBuffers[i] = (uint32_t *) calloc(OD_traceConfig[i].size, sizeof(uint32_t)); CO_traceValueBuffers[i] = (int32_t *) calloc(OD_traceConfig[i].size, sizeof(int32_t)); if(CO_traceTimeBuffers[i] != NULL && CO_traceValueBuffers[i] != NULL) { CO_traceBufferSize[i] = OD_traceConfig[i].size; } else { CO_traceBufferSize[i] = 0; } } #endif } CO_memoryUsed = sizeof(CO_CANmodule_t) + sizeof(CO_CANrx_t) * CO_RXCAN_NO_MSGS + sizeof(CO_CANtx_t) * CO_TXCAN_NO_MSGS + sizeof(CO_SDO_t) * CO_NO_SDO_SERVER + sizeof(CO_OD_extension_t) * CO_OD_NoOfElements + sizeof(CO_EM_t) + sizeof(CO_EMpr_t) + sizeof(CO_NMT_t) + sizeof(CO_SYNC_t) + sizeof(CO_RPDO_t) * CO_NO_RPDO + sizeof(CO_TPDO_t) * CO_NO_TPDO + sizeof(CO_HBconsumer_t) + sizeof(CO_HBconsNode_t) * CO_NO_HB_CONS #if CO_NO_SDO_CLIENT == 1 + sizeof(CO_SDOclient_t) #endif + 0; #if CO_NO_TRACE > 0 CO_memoryUsed += sizeof(CO_trace_t) * CO_NO_TRACE; for(i=0; i<CO_NO_TRACE; i++) { CO_memoryUsed += CO_traceBufferSize[i] * 8; } #endif errCnt = 0; if(CO->CANmodule[0] == NULL) errCnt++; if(CO_CANmodule_rxArray0 == NULL) errCnt++; if(CO_CANmodule_txArray0 == NULL) errCnt++; for(i=0; i<CO_NO_SDO_SERVER; i++){ if(CO->SDO[i] == NULL) errCnt++; } if(CO_SDO_ODExtensions == NULL) errCnt++; if(CO->em == NULL) errCnt++; if(CO->emPr == NULL) errCnt++; if(CO->NMT == NULL) errCnt++; if(CO->SYNC == NULL) errCnt++; for(i=0; i<CO_NO_RPDO; i++){ if(CO->RPDO[i] == NULL) errCnt++; } for(i=0; i<CO_NO_TPDO; i++){ if(CO->TPDO[i] == NULL) errCnt++; } if(CO->HBcons == NULL) errCnt++; if(CO_HBcons_monitoredNodes == NULL) errCnt++; #if CO_NO_SDO_CLIENT == 1 if(CO->SDOclient == NULL) errCnt++; #endif #if CO_NO_TRACE > 0 for(i=0; i<CO_NO_TRACE; i++) { if(CO->trace[i] == NULL) errCnt++; } #endif if(errCnt != 0) return CO_ERROR_OUT_OF_MEMORY; #endif CO->CANmodule[0]->CANnormal = false; CO_CANsetConfigurationMode(CANbaseAddress); /* Verify CANopen Node-ID */ if(nodeId<1 || nodeId>127) nodeId = 0x10; err = CO_CANmodule_init( CO->CANmodule[0], CANbaseAddress, CO_CANmodule_rxArray0, CO_RXCAN_NO_MSGS, CO_CANmodule_txArray0, CO_TXCAN_NO_MSGS, bitRate); if(err){CO_delete(CANbaseAddress); return err;} for (i=0; i<CO_NO_SDO_SERVER; i++) { uint32_t COB_IDClientToServer; uint32_t COB_IDServerToClient; if(i==0){ /*Default SDO server must be located at first index*/ COB_IDClientToServer = CO_CAN_ID_RSDO + nodeId; COB_IDServerToClient = CO_CAN_ID_TSDO + nodeId; }else{ COB_IDClientToServer = OD_SDOServerParameter[i].COB_IDClientToServer; COB_IDServerToClient = OD_SDOServerParameter[i].COB_IDServerToClient; } err = CO_SDO_init( CO->SDO[i], COB_IDClientToServer, COB_IDServerToClient, OD_H1200_SDO_SERVER_PARAM+i, i==0 ? 0 : CO->SDO[0], &CO_OD[0], CO_OD_NoOfElements, CO_SDO_ODExtensions, nodeId, CO->CANmodule[0], CO_RXCAN_SDO_SRV+i, CO->CANmodule[0], CO_TXCAN_SDO_SRV+i); } if(err){CO_delete(CANbaseAddress); return err;} err = CO_EM_init( CO->em, CO->emPr, CO->SDO[0], &OD_errorStatusBits[0], ODL_errorStatusBits_stringLength, &OD_errorRegister, &OD_preDefinedErrorField[0], ODL_preDefinedErrorField_arrayLength, CO->CANmodule[0], CO_TXCAN_EMERG, CO_CAN_ID_EMERGENCY + nodeId); if(err){CO_delete(CANbaseAddress); return err;} err = CO_NMT_init( CO->NMT, CO->emPr, nodeId, 500, CO->CANmodule[0], CO_RXCAN_NMT, CO_CAN_ID_NMT_SERVICE, CO->CANmodule[0], CO_TXCAN_HB, CO_CAN_ID_HEARTBEAT + nodeId); if(err){CO_delete(CANbaseAddress); return err;} #if CO_NO_NMT_MASTER == 1 NMTM_txBuff = CO_CANtxBufferInit(/* return pointer to 8-byte CAN data buffer, which should be populated */ CO->CANmodule[0], /* pointer to CAN module used for sending this message */ CO_TXCAN_NMT, /* index of specific buffer inside CAN module */ 0x0000, /* CAN identifier */ 0, /* rtr */ 2, /* number of data bytes */ 0); /* synchronous message flag bit */ #endif err = CO_SYNC_init( CO->SYNC, CO->em, CO->SDO[0], &CO->NMT->operatingState, OD_COB_ID_SYNCMessage, OD_communicationCyclePeriod, OD_synchronousCounterOverflowValue, CO->CANmodule[0], CO_RXCAN_SYNC, CO->CANmodule[0], CO_TXCAN_SYNC); if(err){CO_delete(CANbaseAddress); return err;} for(i=0; i<CO_NO_RPDO; i++){ CO_CANmodule_t *CANdevRx = CO->CANmodule[0]; uint16_t CANdevRxIdx = CO_RXCAN_RPDO + i; err = CO_RPDO_init( CO->RPDO[i], CO->em, CO->SDO[0], &CO->NMT->operatingState, nodeId, ((i<4) ? (CO_CAN_ID_RPDO_1+i*0x100) : 0), 0, (CO_RPDOCommPar_t*) &OD_RPDOCommunicationParameter[i], (CO_RPDOMapPar_t*) &OD_RPDOMappingParameter[i], OD_H1400_RXPDO_1_PARAM+i, OD_H1600_RXPDO_1_MAPPING+i, CANdevRx, CANdevRxIdx); if(err){CO_delete(CANbaseAddress); return err;} } for(i=0; i<CO_NO_TPDO; i++){ err = CO_TPDO_init( CO->TPDO[i], CO->em, CO->SDO[0], &CO->NMT->operatingState, nodeId, ((i<4) ? (CO_CAN_ID_TPDO_1+i*0x100) : 0), 0, (CO_TPDOCommPar_t*) &OD_TPDOCommunicationParameter[i], (CO_TPDOMapPar_t*) &OD_TPDOMappingParameter[i], OD_H1800_TXPDO_1_PARAM+i, OD_H1A00_TXPDO_1_MAPPING+i, CO->CANmodule[0], CO_TXCAN_TPDO+i); if(err){CO_delete(CANbaseAddress); return err;} } err = CO_HBconsumer_init( CO->HBcons, CO->em, CO->SDO[0], &OD_consumerHeartbeatTime[0], CO_HBcons_monitoredNodes, CO_NO_HB_CONS, CO->CANmodule[0], CO_RXCAN_CONS_HB); if(err){CO_delete(CANbaseAddress); return err;} #if CO_NO_SDO_CLIENT == 1 err = CO_SDOclient_init( CO->SDOclient, CO->SDO[0], (CO_SDOclientPar_t*) &OD_SDOClientParameter[0], CO->CANmodule[0], CO_RXCAN_SDO_CLI, CO->CANmodule[0], CO_TXCAN_SDO_CLI); if(err){CO_delete(CANbaseAddress); return err;} #endif #if CO_NO_TRACE > 0 for(i=0; i<CO_NO_TRACE; i++) { CO_trace_init( CO->trace[i], CO->SDO[0], OD_traceConfig[i].axisNo, CO_traceTimeBuffers[i], CO_traceValueBuffers[i], CO_traceBufferSize[i], &OD_traceConfig[i].map, &OD_traceConfig[i].format, &OD_traceConfig[i].trigger, &OD_traceConfig[i].threshold, &OD_trace[i].value, &OD_trace[i].min, &OD_trace[i].max, &OD_trace[i].triggerTime, OD_INDEX_TRACE_CONFIG + i, OD_INDEX_TRACE + i); } #endif return CO_ERROR_NO; }
int16_t CO_SDO_init( CO_SDO_t *SDO, uint16_t COB_IDClientToServer, uint16_t COB_IDServerToClient, uint16_t ObjDictIndex_SDOServerParameter, CO_SDO_t *parentSDO, const CO_OD_entry_t OD[], uint16_t ODSize, CO_OD_extension_t *ODExtensions, uint8_t nodeId, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx) { /* configure own object dictionary */ if(parentSDO == NULL){ uint16_t i; SDO->ownOD = CO_true; SDO->OD = OD; SDO->ODSize = ODSize; SDO->ODExtensions = ODExtensions; /* clear pointers in ODExtensions */ for(i=0U; i<ODSize; i++){ SDO->ODExtensions[i].pODFunc = NULL; SDO->ODExtensions[i].object = NULL; SDO->ODExtensions[i].flags = NULL; } } /* copy object dictionary from parent */ else{ SDO->ownOD = CO_false; SDO->OD = parentSDO->OD; SDO->ODSize = parentSDO->ODSize; SDO->ODExtensions = parentSDO->ODExtensions; } /* Configure object variables */ SDO->nodeId = nodeId; SDO->state = CO_SDO_ST_IDLE; SDO->CANrxNew = CO_false; SDO->pFunctSignal = 0; SDO->functArg = 0; /* Configure Object dictionary entry at index 0x1200 */ if(ObjDictIndex_SDOServerParameter == OD_H1200_SDO_SERVER_PARAM){ CO_OD_configure(SDO, ObjDictIndex_SDOServerParameter, CO_ODF_1200, (void*)&SDO->nodeId, 0U, 0U); } /* configure SDO server CAN reception */ CO_CANrxBufferInit( CANdevRx, /* CAN device */ CANdevRxIdx, /* rx buffer index */ COB_IDClientToServer, /* CAN identifier */ 0x7FF, /* mask */ 0, /* rtr */ (void*)SDO, /* object passed to receive function */ CO_SDO_receive); /* this function will process received message */ /* configure SDO server CAN transmission */ SDO->CANdevTx = CANdevTx; SDO->CANtxBuff = CO_CANtxBufferInit( CANdevTx, /* CAN device */ CANdevTxIdx, /* index of specific buffer inside CAN module */ COB_IDServerToClient, /* CAN identifier */ 0, /* rtr */ 8, /* number of data bytes */ 0); /* synchronous message flag bit */ return CO_ERROR_NO; }