120 first_sent = 0; |
120 first_sent = 0; |
121 } |
121 } |
122 |
122 |
123 void __retrieve_%(location)s(void) |
123 void __retrieve_%(location)s(void) |
124 { |
124 { |
125 // // send process data |
|
126 // ecrt_rtdm_domain_queque(rt_fd); |
|
127 // ecrt_rtdm_master_send(rt_fd); |
|
128 // |
|
129 // rt_task_sleep(rt_timer_ns2tsc(wait_period_ns)); |
|
130 |
|
131 // receive ethercat |
125 // receive ethercat |
132 if(first_sent){ |
126 if(first_sent){ |
133 ecrt_rtdm_master_recieve(rt_fd); |
127 ecrt_rtdm_master_recieve(rt_fd); |
134 ecrt_rtdm_domain_process(rt_fd); |
128 ecrt_rtdm_domain_process(rt_fd); |
135 %(retrieve_variables)s |
129 %(retrieve_variables)s |
136 } |
130 } |
137 |
131 |
138 } |
132 } |
139 |
133 |
140 static RTIME _last_occur=0; |
134 static RTIME _last_occur=0; |
|
135 static RTIME _last_publish=0; |
141 RTIME _current_lag=0; |
136 RTIME _current_lag=0; |
|
137 RTIME _max_jitter=0; |
|
138 static inline RTIME max(RTIME a,RTIME b){return a>b?a:b;} |
142 |
139 |
143 void __publish_%(location)s(void) |
140 void __publish_%(location)s(void) |
144 { |
141 { |
145 %(publish_variables)s |
142 %(publish_variables)s |
146 ecrt_rtdm_domain_queque(rt_fd); |
143 ecrt_rtdm_domain_queque(rt_fd); |
147 { |
144 { |
148 RTIME _current_time = rt_timer_read(); |
145 RTIME current_time = rt_timer_read(); |
149 RTIME deadline = _last_occur ? |
146 // Limit spining max 1/5 of common_ticktime |
150 _last_occur + common_ticktime__: |
147 RTIME maxdeadline = current_time + (common_ticktime__ / 5); |
151 _current_time; |
148 RTIME deadline = _last_occur ? |
152 _last_occur = _current_time; |
149 _last_occur + common_ticktime__ : |
153 _current_lag = deadline - _current_time; |
150 current_time + _max_jitter; |
154 while(_current_time < deadline) { |
151 if(deadline > maxdeadline) deadline = maxdeadline; |
155 _last_occur = _current_time; |
152 _current_lag = deadline - current_time; |
156 _current_time = rt_timer_read(); |
153 if(_last_publish != 0){ |
|
154 RTIME period = current_time - _last_publish; |
|
155 if(period > common_ticktime__ ) |
|
156 _max_jitter = max(_max_jitter, period - common_ticktime__); |
|
157 else |
|
158 _max_jitter = max(_max_jitter, common_ticktime__ - period); |
|
159 } |
|
160 _last_publish = current_time; |
|
161 _last_occur = current_time; |
|
162 while(current_time < deadline) { |
|
163 _last_occur = current_time; //Drift backward by default |
|
164 current_time = rt_timer_read(); |
|
165 } |
|
166 if( _max_jitter * 10 < common_ticktime__ && _current_lag < _max_jitter){ |
|
167 //Consuming security margin ? |
|
168 _last_occur = current_time; //Drift forward |
157 } |
169 } |
158 } |
170 } |
159 ecrt_rtdm_master_send(rt_fd); |
171 ecrt_rtdm_master_send(rt_fd); |
160 first_sent = 1; |
172 first_sent = 1; |
161 } |
173 } |