Improved dc_user example.

examples/dc_user/Makefile.am

@@ -33,6 +33,6 @@ noinst_PROGRAMS = ec_dc_user_example
 
 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
 
 #------------------------------------------------------------------------------

examples/dc_user/main.c

@@ -35,6 +35,9 @@
 #include <sys/time.h>
 #include <sys/types.h>
 #include <unistd.h>
+#include <time.h>
+#include <sys/mman.h>
+#include <malloc.h>
 
 /****************************************************************************/
 
@@ -43,12 +46,20 @@
 /****************************************************************************/
 
 // Application parameters
-#define FREQUENCY 100
-#define PRIORITY 1
+#define FREQUENCY 1000
+#define CLOCK_TO_USE CLOCK_REALTIME
+#define MEASURE_TIMING
 
-// Optional features
-#define CONFIGURE_PDOS  1
+/****************************************************************************/
+
+#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_master_state_t master_state = {};
 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_in;
 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 @@ void check_master_state(void)
 
     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 check_master_state(void)
 
 void cyclic_task()
 {
-    int i;
+    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
 
-    // receive process data
-    ecrt_master_receive(master);
-    ecrt_domain_process(domain1);
+    // get current time
+    clock_gettime(CLOCK_TO_USE, &wakeupTime);
 
-    // check process data state (optional)
-    check_domain1_state();
+	while(1) {
+		wakeupTime = timespec_add(wakeupTime, cycletime);
+        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
 
-    if (counter) {
-        counter--;
-    } else { // do this at 1 Hz
-        counter = FREQUENCY;
+#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;
 
-        // calculate new process data
-        blink = !blink;
+        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
 
-        // check for master state (optional)
-        check_master_state();
-    }
+		// 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
 
-    if (blink_counter) {
-        blink_counter--;
-    } else {
-        blink_counter = 9;
+			// calculate new process data
+			blink = !blink;
+		}
 
-        // 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 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));
 
-    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 = 1; // sync every cycle
+			ecrt_master_sync_reference_clock(master);
+		}
+		ecrt_master_sync_slave_clocks(master);
 
-    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);
 
-    // send process data
-    ecrt_domain_queue(domain1);
-    ecrt_master_send(master);
-}
-
-/****************************************************************************/
-
-void signal_handler(int signum) {
-    switch (signum) {
-        case SIGALRM:
-            sig_alarms++;
-            break;
-    }
+#ifdef MEASURE_TIMING
+        clock_gettime(CLOCK_TO_USE, &endTime);
+#endif
+	}
 }
 
 /****************************************************************************/
@@ -193,8 +256,11 @@ void signal_handler(int signum) {
 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 @@ int main(int argc, char **argv)
         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 @@ int main(int argc, char **argv)
         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;
 }
 
 /****************************************************************************/