//===========================================================================
void CO_FlashRegisterODFunctions(CO_t* CO)
{
CO_OD_configure(CO->SDO, OD_H1010_STORE_PARAM_FUNC,
CO_ODF_1010_StoreParam, (void*)0, 0, 0);
CO_OD_configure(CO->SDO, OD_H1011_REST_PARAM_FUNC,
CO_ODF_1011_RestoreParam, (void*)0, 0, 0);
}
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;
}
CO_ReturnError_t CO_TPDO_init(
CO_TPDO_t *TPDO,
CO_EM_t *em,
CO_SDO_t *SDO,
uint8_t *operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,
uint8_t restrictionFlags,
const CO_TPDOCommPar_t *TPDOCommPar,
const CO_TPDOMapPar_t *TPDOMapPar,
uint16_t idx_TPDOCommPar,
uint16_t idx_TPDOMapPar,
CO_CANmodule_t *CANdevTx,
uint16_t CANdevTxIdx)
{
/* verify arguments */
if(TPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
TPDOCommPar==NULL || TPDOMapPar==NULL || CANdevTx==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Configure object variables */
TPDO->em = em;
TPDO->SDO = SDO;
TPDO->TPDOCommPar = TPDOCommPar;
TPDO->TPDOMapPar = TPDOMapPar;
TPDO->operatingState = operatingState;
TPDO->nodeId = nodeId;
TPDO->defaultCOB_ID = defaultCOB_ID;
TPDO->restrictionFlags = restrictionFlags;
/* Configure Object dictionary entry at index 0x1800+ and 0x1A00+ */
CO_OD_configure(SDO, idx_TPDOCommPar, CO_ODF_TPDOcom, (void*)TPDO, 0, 0);
CO_OD_configure(SDO, idx_TPDOMapPar, CO_ODF_TPDOmap, (void*)TPDO, 0, 0);
/* configure communication and mapping */
TPDO->CANdevTx = CANdevTx;
TPDO->CANdevTxIdx = CANdevTxIdx;
TPDO->syncCounter = 255;
TPDO->inhibitTimer = 0;
TPDO->eventTimer = ((uint32_t) TPDOCommPar->eventTimer) * 1000;
if(TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1;
CO_TPDOconfigMap(TPDO, TPDOMapPar->numberOfMappedObjects);
CO_TPDOconfigCom(TPDO, TPDOCommPar->COB_IDUsedByTPDO, ((TPDOCommPar->transmissionType<=240) ? 1 : 0));
if((TPDOCommPar->transmissionType>240 &&
TPDOCommPar->transmissionType<254) ||
TPDOCommPar->SYNCStartValue>240){
TPDO->valid = false;
}
return CO_ERROR_NO;
}
void CO_EE_init_2(
CO_EE_t *ee,
CO_ReturnError_t eeStatus,
CO_SDO_t *SDO,
CO_EM_t *em)
{
CO_OD_configure(SDO, OD_H1010_STORE_PARAM_FUNC, CO_ODF_1010, (void*)ee, 0, 0U);
CO_OD_configure(SDO, OD_H1011_REST_PARAM_FUNC, CO_ODF_1011, (void*)ee, 0, 0U);
if(eeStatus != CO_ERROR_NO){
CO_errorReport(em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE, (uint32_t)eeStatus);
}
}
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;
}
int16_t CO_TPDO_init(
CO_TPDO_t *TPDO,
CO_EM_t *EM,
CO_SDO_t *SDO,
uint8_t *operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,
uint8_t restrictionFlags,
const CO_TPDOCommPar_t *TPDOCommPar,
const CO_TPDOMapPar_t *TPDOMapPar,
uint16_t idx_TPDOCommPar,
uint16_t idx_TPDOMapPar,
CO_CANmodule_t *CANdevTx,
uint16_t CANdevTxIdx)
{
/* Configure object variables */
TPDO->EM = EM;
TPDO->SDO = SDO;
TPDO->TPDOCommPar = TPDOCommPar;
TPDO->TPDOMapPar = TPDOMapPar;
TPDO->operatingState = operatingState;
TPDO->nodeId = nodeId;
TPDO->defaultCOB_ID = defaultCOB_ID;
TPDO->restrictionFlags = restrictionFlags;
/* Configure Object dictionary entry at index 0x1800+ and 0x1A00+ */
CO_OD_configure(SDO, idx_TPDOCommPar, CO_ODF_TPDOcom, (void*)TPDO, 0, 0);
CO_OD_configure(SDO, idx_TPDOMapPar, CO_ODF_TPDOmap, (void*)TPDO, 0, 0);
/* configure communication and mapping */
TPDO->CANdevTx = CANdevTx;
TPDO->CANdevTxIdx = CANdevTxIdx;
TPDO->syncCounter = 255;
TPDO->inhibitTimer = 0;
TPDO->eventTimer = TPDOCommPar->eventTimer;
TPDO->SYNCtimerPrevious = 0;
if(TPDOCommPar->transmissionType>=254) TPDO->sendRequest = 1;
CO_TPDOconfigMap(TPDO, TPDOMapPar->numberOfMappedObjects);
CO_TPDOconfigCom(TPDO, TPDOCommPar->COB_IDUsedByTPDO, ((TPDOCommPar->transmissionType<=240) ? 1 : 0));
if((TPDOCommPar->transmissionType>240 &&
TPDOCommPar->transmissionType<254) ||
TPDOCommPar->SYNCStartValue>240){
TPDO->valid = 0;
}
return CO_ERROR_NO;
}
CO_ReturnError_t CO_RPDO_init(
CO_RPDO_t *RPDO,
CO_EM_t *em,
CO_SDO_t *SDO,
uint8_t *operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,
uint8_t restrictionFlags,
const CO_RPDOCommPar_t *RPDOCommPar,
const CO_RPDOMapPar_t *RPDOMapPar,
uint16_t idx_RPDOCommPar,
uint16_t idx_RPDOMapPar,
CO_CANmodule_t *CANdevRx,
uint16_t CANdevRxIdx)
{
/* verify arguments */
if(RPDO==NULL || em==NULL || SDO==NULL || operatingState==NULL ||
RPDOCommPar==NULL || RPDOMapPar==NULL || CANdevRx==NULL){
return CO_ERROR_ILLEGAL_ARGUMENT;
}
/* Configure object variables */
RPDO->em = em;
RPDO->SDO = SDO;
RPDO->RPDOCommPar = RPDOCommPar;
RPDO->RPDOMapPar = RPDOMapPar;
RPDO->operatingState = operatingState;
RPDO->nodeId = nodeId;
RPDO->defaultCOB_ID = defaultCOB_ID;
RPDO->restrictionFlags = restrictionFlags;
/* Configure Object dictionary entry at index 0x1400+ and 0x1600+ */
CO_OD_configure(SDO, idx_RPDOCommPar, CO_ODF_RPDOcom, (void*)RPDO, 0, 0);
CO_OD_configure(SDO, idx_RPDOMapPar, CO_ODF_RPDOmap, (void*)RPDO, 0, 0);
/* configure communication and mapping */
RPDO->CANrxNew = false;
RPDO->CANdevRx = CANdevRx;
RPDO->CANdevRxIdx = CANdevRxIdx;
CO_RPDOconfigMap(RPDO, RPDOMapPar->numberOfMappedObjects);
CO_RPDOconfigCom(RPDO, RPDOCommPar->COB_IDUsedByRPDO);
return CO_ERROR_NO;
}
int16_t CO_RPDO_init(
CO_RPDO_t *RPDO,
CO_EM_t *EM,
CO_SDO_t *SDO,
uint8_t *operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,
uint8_t restrictionFlags,
const CO_RPDOCommPar_t *RPDOCommPar,
const CO_RPDOMapPar_t *RPDOMapPar,
uint16_t idx_RPDOCommPar,
uint16_t idx_RPDOMapPar,
CO_CANmodule_t *CANdevRx,
uint16_t CANdevRxIdx)
{
/* Configure object variables */
RPDO->EM = EM;
RPDO->SDO = SDO;
RPDO->RPDOCommPar = RPDOCommPar;
RPDO->RPDOMapPar = RPDOMapPar;
RPDO->operatingState = operatingState;
RPDO->nodeId = nodeId;
RPDO->defaultCOB_ID = defaultCOB_ID;
RPDO->restrictionFlags = restrictionFlags;
/* Configure Object dictionary entry at index 0x1400+ and 0x1600+ */
CO_OD_configure(SDO, idx_RPDOCommPar, CO_ODF_RPDOcom, (void*)RPDO, 0, 0);
CO_OD_configure(SDO, idx_RPDOMapPar, CO_ODF_RPDOmap, (void*)RPDO, 0, 0);
/* configure communication and mapping */
RPDO->CANrxNew = 0;
RPDO->CANdevRx = CANdevRx;
RPDO->CANdevRxIdx = CANdevRxIdx;
CO_RPDOconfigMap(RPDO, RPDOMapPar->numberOfMappedObjects);
CO_RPDOconfigCom(RPDO, RPDOCommPar->COB_IDUsedByRPDO);
return CO_ERROR_NO;
}
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;
}
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;
}
int main (int argc, char *argv[]) {
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
CO_ReturnError_t odStorStatus_rom, odStorStatus_eeprom;
int CANdevice0Index = 0;
int opt;
bool_t firstRun = true;
char* CANdevice = NULL; /* CAN device, configurable by arguments. */
int nodeId = -1; /* Set to 1..127 by arguments */
bool_t rebootEnable = false; /* Configurable by arguments */
#ifndef CO_SINGLE_THREAD
bool_t commandEnable = false; /* Configurable by arguments */
#endif
if(argc < 3 || strcmp(argv[1], "--help") == 0){
printUsage(argv[0]);
exit(EXIT_SUCCESS);
}
/* Get program options */
while((opt = getopt(argc, argv, "i:p:rc:s:a:")) != -1) {
switch (opt) {
case 'i': nodeId = strtol(optarg, NULL, 0); break;
case 'p': rtPriority = strtol(optarg, NULL, 0); break;
case 'r': rebootEnable = true; break;
#ifndef CO_SINGLE_THREAD
case 'c':
/* In case of empty string keep default name, just enable interface. */
if(strlen(optarg) != 0) {
CO_command_socketPath = optarg;
}
commandEnable = true;
break;
#endif
case 's': odStorFile_rom = optarg; break;
case 'a': odStorFile_eeprom = optarg; break;
default:
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
}
if(optind < argc) {
CANdevice = argv[optind];
CANdevice0Index = if_nametoindex(CANdevice);
}
if(nodeId < 1 || nodeId > 127) {
fprintf(stderr, "Wrong node ID (%d)\n", nodeId);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
if(rtPriority != -1 && (rtPriority < sched_get_priority_min(SCHED_FIFO)
|| rtPriority > sched_get_priority_max(SCHED_FIFO))) {
fprintf(stderr, "Wrong RT priority (%d)\n", rtPriority);
printUsage(argv[0]);
exit(EXIT_FAILURE);
}
if(CANdevice0Index == 0) {
char s[120];
snprintf(s, 120, "Can't find CAN device \"%s\"", CANdevice);
CO_errExit(s);
}
printf("%s - starting CANopen device with Node ID %d(0x%02X)", argv[0], nodeId, nodeId);
/* Verify, if OD structures have proper alignment of initial values */
if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) {
fprintf(stderr, "Program init - %s - Error in CO_OD_RAM.\n", argv[0]);
exit(EXIT_FAILURE);
}
if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) {
fprintf(stderr, "Program init - %s - Error in CO_OD_EEPROM.\n", argv[0]);
exit(EXIT_FAILURE);
}
if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) {
fprintf(stderr, "Program init - %s - Error in CO_OD_ROM.\n", argv[0]);
exit(EXIT_FAILURE);
}
/* initialize Object Dictionary storage */
odStorStatus_rom = CO_OD_storage_init(&odStor, (uint8_t*) &CO_OD_ROM, sizeof(CO_OD_ROM), odStorFile_rom);
odStorStatus_eeprom = CO_OD_storage_init(&odStorAuto, (uint8_t*) &CO_OD_EEPROM, sizeof(CO_OD_EEPROM), odStorFile_eeprom);
/* Catch signals SIGINT and SIGTERM */
if(signal(SIGINT, sigHandler) == SIG_ERR)
CO_errExit("Program init - SIGINIT handler creation failed");
if(signal(SIGTERM, sigHandler) == SIG_ERR)
CO_errExit("Program init - SIGTERM handler creation failed");
/* increase variable each startup. Variable is automatically stored in non-volatile memory. */
printf(", count=%u ...\n", ++OD_powerOnCounter);
while(reset != CO_RESET_APP && reset != CO_RESET_QUIT && CO_endProgram == 0) {
/* CANopen communication reset - initialize CANopen objects *******************/
CO_ReturnError_t err;
printf("%s - communication reset ...\n", argv[0]);
#ifndef CO_SINGLE_THREAD
/* Wait other threads (command interface). */
pthread_mutex_lock(&CO_CAN_VALID_mtx);
#endif
/* Wait rt_thread. */
if(!firstRun) {
CO_LOCK_OD();
CO->CANmodule[0]->CANnormal = false;
CO_UNLOCK_OD();
}
/* Enter CAN configuration. */
CO_CANsetConfigurationMode(CANdevice0Index);
/* initialize CANopen */
err = CO_init(CANdevice0Index, nodeId, 0);
if(err != CO_ERROR_NO) {
char s[120];
snprintf(s, 120, "Communication reset - CANopen initialization failed, err=%d", err);
CO_errExit(s);
}
/* initialize OD objects 1010 and 1011 and verify errors. */
CO_OD_configure(CO->SDO[0], OD_H1010_STORE_PARAM_FUNC, CO_ODF_1010, (void*)&odStor, 0, 0U);
CO_OD_configure(CO->SDO[0], OD_H1011_REST_PARAM_FUNC, CO_ODF_1011, (void*)&odStor, 0, 0U);
if(odStorStatus_rom != CO_ERROR_NO) {
CO_errorReport(CO->em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE, (uint32_t)odStorStatus_rom);
}
if(odStorStatus_eeprom != CO_ERROR_NO) {
CO_errorReport(CO->em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE, (uint32_t)odStorStatus_eeprom + 1000);
}
/* Configure callback functions for task control */
CO_EM_initCallback(CO->em, taskMain_cbSignal);
CO_SDO_initCallback(CO->SDO[0], taskMain_cbSignal);
CO_SDOclient_initCallback(CO->SDOclient, taskMain_cbSignal);
CO_SYNC_initCallback(CO->SYNC, CANrx_lockCbSync);
/* Initialize time */
CO_time_init(&CO_time, CO->SDO[0], &OD_time.epochTimeBaseMs, &OD_time.epochTimeOffsetMs, 0x2130);
/* First time only initialization. */
if(firstRun) {
firstRun = false;
/* Configure epoll for mainline */
mainline_epoll_fd = epoll_create(4);
if(mainline_epoll_fd == -1)
CO_errExit("Program init - epoll_create mainline failed");
/* Init mainline */
taskMain_init(mainline_epoll_fd, &OD_performance[ODA_performance_mainCycleMaxTime]);
#ifdef CO_SINGLE_THREAD
/* Init taskRT */
CANrx_taskTmr_init(mainline_epoll_fd, TMR_TASK_INTERVAL_NS, &OD_performance[ODA_performance_timerCycleMaxTime]);
OD_performance[ODA_performance_timerCycleTime] = TMR_TASK_INTERVAL_NS/1000; /* informative */
/* Set priority for mainline */
if(rtPriority > 0) {
struct sched_param param;
param.sched_priority = rtPriority;
if(sched_setscheduler(0, SCHED_FIFO, ¶m) != 0)
CO_errExit("Program init - mainline set scheduler failed");
}
#else
/* Configure epoll for rt_thread */
rt_thread_epoll_fd = epoll_create(2);
if(rt_thread_epoll_fd == -1)
CO_errExit("Program init - epoll_create rt_thread failed");
/* Init taskRT */
CANrx_taskTmr_init(rt_thread_epoll_fd, TMR_TASK_INTERVAL_NS, &OD_performance[ODA_performance_timerCycleMaxTime]);
OD_performance[ODA_performance_timerCycleTime] = TMR_TASK_INTERVAL_NS/1000; /* informative */
/* Create rt_thread */
if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0)
CO_errExit("Program init - rt_thread creation failed");
/* Set priority for rt_thread */
if(rtPriority > 0) {
struct sched_param param;
param.sched_priority = rtPriority;
if(pthread_setschedparam(rt_thread_id, SCHED_FIFO, ¶m) != 0)
CO_errExit("Program init - rt_thread set scheduler failed");
}
#endif
#ifndef CO_SINGLE_THREAD
/* Initialize socket command interface */
if(commandEnable) {
if(CO_command_init() != 0) {
CO_errExit("Socket command interface initialization failed");
}
printf("%s - Command interface on socket '%s' started ...\n", argv[0], CO_command_socketPath);
}
#endif
/* Execute optional additional application code */
app_programStart();
}
/* Execute optional additional application code */
app_communicationReset();
/* start CAN */
CO_CANsetNormalMode(CO->CANmodule[0]);
#ifndef CO_SINGLE_THREAD
pthread_mutex_unlock(&CO_CAN_VALID_mtx);
#endif
reset = CO_RESET_NOT;
printf("%s - running ...\n", argv[0]);
while(reset == CO_RESET_NOT && CO_endProgram == 0) {
/* loop for normal program execution ******************************************/
int ready;
struct epoll_event ev;
ready = epoll_wait(mainline_epoll_fd, &ev, 1, -1);
if(ready != 1) {
if(errno != EINTR) {
CO_error(0x11100000L + errno);
}
}
#ifdef CO_SINGLE_THREAD
else if(CANrx_taskTmr_process(ev.data.fd)) {
/* code was processed in the above function. Additional code process below */
INCREMENT_1MS(CO_timer1ms);
/* Detect timer large overflow */
if(OD_performance[ODA_performance_timerCycleMaxTime] > TMR_TASK_OVERFLOW_US && rtPriority > 0) {
CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0x22400000L | OD_performance[ODA_performance_timerCycleMaxTime]);
}
}
#endif
else if(taskMain_process(ev.data.fd, &reset, CO_timer1ms)) {
uint16_t timer1msDiff;
static uint16_t tmr1msPrev = 0;
/* Calculate time difference */
timer1msDiff = CO_timer1ms - tmr1msPrev;
tmr1msPrev = CO_timer1ms;
/* code was processed in the above function. Additional code process below */
/* Execute optional additional application code */
app_programAsync(timer1msDiff);
CO_OD_storage_autoSave(&odStorAuto, CO_timer1ms, 60000);
}
else {
/* No file descriptor was processed. */
CO_error(0x11200000L);
}
}
}
/* program exit ***************************************************************/
/* join threads */
#ifndef CO_SINGLE_THREAD
if(commandEnable) {
if(CO_command_clear() != 0) {
CO_errExit("Socket command interface removal failed");
}
}
#endif
CO_endProgram = 1;
#ifndef CO_SINGLE_THREAD
if(pthread_join(rt_thread_id, NULL) != 0) {
CO_errExit("Program end - pthread_join failed");
}
#endif
/* Execute optional additional application code */
app_programEnd();
/* Store CO_OD_EEPROM */
CO_OD_storage_autoSave(&odStorAuto, 0, 0);
CO_OD_storage_autoSaveClose(&odStorAuto);
/* delete objects from memory */
CANrx_taskTmr_close();
taskMain_close();
CO_delete(CANdevice0Index);
printf("%s on %s (nodeId=0x%02X) - finished.\n\n", argv[0], CANdevice, nodeId);
/* Flush all buffers (and reboot) */
if(rebootEnable && reset == CO_RESET_APP) {
sync();
if(reboot(LINUX_REBOOT_CMD_RESTART) != 0) {
CO_errExit("Program end - reboot failed");
}
}
exit(EXIT_SUCCESS);
}
