equal
deleted
inserted
replaced
42 static long long period_ns = 0; |
42 static long long period_ns = 0; |
43 struct timespec next_abs_time; |
43 struct timespec next_abs_time; |
44 |
44 |
45 static void inc_timespec(struct timespec *ts, unsigned long long value_ns) |
45 static void inc_timespec(struct timespec *ts, unsigned long long value_ns) |
46 { |
46 { |
|
47 long long next_ns = ((long long) ts->tv_sec * 1000000000) + ts->tv_nsec + value_ns; |
47 #ifdef __lldiv_t_defined |
48 #ifdef __lldiv_t_defined |
48 lldiv_t next_div = lldiv(ts->tv_sec * 1000000000 + ts->tv_nsec + value_ns, 1000000000); |
49 lldiv_t next_div = lldiv(next_ns, 1000000000); |
49 ts->tv_sec = next_div.quot; |
50 ts->tv_sec = next_div.quot; |
50 ts->tv_nsec = next_div.rem; |
51 ts->tv_nsec = next_div.rem; |
51 #else |
52 #else |
52 long long next_ns = ts->tv_sec * 1000000000 + ts->tv_nsec + value_ns; |
|
53 ts->tv_sec = next_ns / 1000000000; |
53 ts->tv_sec = next_ns / 1000000000; |
54 ts->tv_nsec = next_ns % 1000000000; |
54 ts->tv_nsec = next_ns % 1000000000; |
55 #endif |
55 #endif |
56 } |
56 } |
57 |
57 |
64 clock_gettime(CLOCK_MONOTONIC, &next_abs_time); |
64 clock_gettime(CLOCK_MONOTONIC, &next_abs_time); |
65 inc_timespec(&next_abs_time, next); |
65 inc_timespec(&next_abs_time, next); |
66 // interrupt clock_nanpsleep |
66 // interrupt clock_nanpsleep |
67 pthread_kill(PLC_thread, SIGUSR1); |
67 pthread_kill(PLC_thread, SIGUSR1); |
68 } |
68 } |
69 // |
69 |
70 void catch_signal(int sig) |
70 void catch_signal(int sig) |
71 { |
71 { |
72 // signal(SIGTERM, catch_signal); |
72 // signal(SIGTERM, catch_signal); |
73 signal(SIGINT, catch_signal); |
73 signal(SIGINT, catch_signal); |
74 printf("Got Signal %d\n",sig); |
74 printf("Got Signal %d\n",sig); |
89 { |
89 { |
90 while (!PLC_shutdown) { |
90 while (!PLC_shutdown) { |
91 // Sleep until next PLC run |
91 // Sleep until next PLC run |
92 // TODO check result of clock_nanosleep and wait again or exit eventually |
92 // TODO check result of clock_nanosleep and wait again or exit eventually |
93 int res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_abs_time, NULL); |
93 int res = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_abs_time, NULL); |
94 if(res==EINTR) continue; |
94 if(res==EINTR){ |
95 if(res!=0) return; |
95 continue; |
|
96 } |
|
97 if(res!=0){ |
|
98 printf("PLC thread died with error %d \n", res); |
|
99 return; |
|
100 } |
96 PLC_GetTime(&__CURRENT_TIME); |
101 PLC_GetTime(&__CURRENT_TIME); |
97 __run(); |
102 __run(); |
98 inc_timespec(&next_abs_time, period_ns); |
103 inc_timespec(&next_abs_time, period_ns); |
99 } |
104 } |
100 pthread_exit(0); |
105 pthread_exit(0); |
104 int startPLC(int argc,char **argv) |
109 int startPLC(int argc,char **argv) |
105 { |
110 { |
106 setlocale(LC_NUMERIC, "C"); |
111 setlocale(LC_NUMERIC, "C"); |
107 |
112 |
108 PLC_shutdown = 0; |
113 PLC_shutdown = 0; |
109 |
|
110 pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL); |
|
111 |
114 |
112 pthread_mutex_init(&debug_wait_mutex, NULL); |
115 pthread_mutex_init(&debug_wait_mutex, NULL); |
113 pthread_mutex_init(&debug_mutex, NULL); |
116 pthread_mutex_init(&debug_mutex, NULL); |
114 pthread_mutex_init(&python_wait_mutex, NULL); |
117 pthread_mutex_init(&python_wait_mutex, NULL); |
115 pthread_mutex_init(&python_mutex, NULL); |
118 pthread_mutex_init(&python_mutex, NULL); |
124 /* Signal to end PLC thread */ |
127 /* Signal to end PLC thread */ |
125 signal(SIGUSR2, PLCThreadSignalHandler); |
128 signal(SIGUSR2, PLCThreadSignalHandler); |
126 /* install signal handler for manual break */ |
129 /* install signal handler for manual break */ |
127 signal(SIGINT, catch_signal); |
130 signal(SIGINT, catch_signal); |
128 |
131 |
129 PLC_SetTimer(common_ticktime__,common_ticktime__); |
132 /* initialize next occurence and period */ |
|
133 period_ns = common_ticktime__; |
|
134 clock_gettime(CLOCK_MONOTONIC, &next_abs_time); |
|
135 |
|
136 pthread_create(&PLC_thread, NULL, (void*) &PLC_thread_proc, NULL); |
130 }else{ |
137 }else{ |
131 return 1; |
138 return 1; |
132 } |
139 } |
133 return 0; |
140 return 0; |
134 } |
141 } |