Commit b185a42f authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Update test networks

Fix some setpoint generator quirks
parent 98c41253
......@@ -3,3 +3,5 @@ all:
$(MAKECMDGOALS):
@$(MAKE) --no-print-directory -C .. $(MAKECMDGOALS)
.PHONY: test
......@@ -139,8 +139,8 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
uint64_t command_t;
int64_t t;
command_t = priv->cur_command.t.tv_nsec +
priv->cur_command.t.tv_sec * 1000000000;
command_t = (uint64_t)priv->cur_command.t.tv_nsec +
(uint64_t)priv->cur_command.t.tv_sec * 1000000000;
priv->command_t = command_t;
t = (command_t - controller_time_nseconds) / priv->period_nsec;
......
......@@ -26,6 +26,7 @@
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_sample.h>
#include <log/log.h>
/*
......@@ -95,12 +96,12 @@ struct controller_block_private {
double precision_a;
/* 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;
uint64_t period_nsec;
/* parameters in real world format (time unit: second) */
float max_v_sec;
......@@ -132,7 +133,7 @@ struct controller_block_private {
struct command_entry cur_command;
bool cur_done;
bool cur_start;
uint32_t cur_t_samplenr;
uint64_t command_t;
struct controller_command *command;
};
......@@ -244,6 +245,8 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->start_a = 0.0;
priv->start_j = 0.0;
priv->start_t = 0;
goto set_output;
}
if (priv->cur_done) {
......@@ -262,8 +265,6 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
}
}
if (!priv->cur_done) {
double t;
switch (priv->cur_command.type) {
case COMMAND_PTYPE_SETPOINT:
priv->cmd_x = priv->cur_command.value.f;
......@@ -271,24 +272,21 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->cur_done = true;
break;
case COMMAND_PTYPE_SPEED:
priv->cmd_v = priv->cur_command.value.f * priv->tick;
priv->cmd_v = priv->cur_command.value.f * controller_sample_period();
priv->cur_done = true;
break;
case COMMAND_PTYPE_SETPOINT_TIME:
if (!priv->cur_start) {
double t_samplenr;
t_samplenr = floor(
((double)priv->cur_command.t.tv_nsec * priv->freq) /
1000000.0);
priv->cur_t_samplenr = t_samplenr;
t = (double)(priv->cur_command.t.tv_sec
- controller_time_seconds) * priv->freq +
t_samplenr -
(double)controller_time_samplenr;
uint64_t command_t;
int64_t t;
command_t = (uint64_t)priv->cur_command.t.tv_nsec +
(uint64_t)priv->cur_command.t.tv_sec * 1000000000;
priv->command_t = command_t;
t = (command_t - controller_time_nseconds) / priv->period_nsec;
if (t <= 0.0) {
if (t < 0) {
/* Command is to old, pretend it
is a setpoint without time */
log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%lld < %d)",
......@@ -299,13 +297,12 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
} else {
priv->cmd_v =
(priv->cur_command.value.f -
priv->cmd_x) / (t+1);
priv->cmd_x) / (t+1.0);
priv->cur_start = true;
}
}
if (priv->cur_command.t.tv_sec <=
controller_time_seconds) {
if (priv->command_t <= controller_time_nseconds) {
priv->cur_done = true;
priv->cmd_x =
priv->cur_command.value.f -
......@@ -319,7 +316,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
ignore_x = true;
priv->cmd_x = cur_x;
priv->cmd_x = cur_x + priv->cmd_v;
}
if (priv->cmd_x > priv->max_x)
......@@ -757,6 +754,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->start_t = 0;
}
set_output:
priv->cur_x_out = cur_x;
priv->cur_v_out = cur_v * priv->freq;
priv->cur_a_out = priv->cur_a * priv->freq2;
......@@ -825,8 +823,7 @@ static void param_get(struct controller_block *spg, int param, void *val)
spg->private->cmd_x;
break;
case 1:
*(float*)val =
spg->private->tick;
*(float*)val = controller_sample_period();
break;
case 2:
*(float*)val =
......@@ -866,6 +863,7 @@ 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;
double tick = controller_sample_period();
switch (param) {
case 0:
......@@ -878,7 +876,6 @@ static void param_set(struct controller_block *spg, int param, va_list val)
spg->private->cur_j = 0.0;
break;
case 1:
spg->private->tick = va_arg(val, double);
break;
case 2:
spg->private->max_x = va_arg(val, double);
......@@ -907,18 +904,19 @@ static void param_set(struct controller_block *spg, int param, va_list val)
}
/* Scale all settings to sample time unit */
spg->private->max_v = spg->private->max_v_sec * spg->private->tick;
spg->private->max_a = spg->private->max_a_sec * spg->private->tick * spg->private->tick;
spg->private->max_j = spg->private->max_j_sec * spg->private->tick * spg->private->tick * spg->private->tick;
spg->private->max_v = spg->private->max_v_sec * tick;
spg->private->max_a = spg->private->max_a_sec * tick * tick;
spg->private->max_j = spg->private->max_j_sec * tick * tick * tick;
spg->private->precision_x = spg->private->precision_x_sec;
spg->private->precision_v = spg->private->precision_v_sec * spg->private->tick;
spg->private->precision_a = spg->private->precision_a_sec * spg->private->tick * spg->private->tick;
spg->private->precision_v = spg->private->precision_v_sec * tick;
spg->private->precision_a = spg->private->precision_a_sec * tick * tick;
spg->private->freq = 1.0 / spg->private->tick;
spg->private->freq = 1.0 / 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;
spg->private->period_nsec = controller_sample_period() * 1000000000;
/* Calculate delta v when comming from max_a */
t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
......@@ -967,7 +965,6 @@ struct controller_block * block_setpoint_generator_3d_create(char *name, va_list
goto err_name;
spg->private->cmd_x = 0.0;
spg->private->cmd_v = 0.0;
spg->private->tick = 1.0;
spg->private->max_x = 0.0;
spg->private->min_x = 0.0;
spg->private->max_v = 0.0;
......
frequency 10
blocks {
{ "setpoint_generator_3d", "spg", "spg", "na" }
{ "test_input_bool", "reset" }
{ "test_input_float", "reset_x" }
{ "test_output_float", "setpoint" }
{ "test_output_float", "x" }
{ "test_output_float", "v" }
{ "test_output_float", "a" }
{ "test_output_float", "j" }
{ "test_command", "command" }
}
links {
{ "reset", "value", "spg", "reset", true }
{ "reset_x", "value", "spg", "reset_x", true }
{ "spg", "x", "x", "value", true }
{ "spg", "v", "v", "value", true }
{ "spg", "a", "a", "value", true }
{ "spg", "j", "j", "value", true }
{ "spg", "setpoint", "setpoint", "value", true }
}
set SETPOINT 4
set SPEED 5
set SETPOINT_TIME 6
params {
{ "spg", "min_x", (float) -100.0 }
{ "spg", "max_x", (float) 200.0 }
{ "spg", "max_v", (float) 20.0 }
{ "spg", "max_a", (float) 2.0 }
{ "spg", "max_j", (float) 0.5 }
{ "command", "command", "spg", $[SETPOINT_TIME], 123, 80.0, 0, 500000000 }
{ "command", "command", "spg", $[SETPOINT], 124, -110.0 }
{ "command", "command", "spg", $[SETPOINT], 125, 300.0 }
{ "command", "command", "spg", $[SETPOINT], 126, 61.0 }
{ "command", "command", "spg", $[SPEED], 200, 11.0 }
{ "command", "command", "spg", $[SETPOINT_TIME], 127, -95.0, 1, 100000000 }
{ "command", "command", "spg", $[SPEED], 201, -33.0 }
{ "reset", "value", 12,
(int) { true, false, false, false, false, false, false, false, false, false,
true, false } }
{ "reset_x", "value", 11,
(float) { 50.0, 33.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-94.0 } }
{ "setpoint", "value", 10,
(float) { 50.0, 56.0, 62.0, 68.0, 74.0, 80.0, -100.0, 200.0, 61.0, 51.1395 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-7 }
}
{ "x", "value", 20,
(float) { 50.0, 50.00008, 50.00007, 50.002255, 50.00533, 50.01042, 50.01783, 50.02742, 50.0395, 50.05458,
-94.0, -9.400008e+01, -9.400066e+01, -9.400225e+01, -9.400533e+01,
-9.401041e+01, -9.401800e+01, -9.402858e+01, -9.404266e+01, -9.406075e+01 },
(float) { 0.0, 1e-5, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3,
0.0, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5 }
}
{ "v", "value", 10,
(float) { 0.0, 0.0025, 0.01, 0.0225, 0.04, 0.0625, 0.085, 0.1075, 0.135, 0.1675 },
(float) { 0.0, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7 }
}
{ "a", "value", 20,
(float) { 0.0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.20, 0.25, 0.3, 0.35,
0.0, -0.05,-0.1,-0.15,-0.2,-0.25,-0.30,-0.35,-0.4,-0.45},
(float) { 0.0, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7,
0.0, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7 }
}
{ "j", "value", 20,
(float) { 0.0, 0.5, 0.5, 0.5, 0.5, 0.5, -0.5, 0.5, 0.5, 0.5,
0.0, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5},
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
}
......@@ -77,3 +77,10 @@ CTRL_BLOCK_LIBS += libblock.la
TARGETS += $(BLOCK_TARGETS)
CLEAN += $(BLOCK_TARGETS) $(BLOCK_OBJS)
SRCS += $(BLOCK_SRCS)
CTRL_TESTS += \
$(DIR)/block_gain.test.ctrl \
$(DIR)/block_not.test.ctrl \
$(DIR)/block_setpoint_generator_1d.test.ctrl \
$(DIR)/block_setpoint_generator_3d.test.ctrl
......@@ -347,8 +347,8 @@ static void *sample_thread(void *arg)
t.tv_nsec += nsec_interval;
tsnorm(&t);
controller_time_nseconds = t.tv_sec * 1000000000;
controller_time_nseconds = (uint64_t)t.tv_sec * 1000000000;
controller_time_nseconds += t.tv_nsec;
controller_time_seconds = t.tv_sec;
controller_time_samplenr = t.tv_nsec / nsec_interval;
......
......@@ -19,6 +19,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <inttypes.h>
#include <controller/controller_block.h>
#include <controller/controller_command.h>
......@@ -42,6 +43,8 @@ static void calculate_test_command(struct controller_block *block)
if (!priv->start_sec) {
priv->start_sec = controller_time_seconds;
priv->start_nsec = controller_time_nseconds % 1000000000;
log_send(LOG_T_DEBUG, "%s: Start time: %" PRIu64 ".%09" PRIu32,
block->name, priv->start_sec, priv->start_nsec);
}
if (priv->cur < priv->entries_nr) {
......@@ -56,6 +59,8 @@ static void calculate_test_command(struct controller_block *block)
entry->t.tv_nsec += priv->start_nsec;
}
log_send(LOG_T_DEBUG, "%s: Filter command %d and write to queue",
command->name, priv->cur);
if (command->filter)
r = command->filter(command, entry);
if (r == 0)
......
......@@ -53,14 +53,15 @@ static void calculate_test_output_float(struct controller_block *block)
float value = *priv->value;
float value_good = priv->values[priv->values_cur];
float value_error = priv->errors[priv->values_cur];
float error = fabs(value - value_good);
if (fabs(value - value_good) > value_error) {
if (error > value_error) {
log_send(LOG_T_ERROR,
"%s: value %d does not match: %e (%a) != %e (%a) within %e",
"%s: value %d does not match: %e (%a) != %e (%a) within %e, diff: %e",
block->name, priv->values_cur,
value, value,
value_good, value_good,
value_error);
value_error, error);
} else {
priv->values_ok++;
}
......
......@@ -29,7 +29,5 @@ SRCS += $(TEST_BLOCK_SRCS)
CTRL_TESTS += \
$(DIR)/block_gain.test.ctrl \
$(DIR)/block_not.test.ctrl \
$(DIR)/block_setpoint_generator_1d.test.ctrl
$(DIR)/test.ctrl
frequency 100
blocks {
{ "test_input_bool", "input_bool" }
{ "test_input_float", "input_float" }
{ "test_output_bool", "output_bool" }
{ "test_output_float", "output_float" }
}
links {
{ "input_bool", "value", "output_bool", "value", true }
{ "input_float", "value", "output_float", "value", true }
}
params {
{ "input_bool", "value", 4, (int) { true, false, true, false } }
{ "output_bool", "value", 4, (int) { true, false, true, false } }
{ "input_float", "value", 4,
(float) { 12345678.9, -3.3333333, 0.0, 42.01e-20 },
(float) { 0.0, 0.0, 0.0, 0.0 } }
{ "output_float", "value", 4,
(float) { 12345678.9, -3.3333333, 0.0, 42.01e-20 },
(float) { 0.0, 0.0, 0.0, 0.0 } }
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment