#include <stdio.h>
#include <stdint.h>
#include <errno.h>
#include <getopt.h>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <sys/mman.h>
#include <sched.h>
#include <stdlib.h>
#include <semaphore.h>
#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)
int priority = -1;
int daemonize = 0;
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[] = {
{"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);
}
}
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;
(void)time;
(void)param;
if (value > limit || value < -limit)
return -EINVAL;
memcpy(dst, src, len);
clock_gettime(CLOCK_REALTIME, time);
return 0;
}
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;
}
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);
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;
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};
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()");
if (!(pdserv =
pdserv_create(
"PdServ Test",
"1.234", gettime))) {
err = "Failed to init pdserv.";
goto out;
}
err = "Failed to create task.";
goto out;
}
0666,
pd_double_T, &omega, 1, 0, limit_test, &omega_limit);
"Derivative of [cos,sin]");
{
static const char *text[] = {
"Event message 1",
"Event message 2",
"Event message 3",
"Event message 4",
"Event message 5",
};
}
if (mlockall(MCL_CURRENT | MCL_FUTURE))
fprintf(stderr, "mlockall() failed: %s\n", strerror(errno));
stack_prefault();
{
struct sched_param param = {
.sched_priority = (priority == -1
? sched_get_priority_max(SCHED_FIFO)
: priority),
};
if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
fprintf(stderr,
"Setting SCHED_FIFO with priority %i failed: %s\n",
param.sched_priority, strerror(errno));
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);
if (reset) {
cos = amplitude_set;
sin = 0.0;
}
else if (enable) {
amplitude = cos*cos + sin*sin;
ampl_modulation = 1.0/3.1415
* (amplitude/amplitude_set/amplitude_set - 1);
if (ampl_modulation > 1.0)
ampl_modulation = 1.0;
derivative[0] = -omega*sin - ampl_modulation*cos;
derivative[1] = omega*cos - ampl_modulation*sin;
cos += 1.0e-9*tsample_ns*derivative[0];
sin += 1.0e-9*tsample_ns*derivative[1];
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;
}
if (!--decimation_counter) {
decimation_counter = 10;
++counter;
}
sem_post(&sem);
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;
timer_add(&monotonic_time, tsample_ns);
clock_gettime(CLOCK_MONOTONIC, &end_time);
overruns += DIFF_NS(monotonic_time, end_time) > 0;
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &monotonic_time, 0);
}
sem_destroy(&sem);
out:
if (err) {
fprintf(stderr, "Fatal error: %s\n", err);
return 1;
}
return 0;
}