Commit 9165acfb authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

First set of readability and code improvements

-Safety blocks have some names changed
-Setpoint generator is redesigned.
 +No longer 'unitless' with 'd' as derivative, but physics names (x, v, a, j)
 +Removal of 'mirrored' code.
 +improved tracking at high speeds
 +Addition of 'precision' parameters to limit numerical noise
parent eaeb1f15
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013
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
......@@ -21,6 +21,7 @@
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>
#include "controller_block.h"
#include "block_setpoint_generator.h"
......@@ -33,22 +34,22 @@
--------------------------
| |
----| 0 cur0 0 spg0 |----
----| 0 reset_x 0 x |----
| |
----| 1 reset 1 spg1 |----
----| 1 reset 1 v |----
| |
| 2 spg2 |----
| 2 a |----
| |
| 3 spg3 |----
| 3 j |----
| |
| 4 setpoint |---
| 4 setpoint |----
| |
--------------------------
'setpoint' is the setpoint as the user requested it.
'spg0' is the output of the setpoint generator.
'spg1', 'spg2' and 'spg3' are the first, second and third
order derivatives of 'spg0'.
'x' is the output of the setpoint generator.
'v', 'a' and 'j' are the first, second and third
order derivatives of 'x'.
*/
......@@ -63,43 +64,60 @@ struct spg_command {
};
struct controller_block_private {
float *cur0;
/*
In the setpoint generator blocks the following physics notations
will be used.
x = position
v = velocity (1st derivative of x)
a = acceleration (2nd derivative of x)
j = jerk (3th derivative of x)
*/
float *reset_x;
bool *reset;
double setpoint;
float setpointout;
float derivative;
double step;
float t;
float it;
float max;
float min;
float maxd;
float mind;
float maxdd;
float mindd;
float imaxdd;
float imindd;
float maxddd;
float minddd;
float imaxddd;
float iminddd;
float maxdt;
float mindt;
float maxddt;
float minddt;
float maxdddt;
float mindddt;
double cursp;
double curd;
float curdd;
float curddd;
float curspout;
float curdout;
float curddout;
float curdddout;
/* beware: 'samples' is the time unit, not 'seconds',
all parameters and commands are scaled on block entry/exit */
double cmd_x;
float cmd_x_out; /* float version of the internal cmd_x */
double cmd_v;
float max_x;
float min_x;
float max_v;
float max_a;
float inv_max_a;
float max_j;
float inv_max_j;
double precision_x;
double precision_v;
double precision_a;
/* conversion factor for seconds/tick and its inverse */
float tick; /* seconds per tick */
float freq; /* ticks per second */
/* parameters in real world format (time unit: second) */
float max_v_sec;
float max_a_sec;
float max_j_sec;
float precision_x_sec;
float precision_v_sec;
float precision_a_sec;
/* current internal state */
double cur_x;
double cur_v;
double cur_a;
double cur_j;
float cur_x_out; /* float version of the internal values */
float cur_v_out;
float cur_a_out;
float cur_j_out;
/* Received commands */
struct spg_command current_command;
struct spg_command next_command;
......@@ -107,21 +125,99 @@ struct controller_block_private {
pthread_mutex_t mutex;
};
static bool almost_equal(double d1, double d2)
{
if (fabs(d1 - d2) <= 16 * DBL_EPSILON * fmax(fabs(d1), fabs(d2)) )
return true;
else
return false;
}
/* How many time ticks are needed to go from a to 0 */
static double ticks_to_a(struct controller_block_private *priv, double a_start, double a_at_t)
{
double t;
t = fabs(a_start - a_at_t) * priv->inv_max_j;
return ceil(t);
}
/* time to a constant speed */
static double ticks_to_v(struct controller_block_private *priv,
double v_start, double v_at_t)
{
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.
1/2 v = 1/2 j t^2 -> v = j t^2 -> t = sqrt(v/j)
Second half takes just as long.... return 2*t
*/
t = sqrt(fabs(v_at_t - v_start) * priv->inv_max_j);
return ceil(t) * 2;
}
static double a_after_ticks(struct controller_block_private *priv,
double a, double j, double t)
{
double a_at_ticks;
a_at_ticks =
a +
j * t;
return a_at_ticks;
}
static double v_after_ticks(struct controller_block_private *priv,
double v, double a, double j, double t)
{
double v_at_ticks;
v_at_ticks =
v +
a * t +
1.0/2.0 * j * t * t;
return v_at_ticks;
}
static double x_after_ticks(struct controller_block_private *priv,
double x, double v, double a, double j, double t)
{
double x_at_t;
x_at_t =
x +
v * t +
1.0/2.0 * a * t * t +
1.0/6.0 * j * t * t * t;
return x_at_t;
}
static void calculate(struct controller_block *spg)
{
struct controller_block_private *priv = spg->private;
bool ignore_x = false;
if (*priv->reset) {
priv->setpoint = *priv->cur0;
priv->cursp = priv->setpoint;
priv->cmd_x = *priv->reset_x;
priv->cur_x = priv->cmd_x;
priv->current_command.done = 1;
priv->current_command.type = BLOCK_SPG_SETPOINT;
priv->next_command.done = 1;
priv->step = 0.0;
priv->derivative = 0.0;
priv->curd = 0.0;
priv->curdd = 0.0;
priv->curddd = 0.0;
priv->cmd_v = 0.0;
priv->cur_v = 0.0;
priv->cur_a = 0.0;
priv->cur_j = 0.0;
}
if (priv->current_command.done) {
......@@ -132,9 +228,8 @@ static void calculate(struct controller_block *spg)
} else {
if (priv->current_command.type ==
BLOCK_SPG_SETPOINT_TIME) {
priv->step = 0.0;
priv->derivative = 0.0;
priv->setpoint =
priv->cmd_v = 0.0;
priv->cmd_x =
priv->current_command.setpoint;
}
}
......@@ -144,22 +239,19 @@ static void calculate(struct controller_block *spg)
switch (priv->current_command.type) {
case BLOCK_SPG_SETPOINT:
priv->setpoint = priv->current_command.setpoint;
priv->derivative = 0.0;
priv->step = 0.0;
priv->cmd_x = priv->current_command.setpoint;
priv->cmd_v = 0.0;
priv->current_command.done = 1;
break;
case BLOCK_SPG_SPEED:
priv->derivative = priv->current_command.speed;
priv->step = priv->derivative * priv->t;
priv->cmd_v = priv->current_command.speed * priv->tick;
priv->current_command.done = 1;
break;
case BLOCK_SPG_SETPOINT_TIME:
if (!priv->current_command.start) {
t = (double)(priv->current_command.time
-controller_time_seconds) * priv->it -
-controller_time_seconds) * priv->freq -
-(double)controller_time_samplenr;
t--;
if (t < 1.0 ||
priv->current_command.time <
controller_time_seconds) {
......@@ -167,15 +259,13 @@ static void calculate(struct controller_block *spg)
is a setpoint without time */
log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%d < %d)",
spg->name, priv->current_command.time, controller_time_seconds);
priv->setpoint = priv->current_command.setpoint;
priv->derivative = 0.0;
priv->step = 0.0;
priv->cmd_x = priv->current_command.setpoint;
priv->cmd_v = 0.0;
priv->current_command.done = 1;
} else {
priv->step =
priv->cmd_v =
(priv->current_command.setpoint -
priv->setpoint) / t;
priv->derivative= priv->step * priv->it;
priv->cmd_x) / t;
priv->current_command.start = 1;
}
}
......@@ -183,9 +273,9 @@ static void calculate(struct controller_block *spg)
if (priv->current_command.time <=
controller_time_seconds) {
priv->current_command.done = 1;
priv->setpoint =
priv->cmd_x =
priv->current_command.setpoint -
priv->step;
priv->cmd_v;
}
break;
}
......@@ -199,462 +289,381 @@ static void calculate(struct controller_block *spg)
}
if (priv->current_command.type == BLOCK_SPG_SPEED) {
priv->setpoint = priv->cursp;
ignore_x = true;
priv->cmd_x = priv->cur_x;
} else {
priv->setpoint += priv->step;
priv->cmd_x += priv->cmd_v;
}
if (priv->setpoint > priv->max)
priv->setpoint = priv->max;
if (priv->setpoint < priv->min)
priv->setpoint = priv->min;
if (priv->cmd_x > priv->max_x)
priv->cmd_x = priv->max_x;
if (priv->cmd_x < priv->min_x)
priv->cmd_x = priv->min_x;
if (priv->curd < priv->step) {
/* Our speed is smaller than requested by command */
if (priv->curdd > 0.0) {
/* We are accelerating toward speed requested */
double t, dt, dt2, t2;
double st, st2;
t = priv->curdd * priv->imaxddd;
t = ceil(t);
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;
double x, v, a, j, t;
double req_x, req_v, t_max_a, v_delta_from_max_a;
double error_v_after_a, error_x_at_v;
bool state_at_max_a = false;
bool state_to_max_a = false;
/* procedure: where would we end up if we go to
requested speed.
*/
x = priv->cur_x;
v = priv->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);
j = copysign(priv->max_j, error_v);
dt= priv->curd +
priv->curdd * t +
1.0/2.0 * priv->minddd * t * t;
st= priv->cursp +
priv->curd * t +
1.0/2.0 * priv->curdd * t * t +
1.0/6.0 * priv->minddd * t * t * t;
if ((float)priv->curd < priv->mind - priv->minddd) {
if (st > priv->setpoint + priv->step) {
priv->curddd = 0.0;
}
}
if (priv->curddd == 0.0) {
t = sqrt((priv->curd - priv->step) * priv->iminddd);
t = ceil(t);
if (priv->curdd + t*priv->maxddd > priv->maxdd){
t = priv->maxdd * priv->imaxddd;
}
st= priv->cursp +
priv->curd * t +
1.0/6.0 * priv->maxddd * t * t * t;
dt= priv->curd +
1.0/2.0 * priv->maxddd * t * t;
st= st +
dt * t +
1.0/2.0 * (priv->minddd * t) * t * t +
1.0/6.0 * priv->minddd * t * t * t;
dt2= dt +
priv->maxdd * t +
1.0/2.0 * priv->minddd * t * t;
if (dt2 < priv->step) {
t2 = (priv->step - dt2) * priv->imaxdd;
st2= dt * t2
+ 1.0/2.0 * priv->maxdd * t2 * t2
+ priv->maxdd * t2 * t;
if (st + st2 <=
(priv->setpoint +
priv->step * (t * 2 + t2))) {
priv->curddd = priv->maxddd;
}
}
if (st <= priv->setpoint + priv->step * t * 2) {
priv->curddd = priv->maxddd;
}
if (st >= priv->max) {
priv->curddd = priv->minddd;
}
}
if (dt > priv->step) {
priv->curddd = priv->minddd;
}
} else if (priv->curdd < 0.0) {
/* We are decelerating away from requested speed */
double t, dt, dt2, t2;
double st, st2 = 0.0;
t = priv->curdd * priv->iminddd;
t = ceil(t);
if (fabs(a) > priv->max_j) {
/* Not at constant velocity */
if (signbit(a) != signbit(error_v)) {
/* We are not accelerating towards cmd speed.
Go to constant velocity first
*/
dt= priv->curd +
priv->curdd * t * 2.0 +
1.0/2.0 * priv->maxddd * t * t * 4.0;
st= priv->cursp +
priv->curd * t * 2.0 +
1.0/2.0 * priv->curdd * t * t * 4.0 +
1.0/6.0 * priv->maxddd * t * t * t * 8.0;
st= st +
dt * t +
-1.0/2.0 * priv->curdd * t * t +
1.0/6.0 * priv->minddd * t * t * t;
dt2= dt +
- priv->curdd * t +
1.0/2.0 * priv->minddd * t * t;
if (dt2 < priv->step) {
t2 = (priv->step - dt2) * priv->imaxdd;
st2= dt * t2
+ 1.0/2.0 * priv->maxdd * t2 * t2
+ priv->maxdd * t2 * t;
}
if (st < priv->setpoint + priv->step * t * 3.0) {
priv->curddd = priv->maxddd;
} else if (priv->curdd == priv->mindd) {
if (st + st2 <
priv->setpoint + priv->step * t * 3.0) {
priv->curddd = priv->maxddd;
}
}
} else {
/* We are not accelerating at the moment */
if (priv->curd < priv->mind - priv->minddd) {
/* Going at min speed */
double t, st, dt;
int at_maxdd = 0;
t = sqrt((priv->curd - priv->step) * priv->iminddd);
t = ceil(t);
if (t * priv->maxddd > priv->maxdd){
t = priv->maxdd * priv->imaxddd;
at_maxdd = 1;
}
st= priv->cursp +
priv->curd * t +
1.0/6.0 * priv->maxddd * t * t * t;
t = ticks_to_a(priv, a, 0);
x = x_after_ticks(priv, x, v, a, j, t);
v = v_after_ticks(priv, v, a, j, t);
a = a_after_ticks(priv, a, j, t);
req_x = req_x + req_v * t;
dt= priv->curd +
1.0/2.0 * priv->maxddd * t * t;
state_to_max_a = true;
} else {
/* We are accelerating towards cmd speed. */
double a_peak;
double t_to_max_a, t_a, t_at_max_a, t_a_to_0;
double v_end_max_a, v_start_max_a, v_started_a, v_delta_a;
/* speed we had when starting acceleration */
t_a = ticks_to_a(priv, 0, a);
v_delta_a = v_after_ticks(priv, 0, 0, priv->max_j, t_a);
v_started_a = v - copysign(v_delta_a, a);
/* What is the highest a we get? */
t_a_to_0 = ticks_to_v(priv, v_started_a, req_v) * 0.5;
a_peak = a_after_ticks(priv, 0, j, t_a_to_0);
if (fabs(a) > fabs(a_peak)) {
/* We are accelerating much faster than needed acc down first */
t_a_to_0 = ticks_to_a(priv, a, 0);
if (at_maxdd) {
double lt;
double dt2;
} else if (fabs(a_peak) > priv->max_a) {
a_peak = copysign(priv->max_a, a_peak);
dt2 = priv->mind - dt;
lt = (priv->mind - 2 * dt2) * priv->imindd;
t = ticks_to_a(priv, 0, a_peak);
v_start_max_a = v_after_ticks(priv, v_started_a, 0, j, t);
st+=dt * lt +
1.0/2.0 * (priv->maxdd) * lt * lt;
v_end_max_a = req_v - copysign(v_delta_from_max_a, error_v);
dt = dt2;
}
st= st +
dt * t +
1.0/2.0 * (priv->maxddd * t) * t * t +
1.0/6.0 * priv->minddd * t * t * t;
if (st <= priv->setpoint + priv->step * t) {
priv->curddd = priv->maxddd;
t_at_max_a = fabs(v_end_max_a - v_start_max_a) * priv->inv_max_a;
t_to_max_a = t - t_a;
if (t_to_max_a >= 1.0) {
state_to_max_a = true;
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);
req_x = req_x + req_v * t_to_max_a;
} else {
state_at_max_a = true;
}
x = x_after_ticks(priv, x, v, a, 0, t_at_max_a);
v = v_after_ticks(priv, v, a, 0, t_at_max_a);
a = a_after_ticks(priv, a, 0, t_at_max_a);
req_x = req_x + req_v * t_at_max_a;
t_a_to_0 = t_max_a;
} else {
priv->curdd = 0.0;
priv->curddd = 0.0;
t_to_max_a = t_a_to_0 - t_a;
if (t_to_max_a >= 1.0) {
state_to_max_a = true;
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);
req_x = req_x + req_v * t_to_max_a;
} else {
state_at_max_a = true;
}
}
} else {
priv->curddd = priv->maxddd;
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;
}
} else {
state_to_max_a = true;
}
} else if (priv->curd > priv->step) {
/* Our speed is greater than requested */
if (priv->curdd < 0.0) {
/* We are decelerating towards requested speed */
double t, dt, dt2, t2;
double st, st2;