예제 #1
0
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;
}
예제 #2
0
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);
        }
    }
}
예제 #3
0
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;
}
예제 #4
0
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;
}
예제 #5
0
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);
    }
}
예제 #6
0
파일: main.c 프로젝트: GrandHsu/CANopenNode
/* 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);
    }
}
예제 #7
0
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;
}
예제 #8
0
파일: eeprom.c 프로젝트: yakele/CANopenNode
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);
    }
}
예제 #9
0
/*
 * 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;
}
예제 #10
0
/*
 * 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;
}
예제 #11
0
/* 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;
}
예제 #12
0
/* 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;
}
예제 #13
0
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;
}
예제 #14
0
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;
}
예제 #15
0
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, &param) != 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, &param) != 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);
}
예제 #16
0
/* 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;
}
예제 #18
0
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;
}
예제 #19
0
/* 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;
}