Commit 990cb146 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Add id to setpoint generators

Lots of test updates
parent 4d0ee609
frequency 10
blocks {
{ "limit", "limit" }
{ "test_input_float", "in" }
{ "test_output_float", "out" }
}
links {
{ "in", "value", "limit", "in", true }
{ "limit", "out", "out", "value", true }
}
params {
{ "limit", "min", (float) -11.1 }
{ "limit", "max", (float) 22.2 }
{ "in", "value", 7, (float)
{ 0.0, -100.0, -11.1, -11.0, 22.0, 22.2, 100.0 }
}
{ "out", "value", 7,
(float) { 0.0, -11.1, -11.1, -11.0, 22.0, 22.2, 22.2 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
}
frequency 10
blocks {
{ "matrix_2x2", "matrix" }
{ "test_input_float", "in0" }
{ "test_input_float", "in1" }
{ "test_output_float", "out0" }
{ "test_output_float", "out1" }
}
links {
{ "in0", "value", "matrix", "in0", true }
{ "in1", "value", "matrix", "in1", true }
{ "matrix", "out0", "out0", "value", true }
{ "matrix", "out1", "out1", "value", true }
}
params {
{ "matrix", "constants",
(float) { 2.0, 4.0 },
(float) { 8.0, 16.0 }
}
{ "in0", "value", 4, (float)
{ 0.0, 1.0, 0.0, 1.0 }
}
{ "in1", "value", 4, (float)
{ 0.0, 0.0, 1.0, 1.0 }
}
{ "out0", "value", 4,
(float) { 0.0, 2.0, 4.0, 6.0 },
(float) { 0.0, 0.0, 0.0, 0.0 }
}
{ "out1", "value", 4,
(float) { 0.0, 8.0, 16.0, 24.0 },
(float) { 0.0, 0.0, 0.0, 0.0 }
}
}
frequency 10
blocks {
{ "quantize", "quantize" }
{ "test_input_float", "in" }
{ "test_output_float", "out" }
}
links {
{ "in", "value", "quantize", "in", true }
{ "quantize", "out", "out", "value", true }
}
params {
{ "quantize", "quantum", (float) 0.1 }
{ "in", "value", 7, (float)
{ 0.0, -100.09, -11.13, -11.01, 22.01, 22.19, 100.001 }
}
{ "out", "value", 7,
(float) { 0.0, -100.1, -11.1, -11.0, 22.0, 22.2, 100.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
}
frequency 10
blocks {
{ "rangecheck", "rangecheck" }
{ "test_input_float", "in" }
{ "test_output_bool", "valid" }
{ "test_output_bool", "invalid" }
}
links {
{ "in", "value", "rangecheck", "in", true }
{ "rangecheck", "valid", "valid", "value", true }
{ "rangecheck", "invalid", "invalid", "value", true }
}
params {
{ "rangecheck", "min", (float) -11.1 }
{ "rangecheck", "max", (float) 22.2 }
{ "in", "value", 7, (float)
{ 0.0, -100.0, -11.1, -11.0, 22.0, 22.2, 100.0 }
}
{ "valid", "value", 7, (int)
{ true, false, true, true, true, true, false }
}
{ "invalid", "value", 7, (int)
{ false, true, false, false, false, false, true }
}
}
...@@ -99,6 +99,16 @@ static void servo_state_calculate(struct controller_block *servo_state) ...@@ -99,6 +99,16 @@ static void servo_state_calculate(struct controller_block *servo_state)
return; return;
} }
if (!safe && priv->state != SERVO_STATE_DISABLED) {
priv->enable = false;
priv->reset = true;
priv->state =
SERVO_STATE_DISABLED;
log_send(LOG_T_ERROR,
"%s: Unsafe, going to state DISABLED",
servo_state->name);
}
switch (priv->state) { switch (priv->state) {
case SERVO_STATE_ENABLED: case SERVO_STATE_ENABLED:
priv->out_x = *priv->spg_x; priv->out_x = *priv->spg_x;
...@@ -134,18 +144,6 @@ static void servo_state_calculate(struct controller_block *servo_state) ...@@ -134,18 +144,6 @@ static void servo_state_calculate(struct controller_block *servo_state)
float out_a = 0.0; float out_a = 0.0;
float t = controller_sample_period(); float t = controller_sample_period();
if (!safe) {
priv->enable = false;
priv->reset = true;
priv->state =
SERVO_STATE_DISABLED;
log_send(LOG_T_ERROR,
"%s: Unsafe, going to state DISABLED",
servo_state->name);
break;
}
if (out_v > 0.0) { if (out_v > 0.0) {
out_a = -priv->max_a; out_a = -priv->max_a;
out_v += out_a * t; out_v += out_a * t;
......
frequency 10
blocks {
{ "servo_state", "servo_state" }
{ "test_input_float", "spg_x" }
{ "test_input_float", "spg_v" }
{ "test_input_float", "spg_a" }
{ "test_input_bool", "safe" }
{ "test_input_bool", "override" }
{ "test_output_float", "out_x" }
{ "test_output_float", "out_v" }
{ "test_output_float", "out_a" }
{ "test_output_bool", "reset" }
{ "test_output_bool", "enable" }
{ "test_command", "command" }
}
links {
{ "spg_x", "value", "servo_state", "spg_x", true }
{ "spg_v", "value", "servo_state", "spg_v", true }
{ "spg_a", "value", "servo_state", "spg_a", true }
{ "safe", "value", "servo_state", "safe", true }
{ "override", "value", "servo_state", "override", true }
{ "servo_state", "out_x", "out_x", "value", true }
{ "servo_state", "out_v", "out_v", "value", true }
{ "servo_state", "out_a", "out_a", "value", true }
{ "servo_state", "reset", "reset", "value", true }
{ "servo_state", "enable", "enable", "value", true }
}
set SETPOINT 4
params {
{ "servo_state", "min_x", (float) -10.0 }
{ "servo_state", "max_x", (float) 20.0 }
{ "servo_state", "max_v", (float) 3.0 }
{ "servo_state", "max_a", (float) 1.5 }
{ "command", "command", "servo_state", $[SETPOINT], 42, true }
{ "command", "command", "servo_state", $[SETPOINT], 33, false }
{ "spg_x", "value", 4, (float) { 1.0, 2.0, 3.0, 4.0 } }
{ "spg_v", "value", 4, (float) { 0.3, -0.1, -3.0, 1.0 } }
{ "spg_a", "value", 4, (float) { 1.0, -0.2, 0.3, 0.4 } }
{ "safe", "value", 5, (int) { true, true, true, true, false, false } }
{ "override", "value", 5, (int) { false, false, false, true, false } }
{ "out_x", "value", 5,
(float) { 1.0, 2.0, 2.715, 4.0, 4.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "out_a", "value", 5,
(float) { 0.0, 0.0, 1.5, 0.4, 0.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "out_v", "value", 5,
(float) { 0.0, 0.0, -2.85, 1.0, 0.0 },
(float) { 0.0, 0.0, 0.0, 0.0, 0.0 }
}
{ "reset", "value", 5, (int) { true, true, true, false, true } }
{ "enable", "value", 5, (int) { false, false, true, true, false } }
}
...@@ -81,6 +81,8 @@ struct controller_block_private { ...@@ -81,6 +81,8 @@ struct controller_block_private {
float cur_x; float cur_x;
float cur_v; float cur_v;
uint32_t id;
struct command_entry cur_command; struct command_entry cur_command;
bool cur_done; bool cur_done;
bool cur_start; bool cur_start;
...@@ -105,6 +107,7 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg) ...@@ -105,6 +107,7 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
priv->cur_command.type = COMMAND_PTYPE_SETPOINT; priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0; priv->cmd_v = 0.0;
priv->cur_v = 0.0; priv->cur_v = 0.0;
priv->id = COMMAND_ID_NONE;
return; return;
} }
...@@ -115,12 +118,14 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg) ...@@ -115,12 +118,14 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
if (r == 0) { if (r == 0) {
priv->cur_done = false; priv->cur_done = false;
priv->cur_start = false; priv->cur_start = false;
priv->id = priv->cur_command.id;
} else { } else {
if (priv->cur_command.type == if (priv->cur_command.type ==
COMMAND_PTYPE_SETPOINT_TIME) { COMMAND_PTYPE_SETPOINT_TIME) {
priv->cmd_v = 0.0; priv->cmd_v = 0.0;
priv->cmd_x = priv->cur_command.value.f; priv->cmd_x = priv->cur_command.value.f;
} }
priv->id = COMMAND_ID_NONE;
} }
} }
if (!priv->cur_done) { if (!priv->cur_done) {
...@@ -308,6 +313,7 @@ static struct controller_block_outterm_list outterms[] = { ...@@ -308,6 +313,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x) }, { "x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x) },
{ "v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v) }, { "v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v) },
{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x) }, { "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x) },
{ "id", CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, id) },
{ NULL } { NULL }
}; };
...@@ -344,6 +350,8 @@ struct controller_block * block_setpoint_generator_1d_create(char *name, va_list ...@@ -344,6 +350,8 @@ struct controller_block * block_setpoint_generator_1d_create(char *name, va_list
spg->private->cur_x = 0.0; spg->private->cur_x = 0.0;
spg->private->cur_v = 0.0; spg->private->cur_v = 0.0;
spg->private->id = COMMAND_ID_NONE;
if (controller_block_interm_list_init(spg, interms)) if (controller_block_interm_list_init(spg, interms))
goto err_private; goto err_private;
......
...@@ -9,6 +9,7 @@ blocks { ...@@ -9,6 +9,7 @@ blocks {
{ "test_output_float", "setpoint" } { "test_output_float", "setpoint" }
{ "test_output_float", "x" } { "test_output_float", "x" }
{ "test_output_float", "v" } { "test_output_float", "v" }
{ "test_output_uint32", "id" }
{ "test_command", "command" } { "test_command", "command" }
} }
...@@ -20,6 +21,7 @@ links { ...@@ -20,6 +21,7 @@ links {
{ "spg", "x", "x", "value", true } { "spg", "x", "x", "value", true }
{ "spg", "v", "v", "value", true } { "spg", "v", "v", "value", true }
{ "spg", "setpoint", "setpoint", "value", true } { "spg", "setpoint", "setpoint", "value", true }
{ "spg", "id", "id", "value", true }
} }
set SETPOINT 4 set SETPOINT 4
...@@ -61,4 +63,8 @@ params { ...@@ -61,4 +63,8 @@ params {
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } (float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
} }
{ "id", "value", 20,
(int) { (int)0xffffffff, 123, 123, 123, 123, 123, 124, 125, 126, 200,
(int)0xffffffff, 127, 201,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff}
}
} }
...@@ -129,6 +129,7 @@ struct controller_block_private { ...@@ -129,6 +129,7 @@ struct controller_block_private {
float cur_a_out; float cur_a_out;
float cur_j_out; float cur_j_out;
uint32_t id;
struct command_entry cur_command; struct command_entry cur_command;
bool cur_done; bool cur_done;
...@@ -245,6 +246,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg) ...@@ -245,6 +246,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->start_a = 0.0; priv->start_a = 0.0;
priv->start_j = 0.0; priv->start_j = 0.0;
priv->start_t = 0; priv->start_t = 0;
priv->id = COMMAND_ID_NONE;
goto set_output; goto set_output;
} }
...@@ -256,12 +258,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg) ...@@ -256,12 +258,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
if (r == 0) { if (r == 0) {
priv->cur_done = false; priv->cur_done = false;
priv->cur_start = false; priv->cur_start = false;
priv->id = priv->cur_command.id;
} else { } else {
if (priv->cur_command.type == if (priv->cur_command.type ==
COMMAND_PTYPE_SETPOINT_TIME) { COMMAND_PTYPE_SETPOINT_TIME) {
priv->cmd_v = 0.0; priv->cmd_v = 0.0;
priv->cmd_x = priv->cur_command.value.f; priv->cmd_x = priv->cur_command.value.f;
} }
priv->id = COMMAND_ID_NONE;
} }
} }
if (!priv->cur_done) { if (!priv->cur_done) {
...@@ -937,6 +941,7 @@ static struct controller_block_outterm_list outterms[] = { ...@@ -937,6 +941,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "a", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_a_out) }, { "a", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_a_out) },
{ "j", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_j_out) }, { "j", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_j_out) },
{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x_out) }, { "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x_out) },
{ "id", CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, id) },
{ NULL } { NULL }
}; };
...@@ -999,6 +1004,7 @@ struct controller_block * block_setpoint_generator_3d_create(char *name, va_list ...@@ -999,6 +1004,7 @@ struct controller_block * block_setpoint_generator_3d_create(char *name, va_list
spg->private->cur_a_out = 0.0; spg->private->cur_a_out = 0.0;
spg->private->cur_j_out = 0.0; spg->private->cur_j_out = 0.0;
spg->private->cmd_x_out = 0.0; spg->private->cmd_x_out = 0.0;
spg->private->id = COMMAND_ID_NONE;
if (controller_block_interm_list_init(spg, interms)) if (controller_block_interm_list_init(spg, interms))
goto err_private; goto err_private;
......
...@@ -11,6 +11,7 @@ blocks { ...@@ -11,6 +11,7 @@ blocks {
{ "test_output_float", "v" } { "test_output_float", "v" }
{ "test_output_float", "a" } { "test_output_float", "a" }
{ "test_output_float", "j" } { "test_output_float", "j" }
{ "test_output_uint32", "id" }
{ "test_command", "command" } { "test_command", "command" }
} }
...@@ -24,6 +25,7 @@ links { ...@@ -24,6 +25,7 @@ links {
{ "spg", "a", "a", "value", true } { "spg", "a", "a", "value", true }
{ "spg", "j", "j", "value", true } { "spg", "j", "j", "value", true }
{ "spg", "setpoint", "setpoint", "value", true } { "spg", "setpoint", "setpoint", "value", true }
{ "spg", "id", "id", "value", true }
} }
set SETPOINT 4 set SETPOINT 4
...@@ -81,4 +83,9 @@ params { ...@@ -81,4 +83,9 @@ params {
(float) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 , (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 } 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
} }
{ "id", "value", 20,
(int) { (int)0xffffffff, 123, 123, 123, 123, 123, 124, 125, 126, 200,
(int)0xffffffff, 127, 201,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff,(int)0xffffffff}
}
} }
...@@ -80,7 +80,12 @@ SRCS += $(BLOCK_SRCS) ...@@ -80,7 +80,12 @@ SRCS += $(BLOCK_SRCS)
CTRL_TESTS += \ CTRL_TESTS += \
$(DIR)/block_gain.test.ctrl \ $(DIR)/block_gain.test.ctrl \
$(DIR)/block_limit.test.ctrl \
$(DIR)/block_matrix_2x2.test.ctrl \
$(DIR)/block_not.test.ctrl \ $(DIR)/block_not.test.ctrl \
$(DIR)/block_rangecheck.test.ctrl \
$(DIR)/block_setpoint_generator_1d.test.ctrl \ $(DIR)/block_setpoint_generator_1d.test.ctrl \
$(DIR)/block_setpoint_generator_3d.test.ctrl $(DIR)/block_setpoint_generator_3d.test.ctrl \
$(DIR)/block_servo_state.test.ctrl \
$(DIR)/block_quantize.test.ctrl
...@@ -103,8 +103,13 @@ static void param_set(struct controller_block *block, int param, va_list val) ...@@ -103,8 +103,13 @@ static void param_set(struct controller_block *block, int param, va_list val)
case COMMAND_VALUE_TYPE_FLOAT: case COMMAND_VALUE_TYPE_FLOAT:
entry->value.f = va_arg(val, double); entry->value.f = va_arg(val, double);
break; break;
case COMMAND_VALUE_TYPE_BOOL:
entry->value.b = va_arg(val, int);
break;
default: default:
log_send(LOG_T_ERROR, "not yet supported"); log_send(LOG_T_ERROR, "%s not yet supported",
enum_command_value_type2str(command->value_type));
log_server_flush();
exit(3); exit(3);
} }
......
...@@ -53,7 +53,8 @@ static void calculate_test_input_bool(struct controller_block *block) ...@@ -53,7 +53,8 @@ static void calculate_test_input_bool(struct controller_block *block)
priv->values_cur++; priv->values_cur++;
if (priv->values_cur == priv->values_nr) { if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
test_block_done(block, true); test_block_done(block, true);
} }
} }
......
...@@ -53,7 +53,8 @@ static void calculate_test_input_float(struct controller_block *block) ...@@ -53,7 +53,8 @@ static void calculate_test_input_float(struct controller_block *block)
priv->values_cur++; priv->values_cur++;
if (priv->values_cur == priv->values_nr) { if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
test_block_done(block, true); test_block_done(block, true);
} }
} }
......
/*
Copyright Jeroen Vreeken (jeroen@vreeken.net), 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
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <controller/controller_block.h>
#include <test/test_block.h>
#include <log/log.h>
/*
outputs
nr name
-----------------
| |
| 0 value |----
| |
-----------------
*/
struct controller_block_private {
uint32_t value;
uint32_t *values;
int values_nr;
int values_cur;
};
static void calculate_test_input_uint32(struct controller_block *block)
{
struct controller_block_private *priv = block->private;
if (priv->values_cur < priv->values_nr) {
priv->value = priv->values[priv->values_cur];
}
priv->values_cur++;
if (priv->values_cur == priv->values_nr ||
(priv->values_nr == 0 && priv->values_cur == 1)) {
test_block_done(block, true);
}
}
static struct controller_block_param_list params[] = {
{ "value", false },
{ NULL },
};
static void param_set(struct controller_block *block, int param, va_list val)
{
struct controller_block_private *priv = block->private;
int nr = va_arg(val, int);
int *values = va_arg(val, int *);
int i;
/* we have only one parameter... */