diff -r 09d5d1456616 -r c9deff128c37 etherlab/plc_etherlab.c --- a/etherlab/plc_etherlab.c Sat Jun 23 09:17:20 2018 +0200 +++ b/etherlab/plc_etherlab.c Wed Nov 20 16:57:15 2019 +0100 @@ -32,6 +32,43 @@ %(used_pdo_entry_configuration)s {} }; + +// Distributed Clock variables; +%(dc_variable)s +unsigned long long comp_period_ns = 500000ULL; + +int comp_count = 1; +int comp_count_max; + +#define DC_FILTER_CNT 1024 + +// EtherCAT slave-time-based DC Synchronization variables. +static uint64_t dc_start_time_ns = 0LL; +static uint64_t dc_time_ns = 0; +static uint8_t dc_started = 0; +static int32_t dc_diff_ns = 0; +static int32_t prev_dc_diff_ns = 0; +static int64_t dc_diff_total_ns = 0LL; +static int64_t dc_delta_total_ns = 0LL; +static int dc_filter_idx = 0; +static int64_t dc_adjust_ns; +static int64_t system_time_base = 0LL; + +static uint64_t dc_first_app_time = 0LL; + +unsigned long long frame_period_ns = 0ULL; + +int debug_count = 0; +int slave_dc_used = 0; + +void dc_init(void); +uint64_t system_time_ns(void); +RTIME system2count(uint64_t time); +void sync_distributed_clocks(void); +void update_master_clock(void); +RTIME calculate_sleeptime(uint64_t wakeup_time); +uint64_t calculate_first(void); + /*****************************************************************************/ %(pdos_configuration_declaration)s @@ -50,7 +87,7 @@ LogMessage(level, sbuf, slen);\ } -/* Beremiz plugin functions */ +/* EtherCAT plugin functions */ int __init_%(location)s(int argc,char **argv) { uint32_t abort_code; @@ -81,10 +118,34 @@ ecrt_master_set_send_interval(master, common_ticktime__); // slaves initialization +/* %(slaves_initialization)s +*/ + // configure DC SYNC0/1 Signal +%(config_dc)s + + // select reference clock +#if DC_ENABLE + { + int ret; + + ret = ecrt_master_select_reference_clock(master, slave0); + if (ret <0) { + fprintf(stderr, "Failed to select reference clock : %%s\n", + strerror(-ret)); + return ret; + } + } +#endif // extracting default value for not mapped entry in output PDOs +/* %(slaves_output_pdos_default_values_extraction)s +*/ + +#if DC_ENABLE + dc_init(); +#endif if (ecrt_master_activate(master)){ SLOGF(LOG_CRITICAL, "EtherCAT Master activation failed"); @@ -126,17 +187,20 @@ } +/* static RTIME _last_occur=0; static RTIME _last_publish=0; RTIME _current_lag=0; RTIME _max_jitter=0; static inline RTIME max(RTIME a,RTIME b){return a>b?a:b;} +*/ void __publish_%(location)s(void) { %(publish_variables)s ecrt_domain_queue(domain1); { + /* RTIME current_time = rt_timer_read(); // Limit spining max 1/5 of common_ticktime RTIME maxdeadline = current_time + (common_ticktime__ / 5); @@ -162,7 +226,281 @@ //Consuming security margin ? _last_occur = current_time; //Drift forward } - } + */ + } + +#if DC_ENABLE + if (comp_count == 0) + sync_distributed_clocks(); +#endif + ecrt_master_send(master); first_sent = 1; -} + +#if DC_ENABLE + if (comp_count == 0) + update_master_clock(); + + comp_count++; + + if (comp_count == comp_count_max) + comp_count = 0; +#endif + +} + +/* Test Function For Parameter (SDO) Set */ + +/* +void GetSDOData(void){ + uint32_t abort_code, test_value; + size_t result_size; + uint8_t value[4]; + + abort_code = 0; + result_size = 0; + test_value = 0; + + if (ecrt_master_sdo_upload(master, 0, 0x1000, 0x0, (uint8_t *)value, 4, &result_size, &abort_code)) { + SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value"); + } + test_value = EC_READ_S32((uint8_t *)value); + SLOGF(LOG_INFO, "SDO Value %%d", test_value); +} +*/ + +int GetMasterData(void){ + master = ecrt_open_master(0); + if (!master) { + SLOGF(LOG_CRITICAL, "EtherCAT master request failed!"); + return -1; + } + return 0; +} + +void ReleaseMasterData(void){ + ecrt_release_master(master); +} + +uint32_t GetSDOData(uint16_t slave_pos, uint16_t idx, uint8_t subidx, int size){ + uint32_t abort_code, return_value; + size_t result_size; + uint8_t value[size]; + + abort_code = 0; + result_size = 0; + + if (ecrt_master_sdo_upload(master, slave_pos, idx, subidx, (uint8_t *)value, size, &result_size, &abort_code)) { + SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value %%d %%d", idx, subidx); + } + + return_value = EC_READ_S32((uint8_t *)value); + //SLOGF(LOG_INFO, "SDO Value %%d", return_value); + + return return_value; +} + +/*****************************************************************************/ + +void dc_init(void) +{ + slave_dc_used = 1; + + frame_period_ns = common_ticktime__; + if (frame_period_ns <= comp_period_ns) { + comp_count_max = comp_period_ns / frame_period_ns; + comp_count = 0; + } else { + comp_count_max = 1; + comp_count = 0; + } + + /* Set the initial master time */ + dc_start_time_ns = system_time_ns(); + dc_time_ns = dc_start_time_ns; + + /* by woonggy */ + dc_first_app_time = dc_start_time_ns; + + /* + * Attention : The initial application time is also used for phase + * calculation for the SYNC0/1 interrupts. Please be sure to call it at + * the correct phase to the realtime cycle. + */ + ecrt_master_application_time(master, dc_start_time_ns); +} + +/****************************************************************************/ + +/* + * Get the time in ns for the current cpu, adjusted by system_time_base. + * + * \attention Rather than calling rt_timer_read() directly, all application + * time calls should use this method instead. + * + * \ret The time in ns. + */ +uint64_t system_time_ns(void) +{ + RTIME time = rt_timer_read(); // wkk + + if (unlikely(system_time_base > (SRTIME) time)) { + fprintf(stderr, "%%s() error: system_time_base greater than" + " system time (system_time_base: %%ld, time: %%llu\n", + __func__, system_time_base, time); + return time; + } + else { + return time - system_time_base; + } +} + +/****************************************************************************/ + +// Convert system time to Xenomai time in counts (via the system_time_base). +RTIME system2count(uint64_t time) +{ + RTIME ret; + + if ((system_time_base < 0) && + ((uint64_t) (-system_time_base) > time)) { + fprintf(stderr, "%%s() error: system_time_base less than" + " system time (system_time_base: %%I64d, time: %%ld\n", + __func__, system_time_base, time); + ret = time; + } + else { + ret = time + system_time_base; + } + + return (RTIME) rt_timer_ns2ticks(ret); // wkk +} + +/*****************************************************************************/ + +// Synchronise the distributed clocks +void sync_distributed_clocks(void) +{ + uint32_t ref_time = 0; + RTIME prev_app_time = dc_time_ns; + + // get reference clock time to synchronize master cycle + if(!ecrt_master_reference_clock_time(master, &ref_time)) { + dc_diff_ns = (uint32_t) prev_app_time - ref_time; + } + // call to sync slaves to ref slave + ecrt_master_sync_slave_clocks(master); + // set master time in nano-seconds + dc_time_ns = system_time_ns(); + ecrt_master_application_time(master, dc_time_ns); +} + +/*****************************************************************************/ + +/* + * Return the sign of a number + * ie -1 for -ve value, 0 for 0, +1 for +ve value + * \ret val the sign of the value + */ +#define sign(val) \ + ({ typeof (val) _val = (val); \ + ((_val > 0) - (_val < 0)); }) + +/*****************************************************************************/ + +/* + * Update the master time based on ref slaves time diff + * called after the ethercat frame is sent to avoid time jitter in + * sync_distributed_clocks() + */ +void update_master_clock(void) +{ + // calc drift (via un-normalised time diff) + int32_t delta = dc_diff_ns - prev_dc_diff_ns; + prev_dc_diff_ns = dc_diff_ns; + + // normalise the time diff + dc_diff_ns = dc_diff_ns >= 0 ? + ((dc_diff_ns + (int32_t)(frame_period_ns / 2)) %% + (int32_t)frame_period_ns) - (frame_period_ns / 2) : + ((dc_diff_ns - (int32_t)(frame_period_ns / 2)) %% + (int32_t)frame_period_ns) - (frame_period_ns / 2) ; + + // only update if primary master + if (dc_started) { + // add to totals + dc_diff_total_ns += dc_diff_ns; + dc_delta_total_ns += delta; + dc_filter_idx++; + + if (dc_filter_idx >= DC_FILTER_CNT) { + dc_adjust_ns += dc_delta_total_ns >= 0 ? + ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) : + ((dc_delta_total_ns - (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) ; + + // and add adjustment for general diff (to pull in drift) + dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); + + // limit crazy numbers (0.1%% of std cycle time) + if (dc_adjust_ns < -1000) { + dc_adjust_ns = -1000; + } + if (dc_adjust_ns > 1000) { + dc_adjust_ns = 1000; + } + // reset + dc_diff_total_ns = 0LL; + dc_delta_total_ns = 0LL; + dc_filter_idx = 0; + } + // add cycles adjustment to time base (including a spot adjustment) + system_time_base += dc_adjust_ns + sign(dc_diff_ns); + } + else { + dc_started = (dc_diff_ns != 0); + + if (dc_started) { +#if DC_ENABLE && DEBUG_MODE + // output first diff + fprintf(stderr, "First master diff: %%d\n", dc_diff_ns); +#endif + // record the time of this initial cycle + dc_start_time_ns = dc_time_ns; + } + } +} + +/*****************************************************************************/ + +/* + * Calculate the sleeptime + */ +RTIME calculate_sleeptime(uint64_t wakeup_time) +{ + RTIME wakeup_count = system2count (wakeup_time); + RTIME current_count = rt_timer_read(); + + if ((wakeup_count < current_count) || (wakeup_count > current_count + (50 * frame_period_ns))) { + fprintf(stderr, "%%s(): unexpected wake time! wc = %%lld\tcc = %%lld\n", __func__, wakeup_count, current_count); + } + + return wakeup_count; +} + +/*****************************************************************************/ + +/* + * Calculate the sleeptime + */ +uint64_t calculate_first(void) +{ + uint64_t dc_remainder = 0LL; + uint64_t dc_phase_set_time = 0LL; + + dc_phase_set_time = system_time_ns()+ frame_period_ns * 10; + dc_remainder = (dc_phase_set_time - dc_first_app_time) %% frame_period_ns; + + return dc_phase_set_time + frame_period_ns - dc_remainder; +} + +/*****************************************************************************/