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;      
unsigned int duration_ns = 0;
const char *config = 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"
            "PdServ library version: %s\n"
            "Options:\n"
            "  --duration       -d secs    Set duration <float>\n"
            "  --config         -c conffile Set configuration file\n"
            "  --priority       -p <PRIO>  Set task priority. Default: RT.\n"
            "  --help           -h         Show this help.\n",
            base_name, pdserv_full_version);
}
 
void get_options(int argc, char **argv)
{
    int c, arg_count;
 
    static struct option longOptions[] = {
        //name,           has_arg,           flag, val
        {"duration",      required_argument, NULL, 'd'},
        {"config",        required_argument, NULL, 'c'},
        {"priority",      required_argument, NULL, 'p'},
        {"help",          no_argument,       NULL, 'h'},
        {NULL,            no_argument,       NULL,   0}
    };
 
    do {
        c = getopt_long(argc, argv, "d:c: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 'd':
                duration_ns = atof(optarg) * NSEC_PER_SEC;
                break;
 
            case 'c':
                config = optarg;
                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, stop_time = {0,0}, 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;
    }
 
    if (config)
        pdserv_config_file(pdserv, config);
 
    /* 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);
    pdserv_set_comment(var,
            "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 a vector event */
    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
     */
    int ret = pdserv_prepare(pdserv);
    if (ret) {
        err = "Failed to prepare pdserv.";
        goto out;
    }
 
 
    // 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;
    if (duration_ns) {
        stop_time = last_start_time;
        timer_add(&stop_time, duration_ns);
    }
    while (running && (!duration_ns || DIFF_NS(last_start_time, stop_time) > 0)) {
        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;
}
 
/****************************************************************************/