GCC Code Coverage Report

Directory: ./
File: pdserv-1.1.0/example/example-st.c
Date: 2024-11-17 04:08:36
Exec Total Coverage
Lines: 0 165 0.0%
Branches: 0 98 0.0%
Line Branch Exec Source
1 /*****************************************************************************
2 *
3 * $Id$
4 *
5 * vim: tw=78
6 *
7 * This is a very simple single tasking example.
8 *
9 * Copyright (C) 2017,2018 Richard Hacker <lerichi at gmx dot net>
10 * License: LGPLv3
11 *
12 * It demonstrates:
13 * - signals
14 * - sub-rated signals (signal that is calculated every N-th cycle)
15 * - parameters
16 * - events
17 *
18 * In the cyclic calculation, an oscillator producing a sine and cosine
19 * signal with variable amplitude and frequency is calculated.
20 *
21 * With a decimation of 10 (subrating), a saw tooth signal is also produced
22 *
23 ****************************************************************************/
24
25 #include <pdserv.h> // obviously!
26
27 #include <stdio.h>
28 #include <stdint.h>
29 #include <errno.h>
30 #include <getopt.h> // getopt()
31 #include <unistd.h> // getopt()
32 #include <string.h> // memset(), strcmp(), strerror()
33 #include <time.h> // clock_gettime(), clock_nanosleep()
34 #include <sys/mman.h> // mlockall()
35 #include <sched.h> // sched_setscheduler()
36 #include <stdlib.h> // strtoul(), exit()
37 #include <pthread.h> // pthread_mutex_lock(), pthread_mutex_unlock()
38
39 /****************************************************************************/
40
41 #define MAX_SAFE_STACK (8 * 1024) /** The maximum stack size which is
42 guranteed safe to access without faulting.
43 */
44
45 #define NSEC_PER_SEC (1000000000)
46 #define DIFF_NS(A, B) (((long long) (B).tv_sec - (A).tv_sec) * NSEC_PER_SEC \
47 + (B).tv_nsec - (A).tv_nsec)
48
49 /****************************************************************************/
50
51 /* Command-line option variables. */
52
53 int priority = -1; /**< Task priority, -1 means RT (maximum). */
54 int daemonize = 0; /**< Become a daemon. */
55 unsigned int duration_ns = 0;
56 const char *config = 0;
57
58 /***************************************************************************
59 * Support functions
60 ***************************************************************************/
61
62 /** Increment the time.
63 * Arguments:
64 * - t: timespec pointer
65 * - dt_ns: increment in nanoseconds
66 */
67 ✗ struct timespec* timer_add(struct timespec *t, unsigned int dt_ns)
68 {
69 ✗ t->tv_nsec += dt_ns;
70 ✗ while (t->tv_nsec >= NSEC_PER_SEC) {
71 ✗ t->tv_nsec -= NSEC_PER_SEC;
72 ✗ t->tv_sec++;
73 }
74 ✗ return t;
75 }
76
77 /** Return the current system time.
78 *
79 * This is a callback needed by pdserv.
80 */
81 ✗ int gettime(struct timespec *time)
82 {
83 ✗ return clock_gettime(CLOCK_REALTIME, time);
84 }
85
86 /** Cause a stack fault before entering cyclic operation.
87 */
88 ✗ void stack_prefault(void)
89 {
90 ✗ unsigned char dummy[MAX_SAFE_STACK];
91
92 ✗ memset(dummy, 0, MAX_SAFE_STACK);
93 }
94
95 /** Output the usage.
96 */
97 ✗ void usage(FILE *f, const char *base_name)
98 {
99 ✗ fprintf(f,
100 "Usage: %s [OPTIONS]\n"
101 "Options:\n"
102 " --duration -d secs Set duration <float>\n"
103 " --config -c conffile Set configuration file\n"
104 " --priority -p <PRIO> Set task priority. Default: RT.\n"
105 " --help -h Show this help.\n",
106 base_name);
107 }
108
109 /** Get the command-line options.
110 */
111 ✗ void get_options(int argc, char **argv)
112 {
113 int c, arg_count;
114
115 static struct option longOptions[] = {
116 //name, has_arg, flag, val
117 {"duration", required_argument, NULL, 'd'},
118 {"config", required_argument, NULL, 'c'},
119 {"priority", required_argument, NULL, 'p'},
120 {"help", no_argument, NULL, 'h'},
121 {NULL, no_argument, NULL, 0}
122 };
123
124 do {
125 ✗ c = getopt_long(argc, argv, "d:c:p:h", longOptions, NULL);
126
127 ✗ switch (c) {
128 ✗ case 'p':
129 ✗ if (!strcmp(optarg, "RT")) {
130 ✗ priority = -1;
131 ✗ } else {
132 ✗ char *end;
133 ✗ priority = strtoul(optarg, &end, 10);
134 ✗ if (!*optarg || *end) {
135 ✗ fprintf(stderr, "Invalid priority: %s\n", optarg);
136 ✗ exit(1);
137 }
138 }
139 ✗ break;
140
141 ✗ case 'd':
142 ✗ duration_ns = atof(optarg) * NSEC_PER_SEC;
143 ✗ break;
144
145 ✗ case 'c':
146 ✗ config = optarg;
147 ✗ break;
148
149 ✗ case 'h':
150 ✗ usage(stdout, argv[0]);
151 ✗ exit(0);
152
153 ✗ case '?':
154 ✗ usage(stderr, argv[0]);
155 ✗ exit(1);
156
157 ✗ default:
158 ✗ break;
159 }
160 }
161 ✗ while (c != -1);
162
163 ✗ arg_count = argc - optind;
164
165 ✗ if (arg_count) {
166 ✗ fprintf(stderr, "%s takes no arguments!\n", argv[0]);
167 ✗ usage(stderr, argv[0]);
168 ✗ exit(1);
169 }
170 }
171
172 /* Callback to test the limit of a parameter to be set */
173 ✗ int limit_test(const struct pdvariable* param,
174 void *dst, const void* src, size_t len,
175 struct timespec *time, void* priv_data)
176 {
177 ✗ double value = *(double*)src;
178 ✗ double limit = *(double*)priv_data; /* pointer to limit of double */
179 (void)time;
180 (void)param;
181
182 ✗ if (value > limit || value < -limit)
183 ✗ return -EINVAL;
184
185 ✗ memcpy(dst, src, len);
186 ✗ clock_gettime(CLOCK_REALTIME, time);
187
188 ✗ return 0;
189 }
190
191 /* Callback used to protect signals and parameters */
192 ✗ void lock_fn(int lock, void* priv_data)
193 {
194 ✗ if (lock)
195 ✗ pthread_mutex_lock(priv_data);
196 else
197 ✗ pthread_mutex_unlock(priv_data);
198 }
199
200 enum {
201 NUM_EVENTS = 5,
202 };
203
204 /****************************************************************************
205 * Main function
206 ****************************************************************************/
207 ✗ int main(int argc, char **argv)
208 {
209 struct pdserv* pdserv;
210 struct pdtask* pdtask;
211 const struct pdevent* event;
212 struct pdvariable* var;
213 ✗ unsigned int tsample_ns = (uint64_t)(0.01e9); // 10ms
214 ✗ const char* err = NULL;
215 ✗ int running = 1;
216 ✗ pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
217 double exec_time, cycle_time;
218 ✗ unsigned int overruns = 0;
219 ✗ struct timespec monotonic_time, world_time;
220 ✗ struct timespec start_time, stop_time = {0,0}, end_time, last_start_time;
221
222 // Variables for real time task: sin/cos oscillator and saw tooth generator
223 // Parameters:
224 ✗ double omega = 1.2;
225 ✗ char enable = 1;
226 ✗ char reset = 0;
227 ✗ double omega_limit = 5.0;
228 ✗ double amplitude_set = 10.0;
229 ✗ double ampl_limit = 20.0;
230 ✗ uint32_t event_state[NUM_EVENTS] = {0,0,0,0,0};
231 // Signals:
232 ✗ double sin = 0.0, cos = amplitude_set;
233 ✗ double amplitude;
234 ✗ double ampl_modulation;
235 ✗ double derivative[2];
236 ✗ uint8_t counter;
237 ✗ int decimation_counter = 1;
238
239 ✗ get_options(argc, argv);
240
241 /////////////////////// setup pdserv //////////////////////////////
242
243 /* Create a pdserv instance */
244 ✗ if (!(pdserv = pdserv_create("PdServ Test", "1.234", gettime))) {
245 ✗ err = "Failed to init pdserv.";
246 ✗ goto out;
247 }
248
249 ✗ if (config)
250 ✗ pdserv_config_file(pdserv, config);
251
252 /* Create a task */
253 ✗ if (!(pdtask = pdserv_create_task(pdserv, 1.0e-9*tsample_ns, NULL))) {
254 ✗ err = "Failed to create task.";
255 ✗ goto out;
256 }
257
258 /* Register parameters */
259 ✗ pdserv_parameter(pdserv, "/osc/omega",
260 0666, pd_double_T, &omega, 1, 0, 0, 0);
261 ✗ pdserv_parameter(pdserv, "/osc/amplitude/Setpoint",
262 0666, pd_double_T, &amplitude_set, 1, 0, 0, 0);
263 ✗ pdserv_parameter(pdserv, "/osc/enable",
264 0666, pd_sint8_T, &enable, 1, 0, 0, 0);
265 ✗ pdserv_parameter(pdserv, "/osc/reset",
266 0666, pd_sint8_T, &reset, 1, 0, 0, 0);
267 ✗ pdserv_parameter(pdserv, "/osc/amplitude/Limit",
268 0666, pd_double_T, &ampl_limit, 1, 0, 0, 0);
269 ✗ pdserv_parameter(pdserv, "/Event/State",
270 0666, pd_uint32_T, &event_state, 5, 0, 0, 0);
271
272 /* Register signals */
273 ✗ pdserv_signal(pdtask, 1, "/osc/cos",
274 pd_double_T, &cos, 1, 0);
275 ✗ pdserv_signal(pdtask, 1, "/osc/sin",
276 pd_double_T, &sin, 1, 0);
277 ✗ pdserv_signal(pdtask, 1, "/osc/amplitude",
278 pd_double_T, &amplitude, 1, 0);
279 ✗ pdserv_signal(pdtask, 1, "/osc/amplitude/Modulation",
280 pd_double_T, &ampl_modulation, 1, 0);
281 ✗ var = pdserv_signal(pdtask, 1, "/osc/derivative",
282 pd_double_T, derivative, 2, 0);
283 ✗ pdserv_set_comment(var,
284 "Derivative of [cos,sin]");
285
286 /* This signal is updated every 10th calculation cycle. This has no
287 * consequence to pdserv, it is only an indication to the clients
288 * that the signal is decimated */
289 ✗ pdserv_signal(pdtask, 10, "/SawTooth",
290 pd_uint8_T, &counter, 1, 0);
291
292 static const char *text[] = {
293 "Event message 1",
294 "Event message 2",
295 "Event message 3",
296 "Event message 4",
297 "Event message 5",
298 };
299 /* Register a vector event */
300 ✗ event = pdserv_event(pdserv, "/Limit", WARN_EVENT, NUM_EVENTS, text);
301
302 /* At this time, the setup for pdserv is finished. Up to now
303 * - pdserv instance and pdserv tasks were created
304 * - signals, parameters and events were registered.
305 *
306 * Now tell pdserv to prepare itself. Here it prepares the non-real time
307 * interface (network sockets), support threads, restore persistent
308 * parameters, etc
309 */
310 ✗ int ret = pdserv_prepare(pdserv);
311 ✗ if (ret) {
312 ✗ err = "Failed to prepare pdserv.";
313 ✗ goto out;
314 }
315
316 ///////////////////// setup real time task /////////////////////////
317
318 // Only _after_ pdserv_prepare() was called, the real time setup, like
319 // locking memory, prefaulting the stack, setting scheduler and priority,
320 // etc, is done. This is important, otherwise the real time setup will
321 // leak into non-real time tasks which is not a good idea.
322
323 /* Lock all memory forever - prevents it from being swapped out. */
324 ✗ if (mlockall(MCL_CURRENT | MCL_FUTURE))
325 ✗ fprintf(stderr, "mlockall() failed: %s\n", strerror(errno));
326
327 /* Provoke the first stack fault before cyclic operation. */
328 ✗ stack_prefault();
329
330 /* Set task priority and scheduler. */
331 {
332 ✗ struct sched_param param = {
333 ✗ .sched_priority = (priority == -1
334 ? sched_get_priority_max(SCHED_FIFO)
335 ✗ : priority),
336 };
337
338 ✗ if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
339 ✗ fprintf(stderr,
340 "Setting SCHED_FIFO with priority %i failed: %s\n",
341 ✗ param.sched_priority, strerror(errno));
342
343 /* Reset priority, so that sub-threads start */
344 ✗ priority = -1;
345 }
346 }
347
348 ///////////////////// cyclic task //////////////////////////////////
349
350 ✗ clock_gettime(CLOCK_MONOTONIC, &monotonic_time);
351 ✗ last_start_time = monotonic_time;
352 ✗ if (duration_ns) {
353 ✗ stop_time = last_start_time;
354 ✗ timer_add(&stop_time, duration_ns);
355 }
356 ✗ while (running && (!duration_ns || DIFF_NS(last_start_time, stop_time) > 0)) {
357 ✗ clock_gettime(CLOCK_MONOTONIC, &start_time);
358 ✗ clock_gettime(CLOCK_REALTIME, &world_time);
359
360 /* Get a read lock on parameters and a write lock on signals */
361 ✗ pthread_mutex_lock(&mutex);
362 ✗ pdserv_get_parameters(pdserv, pdtask, &world_time);
363
364 /* Calculation sin/cos oscillator at base rate */
365 ✗ if (reset) {
366 ✗ cos = amplitude_set;
367 ✗ sin = 0.0;
368 }
369 ✗ else if (enable) {
370 // Calculate amplitude
371 ✗ amplitude = cos*cos + sin*sin;
372
373 // Amplitude error, limiting upper value (for very small
374 // amplitudes)
375 ✗ ampl_modulation = 1.0/3.1415
376 ✗ * (amplitude/amplitude_set/amplitude_set - 1);
377 ✗ if (ampl_modulation > 1.0)
378 ✗ ampl_modulation = 1.0;
379
380 // Calculate derivatives
381 ✗ derivative[0] = -omega*sin - ampl_modulation*cos;
382 ✗ derivative[1] = omega*cos - ampl_modulation*sin;
383
384 // Integrate
385 ✗ cos += 1.0e-9*tsample_ns*derivative[0];
386 ✗ sin += 1.0e-9*tsample_ns*derivative[1];
387
388 // Check amplitude
389 ✗ if (cos > ampl_limit) cos = ampl_limit;
390 ✗ if (cos < -ampl_limit) cos = -ampl_limit;
391 ✗ if (sin > ampl_limit) sin = ampl_limit;
392 ✗ if (sin < -ampl_limit) sin = -ampl_limit;
393 }
394
395 /* Sub-rating task for saw tooth */
396 ✗ if (!--decimation_counter) {
397 ✗ decimation_counter = 10; // Reset decimation counter
398
399 ✗ ++counter; // Sawtooth with natural wrap
400 }
401
402 ✗ for (unsigned i = 0; i < NUM_EVENTS; ++i)
403 ✗ pdserv_event_set(event, i, event_state[i], &world_time);
404
405 /* Release locks */
406 ✗ pthread_mutex_unlock(&mutex);
407
408 /* Call at end of calculation task, so that pdserv updates itself */
409 ✗ pdserv_update(pdtask, &world_time);
410
411 /* Calculate timing statistics */
412 ✗ cycle_time = 1.0e-9 * DIFF_NS(last_start_time, start_time);
413 ✗ exec_time = 1.0e-9 * DIFF_NS(last_start_time, end_time);
414 ✗ last_start_time = start_time;
415 ✗ pdserv_update_statistics(pdtask, exec_time, cycle_time, overruns);
416
417 ✗ timer_add(&monotonic_time, tsample_ns); // Increment timer
418
419 ✗ clock_gettime(CLOCK_MONOTONIC, &end_time);
420
421 ✗ overruns += DIFF_NS(monotonic_time, end_time) > 0;
422
423 /* Wait for next cycle */
424 ✗ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &monotonic_time, 0);
425 }
426
427 ///////////////////// clean up /////////////////////////////////////
428
429 /* Clean up */
430 ✗ pdserv_exit(pdserv);
431
432 ✗ pthread_mutex_destroy(&mutex);
433
434 ✗ out:
435 ✗ if (err) {
436 ✗ fprintf(stderr, "Fatal error: %s\n", err);
437 ✗ return 1;
438 }
439 ✗ return 0;
440 }
441
442 /****************************************************************************/
443