fp@2447: /****************************************************************************** fp@2447: * fp@2447: * $Id$ fp@2447: * fp@2447: * Copyright (C) 2011 IgH Andreas Stewering-Bone fp@2447: * 2012 Florian Pose fp@2447: * fp@2447: * This file is part of the IgH EtherCAT master fp@2447: * fp@2447: * The IgH EtherCAT Master is free software; you can redistribute it and/or fp@2447: * modify it under the terms of the GNU General Public License version 2, as fp@2447: * published by the Free Software Foundation. fp@2447: * fp@2447: * The IgH EtherCAT master is distributed in the hope that it will be useful, fp@2447: * but WITHOUT ANY WARRANTY; without even the implied warranty of fp@2447: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General fp@2447: * Public License for more details. fp@2447: * fp@2447: * You should have received a copy of the GNU General Public License along fp@2447: * with the IgH EtherCAT master. If not, see . fp@2447: * fp@2447: * --- fp@2447: * fp@2447: * The license mentioned above concerns the source code only. Using the fp@2447: * EtherCAT technology and brand is only permitted in compliance with the fp@2447: * industrial property and similar rights of Beckhoff Automation GmbH. fp@2447: * fp@2447: *****************************************************************************/ fp@2447: fp@2447: #include fp@2447: #include fp@2447: #include fp@2447: #include fp@2447: #include fp@2447: fp@2447: #include fp@2447: #include fp@2447: fp@2447: #include "ecrt.h" fp@2447: fp@2447: #define rt_printf(X, Y) fp@2447: fp@2447: #define NSEC_PER_SEC 1000000000 fp@2447: fp@2447: RT_TASK *task; fp@2447: fp@2447: static unsigned int cycle_ns = 1000000; /* 1 ms */ fp@2447: fp@2447: static int run = 1; fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: // EtherCAT fp@2447: static ec_master_t *master = NULL; fp@2447: static ec_master_state_t master_state = {}; fp@2447: fp@2447: static ec_domain_t *domain1 = NULL; fp@2447: static ec_domain_state_t domain1_state = {}; fp@2447: fp@2447: static uint8_t *domain1_pd = NULL; fp@2447: fp@2447: static ec_slave_config_t *sc_dig_out_01 = NULL; fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: // EtherCAT distributed clock variables fp@2447: fp@2447: #define DC_FILTER_CNT 1024 fp@2447: #define SYNC_MASTER_TO_REF 1 fp@2447: fp@2447: static uint64_t dc_start_time_ns = 0LL; fp@2447: static uint64_t dc_time_ns = 0; fp@2447: #if SYNC_MASTER_TO_REF fp@2447: static uint8_t dc_started = 0; fp@2447: static int32_t dc_diff_ns = 0; fp@2447: static int32_t prev_dc_diff_ns = 0; fp@2447: static int64_t dc_diff_total_ns = 0LL; fp@2447: static int64_t dc_delta_total_ns = 0LL; fp@2447: static int dc_filter_idx = 0; fp@2447: static int64_t dc_adjust_ns; fp@2447: #endif fp@2447: static int64_t system_time_base = 0LL; fp@2447: static uint64_t wakeup_time = 0LL; fp@2447: static uint64_t overruns = 0LL; fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: // process data fp@2447: fp@2447: #define BusCoupler01_Pos 0, 0 fp@2447: #define DigOutSlave01_Pos 0, 1 fp@2447: fp@2447: #define Beckhoff_EK1100 0x00000002, 0x044c2c52 fp@2447: #define Beckhoff_EL2004 0x00000002, 0x07d43052 fp@2447: fp@2447: // offsets for PDO entries fp@2447: static unsigned int off_dig_out0 = 0; fp@2447: fp@2447: // process data fp@2447: fp@2447: const static ec_pdo_entry_reg_t domain1_regs[] = { fp@2447: {DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, fp@2447: {} fp@2447: }; fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: /* Slave 1, "EL2004" fp@2447: * Vendor ID: 0x00000002 fp@2447: * Product code: 0x07d43052 fp@2447: * Revision number: 0x00100000 fp@2447: */ fp@2447: fp@2447: ec_pdo_entry_info_t slave_1_pdo_entries[] = { fp@2447: {0x7000, 0x01, 1}, /* Output */ fp@2447: {0x7010, 0x01, 1}, /* Output */ fp@2447: {0x7020, 0x01, 1}, /* Output */ fp@2447: {0x7030, 0x01, 1}, /* Output */ fp@2447: }; fp@2447: fp@2447: ec_pdo_info_t slave_1_pdos[] = { fp@2447: {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */ fp@2447: {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */ fp@2447: {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */ fp@2447: {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */ fp@2447: }; fp@2447: fp@2447: ec_sync_info_t slave_1_syncs[] = { fp@2447: {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE}, fp@2447: {0xff} fp@2447: }; fp@2447: fp@2447: /***************************************************************************** fp@2447: * Realtime task fp@2447: ****************************************************************************/ fp@2447: fp@2447: /** Get the time in ns for the current cpu, adjusted by system_time_base. fp@2447: * fp@2447: * \attention Rather than calling rt_get_time_ns() directly, all application fp@2447: * time calls should use this method instead. fp@2447: * fp@2447: * \ret The time in ns. fp@2447: */ fp@2447: uint64_t system_time_ns(void) fp@2447: { fp@2447: RTIME time = rt_get_time_ns(); fp@2447: fp@2447: if (system_time_base > time) { fp@2447: rt_printk("%s() error: system_time_base greater than" fp@2447: " system time (system_time_base: %lld, time: %llu\n", fp@2447: __func__, system_time_base, time); fp@2447: return time; fp@2447: } fp@2447: else { fp@2447: return time - system_time_base; fp@2447: } fp@2447: } fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: /** Convert system time to RTAI time in counts (via the system_time_base). fp@2447: */ fp@2447: RTIME system2count( fp@2447: uint64_t time fp@2447: ) fp@2447: { fp@2447: RTIME ret; fp@2447: fp@2447: if ((system_time_base < 0) && fp@2447: ((uint64_t) (-system_time_base) > time)) { fp@2447: rt_printk("%s() error: system_time_base less than" fp@2447: " system time (system_time_base: %lld, time: %llu\n", fp@2447: __func__, system_time_base, time); fp@2447: ret = time; fp@2447: } fp@2447: else { fp@2447: ret = time + system_time_base; fp@2447: } fp@2447: fp@2447: return nano2count(ret); fp@2447: } fp@2447: fp@2447: /*****************************************************************************/ fp@2447: fp@2447: /** Synchronise the distributed clocks fp@2447: */ fp@2447: void sync_distributed_clocks(void) fp@2447: { fp@2447: #if SYNC_MASTER_TO_REF fp@2447: uint32_t ref_time = 0; fp@2447: uint64_t prev_app_time = dc_time_ns; fp@2447: #endif fp@2447: fp@2447: dc_time_ns = system_time_ns(); fp@2447: fp@2447: // set master time in nano-seconds fp@2447: ecrt_master_application_time(master, dc_time_ns); fp@2447: fp@2447: #if SYNC_MASTER_TO_REF fp@2447: // get reference clock time to synchronize master cycle fp@2447: ecrt_master_reference_clock_time(master, &ref_time); fp@2447: dc_diff_ns = (uint32_t) prev_app_time - ref_time; fp@2447: #else fp@2447: // sync reference clock to master fp@2447: ecrt_master_sync_reference_clock(master); fp@2447: #endif fp@2447: fp@2447: // call to sync slaves to ref slave fp@2447: ecrt_master_sync_slave_clocks(master); fp@2447: } fp@2447: fp@2447: /*****************************************************************************/ fp@2447: fp@2447: /** Return the sign of a number fp@2447: * fp@2447: * ie -1 for -ve value, 0 for 0, +1 for +ve value fp@2447: * fp@2447: * \retval the sign of the value fp@2447: */ fp@2447: #define sign(val) \ fp@2447: ({ typeof (val) _val = (val); \ fp@2447: ((_val > 0) - (_val < 0)); }) fp@2447: fp@2447: /*****************************************************************************/ fp@2447: fp@2447: /** Update the master time based on ref slaves time diff fp@2447: * fp@2447: * called after the ethercat frame is sent to avoid time jitter in fp@2447: * sync_distributed_clocks() fp@2447: */ fp@2447: void update_master_clock(void) fp@2447: { fp@2447: #if SYNC_MASTER_TO_REF fp@2447: // calc drift (via un-normalised time diff) fp@2447: int32_t delta = dc_diff_ns - prev_dc_diff_ns; fp@2447: prev_dc_diff_ns = dc_diff_ns; fp@2447: fp@2447: // normalise the time diff fp@2447: dc_diff_ns = fp@2447: ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); fp@2447: fp@2447: // only update if primary master fp@2447: if (dc_started) { fp@2447: fp@2447: // add to totals fp@2447: dc_diff_total_ns += dc_diff_ns; fp@2447: dc_delta_total_ns += delta; fp@2447: dc_filter_idx++; fp@2447: fp@2447: if (dc_filter_idx >= DC_FILTER_CNT) { fp@2447: // add rounded delta average fp@2447: dc_adjust_ns += fp@2447: ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT); fp@2447: fp@2447: // and add adjustment for general diff (to pull in drift) fp@2447: dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); fp@2447: fp@2447: // limit crazy numbers (0.1% of std cycle time) fp@2447: if (dc_adjust_ns < -1000) { fp@2447: dc_adjust_ns = -1000; fp@2447: } fp@2447: if (dc_adjust_ns > 1000) { fp@2447: dc_adjust_ns = 1000; fp@2447: } fp@2447: fp@2447: // reset fp@2447: dc_diff_total_ns = 0LL; fp@2447: dc_delta_total_ns = 0LL; fp@2447: dc_filter_idx = 0; fp@2447: } fp@2447: fp@2447: // add cycles adjustment to time base (including a spot adjustment) fp@2447: system_time_base += dc_adjust_ns + sign(dc_diff_ns); fp@2447: } fp@2447: else { fp@2447: dc_started = (dc_diff_ns != 0); fp@2447: fp@2447: if (dc_started) { fp@2447: // output first diff fp@2447: rt_printk("First master diff: %d.\n", dc_diff_ns); fp@2447: fp@2447: // record the time of this initial cycle fp@2447: dc_start_time_ns = dc_time_ns; fp@2447: } fp@2447: } fp@2447: #endif fp@2447: } fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: void rt_check_domain_state(void) fp@2447: { fp@2447: ec_domain_state_t ds = {}; fp@2447: fp@2447: ecrt_domain_state(domain1, &ds); fp@2447: fp@2447: if (ds.working_counter != domain1_state.working_counter) { fp@2447: rt_printf("Domain1: WC %u.\n", ds.working_counter); fp@2447: } fp@2447: fp@2447: if (ds.wc_state != domain1_state.wc_state) { fp@2447: rt_printf("Domain1: State %u.\n", ds.wc_state); fp@2447: } fp@2447: fp@2447: domain1_state = ds; fp@2447: } fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: void rt_check_master_state(void) fp@2447: { fp@2447: ec_master_state_t ms; fp@2447: fp@2447: ecrt_master_state(master, &ms); fp@2447: fp@2447: if (ms.slaves_responding != master_state.slaves_responding) { fp@2447: rt_printf("%u slave(s).\n", ms.slaves_responding); fp@2447: } fp@2447: fp@2447: if (ms.al_states != master_state.al_states) { fp@2447: rt_printf("AL states: 0x%02X.\n", ms.al_states); fp@2447: } fp@2447: fp@2447: if (ms.link_up != master_state.link_up) { fp@2447: rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); fp@2447: } fp@2447: fp@2447: master_state = ms; fp@2447: } fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: /** Wait for the next period fp@2447: */ fp@2447: void wait_period(void) fp@2447: { fp@2447: while (1) fp@2447: { fp@2447: RTIME wakeup_count = system2count(wakeup_time); fp@2447: RTIME current_count = rt_get_time(); fp@2447: fp@2447: if ((wakeup_count < current_count) fp@2447: || (wakeup_count > current_count + (50 * cycle_ns))) { fp@2447: rt_printk("%s(): unexpected wake time!\n", __func__); fp@2447: } fp@2447: fp@2447: switch (rt_sleep_until(wakeup_count)) { fp@2447: case RTE_UNBLKD: fp@2447: rt_printk("rt_sleep_until(): RTE_UNBLKD\n"); fp@2447: continue; fp@2447: fp@2447: case RTE_TMROVRN: fp@2447: rt_printk("rt_sleep_until(): RTE_TMROVRN\n"); fp@2447: overruns++; fp@2447: fp@2447: if (overruns % 100 == 0) { fp@2447: // in case wake time is broken ensure other processes get fp@2447: // some time slice (and error messages can get displayed) fp@2447: rt_sleep(cycle_ns / 100); fp@2447: } fp@2447: break; fp@2447: fp@2447: default: fp@2447: break; fp@2447: } fp@2447: fp@2447: // done if we got to here fp@2447: break; fp@2447: } fp@2447: fp@2447: // calc next wake time (in sys time) fp@2447: wakeup_time += cycle_ns; fp@2447: } fp@2447: fp@2447: /****************************************************************************/ fp@2447: fp@2447: void my_cyclic(void) fp@2447: { fp@2447: int cycle_counter = 0; fp@2447: unsigned int blink = 0; fp@2447: fp@2447: // oneshot mode to allow adjustable wake time fp@2447: rt_set_oneshot_mode(); fp@2447: fp@2447: // set first wake time in a few cycles fp@2447: wakeup_time = system_time_ns() + 10 * cycle_ns; fp@2447: fp@2447: // start the timer fp@2447: start_rt_timer(nano2count(cycle_ns)); fp@2447: fp@2447: rt_make_hard_real_time(); fp@2447: fp@2447: while (run) { fp@2447: // wait for next period (using adjustable system time) fp@2447: wait_period(); fp@2447: fp@2447: cycle_counter++; fp@2447: fp@2447: if (!run) { fp@2447: break; fp@2447: } fp@2447: fp@2447: // receive EtherCAT fp@2447: ecrt_master_receive(master); fp@2447: ecrt_domain_process(domain1); fp@2447: fp@2447: rt_check_domain_state(); fp@2447: fp@2447: if (!(cycle_counter % 1000)) { fp@2447: rt_check_master_state(); fp@2447: } fp@2447: fp@2447: if (!(cycle_counter % 200)) { fp@2447: blink = !blink; fp@2447: } fp@2447: fp@2447: EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0F); fp@2447: fp@2447: // queue process data fp@2447: ecrt_domain_queue(domain1); fp@2447: fp@2447: // sync distributed clock just before master_send to set fp@2447: // most accurate master clock time fp@2447: sync_distributed_clocks(); fp@2447: fp@2447: // send EtherCAT data fp@2447: ecrt_master_send(master); fp@2447: fp@2447: // update the master clock fp@2447: // Note: called after ecrt_master_send() to reduce time fp@2447: // jitter in the sync_distributed_clocks() call fp@2447: update_master_clock(); fp@2447: } fp@2447: fp@2447: rt_make_soft_real_time(); fp@2447: stop_rt_timer(); fp@2447: } fp@2447: fp@2447: /**************************************************************************** fp@2447: * Signal handler fp@2447: ***************************************************************************/ fp@2447: fp@2447: void signal_handler(int sig) fp@2447: { fp@2447: run = 0; fp@2447: } fp@2447: fp@2447: /**************************************************************************** fp@2447: * Main function fp@2447: ***************************************************************************/ fp@2447: fp@2447: int main(int argc, char *argv[]) fp@2447: { fp@2447: ec_slave_config_t *sc_ek1100; fp@2447: int ret; fp@2447: fp@2447: signal(SIGTERM, signal_handler); fp@2447: signal(SIGINT, signal_handler); fp@2447: fp@2447: mlockall(MCL_CURRENT | MCL_FUTURE); fp@2447: fp@2447: printf("Requesting master...\n"); fp@2447: master = ecrt_request_master(0); fp@2447: if (!master) { fp@2447: return -1; fp@2447: } fp@2447: fp@2447: domain1 = ecrt_master_create_domain(master); fp@2447: if (!domain1) { fp@2447: return -1; fp@2447: } fp@2447: fp@2447: printf("Creating slave configurations...\n"); fp@2447: fp@2447: // Create configuration for bus coupler fp@2447: sc_ek1100 = fp@2447: ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100); fp@2447: if (!sc_ek1100) { fp@2447: return -1; fp@2447: } fp@2447: fp@2447: sc_dig_out_01 = fp@2447: ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004); fp@2447: if (!sc_dig_out_01) { fp@2447: fprintf(stderr, "Failed to get slave configuration.\n"); fp@2447: return -1; fp@2447: } fp@2447: fp@2447: if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs)) { fp@2447: fprintf(stderr, "Failed to configure PDOs.\n"); fp@2447: return -1; fp@2447: } fp@2447: fp@2447: if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { fp@2447: fprintf(stderr, "PDO entry registration failed!\n"); fp@2447: return -1; fp@2447: } fp@2447: fp@2447: /* Set the initial master time and select a slave to use as the DC fp@2447: * reference clock, otherwise pass NULL to auto select the first capable fp@2447: * slave. Note: This can be used whether the master or the ref slave will fp@2447: * be used as the systems master DC clock. fp@2447: */ fp@2447: dc_start_time_ns = system_time_ns(); fp@2447: dc_time_ns = dc_start_time_ns; fp@2523: fp@2523: /* Attention: The initial application time is also used for phase fp@2523: * calculation for the SYNC0/1 interrupts. Please be sure to call it at fp@2523: * the correct phase to the realtime cycle. fp@2523: */ fp@2447: ecrt_master_application_time(master, dc_start_time_ns); fp@2447: fp@2447: ret = ecrt_master_select_reference_clock(master, sc_ek1100); fp@2447: if (ret < 0) { fp@2447: fprintf(stderr, "Failed to select reference clock: %s\n", fp@2447: strerror(-ret)); fp@2447: return ret; fp@2447: } fp@2447: fp@2447: printf("Activating master...\n"); fp@2447: if (ecrt_master_activate(master)) { fp@2447: return -1; fp@2447: } fp@2447: fp@2447: if (!(domain1_pd = ecrt_domain_data(domain1))) { fp@2447: fprintf(stderr, "Failed to get domain data pointer.\n"); fp@2447: return -1; fp@2447: } fp@2447: fp@2447: /* Create cyclic RT-thread */ fp@2447: struct sched_param param; fp@2447: param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1; fp@2447: if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { fp@2447: puts("ERROR IN SETTING THE SCHEDULER"); fp@2447: perror("errno"); fp@2447: return -1; fp@2447: } fp@2447: fp@2447: task = rt_task_init(nam2num("ec_rtai_rtdm_example"), fp@2447: 0 /* priority */, 0 /* stack size */, 0 /* msg size */); fp@2447: fp@2447: my_cyclic(); fp@2447: fp@2447: rt_task_delete(task); fp@2447: fp@2447: printf("End of Program\n"); fp@2447: ecrt_release_master(master); fp@2447: fp@2447: return 0; fp@2447: } fp@2447: fp@2447: /****************************************************************************/