laurent@2022: /*
edouard@2165: 
edouard@2165: Template C code used to produce target Ethercat C code
edouard@2165: 
edouard@2165: Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT
edouard@2165: 
edouard@2165: Distributed under the terms of the GNU Lesser General Public License as
edouard@2165: published by the Free Software Foundation; either version 2 of the License, or
edouard@2165: (at your option) any later version.
edouard@2165: 
edouard@2165: See COPYING file for copyrights details.
edouard@2165: 
edouard@2165: */
laurent@2022: 
laurent@2022: #include <rtdm/rtdm.h>
laurent@2022: #include <native/task.h>
laurent@2022: #include <native/timer.h>
laurent@2022: 
laurent@2022: #include "ecrt.h"
laurent@2022: 
Edouard@2106: #include "beremiz.h"
Edouard@2106: #include "iec_types_all.h"
laurent@2036: 
laurent@2022: // declaration of interface variables
laurent@2022: %(located_variables_declaration)s
laurent@2022: 
laurent@2022: // process data
laurent@2032: uint8_t *domain1_pd = NULL;
laurent@2022: %(used_pdo_entry_offset_variables_declaration)s
laurent@2022: 
laurent@2022: const static ec_pdo_entry_reg_t domain1_regs[] = {
laurent@2022: %(used_pdo_entry_configuration)s
laurent@2022:     {}
laurent@2022: };
edouard@2641: 
edouard@2641: // Distributed Clock variables;
edouard@2641: %(dc_variable)s
edouard@2641: unsigned long long comp_period_ns = 500000ULL;
edouard@2641: 
edouard@2641: int comp_count = 1;
edouard@2641: int comp_count_max;
edouard@2641: 
edouard@2641: #define DC_FILTER_CNT          1024
edouard@2641: 
edouard@2641: // EtherCAT slave-time-based DC Synchronization variables.
edouard@2641: static uint64_t dc_start_time_ns = 0LL;
edouard@2641: static uint64_t dc_time_ns = 0;
edouard@2641: static uint8_t  dc_started = 0;
edouard@2641: static int32_t  dc_diff_ns = 0;
edouard@2641: static int32_t  prev_dc_diff_ns = 0;
edouard@2641: static int64_t  dc_diff_total_ns = 0LL;
edouard@2641: static int64_t  dc_delta_total_ns = 0LL;
edouard@2641: static int      dc_filter_idx = 0;
edouard@2641: static int64_t  dc_adjust_ns;
edouard@2641: static int64_t  system_time_base = 0LL;
edouard@2641: 
edouard@2641: static uint64_t dc_first_app_time = 0LL;
edouard@2641: 
edouard@2641: unsigned long long frame_period_ns = 0ULL;
edouard@2641: 
edouard@2641: int debug_count = 0;
edouard@2641: int slave_dc_used = 0;
edouard@2641: 
edouard@2641: void dc_init(void);
edouard@2641: uint64_t system_time_ns(void);
edouard@2641: RTIME system2count(uint64_t time);
edouard@2641: void sync_distributed_clocks(void);
edouard@2641: void update_master_clock(void);
edouard@2641: RTIME calculate_sleeptime(uint64_t wakeup_time);
edouard@2641: uint64_t calculate_first(void);
edouard@2641: 
laurent@2022: /*****************************************************************************/
laurent@2022: 
laurent@2022: %(pdos_configuration_declaration)s
laurent@2022: 
laurent@2022: long long wait_period_ns = 100000LL;
laurent@2022: 
laurent@2022: // EtherCAT
laurent@2022: static ec_master_t *master = NULL;
laurent@2022: static ec_domain_t *domain1 = NULL;
Edouard@2075: static int first_sent=0;
laurent@2022: %(slaves_declaration)s
Edouard@2107: #define SLOGF(level, format, args...)\
Edouard@2107: {\
Edouard@2107:     char sbuf[256];\
Edouard@2107:     int slen = snprintf(sbuf , sizeof(sbuf) , format , ##args);\
Edouard@2107:     LogMessage(level, sbuf, slen);\
Edouard@2107: }
laurent@2022: 
edouard@2641: /* EtherCAT plugin functions */
laurent@2022: int __init_%(location)s(int argc,char **argv)
laurent@2022: {
laurent@2032:     uint32_t abort_code;
laurent@2039:     size_t result_size;
laurent@2032:     
Laurent@2123:     abort_code = 0;
Laurent@2123:     result_size = 0;
Laurent@2123: 
Edouard@2117:     master = ecrt_request_master(%(master_number)d);
Edouard@2117:     if (!master) {
Edouard@2116:         SLOGF(LOG_CRITICAL, "EtherCAT master request failed!");
Edouard@2116:         return -1;
Edouard@2116:     }
laurent@2022: 
Edouard@2117:     if(!(domain1 = ecrt_master_create_domain(master))){
Edouard@2116:         SLOGF(LOG_CRITICAL, "EtherCAT Domain Creation failed!");
Edouard@2116:         goto ecat_failed;
Edouard@2116:     }
laurent@2022: 
Laurent@2079:     // slaves PDO configuration
Laurent@2079: %(slaves_configuration)s
laurent@2022: 
laurent@2022:     if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
Edouard@2116:         SLOGF(LOG_CRITICAL, "EtherCAT PDO registration failed!");
Edouard@2116:         goto ecat_failed;
laurent@2022:     }
laurent@2022: 
Edouard@2117:     ecrt_master_set_send_interval(master, common_ticktime__);
laurent@2036: 
Laurent@2079:     // slaves initialization
edouard@2641: /*
laurent@2032: %(slaves_initialization)s
edouard@2641: */
edouard@2641:     // configure DC SYNC0/1 Signal
edouard@2641: %(config_dc)s
edouard@2641: 
edouard@2641:     // select reference clock
edouard@2641: #if DC_ENABLE
edouard@2641:     {
edouard@2641:         int ret;
edouard@2641:         
edouard@2641:         ret = ecrt_master_select_reference_clock(master, slave0);
edouard@2641:         if (ret <0) {
edouard@2641:             fprintf(stderr, "Failed to select reference clock : %%s\n",
edouard@2641:                 strerror(-ret));
edouard@2641:             return ret;
edouard@2641:         }
edouard@2641:     }
edouard@2641: #endif
laurent@2032: 
Laurent@2079:     // extracting default value for not mapped entry in output PDOs
edouard@2641: /*
laurent@2039: %(slaves_output_pdos_default_values_extraction)s
edouard@2641: */
edouard@2641: 
edouard@2641: #if DC_ENABLE
edouard@2641:     dc_init();
edouard@2641: #endif
laurent@2039: 
Edouard@2116:     if (ecrt_master_activate(master)){
Edouard@2116:         SLOGF(LOG_CRITICAL, "EtherCAT Master activation failed");
Edouard@2116:         goto ecat_failed;
Edouard@2116:     }
laurent@2022: 
laurent@2022:     if (!(domain1_pd = ecrt_domain_data(domain1))) {
Edouard@2116:         SLOGF(LOG_CRITICAL, "Failed to map EtherCAT process data");
Edouard@2116:         goto ecat_failed;
laurent@2022:     }
laurent@2022: 
Edouard@2116:     SLOGF(LOG_INFO, "Master %(master_number)d activated.");
Edouard@2075:     
Edouard@2075:     first_sent = 0;
Edouard@2075: 
laurent@2022:     return 0;
Edouard@2116: 
Edouard@2116: ecat_failed:
Edouard@2117:     ecrt_release_master(master);
Edouard@2116:     return -1;
Edouard@2116: 
laurent@2022: }
laurent@2022: 
laurent@2022: void __cleanup_%(location)s(void)
laurent@2022: {
Edouard@2117:     //release master
Edouard@2117:     ecrt_release_master(master);
Edouard@2075:     first_sent = 0;
laurent@2022: }
laurent@2022: 
laurent@2022: void __retrieve_%(location)s(void)
laurent@2022: {
laurent@2022:     // receive ethercat
Edouard@2075:     if(first_sent){
Edouard@2108:         ecrt_master_receive(master);
Edouard@2108:         ecrt_domain_process(domain1);
Edouard@2075: %(retrieve_variables)s
Edouard@2075:     }
laurent@2022: 
laurent@2022: }
laurent@2022: 
edouard@2641: /*
Edouard@2075: static RTIME _last_occur=0;
Edouard@2076: static RTIME _last_publish=0;
Edouard@2075: RTIME _current_lag=0;
Edouard@2076: RTIME _max_jitter=0;
Edouard@2076: static inline RTIME max(RTIME a,RTIME b){return a>b?a:b;}
edouard@2641: */
Edouard@2075: 
laurent@2022: void __publish_%(location)s(void)
laurent@2022: {
laurent@2022: %(publish_variables)s
Edouard@2108:     ecrt_domain_queue(domain1);
Edouard@2075:     {
edouard@2641:         /*
Edouard@2076:         RTIME current_time = rt_timer_read();
Edouard@2076:         // Limit spining max 1/5 of common_ticktime
Edouard@2076:         RTIME maxdeadline = current_time + (common_ticktime__ / 5);
Edouard@2076:         RTIME deadline = _last_occur ? 
Edouard@2076:             _last_occur + common_ticktime__ : 
Edouard@2076:             current_time + _max_jitter; 
Edouard@2076:         if(deadline > maxdeadline) deadline = maxdeadline;
Edouard@2076:         _current_lag = deadline - current_time;
Edouard@2076:         if(_last_publish != 0){
Edouard@2076:             RTIME period = current_time - _last_publish;
Edouard@2076:             if(period > common_ticktime__ )
Edouard@2076:                 _max_jitter = max(_max_jitter, period - common_ticktime__);
Edouard@2076:             else
Edouard@2076:                 _max_jitter = max(_max_jitter, common_ticktime__ - period);
Edouard@2076:         }
Edouard@2076:         _last_publish = current_time;
Edouard@2076:         _last_occur = current_time;
Edouard@2076:         while(current_time < deadline) {
Edouard@2076:             _last_occur = current_time; //Drift backward by default
Edouard@2076:             current_time = rt_timer_read();
Edouard@2076:         }
Edouard@2076:         if( _max_jitter * 10 < common_ticktime__ && _current_lag < _max_jitter){
Edouard@2076:             //Consuming security margin ?
Edouard@2076:             _last_occur = current_time; //Drift forward
Edouard@2075:         }
edouard@2641:         */
edouard@2641:     }
edouard@2641: 
edouard@2641: #if DC_ENABLE
edouard@2641:     if (comp_count == 0)
edouard@2641:         sync_distributed_clocks();
edouard@2641: #endif
edouard@2641: 
Edouard@2108:     ecrt_master_send(master);
Edouard@2075:     first_sent = 1;
edouard@2641: 
edouard@2641: #if DC_ENABLE
edouard@2641:     if (comp_count == 0)
edouard@2641:         update_master_clock();
edouard@2641: 
edouard@2641:     comp_count++;
edouard@2641:     
edouard@2641:     if (comp_count == comp_count_max)
edouard@2641:         comp_count = 0;
edouard@2641: #endif
edouard@2641: 
edouard@2641: }
edouard@2641: 
edouard@2641: /* Test Function For Parameter (SDO) Set */
edouard@2641: 
edouard@2641: /*
edouard@2641: void GetSDOData(void){
edouard@2641:     uint32_t abort_code, test_value;
edouard@2641:     size_t result_size;
edouard@2641:     uint8_t value[4];
edouard@2641: 
edouard@2641:     abort_code = 0;
edouard@2641:     result_size = 0;
edouard@2641:     test_value = 0;
edouard@2641: 
edouard@2641:     if (ecrt_master_sdo_upload(master, 0, 0x1000, 0x0, (uint8_t *)value, 4, &result_size, &abort_code)) {
edouard@2641:         SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value");
edouard@2641:         }
edouard@2641:         test_value = EC_READ_S32((uint8_t *)value);
edouard@2641:         SLOGF(LOG_INFO, "SDO Value %%d", test_value);
edouard@2641: }
edouard@2641: */
edouard@2641: 
edouard@2641: int GetMasterData(void){
edouard@2641:     master = ecrt_open_master(0);
edouard@2641:     if (!master) {
edouard@2641:         SLOGF(LOG_CRITICAL, "EtherCAT master request failed!");
edouard@2641:         return -1;
edouard@2641:     }
edouard@2641:     return 0;
edouard@2641: }
edouard@2641: 
edouard@2641: void ReleaseMasterData(void){
edouard@2641:     ecrt_release_master(master);
edouard@2641: }
edouard@2641: 
edouard@2641: uint32_t GetSDOData(uint16_t slave_pos, uint16_t idx, uint8_t subidx, int size){
edouard@2641:     uint32_t abort_code, return_value;
edouard@2641:     size_t result_size;
edouard@2641:     uint8_t value[size];
edouard@2641: 
edouard@2641:     abort_code = 0;
edouard@2641:     result_size = 0;
edouard@2641: 
edouard@2641:     if (ecrt_master_sdo_upload(master, slave_pos, idx, subidx, (uint8_t *)value, size, &result_size, &abort_code)) {
edouard@2641:         SLOGF(LOG_CRITICAL, "EtherCAT failed to get SDO Value %%d %%d", idx, subidx);
edouard@2641:     }
edouard@2641: 
edouard@2641:     return_value = EC_READ_S32((uint8_t *)value);
edouard@2641:     //SLOGF(LOG_INFO, "SDO Value %%d", return_value);
edouard@2641: 
edouard@2641:     return return_value;
edouard@2641: }
edouard@2641: 
edouard@2641: /*****************************************************************************/
edouard@2641: 
edouard@2641: void dc_init(void)
edouard@2641: {
edouard@2641:     slave_dc_used = 1;
edouard@2641: 
edouard@2641:     frame_period_ns = common_ticktime__;
edouard@2641:     if (frame_period_ns <= comp_period_ns) {
edouard@2641:         comp_count_max = comp_period_ns / frame_period_ns;
edouard@2641:         comp_count = 0;
edouard@2641:     } else  {
edouard@2641:         comp_count_max = 1;
edouard@2641:         comp_count = 0;
edouard@2641:     }
edouard@2641: 
edouard@2641:     /* Set the initial master time */
edouard@2641:     dc_start_time_ns = system_time_ns();
edouard@2641:     dc_time_ns = dc_start_time_ns;
edouard@2641: 
edouard@2641:     /* by woonggy */
edouard@2641:     dc_first_app_time = dc_start_time_ns;
edouard@2641: 
edouard@2641:     /*
edouard@2641:      * Attention : The initial application time is also used for phase
edouard@2641:      * calculation for the SYNC0/1 interrupts. Please be sure to call it at
edouard@2641:      * the correct phase to the realtime cycle.
edouard@2641:      */
edouard@2641:     ecrt_master_application_time(master, dc_start_time_ns);
edouard@2641: }
edouard@2641: 
edouard@2641: /****************************************************************************/
edouard@2641: 
edouard@2641: /*
edouard@2641:  * Get the time in ns for the current cpu, adjusted by system_time_base.
edouard@2641:  *
edouard@2641:  * \attention Rather than calling rt_timer_read() directly, all application
edouard@2641:  * time calls should use this method instead.
edouard@2641:  *
edouard@2641:  * \ret The time in ns.
edouard@2641:  */
edouard@2641: uint64_t system_time_ns(void)
edouard@2641: {
edouard@2641:     RTIME time = rt_timer_read();   // wkk
edouard@2641: 
edouard@2641:     if (unlikely(system_time_base > (SRTIME) time)) {
edouard@2641:         fprintf(stderr, "%%s() error: system_time_base greater than"
edouard@2641:                 " system time (system_time_base: %%ld, time: %%llu\n",
edouard@2641:                 __func__, system_time_base, time);
edouard@2641:         return time;
edouard@2641:     }
edouard@2641:     else {
edouard@2641:         return time - system_time_base;
edouard@2641:     }
edouard@2641: }
edouard@2641: 
edouard@2641: /****************************************************************************/
edouard@2641: 
edouard@2641: // Convert system time to Xenomai time in counts (via the system_time_base).
edouard@2641: RTIME system2count(uint64_t time)
edouard@2641: {
edouard@2641:     RTIME ret;
edouard@2641: 
edouard@2641:     if ((system_time_base < 0) &&
edouard@2641:             ((uint64_t) (-system_time_base) > time)) {
edouard@2641:         fprintf(stderr, "%%s() error: system_time_base less than"
edouard@2641:                 " system time (system_time_base: %%I64d, time: %%ld\n",
edouard@2641:                 __func__, system_time_base, time);
edouard@2641:         ret = time;
edouard@2641:     }
edouard@2641:     else {
edouard@2641:         ret = time + system_time_base;
edouard@2641:     }
edouard@2641: 
edouard@2641:     return (RTIME) rt_timer_ns2ticks(ret); // wkk
edouard@2641: }
edouard@2641: 
edouard@2641: /*****************************************************************************/
edouard@2641: 
edouard@2641: // Synchronise the distributed clocks
edouard@2641: void sync_distributed_clocks(void)
edouard@2641: {
edouard@2641:     uint32_t ref_time = 0;
edouard@2641:     RTIME prev_app_time = dc_time_ns;
edouard@2641: 
edouard@2641:     // get reference clock time to synchronize master cycle
edouard@2641:     if(!ecrt_master_reference_clock_time(master, &ref_time)) {
edouard@2641:         dc_diff_ns = (uint32_t) prev_app_time - ref_time;
edouard@2641:     }
edouard@2641:     // call to sync slaves to ref slave
edouard@2641:     ecrt_master_sync_slave_clocks(master);
edouard@2641:     // set master time in nano-seconds
edouard@2641:     dc_time_ns = system_time_ns();
edouard@2641:     ecrt_master_application_time(master, dc_time_ns);
edouard@2641: }
edouard@2641: 
edouard@2641: /*****************************************************************************/
edouard@2641: 
edouard@2641: /*
edouard@2641:  * Return the sign of a number
edouard@2641:  * ie -1 for -ve value, 0 for 0, +1 for +ve value
edouard@2641:  * \ret val the sign of the value
edouard@2641:  */
edouard@2641: #define sign(val) \
edouard@2641:         ({ typeof (val) _val = (val); \
edouard@2641:         ((_val > 0) - (_val < 0)); })
edouard@2641: 
edouard@2641: /*****************************************************************************/
edouard@2641: 
edouard@2641: /*
edouard@2641:  * Update the master time based on ref slaves time diff
edouard@2641:  * called after the ethercat frame is sent to avoid time jitter in
edouard@2641:  * sync_distributed_clocks()
edouard@2641:  */
edouard@2641: void update_master_clock(void)
edouard@2641: {
edouard@2641:     // calc drift (via un-normalised time diff)
edouard@2641:     int32_t delta = dc_diff_ns - prev_dc_diff_ns;
edouard@2641:     prev_dc_diff_ns = dc_diff_ns;
edouard@2641: 
edouard@2641:     // normalise the time diff
edouard@2641:     dc_diff_ns = dc_diff_ns >= 0 ?
edouard@2641:             ((dc_diff_ns + (int32_t)(frame_period_ns / 2)) %%
edouard@2641:                     (int32_t)frame_period_ns) - (frame_period_ns / 2) :
edouard@2641:                     ((dc_diff_ns - (int32_t)(frame_period_ns / 2)) %%
edouard@2641:                             (int32_t)frame_period_ns) - (frame_period_ns / 2) ;
edouard@2641: 
edouard@2641:     // only update if primary master
edouard@2641:     if (dc_started) {
edouard@2641:         // add to totals
edouard@2641:         dc_diff_total_ns += dc_diff_ns;
edouard@2641:         dc_delta_total_ns += delta;
edouard@2641:         dc_filter_idx++;
edouard@2641: 
edouard@2641:         if (dc_filter_idx >= DC_FILTER_CNT) {
edouard@2641:             dc_adjust_ns += dc_delta_total_ns >= 0 ?
edouard@2641:                     ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) :
edouard@2641:                     ((dc_delta_total_ns - (DC_FILTER_CNT / 2)) / DC_FILTER_CNT) ;
edouard@2641: 
edouard@2641:             // and add adjustment for general diff (to pull in drift)
edouard@2641:             dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT);
edouard@2641: 
edouard@2641:             // limit crazy numbers (0.1%% of std cycle time)
edouard@2641:             if (dc_adjust_ns < -1000) {
edouard@2641:                 dc_adjust_ns = -1000;
edouard@2641:             }
edouard@2641:             if (dc_adjust_ns > 1000) {
edouard@2641:                 dc_adjust_ns =  1000;
edouard@2641:             }
edouard@2641:             // reset
edouard@2641:             dc_diff_total_ns = 0LL;
edouard@2641:             dc_delta_total_ns = 0LL;
edouard@2641:             dc_filter_idx = 0;
edouard@2641:         }
edouard@2641:         // add cycles adjustment to time base (including a spot adjustment)
edouard@2641:         system_time_base += dc_adjust_ns + sign(dc_diff_ns);
edouard@2641:     }
edouard@2641:     else {
edouard@2641:         dc_started = (dc_diff_ns != 0);
edouard@2641: 
edouard@2641:         if (dc_started) {
edouard@2641: #if DC_ENABLE && DEBUG_MODE
edouard@2641:             // output first diff
edouard@2641:             fprintf(stderr, "First master diff: %%d\n", dc_diff_ns);
edouard@2641: #endif
edouard@2641:             // record the time of this initial cycle
edouard@2641:             dc_start_time_ns = dc_time_ns;
edouard@2641:         }
edouard@2641:     }
edouard@2641: }
edouard@2641: 
edouard@2641: /*****************************************************************************/
edouard@2641: 
edouard@2641: /*
edouard@2641:  * Calculate the sleeptime
edouard@2641:  */
edouard@2641: RTIME calculate_sleeptime(uint64_t wakeup_time)
edouard@2641: {
edouard@2641:     RTIME wakeup_count = system2count (wakeup_time);
edouard@2641:     RTIME current_count = rt_timer_read();
edouard@2641: 
edouard@2641:     if ((wakeup_count < current_count) || (wakeup_count > current_count + (50 * frame_period_ns)))  {
edouard@2641:         fprintf(stderr, "%%s(): unexpected wake time! wc = %%lld\tcc = %%lld\n", __func__, wakeup_count, current_count);
edouard@2641:     }
edouard@2641: 
edouard@2641:     return wakeup_count;
edouard@2641: }
edouard@2641: 
edouard@2641: /*****************************************************************************/
edouard@2641: 
edouard@2641: /*
edouard@2641:  * Calculate the sleeptime
edouard@2641:  */
edouard@2641: uint64_t calculate_first(void)
edouard@2641: {
edouard@2641:     uint64_t dc_remainder = 0LL;
edouard@2641:     uint64_t dc_phase_set_time = 0LL;
edouard@2641:     
edouard@2641:     dc_phase_set_time = system_time_ns()+ frame_period_ns * 10;
edouard@2641:     dc_remainder = (dc_phase_set_time - dc_first_app_time) %% frame_period_ns;
edouard@2641: 
edouard@2641:     return dc_phase_set_time + frame_period_ns - dc_remainder;
edouard@2641: }
edouard@2641: 
edouard@2641: /*****************************************************************************/