15 void run(long int tv_sec, long int tv_nsec); |
15 void run(long int tv_sec, long int tv_nsec); |
16 |
16 |
17 #define maxval(a,b) ((a>b)?a:b) |
17 #define maxval(a,b) ((a>b)?a:b) |
18 |
18 |
19 #include "iec_types.h" |
19 #include "iec_types.h" |
|
20 /*#include "stdio.h" /* For debug */ |
20 |
21 |
21 /* |
22 /* |
22 * Functions and variables provied by generated C softPLC |
23 * Functions and variables provied by generated C softPLC |
23 **/ |
24 **/ |
24 void config_run__(int tick); |
25 void config_run__(int tick); |
25 void config_init__(void); |
26 void config_init__(void); |
26 |
27 |
27 /* |
28 /* |
28 * Functions and variables to export to generated C softPLC |
29 * Functions and variables to export to generated C softPLC and plugins |
29 **/ |
30 **/ |
30 |
31 |
31 IEC_TIME __CURRENT_TIME; |
32 IEC_TIME __CURRENT_TIME; |
|
33 int __tick = 0; |
32 |
34 |
33 static int tick = 0; |
35 static int init_level = 0; |
34 static int init_level=0; |
36 static int Debugging = 1; |
35 |
37 |
36 /* |
38 /* |
37 * Prototypes of functions exported by plugins |
39 * Prototypes of functions exported by plugins |
38 **/ |
40 **/ |
39 %(calls_prototypes)s |
41 %(calls_prototypes)s |
42 * Retrieve input variables, run PLC and publish output variables |
44 * Retrieve input variables, run PLC and publish output variables |
43 **/ |
45 **/ |
44 void __run() |
46 void __run() |
45 { |
47 { |
46 %(retrieve_calls)s |
48 %(retrieve_calls)s |
|
49 |
|
50 if(Debugging) __retrieve_debug(); |
47 |
51 |
48 /* |
52 config_run__(__tick); |
49 printf("run tick = %%d\n", tick + 1); |
53 |
50 */ |
54 if(Debugging) __publish_debug(); |
51 config_run__(tick++); |
|
52 |
55 |
53 %(publish_calls)s |
56 %(publish_calls)s |
|
57 |
|
58 __tick++; |
54 } |
59 } |
55 |
60 |
56 /* |
61 /* |
57 * Initialize variables according to PLC's defalut values, |
62 * Initialize variables according to PLC's defalut values, |
58 * and then init plugins with that values |
63 * and then init plugins with that values |
86 static int last_tick = 0; |
91 static int last_tick = 0; |
87 static long long Ttick = 0; |
92 static long long Ttick = 0; |
88 #define mod %% |
93 #define mod %% |
89 /* |
94 /* |
90 * Call this on each external sync, |
95 * Call this on each external sync, |
|
96 * @param sync_align_ratio 0->100 : align ratio, < 0 : no align, calibrate period |
91 **/ |
97 **/ |
92 void align_tick(int calibrate) |
98 void align_tick(int sync_align_ratio) |
93 { |
99 { |
94 /* |
100 /* |
95 printf("align_tick(%%d)\n", calibrate); |
101 printf("align_tick(%%d)\n", calibrate); |
96 */ |
102 */ |
97 if(calibrate){ |
103 if(sync_align_ratio < 0){ /* Calibration */ |
98 if(calibration_count == CALIBRATED) |
104 if(calibration_count == CALIBRATED) |
99 /* Re-calibration*/ |
105 /* Re-calibration*/ |
100 calibration_count = NOT_CALIBRATED; |
106 calibration_count = NOT_CALIBRATED; |
101 if(calibration_count == NOT_CALIBRATED) |
107 if(calibration_count == NOT_CALIBRATED) |
102 /* Calibration start, get time*/ |
108 /* Calibration start, get time*/ |
103 PLC_GetTime(&cal_begin); |
109 PLC_GetTime(&cal_begin); |
104 calibration_count++; |
110 calibration_count++; |
105 }else{ |
111 }else{ /* do alignment (if possible) */ |
106 if(calibration_count >= 0){ |
112 if(calibration_count >= 0){ |
107 /* End of calibration */ |
113 /* End of calibration */ |
108 /* Get final time */ |
114 /* Get final time */ |
109 IEC_TIME cal_end; |
115 IEC_TIME cal_end; |
110 PLC_GetTime(&cal_end); |
116 PLC_GetTime(&cal_end); |
133 long long PhaseCorr; |
139 long long PhaseCorr; |
134 long long PeriodicTcorr; |
140 long long PeriodicTcorr; |
135 PLC_GetTime(&now); |
141 PLC_GetTime(&now); |
136 elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; |
142 elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; |
137 if(Nticks > 0){ |
143 if(Nticks > 0){ |
138 PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*%(sync_align_ratio)d/100; /* to be divided by Nticks */ |
144 PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ |
139 Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; |
145 Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; |
140 if(Nticks < 2){ |
146 if(Nticks < 2){ |
141 /* When Sync source period is near Tick time */ |
147 /* When Sync source period is near Tick time */ |
142 /* PhaseCorr may not be applied to Periodic time given to timer */ |
148 /* PhaseCorr may not be applied to Periodic time given to timer */ |
143 PeriodicTcorr = Ttick + FreqCorr / Nticks; |
149 PeriodicTcorr = Ttick + FreqCorr / Nticks; |
144 }else{ |
150 }else{ |
145 PeriodicTcorr = Tcorr; |
151 PeriodicTcorr = Tcorr; |
146 } |
152 } |
147 }else if(tick > last_tick){ |
153 }else if(__tick > last_tick){ |
148 last_tick = tick; |
154 last_tick = __tick; |
149 PhaseCorr = elapsed - (Tsync*%(sync_align_ratio)d/100); |
155 PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); |
150 PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; |
156 PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; |
151 }else{ |
157 }else{ |
152 /*PLC did not run meanwhile. Nothing to do*/ |
158 /*PLC did not run meanwhile. Nothing to do*/ |
153 return; |
159 return; |
154 } |
160 } |
155 /* DO ALIGNEMENT */ |
161 /* DO ALIGNEMENT */ |
156 PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); |
162 PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); |
157 } |
163 } |
158 } |
164 } |
159 } |
165 } |
|
166 |
|
167 int suspendDebug() |
|
168 { |
|
169 /* Prevent PLC to enter debug code */ |
|
170 Debugging = 0; |
|
171 } |
|
172 |
|
173 int resumeDebug() |
|
174 { |
|
175 /* Let PLC enter debug code */ |
|
176 Debugging = 1; |
|
177 } |