--- 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 <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
-
-// 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;
+}
+
+/****************************************************************************/