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