example-st.c

/*****************************************************************************
 *
 * Copyright (C) 2017-2018 Richard Hacker <lerichi at gmx dot net>
 *                    2023 Florian Pose <fp@igh.de>
 *
 * License: LGPLv3
 *
 * This is a very simple single tasking example.
 *
 * This example serves as demonstration data source for different other
 * EtherLab software packages, that rely on the signal/parameter structure.
 *
 * The example 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.
 *
 ****************************************************************************/

#define _GNU_SOURCE     // for pthread_setname_np()
                        //
#include <pdserv.h>     // obviously!

#include <errno.h>
#include <getopt.h>     // getopt()
#include <limits.h>     // PTHREAD_STACK_MIN
#include <pthread.h>    // pthread_mutex_lock(), pthread_mutex_unlock()
#include <sched.h>      // sched_setscheduler()
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>     // strtoul(), exit()
#include <string.h>     // memset(), strcmp(), strerror()
#include <sys/mman.h>   // mlockall()
#include <time.h>       // clock_gettime(), clock_nanosleep()
#include <unistd.h>     // getopt()

/****************************************************************************/

#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.  */

#define RT_PRIO (80)
int priority = -1; 
const char *config = 0;

/****************************************************************************/

struct pdserv* pdserv = 0;
struct pdtask* pdtask = 0;
struct pdevent* event = 0;

unsigned int tsample_ns = 0.01e9; // 10ms
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

// 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;
#define AMP_INIT (10.0)
double amplitude_set = AMP_INIT;
double ampl_limit = 20.0;
unsigned int event_state[5] = {0, 0, 0, 0, 0};

// Signals:
double sine = 0.0, cosine = AMP_INIT;
double amplitude;
double ampl_modulation;
double derivative[2];
uint8_t counter;
int decimation_counter = 1;
size_t matrix_dim[] = {2, 3};
double matrix[2][3] = {{0, 1, 2}, {3, 4, 5}}; // two rows, three columns

/***************************************************************************
 * 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 usage(FILE *f, const char *base_name)
{
    fprintf(f,
            "Usage: %s [OPTIONS]\n"
            "PdServ library version: %s\n"
            "Options:\n"
            "  --config      -c conffile  Set configuration file\n"
            "  --priority    -p <PRIO>    Set task priority.\n"
            "                             Default is non-realtime.\n"
            "  --cycle-time  -t <PERIOD>  Cycle time in µs.\n"
            "                             Default is 10000 (10 ms).\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
        {"config",        required_argument, NULL, 'c'},
        {"priority",      required_argument, NULL, 'p'},
        {"cycle-time",    required_argument, NULL, 't'},
        {"help",          no_argument,       NULL, 'h'},
        {NULL,            no_argument,       NULL,   0}
    };

    do {
        c = getopt_long(argc, argv, "c:p:t:h", longOptions, NULL);

        switch (c) {
            case 'c':
                config = optarg;
                break;

            case 'p':
                if (!strcmp(optarg, "RT")) {
                    priority = RT_PRIO;
                } else {
                    char *end;
                    priority = strtoul(optarg, &end, 10);
                    if (!*optarg || *end) {
                        fprintf(stderr, "Invalid priority: %s\n", optarg);
                        exit(1);
                    }
                }
                break;

            case 't':
                {
                    char *end;
                    unsigned long cycle_time_us = strtoul(optarg, &end, 10);
                    if (!*optarg || *end || cycle_time_us > 1000000) {
                        fprintf(stderr, "Invalid cycle time: %s\n", optarg);
                        exit(1);
                    }
                    tsample_ns = cycle_time_us * 1000;
                }
                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);
}

/****************************************************************************/

int init_pdserv()
{
    struct pdvariable* var;

    /* Create a pdserv instance */
    if (!(pdserv = pdserv_create("PdServ Example", "1.234", gettime))) {
        fprintf(stderr, "Failed to init pdserv.");
        return -1;
    }

    /* Create a task */
    if (!(pdtask = pdserv_create_task(pdserv, 1.0e-9 * tsample_ns, NULL))) {
        fprintf(stderr, "Failed to create task.");
        return -1; /* failed to create a task. */
    }

    /* Apply configuration file */
    if (config) {
        pdserv_config_file(pdserv, config);
    }


    /* 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);
    pdserv_parameter(pdserv, "/Matrix",
            0666, pd_double_T, matrix, sizeof(matrix_dim) / sizeof(size_t),
            matrix_dim, 0, 0);

    /* Register signals */
    pdserv_signal(pdtask, 1, "/osc/cos",
            pd_double_T, &cosine, 1, 0);
    pdserv_signal(pdtask, 1, "/osc/sin",
            pd_double_T, &sine, 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) {
        fprintf(stderr, "Failed to prepare PdServ.\n");
        return -1;
    }

    return 0;
}

/****************************************************************************/

void *cyclic_task(__attribute__((unused)) void *data)
{
    double exec_time, cycle_time;
    unsigned int overruns = 0;
    struct timespec monotonic_time, world_time;
    struct timespec start_time, end_time, last_start_time;

    fprintf(stderr, "Starting cyclic thread with a period of %u µs.\n",
            tsample_ns / 1000);

    clock_gettime(CLOCK_MONOTONIC, &monotonic_time);
    last_start_time = monotonic_time;

    while (1) {
        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) {
            cosine = amplitude_set;
            sine = 0.0;
        }
        else if (enable) {
            // Calculate amplitude
            amplitude = cosine * cosine + sine * sine;

            // 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 * sine - ampl_modulation * cosine;
            derivative[1] = omega * cosine - ampl_modulation * sine;

            // Integrate
            cosine += 1.0e-9 * tsample_ns * derivative[0];
            sine += 1.0e-9 * tsample_ns * derivative[1];

            // Check amplitude
            if (cosine >  ampl_limit) cosine =  ampl_limit;
            if (cosine < -ampl_limit) cosine = -ampl_limit;
            if (sine >  ampl_limit) sine =  ampl_limit;
            if (sine < -ampl_limit) sine = -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);
    }

    return NULL;
}

/****************************************************************************
 * Main function
 ****************************************************************************/

int main(int argc, char **argv)
{
    pthread_attr_t attr;
    pthread_t thread;
    int ret = 0;

    get_options(argc, argv);

    ret = init_pdserv();
    if (ret) {
        fprintf(stderr, "Failed to init PdServ.\n");
        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 memory */
    ret = mlockall(MCL_CURRENT | MCL_FUTURE);
    if (ret) {
        fprintf(stderr, "mlockall() failed: %m\n");
    }

    /* Initialize pthread attributes (default values) */
    ret = pthread_attr_init(&attr);
    if (ret) {
        fprintf(stderr, "init pthread attributes failed\n");
        goto out_pdserv;
    }

    /* Set a specific stack size  */
    ret = pthread_attr_setstacksize(&attr,
            PTHREAD_STACK_MIN + MAX_SAFE_STACK);
    if (ret) {
        fprintf(stderr, "pthread setstacksize failed\n");
        goto out_pdserv;
    }

    if (priority > -1) {
        /* Set scheduler policy and priority of pthread */
        ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
        if (ret) {
            fprintf(stderr, "pthread setschedpolicy failed\n");
            goto out_pdserv;
        }

        struct sched_param param;
        param.sched_priority = priority;
        ret = pthread_attr_setschedparam(&attr, &param);
        if (ret) {
            fprintf(stderr, "pthread setschedparam failed\n");
            goto out_pdserv;
        }

        /* Use scheduling parameters of attr */
        ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
        if (ret) {
            fprintf(stderr, "pthread setinheritsched failed\n");
            goto out_pdserv;
        }
    }

    /* Create a pthread with specified attributes */
    ret = pthread_create(&thread, &attr, cyclic_task, NULL);
    if (ret) {
        fprintf(stderr, "pthread create failed: %s\n", strerror(ret));
        goto out_pdserv;
    }

    /* Name the thread */
    ret = pthread_setname_np(thread, "pdserv-rt");
    if (ret) {
        fprintf(stderr, "Failed to set thread name: %s\n", strerror(ret));
    }

    /* Join the thread and wait until it is done */
    ret = pthread_join(thread, NULL);
    if (ret) {
        fprintf(stderr, "Join pthread failed: %m\n");
    }

out_pdserv:
    pdserv_exit(pdserv);

out:
    pthread_mutex_destroy(&mutex);
    return ret;
}

/****************************************************************************/