13 #include <semaphore.h> |
13 #include <semaphore.h> |
14 #ifdef REALTIME_LINUX |
14 #ifdef REALTIME_LINUX |
15 #include <sys/mman.h> |
15 #include <sys/mman.h> |
16 #endif |
16 #endif |
17 |
17 |
|
18 #define _Log(level,text,...) \ |
|
19 {\ |
|
20 char mstr[256];\ |
|
21 snprintf(mstr, 255, text, ##__VA_ARGS__);\ |
|
22 LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\ |
|
23 } |
|
24 |
|
25 #define _LogError(text,...) _Log(LOG_CRITICAL, text, ##__VA_ARGS__) |
|
26 #define _LogWarning(text,...) _Log(LOG_WARNING, text, ##__VA_ARGS__) |
|
27 |
18 static unsigned long __debug_tick; |
28 static unsigned long __debug_tick; |
19 |
29 |
20 static pthread_t PLC_thread; |
30 static pthread_t PLC_thread; |
21 static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER; |
31 static pthread_mutex_t python_wait_mutex = PTHREAD_MUTEX_INITIALIZER; |
22 static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER; |
32 static pthread_mutex_t python_mutex = PTHREAD_MUTEX_INITIALIZER; |
41 CURRENT_TIME->tv_sec = tmp.tv_sec; |
51 CURRENT_TIME->tv_sec = tmp.tv_sec; |
42 CURRENT_TIME->tv_nsec = tmp.tv_nsec; |
52 CURRENT_TIME->tv_nsec = tmp.tv_nsec; |
43 } |
53 } |
44 |
54 |
45 static long long period_ns = 0; |
55 static long long period_ns = 0; |
46 struct timespec next_abs_time; |
56 struct timespec next_cycle_time; |
47 |
57 |
48 static void inc_timespec(struct timespec *ts, unsigned long long value_ns) |
58 static void inc_timespec(struct timespec *ts, unsigned long long value_ns) |
49 { |
59 { |
50 long long next_ns = ((long long) ts->tv_sec * 1000000000) + ts->tv_nsec + value_ns; |
60 long long next_ns = ((long long) ts->tv_sec * 1000000000) + ts->tv_nsec + value_ns; |
51 #ifdef __lldiv_t_defined |
61 #ifdef __lldiv_t_defined |
62 { |
72 { |
63 /* |
73 /* |
64 printf("SetTimer(%lld,%lld)\n",next, period); |
74 printf("SetTimer(%lld,%lld)\n",next, period); |
65 */ |
75 */ |
66 period_ns = period; |
76 period_ns = period; |
67 clock_gettime(CLOCK_MONOTONIC, &next_abs_time); |
77 clock_gettime(CLOCK_MONOTONIC, &next_cycle_time); |
68 inc_timespec(&next_abs_time, next); |
78 inc_timespec(&next_cycle_time, next); |
69 // interrupt clock_nanpsleep |
79 // interrupt clock_nanpsleep |
70 pthread_kill(PLC_thread, SIGUSR1); |
80 pthread_kill(PLC_thread, SIGUSR1); |
71 } |
81 } |
72 |
82 |
73 void catch_signal(int sig) |
83 void catch_signal(int sig) |
86 |
96 |
87 int ForceSaveRetainReq(void) { |
97 int ForceSaveRetainReq(void) { |
88 return PLC_shutdown; |
98 return PLC_shutdown; |
89 } |
99 } |
90 |
100 |
|
101 #define MAX_JITTER period_ns/10 |
|
102 #define MIN_IDLE_TIME_NS 1000000 /* 1ms */ |
|
103 /* Macro to compare timespec, evaluate to True if a is past b */ |
|
104 #define timespec_gt(a,b) (a.tv_sec > b.tv_sec || (a.tv_sec == b.tv_sec && a.tv_nsec > b.tv_nsec)) |
91 void PLC_thread_proc(void *arg) |
105 void PLC_thread_proc(void *arg) |
92 { |
106 { |
|
107 /* initialize next occurence and period */ |
|
108 period_ns = common_ticktime__; |
|
109 clock_gettime(CLOCK_MONOTONIC, &next_cycle_time); |
|
110 |
93 while (!PLC_shutdown) { |
111 while (!PLC_shutdown) { |
|
112 int res; |
|
113 struct timespec plc_end_time; |
|
114 int periods = 0; |
|
115 #ifdef REALTIME_LINUX |
|
116 struct timespec deadline_time; |
|
117 struct timespec plc_start_time; |
|
118 #endif |
|
119 |
94 // Sleep until next PLC run |
120 // Sleep until next PLC run |
95 // TODO check result of clock_nanosleep and wait again or exit eventually |
121 res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_cycle_time, NULL); |
96 int res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_abs_time, NULL); |
|
97 if(res==EINTR){ |
122 if(res==EINTR){ |
98 continue; |
123 continue; |
99 } |
124 } |
100 if(res!=0){ |
125 if(res!=0){ |
101 printf("PLC thread died with error %d \n", res); |
126 _LogError("PLC thread timer returned error %d \n", res); |
102 return; |
127 return; |
103 } |
128 } |
|
129 |
|
130 #ifdef REALTIME_LINUX |
|
131 // timer overrun detection |
|
132 clock_gettime(CLOCK_MONOTONIC, &plc_start_time); |
|
133 deadline_time=next_cycle_time; |
|
134 inc_timespec(&deadline_time, MAX_JITTER); |
|
135 if(timespec_gt(plc_start_time, deadline_time)){ |
|
136 _LogWarning("PLC thread woken up too late. PLC cyclic task interval is too small.\n"); |
|
137 } |
|
138 #endif |
|
139 |
104 PLC_GetTime(&__CURRENT_TIME); |
140 PLC_GetTime(&__CURRENT_TIME); |
105 __run(); |
141 __run(); |
106 inc_timespec(&next_abs_time, period_ns); |
142 |
107 } |
143 // ensure next PLC cycle occurence is in the future |
|
144 clock_gettime(CLOCK_MONOTONIC, &plc_end_time); |
|
145 while(timespec_gt(plc_end_time, next_cycle_time)){ |
|
146 periods += 1; |
|
147 inc_timespec(&next_cycle_time, period_ns); |
|
148 } |
|
149 |
|
150 // plc execution time overrun detection |
|
151 if(periods > 1) { |
|
152 // Mitigate CPU hogging, in case of too small cyclic task interval: |
|
153 // - since cycle deadline already missed, better keep system responsive |
|
154 // - test if next cycle occurs after minimal idle |
|
155 // - enforce minimum idle time if not |
|
156 |
|
157 struct timespec earliest_possible_time = plc_end_time; |
|
158 inc_timespec(&earliest_possible_time, MIN_IDLE_TIME_NS); |
|
159 while(timespec_gt(earliest_possible_time, next_cycle_time)){ |
|
160 periods += 1; |
|
161 inc_timespec(&next_cycle_time, period_ns); |
|
162 } |
|
163 |
|
164 // increment tick count anyhow, so that task scheduling keeps consistent |
|
165 __tick+=periods-1; |
|
166 |
|
167 _LogWarning("PLC execution time is longer than requested PLC cyclic task interval. %d cycles skipped\n", periods); |
|
168 } |
|
169 } |
|
170 |
108 pthread_exit(0); |
171 pthread_exit(0); |
109 } |
172 } |
110 |
173 |
111 #define _LogError(text,...) \ |
|
112 {\ |
|
113 char mstr[256];\ |
|
114 snprintf(mstr, 255, text, ##__VA_ARGS__);\ |
|
115 LogMessage(LOG_CRITICAL, mstr, strlen(mstr));\ |
|
116 } |
|
117 #define maxval(a,b) ((a>b)?a:b) |
174 #define maxval(a,b) ((a>b)?a:b) |
118 int startPLC(int argc,char **argv) |
175 int startPLC(int argc,char **argv) |
119 { |
176 { |
120 |
177 |
121 int ret; |
178 int ret; |
179 /* Signal to end PLC thread */ |
236 /* Signal to end PLC thread */ |
180 signal(SIGUSR2, PLCThreadSignalHandler); |
237 signal(SIGUSR2, PLCThreadSignalHandler); |
181 /* install signal handler for manual break */ |
238 /* install signal handler for manual break */ |
182 signal(SIGINT, catch_signal); |
239 signal(SIGINT, catch_signal); |
183 |
240 |
184 /* initialize next occurence and period */ |
|
185 period_ns = common_ticktime__; |
|
186 clock_gettime(CLOCK_MONOTONIC, &next_abs_time); |
|
187 |
|
188 ret = pthread_create(&PLC_thread, pattr, (void*) &PLC_thread_proc, NULL); |
241 ret = pthread_create(&PLC_thread, pattr, (void*) &PLC_thread_proc, NULL); |
189 if (ret) { |
242 if (ret) { |
190 _LogError("create pthread failed\n"); |
243 _LogError("create pthread failed\n"); |
191 return ret; |
244 return ret; |
192 } |
245 } |