INTEGER16 CO_SYNC_receive(void *object, CO_CANrxMsg_t *msg){ CO_SYNC_t *SYNC; SYNC = (CO_SYNC_t*)object; //this is the correct pointer type of the first argument if((*SYNC->operatingState == CO_NMT_OPERATIONAL || *SYNC->operatingState == CO_NMT_PRE_OPERATIONAL)){ if(SYNC->counterOverflowValue){ if(msg->DLC != 1){ CO_errorReport(SYNC->EM, ERROR_SYNC_LENGTH, msg->DLC | 0x0100); return CO_ERROR_NO; } SYNC->counter = msg->data[0]; } else{ if(msg->DLC != 0){ CO_errorReport(SYNC->EM, ERROR_SYNC_LENGTH, msg->DLC); return CO_ERROR_NO; } } SYNC->running = 1; SYNC->timer = 0; } return CO_ERROR_NO; }
void CO_CANverifyErrors(CO_CANmodule_t *CANmodule){ uint16_t rxErrors, txErrors, overflow; CO_EM_t* em = (CO_EM_t*)CANmodule->em; uint32_t err; /* get error counters from module. Id possible, function may use different way to * determine errors. */ //With the CANUSB module we can't get this data, possibly stub this out to the driver dll for other hardware?? rxErrors = 0; txErrors = 0; overflow = 0; err = ((uint32_t)txErrors << 16) | ((uint32_t)rxErrors << 8) | overflow; if(CANmodule->errOld != err){ CANmodule->errOld = err; if(txErrors >= 256U){ /* bus off */ CO_errorReport(em, CO_EM_CAN_TX_BUS_OFF, CO_EMC_BUS_OFF_RECOVERED, err); } else{ /* not bus off */ CO_errorReset(em, CO_EM_CAN_TX_BUS_OFF, err); if((rxErrors >= 96U) || (txErrors >= 96U)){ /* bus warning */ CO_errorReport(em, CO_EM_CAN_BUS_WARNING, CO_EMC_NO_ERROR, err); } if(rxErrors >= 128U){ /* RX bus passive */ CO_errorReport(em, CO_EM_CAN_RX_BUS_PASSIVE, CO_EMC_CAN_PASSIVE, err); } else{ CO_errorReset(em, CO_EM_CAN_RX_BUS_PASSIVE, err); } if(txErrors >= 128U){ /* TX bus passive */ if(!CANmodule->firstCANtxMessage){ CO_errorReport(em, CO_EM_CAN_TX_BUS_PASSIVE, CO_EMC_CAN_PASSIVE, err); } } else{ bool_t isError = CO_isError(em, CO_EM_CAN_TX_BUS_PASSIVE); if(isError){ CO_errorReset(em, CO_EM_CAN_TX_BUS_PASSIVE, err); CO_errorReset(em, CO_EM_CAN_TX_OVERFLOW, err); } } if((rxErrors < 96U) && (txErrors < 96U)){ /* no error */ CO_errorReset(em, CO_EM_CAN_BUS_WARNING, err); } } if(overflow != 0U){ /* CAN RX bus overflow */ CO_errorReport(em, CO_EM_CAN_RXB_OVERFLOW, CO_EMC_CAN_OVERRUN, err); } } }
void __ISR(_TIMER_2_VECTOR, IPL3SOFT) CO_TimerInterruptHandler(void){ bool_t syncWas; CO_TMR_ISR_FLAG = 0; CO_timer1ms++; // if(CO->CANmodule[0]->CANnormal) { // bool_t syncWas; /* Process Sync and read inputs */ syncWas = CO_process_SYNC_RPDO(CO, 1000); /* Re-enable CANrx, if it was disabled by SYNC callback */ // TODO this and outer loop /* Further I/O or nonblocking application code may go here. */ program1ms(); /* Write outputs */ CO_process_TPDO(CO, syncWas, 1000); // } /* verify timer overflow */ if(CO_TMR_ISR_FLAG == 1){ CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0); CO_TMR_ISR_FLAG = 0; } /* calculate cycle time for performance measurement */ uint16_t t = CO_TMR_TMR / (CO_PBCLK / 100); OD_performance[ODA_performance_timerCycleTime] = t; if(t > OD_performance[ODA_performance_timerCycleMaxTime]) OD_performance[ODA_performance_timerCycleMaxTime] = t; }
CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer){ CO_ReturnError_t err = CO_ERROR_NO; /* Verify overflow */ if(buffer->bufferFull){ if(!CANmodule->firstCANtxMessage){ /* don't set error, if bootup message is still on buffers */ CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_CAN_TX_OVERFLOW, CO_EMC_CAN_OVERRUN, buffer->ident); } err = CO_ERROR_TX_OVERFLOW; } CO_LOCK_CAN_SEND(); /* if CAN TX buffer is free, copy message to it */ if(1 && CANmodule->CANtxCount == 0){ CANmodule->bufferInhibitFlag = buffer->syncFlag; /* copy message and txRequest */ WriteCanPacket(buffer); } /* if no buffer is free, message will be sent by interrupt */ else{ buffer->bufferFull = true; CANmodule->CANtxCount++; } CO_UNLOCK_CAN_SEND(); return err; }
void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule){ uint32_t tpdoDeleted = 0U; CO_LOCK_CAN_SEND(); /* Abort message from CAN module, if there is synchronous TPDO. * Take special care with this functionality. */ if(/*messageIsOnCanBuffer && */CANmodule->bufferInhibitFlag){ /* clear TXREQ */ CANmodule->bufferInhibitFlag = false; tpdoDeleted = 1U; } /* delete also pending synchronous TPDOs in TX buffers */ if(CANmodule->CANtxCount != 0U){ uint16_t i; CO_CANtx_t *buffer = &CANmodule->txArray[0]; for(i = CANmodule->txSize; i > 0U; i--){ if(buffer->bufferFull){ if(buffer->syncFlag){ buffer->bufferFull = false; CANmodule->CANtxCount--; tpdoDeleted = 2U; } } buffer++; } } CO_UNLOCK_CAN_SEND(); if(tpdoDeleted != 0U){ CO_errorReport((CO_EM_t*)CANmodule->em, CO_EM_TPDO_OUTSIDE_WINDOW, CO_EMC_COMMUNICATION, tpdoDeleted); } }
/* timer thread executes in constant intervals ********************************/ static void tmrTask_thread(void){ for(;;) { /* sleep for interval */ INCREMENT_1MS(CO_timer1ms); if(CO_CAN_OK) { bool_t syncWas; /* Process Sync and read inputs */ syncWas = CO_process_SYNC_RPDO(CO, TMR_TASK_INTERVAL); /* Reenable CANrx, if it was disabled by SYNC callback */ /* Further I/O or nonblocking application code may go here. */ /* Write outputs */ CO_process_TPDO(CO, syncWas, TMR_TASK_INTERVAL); } } /* verify timer overflow */ if(0){ CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0U); } }
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; }
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); } }
/* * Configure TPDO Mapping parameter. * * Function is called from communication reset or when parameter changes. * * Function configures following variables from CO_TPDO_t: _dataLength_, * _mapPointer_ and _sendIfCOSFlags_. * * @param TPDO TPDO object. * @param noOfMappedObjects Number of mapped object (from OD). * * @return 0 on success, otherwise SDO abort code. */ static uint32_t CO_TPDOconfigMap(CO_TPDO_t* TPDO, uint8_t noOfMappedObjects){ int16_t i; uint8_t length = 0; uint32_t ret = 0; const uint32_t* pMap = &TPDO->TPDOMapPar->mappedObject1; TPDO->sendIfCOSFlags = 0; for(i=noOfMappedObjects; i>0; i--){ int16_t j; uint8_t* pData; uint8_t prevLength = length; uint8_t MBvar; uint32_t map = *(pMap++); /* function do much checking of errors in map */ ret = CO_PDOfindMap( TPDO->SDO, map, 1, &pData, &length, &TPDO->sendIfCOSFlags, &MBvar); if(ret){ length = 0; CO_errorReport(TPDO->em, CO_EM_PDO_WRONG_MAPPING, CO_EMC_PROTOCOL_ERROR, map); break; } /* write PDO data pointers */ #ifdef CO_BIG_ENDIAN if(MBvar){ for(j=length-1; j>=prevLength; j--) TPDO->mapPointer[j] = pData++; } else{ for(j=prevLength; j<length; j++) TPDO->mapPointer[j] = pData++; } #else for(j=prevLength; j<length; j++){ TPDO->mapPointer[j] = pData++; } #endif } TPDO->dataLength = length; return ret; }
/* * Configure RPDO Mapping parameter. * * Function is called from communication reset or when parameter changes. * * Function configures following variables from CO_RPDO_t: _dataLength_ and * _mapPointer_. * * @param RPDO RPDO object. * @param noOfMappedObjects Number of mapped object (from OD). * * @return 0 on success, otherwise SDO abort code. */ static uint32_t CO_RPDOconfigMap(CO_RPDO_t* RPDO, uint8_t noOfMappedObjects){ int16_t i; uint8_t length = 0; uint32_t ret = 0; const uint32_t* pMap = &RPDO->RPDOMapPar->mappedObject1; for(i=noOfMappedObjects; i>0; i--){ int16_t j; uint8_t* pData; uint8_t dummy = 0; uint8_t prevLength = length; uint8_t MBvar; uint32_t map = *(pMap++); /* function do much checking of errors in map */ ret = CO_PDOfindMap( RPDO->SDO, map, 0, &pData, &length, &dummy, &MBvar); if(ret){ length = 0; CO_errorReport(RPDO->EM, ERROR_PDO_WRONG_MAPPING, map); break; } /* write PDO data pointers */ #ifdef BIG_ENDIAN if(MBvar){ for(j=length-1; j>=prevLength; j--) RPDO->mapPointer[j] = pData++; } else{ for(j=prevLength; j<length; j++) RPDO->mapPointer[j] = pData++; } #else for(j=prevLength; j<length; j++){ RPDO->mapPointer[j] = pData++; } #endif } RPDO->dataLength = length; return ret; }
/* Realtime thread for CAN receive and taskTmr ********************************/ static void* rt_thread(void* arg) { /* Endless loop */ while(CO_endProgram == 0) { int ready; struct epoll_event ev; ready = epoll_wait(rt_thread_epoll_fd, &ev, 1, -1); if(ready != 1) { if(errno != EINTR) { CO_error(0x12100000L + errno); } } else if(CANrx_taskTmr_process(ev.data.fd)) { int i; /* code was processed in the above function. Additional code process below */ INCREMENT_1MS(CO_timer1ms); /* Monitor variables with trace objects */ CO_time_process(&CO_time); #if CO_NO_TRACE > 0 for(i=0; i<OD_traceEnable && i<CO_NO_TRACE; i++) { CO_trace_process(CO->trace[i], *CO_time.epochTimeOffsetMs); } #endif /* Execute optional additional application code */ app_program1ms(); /* Detect timer large overflow */ if(OD_performance[ODA_performance_timerCycleMaxTime] > TMR_TASK_OVERFLOW_US && rtPriority > 0 && CO->CANmodule[0]->CANnormal) { CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0x22400000L | OD_performance[ODA_performance_timerCycleMaxTime]); } } else { /* No file descriptor was processed. */ CO_error(0x12200000L); } } return NULL; }
/* timer interrupt function executes every millisecond ************************/ CO_TIMER_ISR(){ /* clear interrupt flag bit */ CO_TMR_ISR_FLAG = 0; CO_timer1ms++; CO_process_RPDO(CO); /* read RPDO and show it on example LEDS on Explorer16 */ uint8_t leds = OD_writeOutput8Bit[0]; LATAbits.LATA2 = (leds&0x04) ? 1 : 0; LATAbits.LATA3 = (leds&0x08) ? 1 : 0; LATAbits.LATA4 = (leds&0x10) ? 1 : 0; LATAbits.LATA5 = (leds&0x20) ? 1 : 0; LATAbits.LATA6 = (leds&0x40) ? 1 : 0; LATAbits.LATA7 = (leds&0x80) ? 1 : 0; /* prepare TPDO from example buttons on Explorer16 */ uint8_t but = 0; if(!PORTDbits.RD6) but |= 0x08; if(!PORTDbits.RD7) but |= 0x04; if(!PORTDbits.RD13) but |= 0x01; OD_readInput8Bit[0] = but; CO_process_TPDO(CO); /* verify timer overflow */ if(CO_TMR_ISR_FLAG == 1){ CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0); CO_TMR_ISR_FLAG = 0; } /* calculate cycle time for performance measurement */ uint16_t t = CO_TMR_TMR / (CO_FCY / 100); OD_performance[ODA_performance_timerCycleTime] = t; if(t > OD_performance[ODA_performance_timerCycleMaxTime]) OD_performance[ODA_performance_timerCycleMaxTime] = t; }
void __ISR(_TIMER_2_VECTOR, ipl3SOFT) CO_TimerInterruptHandler(void){ CO_TMR_ISR_FLAG = 0; CO_timer1ms++; CO_process_RPDO(CO); program1ms(); CO_process_TPDO(CO); /* verify timer overflow */ if(CO_TMR_ISR_FLAG == 1){ CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0); CO_TMR_ISR_FLAG = 0; } /* calculate cycle time for performance measurement */ uint16_t t = CO_TMR_TMR / (CO_PBCLK / 100); OD_performance[ODA_performance_timerCycleTime] = t; if(t > OD_performance[ODA_performance_timerCycleMaxTime]) OD_performance[ODA_performance_timerCycleMaxTime] = t; }
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; }
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); }
/* send CANopen generic emergency message */ void CO_error(const uint32_t info) { CO_errorReport(CO->em, CO_EM_GENERIC_SOFTWARE_ERROR, CO_EMC_SOFTWARE_INTERNAL, info); fprintf(stderr, "canopend generic error: 0x%X\n", info); }
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; }
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; }
/* timer interrupt function executes every millisecond ************************/ CO_TIMER_ISR(){ /* clear interrupt flag bit */ CO_TMR_ISR_FLAG = 0; CO_timer1ms++; if(CO->CANmodule[0]->CANnormal) { bool_t syncWas; /* Process Sync and read inputs */ syncWas = CO_process_SYNC_RPDO(CO, 1000); /* Re-enable CANrx, if it was disabled by SYNC callback */ CO_CAN_ISR_ENABLE = 1; /* Further I/O or nonblocking application code may go here. */ /* read RPDO and show it on example LEDS on Explorer16 */ uint8_t leds = OD_writeOutput8Bit[0]; LATAbits.LATA3 = (leds&0x08) ? 1 : 0; LATAbits.LATA4 = (leds&0x10) ? 1 : 0; LATAbits.LATA5 = (leds&0x20) ? 1 : 0; LATAbits.LATA6 = (leds&0x40) ? 1 : 0; LATAbits.LATA7 = (leds&0x80) ? 1 : 0; /* prepare TPDO from example buttons on Explorer16 */ uint8_t but = 0; if(!PORTDbits.RD6) but |= 0x08; if(!PORTDbits.RD7) but |= 0x04; if(!PORTDbits.RD13) but |= 0x01; OD_readInput8Bit[0] = but; #if 0 /* Debug - disable CANrx for 650 ms, if button pressed. */ static uint16_t tmrDebug = 0; if(!PORTDbits.RD13) { if(tmrDebug < 650) { CO_CAN_ISR_ENABLE = 0; tmrDebug++; } else { CO_CAN_ISR_ENABLE = 1; } } else { CO_CAN_ISR_ENABLE = 1; tmrDebug = 0; } #endif /* Write outputs */ CO_process_TPDO(CO, syncWas, 1000); /* verify timer overflow */ if(CO_TMR_ISR_FLAG == 1){ CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0); CO_TMR_ISR_FLAG = 0; } } /* calculate cycle time for performance measurement */ uint16_t t = CO_TMR_TMR / (CO_FCY / 100); OD_performance[ODA_performance_timerCycleTime] = t; if(t > OD_performance[ODA_performance_timerCycleMaxTime]) OD_performance[ODA_performance_timerCycleMaxTime] = t; }