Beispiel #1
0
/* main ***********************************************************************/
int main (void){
    unsigned int temp_ui;
    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;

    /* Configure system for maximum performance. plib is necessary for that.*/
    /* SYSTEMConfig(CO_FSYS*1000, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE); */

    /* Enable system multi vectored interrupts */
    INTCONbits.MVEC = 1;
    __builtin_enable_interrupts();

    /* Disable JTAG and trace port */
    DDPCONbits.JTAGEN = 0;
    DDPCONbits.TROEN = 0;


    /* Verify, if OD structures have proper alignment of initial values */
    if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) while(1) CO_clearWDT();
    if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) while(1) CO_clearWDT();
    if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) while(1) CO_clearWDT();


    /* initialize EEPROM - part 1 */
#ifdef USE_EEPROM
    CO_ReturnError_t eeStatus = CO_EE_init_1(&CO_EEO, (uint8_t*) &CO_OD_EEPROM, sizeof(CO_OD_EEPROM),
                            (uint8_t*) &CO_OD_ROM, sizeof(CO_OD_ROM));
#endif


    programStart();


    /* increase variable each startup. Variable is stored in eeprom. */
    OD_powerOnCounter++;


    while(reset != CO_RESET_APP){
/* CANopen communication reset - initialize CANopen objects *******************/
        CO_ReturnError_t err;
        uint16_t timer1msPrevious;
        uint16_t TMR_TMR_PREV = 0;
        uint8_t nodeId;
        uint16_t CANBitRate;

        /* disable timer and CAN interrupts */
        CO_TMR_ISR_ENABLE = 0;
        CO_CAN_ISR_ENABLE = 0;
        CO_CAN_ISR2_ENABLE = 0;

        /* Read CANopen Node-ID and CAN bit-rate from object dictionary */
        nodeId = OD_CANNodeID;
        if(nodeId<1 || nodeId>127) nodeId = 0x10;
        CANBitRate = OD_CANBitRate;/* in kbps */

        /* initialize CANopen */
        err = CO_init(ADDR_CAN1, nodeId, CANBitRate);
        if(err != CO_ERROR_NO){
            while(1) CO_clearWDT();
            /* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
        }


        /* initialize eeprom - part 2 */
#ifdef USE_EEPROM
        CO_EE_init_2(&CO_EEO, eeStatus, CO->SDO, CO->em);
#endif


        /* initialize variables */
        timer1msPrevious = CO_timer1ms;
        OD_performance[ODA_performance_mainCycleMaxTime] = 0;
        OD_performance[ODA_performance_timerCycleMaxTime] = 0;
        reset = CO_RESET_NOT;



        /* Configure Timer interrupt function for execution every 1 millisecond */
        CO_TMR_CON = 0;
        CO_TMR_TMR = 0;
        #if CO_PBCLK > 65000
            #error wrong timer configuration
        #endif
        CO_TMR_PR = CO_PBCLK - 1;  /* Period register */
        CO_TMR_CON = 0x8000;       /* start timer (TON=1) */
        CO_TMR_ISR_FLAG = 0;       /* clear interrupt flag */
        CO_TMR_ISR_PRIORITY = 3;   /* interrupt - set lower priority than CAN (set the same value in interrupt) */

        /* Configure CAN1 Interrupt (Combined) */
        CO_CAN_ISR_FLAG = 0;       /* CAN1 Interrupt - Clear flag */
        CO_CAN_ISR_PRIORITY = 5;   /* CAN1 Interrupt - Set higher priority than timer (set the same value in '#define CO_CAN_ISR_PRIORITY') */
        CO_CAN_ISR2_FLAG = 0;      /* CAN2 Interrupt - Clear flag */
        CO_CAN_ISR2_PRIORITY = 5;  /* CAN Interrupt - Set higher priority than timer (set the same value in '#define CO_CAN_ISR_PRIORITY') */


        communicationReset();


        /* start CAN and enable interrupts */
        CO_CANsetNormalMode(ADDR_CAN1);
        CO_TMR_ISR_ENABLE = 1;
        CO_CAN_ISR_ENABLE = 1;

#if CO_NO_CAN_MODULES >= 2
        CO_CANsetNormalMode(ADDR_CAN2);
        CO_CAN_ISR2_ENABLE = 1;
#endif


        while(reset == CO_RESET_NOT){
/* loop for normal program execution ******************************************/
            uint16_t timer1msCopy, timer1msDiff;

            CO_clearWDT();


            /* calculate cycle time for performance measurement */
            timer1msCopy = CO_timer1ms;
            timer1msDiff = timer1msCopy - timer1msPrevious;
            timer1msPrevious = timer1msCopy;
            uint16_t t0 = CO_TMR_TMR;
            uint16_t t = t0;
            if(t >= TMR_TMR_PREV){
                t = t - TMR_TMR_PREV;
                t = (timer1msDiff * 100) + (t / (CO_PBCLK / 100));
            }
            else if(timer1msDiff){
                t = TMR_TMR_PREV - t;
                t = (timer1msDiff * 100) - (t / (CO_PBCLK / 100));
            }
            else t = 0;
            OD_performance[ODA_performance_mainCycleTime] = t;
            if(t > OD_performance[ODA_performance_mainCycleMaxTime])
                OD_performance[ODA_performance_mainCycleMaxTime] = t;
            TMR_TMR_PREV = t0;


            /* Application asynchronous program */
            programAsync(timer1msDiff);

            CO_clearWDT();


            /* CANopen process */
            reset = CO_process(CO, timer1msDiff, NULL);

            CO_clearWDT();


#ifdef USE_EEPROM
            CO_EE_process(&CO_EEO);
#endif
        }
    }


/* program exit ***************************************************************/
//    CO_DISABLE_INTERRUPTS();

    /* delete objects from memory */
    programEnd();
    CO_delete(ADDR_CAN1);

    /* reset */
    SYSKEY = 0x00000000;
    SYSKEY = 0xAA996655;
    SYSKEY = 0x556699AA;
    RSWRSTSET = 1;
    temp_ui = RSWRST;
    while(1);
}
/* main ***********************************************************************/
int main(void) {
    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;

    InitCanLeds();
    DBGU_Configure(115200);
    TRACE_INFO_WP("\n\rCanOpenNode %s (%s %s)\n\r", cVer, __DATE__, __TIME__);
    /* Configure Timer interrupt function for execution every 1 millisecond */
    if (SysTick_Config(SysTick_1ms))
        TRACE_FATAL("SysTick_Config\n\r");
    initTimer(getTimer_us);

    /* Todo: initialize EEPROM */

    /*  Todo: Loading COD */
    TRACE_INFO("Loading COD\n\r");

    /* Verify, if OD structures have proper alignment of initial values */
    TRACE_DEBUG("Checking COD in RAM (size=%d)\n\r", &CO_OD_RAM.LastWord - &CO_OD_RAM.FirstWord);
    if (CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord)
        TRACE_FATAL("Err COD in RAM\n\r");
    TRACE_DEBUG("Checking COD in EEPROM (size=%d)\n\r", &CO_OD_EEPROM.LastWord - &CO_OD_EEPROM.FirstWord);
    if (CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord)
        TRACE_FATAL("Err COD in EEPROM\n\r");
    TRACE_DEBUG("Checking COD in ROM (size=%d)\n\r", &CO_OD_ROM.LastWord - &CO_OD_ROM.FirstWord);
    if (CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord)
        TRACE_FATAL("Err COD in ROM\n\r");

    /* increase variable each startup. Variable is stored in eeprom. */
    OD_powerOnCounter++;

    TRACE_INFO("CO power-on (BTR=%dk Node=0x%x)\n\r", CO_OD_ROM.CANBitRate, CO_OD_ROM.CANNodeID);
    ttimer tprof;
    while (reset != CO_RESET_APP) {
        /* CANopen communication reset - initialize CANopen objects *******************/
        static uint32_t timer1msPrevious;
        CO_ReturnError_t err;

        /* disable timer interrupts, turn on red LED */
        canTimerOff = 1;
        CanLedsSet(eCoLed_Red);

        /* initialize CANopen */
        err = CO_init();
        if (err) {
            TRACE_FATAL("CO_init\n\r");
            /* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
        }

        /* start Timer */
        canTimerOff = 0;

        reset = CO_RESET_NOT;
        timer1msPrevious = CO_timer1ms;

        TRACE_INFO("CO (re)start\n\r");
        while (reset == CO_RESET_NOT) {
            saveTime(&tprof);
            /* loop for normal program execution ******************************************/
            uint32_t timer1msDiff;

            timer1msDiff = CO_timer1ms - timer1msPrevious;
            timer1msPrevious = CO_timer1ms;

            ClearWDT();

            /* CANopen process */
            reset = CO_process(CO, timer1msDiff);

            CanLedsSet((LED_GREEN_RUN(CO->NMT)>0 ? eCoLed_Green : 0) | (LED_RED_ERROR(CO->NMT)>0 ? eCoLed_Red : 0));

            ClearWDT();

            /* (not implemented) eeprom_process(&eeprom); */
            uint32_t t = getTime_us(&tprof);
            OD_performance[ODA_performance_mainCycleTime] = t;
            if (t > OD_performance[ODA_performance_mainCycleMaxTime])
                OD_performance[ODA_performance_mainCycleMaxTime] = t;

        } /*  while (reset != 0) */
    } /*  while (reset != 2) */
    /* program exit ***************************************************************/
    /* save variables to eeprom */
    CO_DISABLE_INTERRUPTS();

    CanLedsSet(eCoLed_None);
    /* (not implemented) eeprom_saveAll(&eeprom); */
    CanLedsSet(eCoLed_Red);
    /* delete CANopen object from memory */
    CO_delete();

    /* reset - by WD */
    return 0;
}
Beispiel #3
0
/* main ***********************************************************************/
int main (void){
    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;

    /* Configure microcontroller. */


    /* initialize EEPROM */


    /* increase variable each startup. Variable is stored in EEPROM. */
    OD_powerOnCounter++;


    while(reset != CO_RESET_APP){
/* CANopen communication reset - initialize CANopen objects *******************/
        CO_ReturnError_t err;
        uint16_t timer1msPrevious;

        /* disable timer and CAN interrupts */
        CO_CAN_OK = false;


        /* initialize CANopen */
        err = CO_init(0/* CAN module address */, 10/* NodeID */, 125 /* bit rate */);
        if(err != CO_ERROR_NO){
            while(1);
            /* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
        }


        /* set callback functions for task control. */


        /* Configure Timer interrupt function for execution every 1 millisecond */


        /* Configure CAN transmit and receive interrupt */


        /* start CAN */
        CO_CANsetNormalMode(CO->CANmodule[0]);
        CO_CAN_OK = true;

        reset = CO_RESET_NOT;
        timer1msPrevious = CO_timer1ms;

        while(reset == CO_RESET_NOT){
/* loop for normal program execution ******************************************/
            uint16_t timer1msCopy, timer1msDiff;

            timer1msCopy = CO_timer1ms;
            timer1msDiff = timer1msCopy - timer1msPrevious;
            timer1msPrevious = timer1msCopy;


            /* CANopen process */
            reset = CO_process(CO, timer1msDiff);

            /* Nonblocking application code may go here. */

            /* Process EEPROM */
        }
    }


/* program exit ***************************************************************/
    /* stop threads */


    /* delete objects from memory */
    CO_delete(0/* CAN module address */);


    /* reset */
    return 0;
}
Beispiel #4
0
//===========================================================================
// CANopen stack main entry point
//===========================================================================
void CO_main(void)
{
	CO_FlashInit();
	while (1)
	{
		reset = CO_RESET_NOT;
		// Application interface
		programStart();

		// increase variable each startup. Variable is stored in EEPROM.
		OD_powerOnCounter++;
		while (reset < CO_RESET_APP)
		{
			// CANopen communication reset - initialize CANopen objects
			int16_t err;
			cyg_uint64 timer1msPrevious = CO_TmrGetMilliSec();

			// initialize CANopen
			err = CO_init();
			if(err)
			{
				CO_eCos_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR,
					CO_EMC_SOFTWARE_INTERNAL, err);
				while(1);
			}
			// register object dictionary functions to support store and restore
			// of parameters via objects 0x1010 and 0x1011
			CO_FlashRegisterODFunctions(CO);

			// initialize variables
			reset = CO_RESET_NOT;

			// Application interface
			communicationReset();

			// start CAN and enable interrupts
			CO_CANsetNormalMode(ADDR_CAN1);

			while (CO_RESET_NOT == reset)
			{
				//diag_print_reg("CAN GSR", CAN_CTRL_1_REG_BASE + CANREG_GSR);
				// loop for normal program execution
				cyg_uint64 timer1ms = CO_TmrGetMilliSec();
				uint16_t timer1msDiff = timer1ms - timer1msPrevious;
				timer1msPrevious = timer1ms;

				// Application interface
				programAsync(timer1msDiff);

				// CANopen process
				reset = CO_process(CO, timer1msDiff);
				if (CO_TmrIsExpired(CANopenPollingTimer))
				{
					CANopenPollingTimer = CO_TmrStartFrom(CANopenPollingTimer, 1);
					CO_process_RPDO(CO);
					program1ms();
					CO_process_TPDO(CO);
				}
			}
		}
	}
}
DWORD WINAPI CanOpen_run(LPVOID lpParam)
{
	

	/* initialize EEPROM - part 1 */
#ifdef USE_EEPROM
	CO_ReturnError_t eeStatus = CO_EE_init_1(&CO_EEO, (uint8_t*)&CO_OD_EEPROM, sizeof(CO_OD_EEPROM),
		(uint8_t*)&CO_OD_ROM, sizeof(CO_OD_ROM));
#endif

	programStart();

	/* increase variable each startup. Variable is stored in eeprom. */
	OD_powerOnCounter++;

	while (reset != CO_RESET_APP) {
		/* CANopen communication reset - initialize CANopen objects *******************/
		CO_ReturnError_t err;
		uint16_t timer1msPrevious;
		uint16_t TMR_TMR_PREV = 0;
		uint8_t nodeId;
		uint16_t CANBitRate;

		/* disable CAN and CAN interrupts */
		CO_CAN_ISR_ENABLE = 0;
		CO_CAN_ISR2_ENABLE = 0;

		/* Read CANopen Node-ID and CAN bit-rate from object dictionary */
		nodeId = OD_CANNodeID;
		if (nodeId<1 || nodeId>127) nodeId = 0x10;
		CANBitRate = OD_CANBitRate;/* in kbps */

								   /* initialize CANopen */
		err = CO_init(ADDR_CAN1, nodeId, CANBitRate);
		if (err != CO_ERROR_NO) {
			//FIXME do something here?
			/* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
		}


		/* initialize eeprom - part 2 */
#ifdef USE_EEPROM
		CO_EE_init_2(&CO_EEO, eeStatus, CO->SDO[0], CO->em);
#endif


		/* Configure callback functions */
		CO_SYNC_initCallback(CO->SYNC, CANrx_lockCbSync);

		/* initialize variables */
		timer1msPrevious = CO_timer1ms;
		OD_performance[ODA_performance_mainCycleMaxTime] = 0;
		OD_performance[ODA_performance_timerCycleMaxTime] = 0;
		reset = CO_RESET_NOT;

		/* Configure Timer interrupt function for execution every 1 millisecond */
		/* Not sure if this is the best type of timer to use */
		/* The jitter on the timing is not great */
		BOOL success = CreateTimerQueueTimer(
			&m_timerHandle,
			NULL,
			TimerProc,
			NULL,
			0,
			1, //Period in ms
			WT_EXECUTEINTIMERTHREAD);

#if CO_PBCLK > 65000
#error wrong timer configuration
#endif
		communicationReset();

		/* start CAN and enable interrupts */
		CO_CANsetNormalMode(CO->CANmodule[0]);
		
#if CO_NO_CAN_MODULES >= 2
		CO_CANsetNormalMode(CO->CANmodule[1]);
		CO_CAN_ISR2_ENABLE = 1;
#endif


		while (reset == CO_RESET_NOT) {
			/* loop for normal program execution ******************************************/
			uint16_t timer1msCopy, timer1msDiff;

			/* calculate cycle time for performance measurement */
			
			timer1msCopy = CO_timer1ms;
			timer1msDiff = timer1msCopy - timer1msPrevious;
			timer1msPrevious = timer1msCopy;

			LARGE_INTEGER Frequency;
			LARGE_INTEGER StartingTime;
			LARGE_INTEGER ElapsedMicroseconds;

			QueryPerformanceFrequency(&Frequency);
			QueryPerformanceCounter(&StartingTime);
			ElapsedMicroseconds.QuadPart = StartingTime.QuadPart - li_main.QuadPart;
			ElapsedMicroseconds.QuadPart *= 1000000;
			ElapsedMicroseconds.QuadPart /= Frequency.QuadPart;

			double t = ElapsedMicroseconds.QuadPart / 1000.0;
			t = 1000.0 / t;

			OD_performance[ODA_performance_mainCycleTime] = t;

			if (t > OD_performance[ODA_performance_mainCycleMaxTime])
					OD_performance[ODA_performance_mainCycleMaxTime] = t;
	
			li_main = StartingTime;

			/* Application asynchronous program */
			programAsync(timer1msDiff);

			/* Pump the SDO master */
			/* TODO In theory we should support as many masters as is defined in OD*/
			canprocessSDOmaster(timer1msDiff);

			/* CANopen process */
			reset = CO_process(CO, timer1msDiff, NULL);

			Sleep(1); //add some grace time to the thread as we do not want to hammer the async thread at 100%

		}
#ifdef USE_EEPROM
			CO_EE_process(&CO_EEO);
#endif
		}

		/* program exit ***************************************************************/

		/* delete objects from memory */
		programEnd();
		CO_delete(ADDR_CAN1);

		// fix me graceful shutdown on win32
		return 0;
}
Beispiel #6
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);
}
Beispiel #7
0
/* main ***********************************************************************/
int main (void){
    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;

    /* Initialize two CAN led diodes */
    TRISAbits.TRISA0 = 0; LATAbits.LATA0 = 0;
    TRISAbits.TRISA1 = 0; LATAbits.LATA1 = 1;
    #define CAN_RUN_LED        LATAbits.LATA0
    #define CAN_ERROR_LED      LATAbits.LATA1

    /* Initialize other LED diodes for RPDO */
    TRISAbits.TRISA2 = 0; LATAbits.LATA2 = 0;
    TRISAbits.TRISA3 = 0; LATAbits.LATA3 = 0;
    TRISAbits.TRISA4 = 0; LATAbits.LATA4 = 0;
    TRISAbits.TRISA5 = 0; LATAbits.LATA5 = 0;
    TRISAbits.TRISA6 = 0; LATAbits.LATA6 = 0;
    TRISAbits.TRISA7 = 0; LATAbits.LATA7 = 0;


    /* Configure Oscillator */
    /* Fosc = Fin*M/(N1*N2), Fcy=Fosc/2 */
    /* Fosc = 8M*24(2*2) = 48MHz -> Fcy = 24MHz */
    PLLFBD=22;              /*  M=24 */
    CLKDIVbits.PLLPOST=0;   /*  N1=2 */
    CLKDIVbits.PLLPRE=0;    /*  N2=2 */
    OSCTUN=0;               /*  Tune FRC oscillator, if FRC is used */
    while(OSCCONbits.LOCK!=1) ClrWdt(); /* wait for PLL to lock */


    /* Verify, if OD structures have proper alignment of initial values */
    if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) while(1) ClrWdt();
    if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) while(1) ClrWdt();
    if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) while(1) ClrWdt();


    /* initialize EEPROM */
        /* (not implemented) */

    /* increase variable each startup. Variable is stored in eeprom. */
    OD_powerOnCounter++;

    while(reset != CO_RESET_APP){
/* CANopen communication reset - initialize CANopen objects *******************/
        static uint16_t timer1msPrevious;
        CO_ReturnError_t err;

        /* disable timer and CAN interrupts, turn on red LED */
        CO_TMR_ISR_ENABLE = 0;
        CO_CAN_ISR_ENABLE = 0;
        CAN_RUN_LED = 0;
        CAN_ERROR_LED = 1;

        /* Initialize digital outputs */
        TRISAbits.TRISA2 = 0; LATAbits.LATA2 = 0;
        TRISAbits.TRISA3 = 0; LATAbits.LATA3 = 0;
        TRISAbits.TRISA4 = 0; LATAbits.LATA4 = 0;
        TRISAbits.TRISA5 = 0; LATAbits.LATA5 = 0;
        TRISAbits.TRISA6 = 0; LATAbits.LATA6 = 0;
        TRISAbits.TRISA7 = 0; LATAbits.LATA7 = 0;
        OD_writeOutput8Bit[0] = 0;
        OD_writeOutput8Bit[1] = 0;


        /* initialize CANopen */
        err = CO_init();
        if(err != CO_ERROR_NO){
            while(1) ClrWdt();
            /* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
        }

        /* start CAN */
        CO_CANsetNormalMode(ADDR_CAN1);


        /* Configure Timer interrupt function for execution every 1 millisecond */
        CO_TMR_CON = 0;
        CO_TMR_TMR = 0;
        CO_TMR_PR = CO_FCY - 1;    /* Period register */
        CO_TMR_CON = 0x8000;       /* start timer (TON=1) */
        CO_timer1ms = 0;
        CO_TMR_ISR_FLAG = 0;       /* clear interrupt flag */
        CO_TMR_ISR_PRIORITY = 3;   /* interrupt - set lower priority than CAN */
        CO_TMR_ISR_ENABLE = 1;     /* enable interrupt */
        /* Configure CAN1 Interrupt (Combined) */
        CO_CAN_ISR_FLAG = 0;       /* CAN1 Interrupt - Clear flag */
        CO_CAN_ISR_PRIORITY = 5;   /* CAN1 Interrupt - Set higher priority than timer */
        CO_CAN_ISR_ENABLE = 1;     /* CAN1 Interrupt - Enable interrupt */

        reset = CO_RESET_NOT;
        timer1msPrevious = CO_timer1ms;

        while(reset == CO_RESET_NOT){
/* loop for normal program execution ******************************************/
            uint16_t timer1msCopy, timer1msDiff;
            static uint16_t TMR_TMR_PREV = 0;

            timer1msCopy = CO_timer1ms;
            timer1msDiff = timer1msCopy - timer1msPrevious;
            timer1msPrevious = timer1msCopy;

            ClrWdt();

            /* calculate cycle time for performance measurement */
            uint16_t t0 = CO_TMR_TMR;
            uint16_t t = t0;
            if(t >= TMR_TMR_PREV){
                t = t - TMR_TMR_PREV;
                t = (timer1msDiff * 100) + (t / (CO_FCY / 100));
            }
            else if(timer1msDiff){
                t = TMR_TMR_PREV - t;
                t = (timer1msDiff * 100) - (t / (CO_FCY / 100));
            }
            else t = 0;
            OD_performance[ODA_performance_mainCycleTime] = t;
            if(t > OD_performance[ODA_performance_mainCycleMaxTime])
                OD_performance[ODA_performance_mainCycleMaxTime] = t;
            TMR_TMR_PREV = t0;

            /* CANopen process */
            reset = CO_process(CO, timer1msDiff);

            CAN_RUN_LED = LED_GREEN_RUN(CO->NMT);
            CAN_ERROR_LED = LED_RED_ERROR(CO->NMT);

            ClrWdt();

            /* (not implemented) eeprom_process(&eeprom); */
        }
    }
/* program exit ***************************************************************/
    /* save variables to eeprom */
    RESTORE_CPU_IPL(7);           /* disable interrupts */
    CAN_RUN_LED = 0;
    /* CAN_ERROR_LED = 0; */
    /* (not implemented) eeprom_saveAll(&eeprom); */
    CAN_ERROR_LED = 1;

    /* delete CANopen object from memory */
    CO_delete();

    /* reset */
    return 0;
}