Commit 8d1a320c authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Merge branch 'jeroen' into controller_bus

parents e8a77b98 6606002f
......@@ -11,7 +11,7 @@ LIBUTILOBJS= $(UTILSRCS:.c=.lo)
all: $(LIBUTILOBJS) weather_test libutils.la libutils.la_install
libutils.la_LDFLAGS= -lm -rpath ${CURDIR}/../lib
libutils.la_LDFLAGS= -lm -rpath ${CURDIR}/../lib -lrt
libutils.la: $(LIBUTILOBJS)
${CURDIR}/../lib:
......
......@@ -8,7 +8,7 @@ CFLAGS= -Wall -O3 -I../../common/utils \
-I../../common/log \
-I../../include
LDFLAGS+= -L../../common/lib/ -L../../lib/ -L./lib -lutils -ltrace -static -llog
LDFLAGS+= -L../../common/lib/ -L../../lib/ -L./lib -lutils -ltrace -llog
LIBNOVA=-lnova -L../../lib/
......
......@@ -70,7 +70,7 @@ il2cdir:
$(BLOCKSRCS): il2cdir
libblock.la_LDFLAGS=-rpath ${CURDIR}/../lib
libblock.la_LDFLAGS=-rpath ${CURDIR}/../lib -lm
libblock.la: $(BLOCKS)
libblock.la_install: libblock.la
......
......@@ -97,6 +97,10 @@ struct controller_block_private {
/* conversion factor for seconds/tick and its inverse */
float tick; /* seconds per tick */
float freq; /* ticks per second */
float freq2;
float freq3;
double t_max_a;
double v_delta_from_max_a;
/* parameters in real world format (time unit: second) */
float max_v_sec;
......@@ -107,6 +111,13 @@ struct controller_block_private {
float precision_v_sec;
float precision_a_sec;
/* state at last change */
double start_x;
double start_v;
double start_a;
double start_j;
int start_t;
/* current internal state */
double cur_x;
double cur_v;
......@@ -151,8 +162,8 @@ static double ticks_to_v(struct controller_block_private *priv,
double t;
/* From a constant speed to a constant speed is done in two halves:
First halve (untill half the speed difference is reached is done
at max jerk, second half is done at -max jerk.
First halve (untill half the speed difference is reached) is
done at max jerk, second half is done at -max jerk.
1/2 v = 1/2 j t^2 -> v = j t^2 -> t = sqrt(v/j)
......@@ -203,22 +214,36 @@ static double x_after_ticks(struct controller_block_private *priv,
return x_at_t;
}
static void calculate(struct controller_block *spg)
static void setpoint_generator_calculate(struct controller_block *spg)
{
struct controller_block_private *priv = spg->private;
double cur_x, cur_v;
double t_max_a;
bool ignore_x = false;
bool must_brake = false;
bool good_x;
bool good_v;
cur_x = priv->cur_x;
cur_v = priv->cur_v;
t_max_a = priv->t_max_a;
if (*priv->reset) {
priv->cmd_x = *priv->reset_x;
priv->cur_x = priv->cmd_x;
cur_x = priv->cmd_x;
priv->current_command.done = 1;
priv->current_command.type = BLOCK_SPG_SETPOINT;
priv->next_command.done = 1;
priv->cmd_v = 0.0;
priv->cur_v = 0.0;
cur_v = 0.0;
priv->cur_a = 0.0;
priv->cur_j = 0.0;
priv->start_x = cur_x;
priv->start_v = 0.0;
priv->start_a = 0.0;
priv->start_j = 0.0;
priv->start_t = 0;
}
if (priv->current_command.done) {
......@@ -266,7 +291,7 @@ static void calculate(struct controller_block *spg)
} else {
priv->cmd_v =
(priv->current_command.setpoint -
priv->cmd_x) / t;
priv->cmd_x) / (t+1);
priv->current_command.start = 1;
}
}
......@@ -291,9 +316,7 @@ static void calculate(struct controller_block *spg)
if (priv->current_command.type == BLOCK_SPG_SPEED) {
ignore_x = true;
priv->cmd_x = priv->cur_x;
} else {
priv->cmd_x += priv->cmd_v;
priv->cmd_x = cur_x;
}
if (priv->cmd_x > priv->max_x)
......@@ -301,13 +324,20 @@ static void calculate(struct controller_block *spg)
if (priv->cmd_x < priv->min_x)
priv->cmd_x = priv->min_x;
if (!almost_equal(priv->cur_v, priv->cmd_v) ||
!almost_equal(priv->cur_x, priv->cmd_x)) {
double error_x = priv->cmd_x - priv->cur_x;
double error_v = priv->cmd_v - priv->cur_v;
good_x = almost_equal(cur_x, priv->cmd_x);
good_v = almost_equal(cur_v, priv->cmd_v);
if (!good_v || !good_x) {
double error_x;
double req_x_1;
double error_x_jpos;
double error_x_j0;
double error_x_jneg;
double error_v = priv->cmd_v - cur_v;
double x, v, a, j, t;
double req_x, req_v, t_max_a, v_delta_from_max_a;
double req_x, req_v, v_delta_from_max_a;
double error_v_after_a, error_x_at_v;
double j_from_pos;
bool state_at_max_a = false;
bool state_to_max_a = false;
......@@ -315,19 +345,34 @@ static void calculate(struct controller_block *spg)
requested speed.
*/
x = priv->cur_x;
v = priv->cur_v;
x = cur_x;
v = cur_v;
a = priv->cur_a;
req_x = priv->cmd_x;
req_v = priv->cmd_v;
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(priv, 0, priv->max_a);
v_delta_from_max_a = v_after_ticks(priv, 0, 0, priv->max_j, t_max_a);
req_x_1 = req_x + req_v;
error_x_jpos = req_x_1 - x_after_ticks(priv, x, v, a, priv->max_j, 1);
error_x_j0 = req_x_1 - x_after_ticks(priv, x, v, a, 0, 1);
error_x_jneg = req_x_1 - x_after_ticks(priv, x, v, a, -priv->max_j, 1);
if (fabs(error_x_jpos) < fabs(error_x_jneg)) {
error_x = error_x_jpos;
j_from_pos = priv->max_j;
} else {
error_x = error_x_jneg;
j_from_pos = -priv->max_j;
}
if (fabs(error_x_j0) < fabs(error_x)) {
error_x = error_x_j0;
j_from_pos = 0;
}
v_delta_from_max_a = priv->v_delta_from_max_a;
j = copysign(priv->max_j, error_v);
if (fabs(a) > priv->max_j) {
if (fabs(a) >= priv->max_j) {
/* Not at constant velocity */
if (signbit(a) != signbit(error_v)) {
......@@ -362,6 +407,7 @@ static void calculate(struct controller_block *spg)
t_a_to_0 = ticks_to_a(priv, a, 0);
} else if (fabs(a_peak) > priv->max_a) {
/* We are going to hit maximum acc */
a_peak = copysign(priv->max_a, a_peak);
t = ticks_to_a(priv, 0, a_peak);
......@@ -394,6 +440,7 @@ static void calculate(struct controller_block *spg)
if (t_to_max_a >= 1.0) {
state_to_max_a = true;
/* accelerate to max a */
x = x_after_ticks(priv, x, v, a, j, t_to_max_a);
v = v_after_ticks(priv, v, a, j, t_to_max_a);
a = a_after_ticks(priv, a, j, t_to_max_a);
......@@ -402,9 +449,9 @@ static void calculate(struct controller_block *spg)
state_at_max_a = true;
}
}
/* decellerate to a==0 */
x = x_after_ticks(priv, x, v, a, -j, t_a_to_0);
v = v_after_ticks(priv, v, a, -j, t_a_to_0);
a = a_after_ticks(priv, a, -j, t_a_to_0);
req_x = req_x + req_v * t_a_to_0;
a = 0;
}
......@@ -427,7 +474,7 @@ static void calculate(struct controller_block *spg)
x = x_after_ticks(priv, x, v, 0, j, t_max_a);
v = v_after_ticks(priv, v, 0, j, t_max_a);
a = a_after_ticks(priv, 0, j, t_max_a);
a = copysign(priv->max_a, j);
req_x = req_x + req_v * t_max_a;
t_at_max_a = (fabs(error_v_after_a) - v_delta_from_max_a*2) * priv->inv_max_a;
......@@ -439,7 +486,6 @@ static void calculate(struct controller_block *spg)
x = x_after_ticks(priv, x, v, a, -j, t_max_a);
v = v_after_ticks(priv, v, a, -j, t_max_a);
req_x = req_x + req_v * t_max_a;
a = a_after_ticks(priv, a, -j, t_max_a);
a = 0;
} else {
/* Simple profile: triangle
......@@ -463,11 +509,13 @@ static void calculate(struct controller_block *spg)
error_x_at_v = req_x - x;
if (fabs(error_x_at_v) < fabs(error_x) || ignore_x) {
if (fabs(error_x_at_v) > req_v && !ignore_x &&
if (fabs(error_x_at_v) < fabs(error_x) ||
(signbit(error_x) != signbit(error_x_at_v) && !state_to_max_a && !state_at_max_a) ||
ignore_x) {
if (!ignore_x &&
signbit(error_x_at_v) == signbit(error_x) ) {
/* position got better, but not yet enough */
priv->cur_j = copysign(priv->max_j, error_x);
priv->cur_j = j_from_pos;
} else {
if (state_to_max_a) {
priv->cur_j = j;
......@@ -481,17 +529,18 @@ static void calculate(struct controller_block *spg)
/* going to requested speed would make position error
bigger, first go to position.
*/
priv->cur_j = copysign(priv->max_j, error_x);
priv->cur_j = j_from_pos;
}
}
/* When moving can we brake before the position limits? */
if (fabs(priv->cur_v) > 0.0) {
if (fabs(cur_v) > 0.0) {
double t, x, v, a, j;
bool done = false;
x = priv->cur_x;
v = priv->cur_v;
x = cur_x;
v = cur_v;
a = priv->cur_a;
/* add 1 tick, we want to know if we are still safe untill the next
......@@ -541,7 +590,7 @@ static void calculate(struct controller_block *spg)
/* Assuming we have a constant v,
check if we have to start deceleration now. */
t = ticks_to_v(priv, priv->cur_v, 0);
t = ticks_to_v(priv, cur_v, 0);
/* will we hit max a? */
if (fabs(a_after_ticks(priv, a, j, t/2)) > priv->max_a) {
......@@ -549,7 +598,7 @@ static void calculate(struct controller_block *spg)
double v_start_3, t_2;
t = ticks_to_a(priv, 0, priv->max_a);
t = t_max_a;
x = x_after_ticks(priv, x, v, a, j, t);
v = v_after_ticks(priv, v, a, j, t);
......@@ -585,10 +634,11 @@ static void calculate(struct controller_block *spg)
}
}
/* If accelerating, can we decelerate before going beyond our max vel */
if (fabs(priv->cur_a) > 0.0) {
double t, v, a, j;
v = priv->cur_v;
v = cur_v;
a = priv->cur_a;
/* add 1 tick, we want to know if we are still safe untill the next
......@@ -615,58 +665,102 @@ static void calculate(struct controller_block *spg)
}
}
if (!ignore_x) {
priv->cmd_x += priv->cmd_v;
}
/* Is the difference between spg and command small enough?
If so, make outputs equal to command */
if (!must_brake &&
fabs(priv->cmd_x - priv->cur_x) < priv->precision_x &&
fabs(priv->cmd_v - priv->cur_v) < priv->precision_v &&
fabs(priv->cmd_x - cur_x) < priv->precision_x &&
fabs(priv->cmd_v - cur_v) < priv->precision_v &&
fabs(priv->cur_a) < priv->precision_a) {
priv->cur_j = 0.0;
priv->cur_a = 0.0;
priv->cur_v = priv->cmd_v;
priv->cur_x = priv->cmd_x;
cur_v = priv->cmd_v;
cur_x = priv->cmd_x;
}
priv->cur_a += priv->cur_j;
/* new jerk? */
if (!almost_equal(priv->cur_j, priv->start_j)) {
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
priv->start_t++;
priv->cur_a = a_after_ticks(priv,
priv->start_a, priv->start_j, priv->start_t);
cur_v = v_after_ticks(priv,
priv->start_v, priv->start_a, priv->start_j, priv->start_t);
cur_x = x_after_ticks(priv,
priv->start_x, priv->start_v, priv->start_a, priv->start_j,
priv->start_t);
if (fabs(priv->cur_a) > priv->max_a) {
priv->cur_a = copysign(priv->max_a, priv->cur_a);
priv->cur_j = 0;
}
priv->cur_v += priv->cur_a;
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
if (fabs(priv->cur_v) >= priv->max_v) {
if (fabs(cur_v) >= priv->max_v) {
/* prevent further acceleration beyond max v */
if (signbit(priv->cur_v) == signbit(priv->cur_a)) {
if (signbit(cur_v) == signbit(priv->cur_a)) {
priv->cur_a = 0.0;
}
if (signbit(priv->cur_v) == signbit(priv->cur_j)) {
if (signbit(cur_v) == signbit(priv->cur_j)) {
priv->cur_j = 0.0;
}
priv->cur_v = copysign(priv->max_v, priv->cur_v);
cur_v = copysign(priv->max_v, cur_v);
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
priv->cur_x += priv->cur_v;
if (priv->cur_x > priv->max_x) {
priv->cur_x = priv->max_x;
priv->cur_v = 0;
if (cur_x > priv->max_x) {
cur_x = priv->max_x;
cur_v = 0;
priv->cur_a = 0;
priv->cur_j = 0;
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
if (priv->cur_x < priv->min_x) {
priv->cur_x = priv->min_x;
priv->cur_v = 0;
if (cur_x < priv->min_x) {
cur_x = priv->min_x;
cur_v = 0;
priv->cur_a = 0;
priv->cur_j = 0;
priv->start_j = priv->cur_j;
priv->start_a = priv->cur_a;
priv->start_v = cur_v;
priv->start_x = cur_x;
priv->start_t = 0;
}
priv->cur_x_out = priv->cur_x;
priv->cur_v_out = priv->cur_v * priv->freq;
priv->cur_a_out = priv->cur_a * priv->freq * priv->freq;
priv->cur_j_out = priv->cur_j * priv->freq * priv->freq * priv->freq;
priv->cur_x_out = cur_x;
priv->cur_v_out = cur_v * priv->freq;
priv->cur_a_out = priv->cur_a * priv->freq2;
priv->cur_j_out = priv->cur_j * priv->freq3;
priv->cmd_x_out = priv->cmd_x;
priv->cur_x = cur_x;
priv->cur_v = cur_v;
}
static bool block_setpoint_generator_queue_space(struct controller_block *spg)
......@@ -763,6 +857,8 @@ static void param_get(struct controller_block *spg, int param, void *val)
static void param_set(struct controller_block *spg, int param, va_list val)
{
double t_max_a;
switch (param) {
case 0:
spg->private->cmd_x = va_arg(val, double);
......@@ -811,8 +907,16 @@ static void param_set(struct controller_block *spg, int param, va_list val)
spg->private->precision_a = spg->private->precision_a_sec * spg->private->tick * spg->private->tick;
spg->private->freq = 1.0 / spg->private->tick;
spg->private->freq2 = spg->private->freq * spg->private->freq;
spg->private->freq3 = spg->private->freq2 * spg->private->freq;
spg->private->inv_max_j = 1.0 / spg->private->max_j;
spg->private->inv_max_a = 1.0 / spg->private->max_a;
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
spg->private->v_delta_from_max_a =
v_after_ticks(spg->private, 0, 0, spg->private->max_j, t_max_a);
spg->private->t_max_a = t_max_a;
}
static struct controller_block_interm_list interms[] = {
......@@ -876,6 +980,8 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
spg->private->max_a_sec = 0.0;
spg->private->max_j_sec = 0.0;
spg->private->freq = 1.0;
spg->private->freq2 = 1.0;
spg->private->freq3 = 1.0;
spg->private->precision_x = 0.0;
spg->private->precision_v = 0.0;
spg->private->precision_a = 0.0;
......@@ -887,6 +993,11 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
spg->private->cur_v = 0.0;
spg->private->cur_a = 0.0;
spg->private->cur_j = 0.0;
spg->private->start_x = 0.0;
spg->private->start_v = 0.0;
spg->private->start_a = 0.0;
spg->private->start_j = 0.0;
spg->private->start_t = 0;
spg->private->cur_x_out = 0.0;
spg->private->cur_v_out = 0.0;
spg->private->cur_a_out = 0.0;
......@@ -899,7 +1010,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
if (controller_block_outterm_list_init(spg, outterms))
goto err_input;
spg->calculate = calculate;
spg->calculate = setpoint_generator_calculate;
if (controller_block_param_list_init(spg, params))
goto err_output;
......
......@@ -28,7 +28,7 @@ controller_load_parser.tab.lo: controller_load_parser.yy.c
controller_load.lo: controller_load_parser.tab.h controller_load_parser.yy.h
libcontroller.la_LDFLAGS=-rpath ${CURDIR}/../lib -lshell
libcontroller.la_LDFLAGS=-rpath ${CURDIR}/../lib -lshell -ldl -lpthread -lrt
libcontroller.la: $(CONTROLLER)
libcontroller.la_install: libcontroller.la
......
......@@ -29,6 +29,7 @@
#include <string.h>
#include <sys/mman.h>
#include <errno.h>
#include <signal.h>
#include "controller_block.h"
#include "controller_sample.h"
......@@ -125,6 +126,34 @@ static ssize_t sample_timing_snprintf(struct sample_timing *st,
}
static int sample_timing_sane(struct timespec *t_exp, struct timespec *t_act)
{
long s_diff;
long n_diff;
s_diff = t_act->tv_sec - t_exp->tv_sec;
if (s_diff < 0) {
return -1;
}
if (s_diff > 1) {
return 1;
}
n_diff = s_diff * 1000 * 1000 * 1000;
n_diff += t_act->tv_nsec - t_exp->tv_nsec;
if (n_diff > nsec_interval * 10) {
return 1;
}
return 0;
}
static void alarm_handler(int d)
{
log_send(LOG_T_ERROR, "Sample has been inactive to long!");
}
/*
This thread should be realtime....
*/
......@@ -142,6 +171,16 @@ static void *sample_thread(void *arg)
pthread_mutex_t lock;
pthread_cond_t cv;
#endif
sigset_t sigset;
struct sigaction sigact;
sigemptyset (&sigset);
sigaddset(&sigset, SIGALRM);
sigprocmask(SIG_UNBLOCK, &sigset, NULL);
sigact.sa_handler = alarm_handler;
sigemptyset (&sigact.sa_mask);
sigact.sa_flags = 0;
sigaction(SIGALRM, &sigact, NULL);
log_send(LOG_T_DEBUG, "Starting sample thread");
controller_sample_thread_running = true;
......@@ -206,14 +245,26 @@ static void *sample_thread(void *arg)
clock_gettime(clock, &t_end);
sample_timing_add(&st_start, &t, &t_start);
sample_timing_add(&st_io, &t_start, &t_io);
sample_timing_add(&st_end, &t, &t_end);
sample_timing_add(&st_sample, &t_start, &t_end);
if (sample_timing_diff_nsec(&t, &t_end) > nsec_interval) {
controller_sampleoverruns++;
}
if (sample_timing_sane(&t, &t_start)) {
log_send(LOG_T_ERROR,
"Time sanity check triggered: %lld.%09ld %lld.%09ld",
(long long)t.tv_sec, t.tv_nsec,
(long long)t_start.tv_sec, t_start.tv_nsec);
t.tv_sec = t_start.tv_sec;
t.tv_nsec = (t_start.tv_nsec / nsec_interval) * nsec_interval;
log_send(LOG_T_WARNING,
"Using current time for next sampe: %lld.%09ld",
(long long)t.tv_sec, t.tv_nsec);
} else {
sample_timing_add(&st_start, &t, &t_start);
sample_timing_add(&st_io, &t_start, &t_io);
sample_timing_add(&st_end, &t, &t_end);
sample_timing_add(&st_sample, &t_start, &t_end);
}
alarm(1);
}
return NULL;
}
......
......@@ -33,6 +33,7 @@
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <signal.h>
#include "dynarg.h"
......@@ -53,7 +54,12 @@ int main(int argc, char **argv)
{
char *ctrl_filename;
char *dot_filename;
sigset_t sigset;
sigemptyset (&sigset);
sigaddset(&sigset, SIGALRM);
sigprocmask(SIG_BLOCK, &sigset, NULL);
log_server_start(CTRL_LOG_PORT, LOG_T_DEBUG, LOG_T_INFO);