beremiz: comparison etherlab/plc

equal deleted inserted replaced

-:09d5d1456616
+:c9deff128c37
 const static ec_pdo_entry_reg_t domain1_regs[] = {
 %(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
 long long wait_period_ns = 100000LL;
 char sbuf[256];\
 int slen = snprintf(sbuf , sizeof(sbuf) , format , ##args);\
 LogMessage(level, sbuf, slen);\
 }
-/* Beremiz plugin functions */
+/* EtherCAT plugin functions */
 int __init_%(location)s(int argc,char **argv)
 {
 uint32_t abort_code;
 size_t result_size;
 }
 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");
 goto ecat_failed;
 }
 %(retrieve_variables)s
 }
 }
+/*
 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);
 RTIME deadline = _last_occur ?
 _last_occur + common_ticktime__ :
 }
 if( _max_jitter * 10 < common_ticktime__ && _current_lag < _max_jitter){
 //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;
+}
+/*****************************************************************************/

branch	ethercat_from_kosmos
changeset 2641	c9deff128c37
parent 2165	02a2b5dee5e3