# HG changeset patch # User Florian Pose # Date 1288014383 -7200 # Node ID 00e18cef9fc874487f0055a80c3ebf31b35d8583 # Parent 9b0260a60c28ee7efa5bd96018e006785ee62ba3 Improved dc_user example. diff -r 9b0260a60c28 -r 00e18cef9fc8 examples/dc_user/Makefile.am --- a/examples/dc_user/Makefile.am Mon Oct 25 15:27:57 2010 +0200 +++ b/examples/dc_user/Makefile.am Mon Oct 25 15:46:23 2010 +0200 @@ -33,6 +33,6 @@ ec_dc_user_example_SOURCES = main.c ec_dc_user_example_CFLAGS = -I$(top_srcdir)/include -ec_dc_user_example_LDFLAGS = -L$(top_builddir)/lib/.libs -lethercat +ec_dc_user_example_LDFLAGS = -L$(top_builddir)/lib/.libs -lethercat -lrt #------------------------------------------------------------------------------ diff -r 9b0260a60c28 -r 00e18cef9fc8 examples/dc_user/main.c --- a/examples/dc_user/main.c Mon Oct 25 15:27:57 2010 +0200 +++ b/examples/dc_user/main.c Mon Oct 25 15:46:23 2010 +0200 @@ -35,6 +35,9 @@ #include #include #include +#include +#include +#include /****************************************************************************/ @@ -43,12 +46,20 @@ /****************************************************************************/ // Application parameters -#define FREQUENCY 100 -#define PRIORITY 1 - -// Optional features -#define CONFIGURE_PDOS 1 - +#define FREQUENCY 1000 +#define CLOCK_TO_USE CLOCK_REALTIME +#define MEASURE_TIMING + +/****************************************************************************/ + +#define NSEC_PER_SEC (1000000000L) +#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) + +#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \ + (B).tv_nsec - (A).tv_nsec) + +#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) + /****************************************************************************/ // EtherCAT @@ -58,10 +69,6 @@ static ec_domain_t *domain1 = NULL; static ec_domain_state_t domain1_state = {}; -// Timer -static unsigned int sig_alarms = 0; -static unsigned int user_alarms = 0; - /****************************************************************************/ // process data @@ -81,10 +88,26 @@ static int off_counter_out; static unsigned int counter = 0; -static unsigned int blink_counter = 0; static unsigned int blink = 0; static unsigned int sync_ref_counter = 0; -struct timeval app_time; +const struct timespec cycletime = {0, PERIOD_NS}; + +/*****************************************************************************/ + +struct timespec timespec_add(struct timespec time1, struct timespec time2) +{ + struct timespec result; + + if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { + result.tv_sec = time1.tv_sec + time2.tv_sec + 1; + result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; + } else { + result.tv_sec = time1.tv_sec + time2.tv_sec; + result.tv_nsec = time1.tv_nsec + time2.tv_nsec; + } + + return result; +} /*****************************************************************************/ @@ -110,7 +133,7 @@ ecrt_master_state(master, &ms); - if (ms.slaves_responding != master_state.slaves_responding) + if (ms.slaves_responding != master_state.slaves_responding) printf("%u slave(s).\n", ms.slaves_responding); if (ms.al_states != master_state.al_states) printf("AL states: 0x%02X.\n", ms.al_states); @@ -124,68 +147,108 @@ void cyclic_task() { - int i; - - // receive process data - ecrt_master_receive(master); - ecrt_domain_process(domain1); - - // check process data state (optional) - check_domain1_state(); - - if (counter) { - counter--; - } else { // do this at 1 Hz - counter = FREQUENCY; - - // calculate new process data - blink = !blink; - - // check for master state (optional) - check_master_state(); - } - - if (blink_counter) { - blink_counter--; - } else { - blink_counter = 9; - - // calculate new process data - blink = !blink; - } - - // write process data - EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x66 : 0x99); - EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02); - - app_time.tv_usec += 1000000 / FREQUENCY; - if (app_time.tv_usec >= 1000000) { - app_time.tv_usec -= 1000000; - app_time.tv_sec++; - } - ecrt_master_application_time(master, EC_TIMEVAL2NANO(app_time)); - - if (sync_ref_counter) { - sync_ref_counter--; - } else { - sync_ref_counter = 9; - ecrt_master_sync_reference_clock(master); - } - ecrt_master_sync_slave_clocks(master); - - // send process data - ecrt_domain_queue(domain1); - ecrt_master_send(master); -} - -/****************************************************************************/ - -void signal_handler(int signum) { - switch (signum) { - case SIGALRM: - sig_alarms++; - break; - } + struct timespec wakeupTime, time; +#ifdef MEASURE_TIMING + struct timespec startTime, endTime, lastStartTime = {}; + uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0, + latency_min_ns = 0, latency_max_ns = 0, + period_min_ns = 0, period_max_ns = 0, + exec_min_ns = 0, exec_max_ns = 0; +#endif + + // get current time + clock_gettime(CLOCK_TO_USE, &wakeupTime); + + while(1) { + wakeupTime = timespec_add(wakeupTime, cycletime); + clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); + +#ifdef MEASURE_TIMING + clock_gettime(CLOCK_TO_USE, &startTime); + latency_ns = DIFF_NS(wakeupTime, startTime); + period_ns = DIFF_NS(lastStartTime, startTime); + exec_ns = DIFF_NS(lastStartTime, endTime); + lastStartTime = startTime; + + if (latency_ns > latency_max_ns) { + latency_max_ns = latency_ns; + } + if (latency_ns < latency_min_ns) { + latency_min_ns = latency_ns; + } + if (period_ns > period_max_ns) { + period_max_ns = period_ns; + } + if (period_ns < period_min_ns) { + period_min_ns = period_ns; + } + if (exec_ns > exec_max_ns) { + exec_max_ns = exec_ns; + } + if (exec_ns < exec_min_ns) { + exec_min_ns = exec_ns; + } +#endif + + // receive process data + ecrt_master_receive(master); + ecrt_domain_process(domain1); + + // check process data state (optional) + check_domain1_state(); + + if (counter) { + counter--; + } else { // do this at 1 Hz + counter = FREQUENCY; + + // check for master state (optional) + check_master_state(); + +#ifdef MEASURE_TIMING + // output timing stats + printf("period %10u ... %10u\n", + period_min_ns, period_max_ns); + printf("exec %10u ... %10u\n", + exec_min_ns, exec_max_ns); + printf("latency %10u ... %10u\n", + latency_min_ns, latency_max_ns); + period_max_ns = 0; + period_min_ns = 0xffffffff; + exec_max_ns = 0; + exec_min_ns = 0xffffffff; + latency_max_ns = 0; + latency_min_ns = 0xffffffff; +#endif + + // calculate new process data + blink = !blink; + } + + // write process data + EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x66 : 0x99); + EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02); + + // write application time to master + clock_gettime(CLOCK_TO_USE, &time); + ecrt_master_application_time(master, timespec2u64(time)); + + if (sync_ref_counter) { + sync_ref_counter--; + } else { + sync_ref_counter = 1; // sync every cycle + ecrt_master_sync_reference_clock(master); + } + ecrt_master_sync_slave_clocks(master); + + // send process data + ecrt_domain_queue(domain1); + ecrt_master_send(master); + +#ifdef MEASURE_TIMING + clock_gettime(CLOCK_TO_USE, &endTime); +#endif + } } /****************************************************************************/ @@ -193,8 +256,11 @@ int main(int argc, char **argv) { ec_slave_config_t *sc; - struct sigaction sa; - struct itimerval tv; + + if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { + perror("mlockall failed"); + return -1; + } master = ecrt_request_master(0); if (!master) @@ -237,7 +303,7 @@ return -1; // configure SYNC signals for this slave - ecrt_slave_config_dc(sc, 0x0700, 10000000, 4400000, 0, 0); + ecrt_slave_config_dc(sc, 0x0700, PERIOD_NS, 4400000, 0, 0); printf("Activating master...\n"); if (ecrt_master_activate(master)) @@ -247,50 +313,15 @@ return -1; } -#if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); -#endif - - sa.sa_handler = signal_handler; - sigemptyset(&sa.sa_mask); - sa.sa_flags = 0; - if (sigaction(SIGALRM, &sa, 0)) { - fprintf(stderr, "Failed to install signal handler!\n"); - return -1; - } - - printf("Starting timer...\n"); - tv.it_interval.tv_sec = 0; - tv.it_interval.tv_usec = 1000000 / FREQUENCY; - tv.it_value.tv_sec = 0; - tv.it_value.tv_usec = 1000; - if (setitimer(ITIMER_REAL, &tv, NULL)) { - fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); - return 1; - } - - gettimeofday(&app_time, NULL); - - printf("Started.\n"); - while (1) { - pause(); - -#if 0 - struct timeval t; - gettimeofday(&t, NULL); - printf("%u.%06u\n", t.tv_sec, t.tv_usec); -#endif - - while (sig_alarms != user_alarms) { - cyclic_task(); - user_alarms++; - } - } - - return 0; -} - -/****************************************************************************/ + + printf("Starting cyclic function.\n"); + cyclic_task(); + + return 0; +} + +/****************************************************************************/