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 laurent@2022: #include laurent@2022: #include 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: }; 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: laurent@2022: /* Beremiz 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 laurent@2032: %(slaves_initialization)s laurent@2032: Laurent@2079: // extracting default value for not mapped entry in output PDOs laurent@2039: %(slaves_output_pdos_default_values_extraction)s 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@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@2075: laurent@2022: void __publish_%(location)s(void) laurent@2022: { laurent@2022: %(publish_variables)s Edouard@2108: ecrt_domain_queue(domain1); Edouard@2075: { 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@2075: } Edouard@2108: ecrt_master_send(master); Edouard@2075: first_sent = 1; laurent@2022: }