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