Commit 3bd8b0bf authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Merge branch 'controller_bus' into sercos

Conflicts:
	controller/dt_ctrl.c
	controller/ec/esc.c
	controller/ec/esc_coe.c
	controller/ec/esc_device.c
parents 6fb8f235 64b1a680
......@@ -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/
......
......@@ -128,7 +128,7 @@ function dt_traceval(element)
"<div style='float: left'>" +
this.label + filler +
"</div>" +
"<div align='right'>" +
"<div align='right' style='padding-right: 5px' >" +
new_value + this.suffix +
"</div>";
changed = true;
......
......@@ -265,7 +265,7 @@
<td valign="top" width="30%" style="border:thin solid black">
<center>
<input type="button" value="Focusbox 134.7,-0.05" onclick="azel_set_coord('134.7','-0.05');"><br>
<input type="button" value="Show -6,30" onclick="azel_set_coord('-6','30');"><br>
<input type="button" value="Show 30,30" onclick="azel_set_coord('30','30');"><br>
</center>
</td>
</tr>
......@@ -1110,12 +1110,12 @@ Elevation_Setpoint_trace.open(trace_url, 'Elevation_Setpoint', 2);
*/
var focusbox_position = new dt_traceval("Focusbox_Position");
focusbox_position.number_set(true, 2);
focusbox_position.number_set(true, 0);
focusbox_position.highlight_set(true, "yellow", "white");
function Focusbox_Position_trace_value(obj, time, value, unit)
{
focusbox_position.value(value);
focusbox_position.value(value * 10);
}
function Focusbox_Position_trace_timeout(obj)
......
......@@ -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
......
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2013
Copyright Jeroen Vreeken (jeroen@vreeken.net), 2013, 2014
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
......@@ -54,6 +54,8 @@ struct controller_bus *controller_bus_create(char *type, char *name,
(*entry)->owner = owner;
(*entry)->input_block = owner;
(*entry)->output_block = owner;
(*entry)->state = CONTROLLER_BUS_STATE_OK;
log_send(LOG_T_DEBUG, "New bus '%s' created of type '%s'",
name, type);
......@@ -87,6 +89,15 @@ int controller_bus_output_set(struct controller_bus *bus,
return 0;
}
int controller_bus_poll_set(struct controller_bus *bus,
enum controller_bus_state (*poll)(struct controller_bus *bus),
int (*recover)(struct controller_bus *bus))
{
bus->poll = poll;
bus->recover = recover;
return 0;
}
struct controller_bus *controller_bus_find(char *name)
{
......@@ -130,7 +141,10 @@ struct controller_bus_client *controller_bus_add_input_client(
bus->input_client = ic;
bus->input_client[bus->input_clients].block = client;
bus->input_client[bus->input_clients].callback = NULL;
bus->input_client[bus->output_clients].poll = NULL;
bus->input_client[bus->output_clients].recover = NULL;
bus->input_client[bus->input_clients].offset = 0;
bus->input_client[bus->output_clients].state = CONTROLLER_BUS_STATE_OK;
bus->input_clients++;
log_send(LOG_T_DEBUG, "Block '%s' is an input client of bus '%s'",
......@@ -159,7 +173,10 @@ struct controller_bus_client *controller_bus_add_output_client(
bus->output_client = oc;
bus->output_client[bus->output_clients].block = client;
bus->output_client[bus->output_clients].callback = NULL;
bus->output_client[bus->output_clients].poll = NULL;
bus->output_client[bus->output_clients].recover = NULL;
bus->output_client[bus->output_clients].offset = 0;
bus->output_client[bus->output_clients].state = CONTROLLER_BUS_STATE_OK;
bus->output_clients++;
log_send(LOG_T_DEBUG, "Block '%s' is an output client of bus '%s'",
......@@ -215,3 +232,164 @@ struct controller_bus *controller_bus_linked_bus(struct controller_block *src,
return NULL;
}
int controller_bus_poll_states(int *oks, int *errors, int *recoverables)
{
struct controller_bus *entry;
int error = 0;
int recoverable = 0;
int ok = 0;
for (entry = bus_list; entry; entry = entry->next) {
int i;
int entry_error = 0;
int entry_recoverable = 0;
if (entry->poll) {
enum controller_bus_state newstate;
newstate = entry->poll(entry);