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 <semaphore.h>  // sem_wait(), sem_post()

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

#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 to copy a signal during poll */
int read_signal(
        const struct pdvariable* signal,
        void *dst,
        const void *src,
        size_t len,
        struct timespec *time,
        void *priv_data)
{
    (void)signal;

    sem_t *sem = priv_data;

    sem_wait(sem);
    memcpy(dst, src, len);
    sem_post(sem);

    if (time)
        clock_gettime(CLOCK_REALTIME, time);

    return 0;
}

/****************************************************************************
 * 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;
    sem_t sem;
    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);

    if (sem_init(&sem, 0, 1))
        perror("sem_init()");


    /* 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;
    }

    /* 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);

    /* This signal will use the read callback to do some locking. It
     * communicates the semaphore to the callback using priv_data */
    var = pdserv_signal_cb(pdtask, 1, "/osc/derivative",
            pd_double_T, derivative, 2, 0, read_signal, &sem);
    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 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 _atfter_ 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);

        sem_wait(&sem);

        /* 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);

        sem_post(&sem);

        /* 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);

    sem_destroy(&sem);

out:
    if (err) {
        fprintf(stderr, "Fatal error: %s\n", err);
        return 1;
    }
    return 0;
}

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