1 /** |
|
2 * Code common to all C targets |
|
3 **/ |
|
4 |
|
5 #include "iec_types.h" |
|
6 /* |
|
7 * Prototypes of functions provided by generated C softPLC |
|
8 **/ |
|
9 void config_run__(unsigned long tick); |
|
10 void config_init__(void); |
|
11 |
|
12 /* |
|
13 * Prototypes of functions provided by generated target C code |
|
14 * */ |
|
15 void __init_debug(void); |
|
16 void __cleanup_debug(void); |
|
17 /*void __retrieve_debug(void);*/ |
|
18 void __publish_debug(void); |
|
19 |
|
20 /* |
|
21 * Variables used by generated C softPLC and plugins |
|
22 **/ |
|
23 IEC_TIME __CURRENT_TIME; |
|
24 IEC_BOOL __DEBUG = 0; |
|
25 unsigned long __tick = 0; |
|
26 |
|
27 /* |
|
28 * Variable generated by C softPLC and plugins |
|
29 **/ |
|
30 extern unsigned long greatest_tick_count__; |
|
31 |
|
32 /* Help to quit cleanly when init fail at a certain level */ |
|
33 static int init_level = 0; |
|
34 |
|
35 /* |
|
36 * Prototypes of functions exported by plugins |
|
37 **/ |
|
38 %(calls_prototypes)s |
|
39 |
|
40 /* |
|
41 * Retrieve input variables, run PLC and publish output variables |
|
42 **/ |
|
43 void __run(void) |
|
44 { |
|
45 __tick++; |
|
46 if (greatest_tick_count__) |
|
47 __tick %%= greatest_tick_count__; |
|
48 |
|
49 %(retrieve_calls)s |
|
50 |
|
51 /*__retrieve_debug();*/ |
|
52 |
|
53 config_run__(__tick); |
|
54 |
|
55 __publish_debug(); |
|
56 |
|
57 %(publish_calls)s |
|
58 |
|
59 } |
|
60 |
|
61 /* |
|
62 * Initialize variables according to PLC's default values, |
|
63 * and then init plugins with that values |
|
64 **/ |
|
65 int __init(int argc,char **argv) |
|
66 { |
|
67 int res = 0; |
|
68 init_level = 0; |
|
69 config_init__(); |
|
70 __init_debug(); |
|
71 %(init_calls)s |
|
72 return res; |
|
73 } |
|
74 /* |
|
75 * Calls plugin cleanup proc. |
|
76 **/ |
|
77 void __cleanup(void) |
|
78 { |
|
79 %(cleanup_calls)s |
|
80 __cleanup_debug(); |
|
81 } |
|
82 |
|
83 |
|
84 void PLC_GetTime(IEC_TIME *CURRENT_TIME); |
|
85 void PLC_SetTimer(unsigned long long next, unsigned long long period); |
|
86 |
|
87 #define CALIBRATED -2 |
|
88 #define NOT_CALIBRATED -1 |
|
89 static int calibration_count = NOT_CALIBRATED; |
|
90 static IEC_TIME cal_begin; |
|
91 static long long Tsync = 0; |
|
92 static long long FreqCorr = 0; |
|
93 static int Nticks = 0; |
|
94 static unsigned long last_tick = 0; |
|
95 static long long Ttick = 0; |
|
96 #define mod %% |
|
97 /* |
|
98 * Call this on each external sync, |
|
99 * @param sync_align_ratio 0->100 : align ratio, < 0 : no align, calibrate period |
|
100 **/ |
|
101 void align_tick(int sync_align_ratio) |
|
102 { |
|
103 /* |
|
104 printf("align_tick(%%d)\n", calibrate); |
|
105 */ |
|
106 if(sync_align_ratio < 0){ /* Calibration */ |
|
107 if(calibration_count == CALIBRATED) |
|
108 /* Re-calibration*/ |
|
109 calibration_count = NOT_CALIBRATED; |
|
110 if(calibration_count == NOT_CALIBRATED) |
|
111 /* Calibration start, get time*/ |
|
112 PLC_GetTime(&cal_begin); |
|
113 calibration_count++; |
|
114 }else{ /* do alignment (if possible) */ |
|
115 if(calibration_count >= 0){ |
|
116 /* End of calibration */ |
|
117 /* Get final time */ |
|
118 IEC_TIME cal_end; |
|
119 PLC_GetTime(&cal_end); |
|
120 /*adjust calibration_count*/ |
|
121 calibration_count++; |
|
122 /* compute mean of Tsync, over calibration period */ |
|
123 Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 + |
|
124 (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count; |
|
125 if( (Nticks = (Tsync / Ttick)) > 0){ |
|
126 FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */ |
|
127 }else{ |
|
128 FreqCorr = Tsync - (Ttick mod Tsync); |
|
129 } |
|
130 /* |
|
131 printf("Tsync = %%ld\n", Tsync); |
|
132 printf("calibration_count = %%d\n", calibration_count); |
|
133 printf("Nticks = %%d\n", Nticks); |
|
134 */ |
|
135 calibration_count = CALIBRATED; |
|
136 } |
|
137 if(calibration_count == CALIBRATED){ |
|
138 /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */ |
|
139 IEC_TIME now; |
|
140 long long elapsed; |
|
141 long long Tcorr; |
|
142 long long PhaseCorr; |
|
143 long long PeriodicTcorr; |
|
144 PLC_GetTime(&now); |
|
145 elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; |
|
146 if(Nticks > 0){ |
|
147 PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ |
|
148 Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; |
|
149 if(Nticks < 2){ |
|
150 /* When Sync source period is near Tick time */ |
|
151 /* PhaseCorr may not be applied to Periodic time given to timer */ |
|
152 PeriodicTcorr = Ttick + FreqCorr / Nticks; |
|
153 }else{ |
|
154 PeriodicTcorr = Tcorr; |
|
155 } |
|
156 }else if(__tick > last_tick){ |
|
157 last_tick = __tick; |
|
158 PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); |
|
159 PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; |
|
160 }else{ |
|
161 /*PLC did not run meanwhile. Nothing to do*/ |
|
162 return; |
|
163 } |
|
164 /* DO ALIGNEMENT */ |
|
165 PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); |
|
166 } |
|
167 } |
|
168 } |
|
169 |
|
170 /** |
|
171 * Prototypes for function provided by arch-specific code (main) |
|
172 * is concatained hereafter |
|
173 **/ |
|