laurent@2022: /* laurent@2032: * Etherlab execution code laurent@2022: * laurent@2022: * */ laurent@2022: laurent@2022: #include laurent@2022: #include laurent@2022: #include laurent@2022: laurent@2022: #include "ecrt.h" laurent@2022: #include "ec_rtdm.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: int rt_fd = -1; laurent@2022: CstructMstrAttach MstrAttach; laurent@2022: char rt_dev_file[64]; 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@2081: int rtstatus; laurent@2032: laurent@2022: MstrAttach.masterindex = %(master_number)d; laurent@2022: laurent@2022: master = ecrt_request_master(MstrAttach.masterindex); laurent@2022: if (!master) return -1; laurent@2022: laurent@2022: domain1 = ecrt_master_create_domain(master); laurent@2022: if (!domain1) return -1; 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@2107: SLOGF(LOG_CRITICAL, "PDO entry registration failed!\n"); laurent@2022: return -1; laurent@2022: } laurent@2022: laurent@2036: 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: laurent@2022: sprintf(&rt_dev_file[0],"%%s%%u",EC_RTDM_DEV_FILE_NAME,0); laurent@2022: rt_fd = rt_dev_open( &rt_dev_file[0], 0); laurent@2022: if (rt_fd < 0) { Edouard@2107: SLOGF(LOG_CRITICAL, "Can't open %%s\n", &rt_dev_file[0]); laurent@2022: return -1; laurent@2022: } laurent@2022: laurent@2022: // attach the master over rtdm driver laurent@2022: MstrAttach.domainindex = ecrt_domain_index(domain1); laurent@2022: rtstatus = ecrt_rtdm_master_attach(rt_fd, &MstrAttach); laurent@2022: if (rtstatus < 0) { Edouard@2107: SLOGF(LOG_CRITICAL, "Cannot attach to master over rtdm\n"); laurent@2022: return -1; laurent@2022: } laurent@2022: laurent@2022: if (ecrt_master_activate(master)) laurent@2022: return -1; laurent@2022: laurent@2022: if (!(domain1_pd = ecrt_domain_data(domain1))) { Edouard@2107: SLOGF(LOG_CRITICAL, "domain1_pd: 0x%%.6lx\n", (unsigned long)domain1_pd); laurent@2022: return -1; laurent@2022: } laurent@2022: Edouard@2107: SLOGF(LOG_INFO, "Master %(master_number)d activated...\n"); Edouard@2075: Edouard@2075: first_sent = 0; Edouard@2075: laurent@2022: return 0; laurent@2022: } laurent@2022: laurent@2022: void __cleanup_%(location)s(void) laurent@2022: { laurent@2022: if (rt_fd >= 0) { laurent@2022: rt_dev_close(rt_fd); laurent@2022: } dgkang@2024: //release master dgkang@2025: 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@2075: ecrt_rtdm_master_recieve(rt_fd); Edouard@2075: ecrt_rtdm_domain_process(rt_fd); 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@2075: ecrt_rtdm_domain_queque(rt_fd); 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@2075: ecrt_rtdm_master_send(rt_fd); Edouard@2075: first_sent = 1; laurent@2022: }