61 void __cleanup() |
70 void __cleanup() |
62 { |
71 { |
63 %(cleanup_calls)s |
72 %(cleanup_calls)s |
64 } |
73 } |
65 |
74 |
|
75 |
|
76 void PLC_GetTime(IEC_TIME *CURRENT_TIME); |
|
77 void PLC_SetTimer(long long next, long long period); |
|
78 |
|
79 #define CALIBRATED -2 |
|
80 #define NOT_CALIBRATED -1 |
|
81 static int calibration_count = NOT_CALIBRATED; |
|
82 static IEC_TIME cal_begin; |
|
83 static long long Tsync = 0; |
|
84 static long long FreqCorr = 0; |
|
85 static int Nticks = 0; |
|
86 static int last_tick = 0; |
|
87 static long long Ttick = 0; |
|
88 #define mod %% |
|
89 /* |
|
90 * Call this on each external sync, |
|
91 **/ |
|
92 void align_tick(int calibrate) |
|
93 { |
|
94 /* |
|
95 printf("align_tick(%%d)\n", calibrate); |
|
96 */ |
|
97 if(calibrate){ |
|
98 if(calibration_count == CALIBRATED) |
|
99 /* Re-calibration*/ |
|
100 calibration_count = NOT_CALIBRATED; |
|
101 if(calibration_count == NOT_CALIBRATED) |
|
102 /* Calibration start, get time*/ |
|
103 PLC_GetTime(&cal_begin); |
|
104 calibration_count++; |
|
105 }else{ |
|
106 if(calibration_count >= 0){ |
|
107 /* End of calibration */ |
|
108 /* Get final time */ |
|
109 IEC_TIME cal_end; |
|
110 PLC_GetTime(&cal_end); |
|
111 /*adjust calibration_count*/ |
|
112 calibration_count++; |
|
113 /* compute mean of Tsync, over calibration period */ |
|
114 Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 + |
|
115 (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count; |
|
116 if( (Nticks = (Tsync / Ttick)) > 0){ |
|
117 FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */ |
|
118 }else{ |
|
119 FreqCorr = Tsync - (Ttick mod Tsync); |
|
120 } |
|
121 /* |
|
122 printf("Tsync = %%ld\n", Tsync); |
|
123 printf("calibration_count = %%d\n", calibration_count); |
|
124 printf("Nticks = %%d\n", Nticks); |
|
125 */ |
|
126 calibration_count = CALIBRATED; |
|
127 } |
|
128 if(calibration_count == CALIBRATED){ |
|
129 /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */ |
|
130 IEC_TIME now; |
|
131 long long elapsed; |
|
132 long long Tcorr; |
|
133 long long PhaseCorr; |
|
134 long long PeriodicTcorr; |
|
135 PLC_GetTime(&now); |
|
136 elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; |
|
137 if(Nticks > 0){ |
|
138 PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*%(sync_align_ratio)d/100; /* to be divided by Nticks */ |
|
139 Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; |
|
140 if(Nticks < 2){ |
|
141 /* When Sync source period is near Tick time */ |
|
142 /* PhaseCorr may not be applied to Periodic time given to timer */ |
|
143 PeriodicTcorr = Ttick + FreqCorr / Nticks; |
|
144 }else{ |
|
145 PeriodicTcorr = Tcorr; |
|
146 } |
|
147 }else if(tick > last_tick){ |
|
148 last_tick = tick; |
|
149 PhaseCorr = elapsed - (Tsync*%(sync_align_ratio)d/100); |
|
150 PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; |
|
151 }else{ |
|
152 /*PLC did not run meanwhile. Nothing to do*/ |
|
153 return; |
|
154 } |
|
155 /* DO ALIGNEMENT */ |
|
156 PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); |
|
157 } |
|
158 } |
|
159 } |