fp@1414: /*****************************************************************************
fp@1414:  *
fp@1414:  *  $Id$
fp@1414:  *
fp@1414:  *  Copyright (C) 2007-2009  Florian Pose, Ingenieurgemeinschaft IgH
fp@1414:  *
fp@1414:  *  This file is part of the IgH EtherCAT Master.
fp@1414:  *
fp@1414:  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
fp@1414:  *  modify it under the terms of the GNU General Public License version 2, as
fp@1414:  *  published by the Free Software Foundation.
fp@1414:  *
fp@1414:  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
fp@1414:  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
fp@1414:  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
fp@1414:  *  Public License for more details.
fp@1414:  *
fp@1414:  *  You should have received a copy of the GNU General Public License along
fp@1414:  *  with the IgH EtherCAT Master; if not, write to the Free Software
fp@1414:  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
fp@1414:  *
fp@1414:  *  ---
fp@1414:  *
fp@1414:  *  The license mentioned above concerns the source code only. Using the
fp@1414:  *  EtherCAT technology and brand is only permitted in compliance with the
fp@1414:  *  industrial property and similar rights of Beckhoff Automation GmbH.
fp@1414:  *
fp@1414:  ****************************************************************************/
fp@1414: 
fp@1414: #include <errno.h>
fp@1414: #include <signal.h>
fp@1414: #include <stdio.h>
fp@1414: #include <string.h>
fp@1414: #include <sys/resource.h>
fp@1414: #include <sys/time.h>
fp@1414: #include <sys/types.h>
fp@1414: #include <unistd.h>
fp@1970: #include <time.h>
fp@1970: #include <sys/mman.h>
fp@1970: #include <malloc.h>
fp@1414: 
fp@1414: /****************************************************************************/
fp@1414: 
fp@1414: #include "ecrt.h"
fp@1414: 
fp@1414: /****************************************************************************/
fp@1414: 
fp@1414: // Application parameters
fp@1970: #define FREQUENCY 1000
fp@1970: #define CLOCK_TO_USE CLOCK_REALTIME
fp@1970: #define MEASURE_TIMING
fp@1970: 
fp@1970: /****************************************************************************/
fp@1970: 
fp@1970: #define NSEC_PER_SEC (1000000000L)
fp@1970: #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)
fp@1970: 
fp@1970: #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
fp@1970: 	(B).tv_nsec - (A).tv_nsec)
fp@1970: 
fp@1970: #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
fp@2421: 
fp@1414: /****************************************************************************/
fp@1414: 
fp@1414: // EtherCAT
fp@1414: static ec_master_t *master = NULL;
fp@1414: static ec_master_state_t master_state = {};
fp@1414: 
fp@1414: static ec_domain_t *domain1 = NULL;
fp@1414: static ec_domain_state_t domain1_state = {};
fp@1414: 
fp@1414: /****************************************************************************/
fp@1414: 
fp@1414: // process data
fp@1414: static uint8_t *domain1_pd = NULL;
fp@1414: 
fp@1414: #define BusCouplerPos    0, 0
fp@1414: #define DigOutSlavePos   0, 1
fp@1414: #define CounterSlavePos  0, 2
fp@1414: 
fp@1414: #define Beckhoff_EK1100 0x00000002, 0x044c2c52
fp@1414: #define Beckhoff_EL2008 0x00000002, 0x07d83052
fp@1414: #define IDS_Counter     0x000012ad, 0x05de3052
fp@1414: 
fp@1414: // offsets for PDO entries
fp@1414: static int off_dig_out;
fp@1414: static int off_counter_in;
fp@1414: static int off_counter_out;
fp@1414: 
fp@1414: static unsigned int counter = 0;
fp@1414: static unsigned int blink = 0;
fp@1414: static unsigned int sync_ref_counter = 0;
fp@1970: const struct timespec cycletime = {0, PERIOD_NS};
fp@1970: 
fp@1970: /*****************************************************************************/
fp@1970: 
fp@1970: struct timespec timespec_add(struct timespec time1, struct timespec time2)
fp@1970: {
fp@1970: 	struct timespec result;
fp@1970: 
fp@1970: 	if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
fp@1970: 		result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
fp@1970: 		result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
fp@1970: 	} else {
fp@1970: 		result.tv_sec = time1.tv_sec + time2.tv_sec;
fp@1970: 		result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
fp@1970: 	}
fp@1970: 
fp@1970: 	return result;
fp@1970: }
fp@1414: 
fp@1414: /*****************************************************************************/
fp@1414: 
fp@1414: void check_domain1_state(void)
fp@1414: {
fp@1414:     ec_domain_state_t ds;
fp@1414: 
fp@1414:     ecrt_domain_state(domain1, &ds);
fp@1414: 
fp@1414:     if (ds.working_counter != domain1_state.working_counter)
fp@1414:         printf("Domain1: WC %u.\n", ds.working_counter);
fp@1414:     if (ds.wc_state != domain1_state.wc_state)
fp@1414:         printf("Domain1: State %u.\n", ds.wc_state);
fp@1414: 
fp@1414:     domain1_state = ds;
fp@1414: }
fp@1414: 
fp@1414: /*****************************************************************************/
fp@1414: 
fp@1414: void check_master_state(void)
fp@1414: {
fp@1414:     ec_master_state_t ms;
fp@1414: 
fp@1414:     ecrt_master_state(master, &ms);
fp@1414: 
fp@1970: 	if (ms.slaves_responding != master_state.slaves_responding)
fp@1414:         printf("%u slave(s).\n", ms.slaves_responding);
fp@1414:     if (ms.al_states != master_state.al_states)
fp@1414:         printf("AL states: 0x%02X.\n", ms.al_states);
fp@1414:     if (ms.link_up != master_state.link_up)
fp@1414:         printf("Link is %s.\n", ms.link_up ? "up" : "down");
fp@1414: 
fp@1414:     master_state = ms;
fp@1414: }
fp@1414: 
fp@1414: /****************************************************************************/
fp@1414: 
fp@1414: void cyclic_task()
fp@1414: {
fp@1970:     struct timespec wakeupTime, time;
fp@1970: #ifdef MEASURE_TIMING
fp@1970:     struct timespec startTime, endTime, lastStartTime = {};
fp@1970:     uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0,
fp@1970:              latency_min_ns = 0, latency_max_ns = 0,
fp@1970:              period_min_ns = 0, period_max_ns = 0,
fp@1970:              exec_min_ns = 0, exec_max_ns = 0;
fp@1970: #endif
fp@1970: 
fp@1970:     // get current time
fp@1970:     clock_gettime(CLOCK_TO_USE, &wakeupTime);
fp@1970: 
fp@1970: 	while(1) {
fp@1970: 		wakeupTime = timespec_add(wakeupTime, cycletime);
fp@1970:         clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
fp@1970: 
fp@1970: #ifdef MEASURE_TIMING
fp@1970:         clock_gettime(CLOCK_TO_USE, &startTime);
fp@1970:         latency_ns = DIFF_NS(wakeupTime, startTime);
fp@1970:         period_ns = DIFF_NS(lastStartTime, startTime);
fp@1970:         exec_ns = DIFF_NS(lastStartTime, endTime);
fp@1970:         lastStartTime = startTime;
fp@1970: 
fp@1970:         if (latency_ns > latency_max_ns) {
fp@1970:             latency_max_ns = latency_ns;
fp@1970:         }
fp@1970:         if (latency_ns < latency_min_ns) {
fp@1970:             latency_min_ns = latency_ns;
fp@1970:         }
fp@1970:         if (period_ns > period_max_ns) {
fp@1970:             period_max_ns = period_ns;
fp@1970:         }
fp@1970:         if (period_ns < period_min_ns) {
fp@1970:             period_min_ns = period_ns;
fp@1970:         }
fp@1970:         if (exec_ns > exec_max_ns) {
fp@1970:             exec_max_ns = exec_ns;
fp@1970:         }
fp@1970:         if (exec_ns < exec_min_ns) {
fp@1970:             exec_min_ns = exec_ns;
fp@1970:         }
fp@1970: #endif
fp@1970: 
fp@1970: 		// receive process data
fp@1970: 		ecrt_master_receive(master);
fp@1970: 		ecrt_domain_process(domain1);
fp@1970: 
fp@1970: 		// check process data state (optional)
fp@1970: 		check_domain1_state();
fp@1970: 
fp@1970: 		if (counter) {
fp@1970: 			counter--;
fp@1970: 		} else { // do this at 1 Hz
fp@1970: 			counter = FREQUENCY;
fp@1970: 
fp@1970: 			// check for master state (optional)
fp@1970: 			check_master_state();
fp@1970: 
fp@1970: #ifdef MEASURE_TIMING
fp@1970:             // output timing stats
fp@1970:             printf("period     %10u ... %10u\n",
fp@2421:                     period_min_ns, period_max_ns);
fp@1970:             printf("exec       %10u ... %10u\n",
fp@2421:                     exec_min_ns, exec_max_ns);
fp@1970:             printf("latency    %10u ... %10u\n",
fp@2421:                     latency_min_ns, latency_max_ns);
fp@1970:             period_max_ns = 0;
fp@1970:             period_min_ns = 0xffffffff;
fp@1970:             exec_max_ns = 0;
fp@1970:             exec_min_ns = 0xffffffff;
fp@1970:             latency_max_ns = 0;
fp@1970:             latency_min_ns = 0xffffffff;
fp@1970: #endif
fp@1970: 
fp@1970: 			// calculate new process data
fp@1970: 			blink = !blink;
fp@1970: 		}
fp@1970: 
fp@1970: 		// write process data
fp@1970: 		EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x66 : 0x99);
fp@1970: 		EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
fp@1970: 
fp@1970: 		// write application time to master
fp@1970: 		clock_gettime(CLOCK_TO_USE, &time);
fp@1971: 		ecrt_master_application_time(master, TIMESPEC2NS(time));
fp@1970: 
fp@1970: 		if (sync_ref_counter) {
fp@1970: 			sync_ref_counter--;
fp@1970: 		} else {
fp@1970: 			sync_ref_counter = 1; // sync every cycle
fp@1970: 			ecrt_master_sync_reference_clock(master);
fp@1970: 		}
fp@1970: 		ecrt_master_sync_slave_clocks(master);
fp@1970: 
fp@1970: 		// send process data
fp@1970: 		ecrt_domain_queue(domain1);
fp@1970: 		ecrt_master_send(master);
fp@1970: 
fp@1970: #ifdef MEASURE_TIMING
fp@1970:         clock_gettime(CLOCK_TO_USE, &endTime);
fp@1970: #endif
fp@1970: 	}
fp@1414: }
fp@1414: 
fp@1414: /****************************************************************************/
fp@1414: 
fp@1414: int main(int argc, char **argv)
fp@1414: {
fp@1804:     ec_slave_config_t *sc;
fp@1970: 
fp@1970: 	if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
fp@1970: 		perror("mlockall failed");
fp@1970: 		return -1;
fp@1970: 	}
fp@2421: 
fp@1414:     master = ecrt_request_master(0);
fp@1804:     if (!master)
fp@1804:         return -1;
fp@1414: 
fp@1414:     domain1 = ecrt_master_create_domain(master);
fp@1414:     if (!domain1)
fp@1414:         return -1;
fp@1414: 
fp@1414:     // Create configuration for bus coupler
fp@1414:     sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100);
fp@1414:     if (!sc)
fp@1414:         return -1;
fp@1414: 
fp@1414:     if (!(sc = ecrt_master_slave_config(master,
fp@1414:                     DigOutSlavePos, Beckhoff_EL2008))) {
fp@1414:         fprintf(stderr, "Failed to get slave configuration.\n");
fp@1414:         return -1;
fp@1414:     }
fp@1414: 
fp@1414:     off_dig_out = ecrt_slave_config_reg_pdo_entry(sc,
fp@1414:             0x7000, 1, domain1, NULL);
fp@1414:     if (off_dig_out < 0)
fp@1414:         return -1;
fp@1414: 
fp@1414: 	if (!(sc = ecrt_master_slave_config(master,
fp@1414: 					CounterSlavePos, IDS_Counter))) {
fp@1414:         fprintf(stderr, "Failed to get slave configuration.\n");
fp@1414:         return -1;
fp@1414: 	}
fp@1414: 
fp@1414: 	off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,
fp@1414: 			0x6020, 0x11, domain1, NULL);
fp@1414: 	if (off_counter_in < 0)
fp@1414:         return -1;
fp@1414: 
fp@1414: 	off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,
fp@1414: 			0x7020, 1, domain1, NULL);
fp@1414: 	if (off_counter_out < 0)
fp@1414:         return -1;
fp@1414: 
fp@1414:     // configure SYNC signals for this slave
fp@1970: 	ecrt_slave_config_dc(sc, 0x0700, PERIOD_NS, 4400000, 0, 0);
fp@1414: 
fp@1414:     printf("Activating master...\n");
fp@1414:     if (ecrt_master_activate(master))
fp@1414:         return -1;
fp@1414: 
fp@1414:     if (!(domain1_pd = ecrt_domain_data(domain1))) {
fp@1414:         return -1;
fp@1414:     }
fp@1414: 
fp@1414:     pid_t pid = getpid();
fp@1414:     if (setpriority(PRIO_PROCESS, pid, -19))
fp@1414:         fprintf(stderr, "Warning: Failed to set priority: %s\n",
fp@1414:                 strerror(errno));
fp@1970: 
fp@1970: 	printf("Starting cyclic function.\n");
fp@1970:     cyclic_task();
fp@2421: 
fp@1970:     return 0;
fp@1970: }
fp@1970: 
fp@1970: /****************************************************************************/