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