pdserv  3.1
Process data server
example-st.c
/*****************************************************************************
*
* $Id$
*
* vim: tw=78
*
* This is a very simple single tasking example.
*
* Copyright (C) 2017,2018 Richard Hacker <lerichi at gmx dot net>
* License: LGPLv3
*
* It demonstrates:
* - signals
* - sub-rated signals (signal that is calculated every N-th cycle)
* - parameters
* - events
*
* In the cyclic calculation, an oscillator producing a sine and cosine
* signal with variable amplitude and frequency is calculated.
*
* With a decimation of 10 (subrating), a saw tooth signal is also produced
*
****************************************************************************/
#include <pdserv.h> // obviously!
#include <stdio.h>
#include <stdint.h>
#include <errno.h>
#include <getopt.h> // getopt()
#include <unistd.h> // getopt()
#include <string.h> // memset(), strcmp(), strerror()
#include <time.h> // clock_gettime(), clock_nanosleep()
#include <sys/mman.h> // mlockall()
#include <sched.h> // sched_setscheduler()
#include <stdlib.h> // strtoul(), exit()
#include <pthread.h> // pthread_mutex_lock(), pthread_mutex_unlock()
/****************************************************************************/
#define MAX_SAFE_STACK (8 * 1024)
#define NSEC_PER_SEC (1000000000)
#define DIFF_NS(A, B) (((long long) (B).tv_sec - (A).tv_sec) * NSEC_PER_SEC \
+ (B).tv_nsec - (A).tv_nsec)
/****************************************************************************/
/* Command-line option variables. */
int priority = -1;
int daemonize = 0;
/***************************************************************************
* Support functions
***************************************************************************/
struct timespec* timer_add(struct timespec *t, unsigned int dt_ns)
{
t->tv_nsec += dt_ns;
while (t->tv_nsec >= NSEC_PER_SEC) {
t->tv_nsec -= NSEC_PER_SEC;
t->tv_sec++;
}
return t;
}
int gettime(struct timespec *time)
{
return clock_gettime(CLOCK_REALTIME, time);
}
void stack_prefault(void)
{
unsigned char dummy[MAX_SAFE_STACK];
memset(dummy, 0, MAX_SAFE_STACK);
}
void usage(FILE *f, const char *base_name)
{
fprintf(f,
"Usage: %s [OPTIONS]\n"
"Options:\n"
" --priority -p <PRIO> Set task priority. Default: RT.\n"
" --help -h Show this help.\n",
base_name);
}
void get_options(int argc, char **argv)
{
int c, arg_count;
static struct option longOptions[] = {
//name, has_arg, flag, val
{"priority", required_argument, NULL, 'p'},
{"help", no_argument, NULL, 'h'},
{NULL, no_argument, NULL, 0}
};
do {
c = getopt_long(argc, argv, "p:h", longOptions, NULL);
switch (c) {
case 'p':
if (!strcmp(optarg, "RT")) {
priority = -1;
} else {
char *end;
priority = strtoul(optarg, &end, 10);
if (!*optarg || *end) {
fprintf(stderr, "Invalid priority: %s\n", optarg);
exit(1);
}
}
break;
case 'h':
usage(stdout, argv[0]);
exit(0);
case '?':
usage(stderr, argv[0]);
exit(1);
default:
break;
}
}
while (c != -1);
arg_count = argc - optind;
if (arg_count) {
fprintf(stderr, "%s takes no arguments!\n", argv[0]);
usage(stderr, argv[0]);
exit(1);
}
}
/* Callback to test the limit of a parameter to be set */
int limit_test(const struct pdvariable* param,
void *dst, const void* src, size_t len,
struct timespec *time, void* priv_data)
{
double value = *(double*)src;
double limit = *(double*)priv_data; /* pointer to limit of double */
(void)time;
(void)param;
if (value > limit || value < -limit)
return -EINVAL;
memcpy(dst, src, len);
clock_gettime(CLOCK_REALTIME, time);
return 0;
}
/* Callback used to protect signals and parameters */
void lock_fn(int lock, void* priv_data)
{
if (lock)
pthread_mutex_lock(priv_data);
else
pthread_mutex_unlock(priv_data);
}
/****************************************************************************
* Main function
****************************************************************************/
int main(int argc, char **argv)
{
struct pdserv* pdserv;
struct pdtask* pdtask;
struct pdevent* event;
struct pdvariable* var;
unsigned int tsample_ns = (uint64_t)(0.01e9); // 10ms
const char* err = NULL;
int running = 1;
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
double exec_time, cycle_time;
unsigned int overruns = 0;
struct timespec monotonic_time, world_time;
struct timespec start_time, end_time, last_start_time;
// Variables for real time task: sin/cos oscillator and saw tooth generator
// Parameters:
double omega = 1.2;
char enable = 1;
char reset = 0;
double omega_limit = 5.0;
double amplitude_set = 10.0;
double ampl_limit = 20.0;
unsigned int event_state[5] = {0,0,0,0,0};
// Signals:
double sin = 0.0, cos = amplitude_set;
double amplitude;
double ampl_modulation;
double derivative[2];
uint8_t counter;
int decimation_counter = 1;
get_options(argc, argv);
/* Create a pdserv instance */
if (!(pdserv = pdserv_create("PdServ Test", "1.234", gettime))) {
err = "Failed to init pdserv.";
goto out;
}
/* Create a task */
if (!(pdtask = pdserv_create_task(pdserv, 1.0e-9*tsample_ns, NULL))) {
err = "Failed to create task.";
goto out;
}
/* Pass lock callbacks to use */
pdserv_set_signal_readlock_cb(pdtask, lock_fn, &mutex);
pdserv_set_parameter_writelock_cb(pdserv, lock_fn, &mutex);
/* Register parameters */
pdserv_parameter(pdserv, "/osc/omega",
0666, pd_double_T, &omega, 1, 0, limit_test, &omega_limit);
pdserv_parameter(pdserv, "/osc/amplitude/Setpoint",
0666, pd_double_T, &amplitude_set, 1, 0, 0, 0);
pdserv_parameter(pdserv, "/osc/enable",
0666, pd_sint8_T, &enable, 1, 0, 0, 0);
pdserv_parameter(pdserv, "/osc/reset",
0666, pd_sint8_T, &reset, 1, 0, 0, 0);
pdserv_parameter(pdserv, "/osc/amplitude/Limit",
0666, pd_double_T, &ampl_limit, 1, 0, 0, 0);
pdserv_parameter(pdserv, "/Event/State",
0666, pd_uint_T, &event_state, 5, 0, 0, 0);
/* Register signals */
pdserv_signal(pdtask, 1, "/osc/cos",
pd_double_T, &cos, 1, 0);
pdserv_signal(pdtask, 1, "/osc/sin",
pd_double_T, &sin, 1, 0);
pdserv_signal(pdtask, 1, "/osc/amplitude",
pd_double_T, &amplitude, 1, 0);
pdserv_signal(pdtask, 1, "/osc/amplitude/Modulation",
pd_double_T, &ampl_modulation, 1, 0);
var = pdserv_signal(pdtask, 1, "/osc/derivative",
pd_double_T, derivative, 2, 0);
"Derivative of [cos,sin]");
/* This signal is updated every 10th calculation cycle. This has no
* consequence to pdserv, it is only an indication to the clients
* that the signal is decimated */
pdserv_signal(pdtask, 10, "/SawTooth",
pd_uint8_T, &counter, 1, 0);
/* Register an event with 2 elements */
event = pdserv_event(pdserv, "/Limit", 5);
{
static const char *text[] = {
"Event message 1",
"Event message 2",
"Event message 3",
"Event message 4",
"Event message 5",
};
pdserv_event_set_text(event, text);
}
/* At this time, the setup for pdserv is finished. Up to now
* - pdserv instance and pdserv tasks were created
* - signals, parameters and events were registered.
*
* Now tell pdserv to prepare itself. Here it prepares the non-real time
* interface (network sockets), support threads, restore persistent
* parameters, etc
*/
pdserv_prepare(pdserv);
// Only _after_ pdserv_prepare() was called, the real time setup, like
// locking memory, prefaulting the stack, setting scheduler and priority,
// etc, is done. This is important, otherwise the real time setup will
// leak into non-real time tasks which is not a good idea.
/* Lock all memory forever - prevents it from being swapped out. */
if (mlockall(MCL_CURRENT | MCL_FUTURE))
fprintf(stderr, "mlockall() failed: %s\n", strerror(errno));
/* Provoke the first stack fault before cyclic operation. */
stack_prefault();
/* Set task priority and scheduler. */
{
struct sched_param param = {
.sched_priority = (priority == -1
? sched_get_priority_max(SCHED_FIFO)
: priority),
};
if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
fprintf(stderr,
"Setting SCHED_FIFO with priority %i failed: %s\n",
param.sched_priority, strerror(errno));
/* Reset priority, so that sub-threads start */
priority = -1;
}
}
clock_gettime(CLOCK_MONOTONIC, &monotonic_time);
last_start_time = monotonic_time;
while (running) {
clock_gettime(CLOCK_MONOTONIC, &start_time);
clock_gettime(CLOCK_REALTIME, &world_time);
/* Get a read lock on parameters and a write lock on signals */
pthread_mutex_lock(&mutex);
/* Calculation sin/cos oscillator at base rate */
if (reset) {
cos = amplitude_set;
sin = 0.0;
}
else if (enable) {
// Calculate amplitude
amplitude = cos*cos + sin*sin;
// Amplitude error, limiting upper value (for very small
// amplitudes)
ampl_modulation = 1.0/3.1415
* (amplitude/amplitude_set/amplitude_set - 1);
if (ampl_modulation > 1.0)
ampl_modulation = 1.0;
// Calculate derivatives
derivative[0] = -omega*sin - ampl_modulation*cos;
derivative[1] = omega*cos - ampl_modulation*sin;
// Integrate
cos += 1.0e-9*tsample_ns*derivative[0];
sin += 1.0e-9*tsample_ns*derivative[1];
// Check amplitude
if (cos > ampl_limit) cos = ampl_limit;
if (cos < -ampl_limit) cos = -ampl_limit;
if (sin > ampl_limit) sin = ampl_limit;
if (sin < -ampl_limit) sin = -ampl_limit;
}
/* Sub-rating task for saw tooth */
if (!--decimation_counter) {
decimation_counter = 10; // Reset decimation counter
++counter; // Sawtooth with natural wrap
}
pdserv_event_set_all(event, event_state, &world_time);
/* Release locks */
pthread_mutex_unlock(&mutex);
/* Call at end of calculation task, so that pdserv updates itself */
pdserv_update(pdtask, &world_time);
/* Calculate timing statistics */
cycle_time = 1.0e-9 * DIFF_NS(last_start_time, start_time);
exec_time = 1.0e-9 * DIFF_NS(last_start_time, end_time);
last_start_time = start_time;
pdserv_update_statistics(pdtask, exec_time, cycle_time, overruns);
timer_add(&monotonic_time, tsample_ns); // Increment timer
clock_gettime(CLOCK_MONOTONIC, &end_time);
overruns += DIFF_NS(monotonic_time, end_time) > 0;
/* Wait for next cycle */
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &monotonic_time, 0);
}
/* Clean up */
pdserv_exit(pdserv);
pthread_mutex_destroy(&mutex);
out:
if (err) {
fprintf(stderr, "Fatal error: %s\n", err);
return 1;
}
return 0;
}
/****************************************************************************/