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)
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) {
case SERVO_STATE_ENABLED:
priv->out_x = *priv->spg_x;
......@@ -134,18 +144,6 @@ static void servo_state_calculate(struct controller_block *servo_state)
float out_a = 0.0;
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) {
out_a = -priv->max_a;
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 {
float cur_x;
float cur_v;
uint32_t id;
struct command_entry cur_command;
bool cur_done;
bool cur_start;
......@@ -105,6 +107,7 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0;
priv->cur_v = 0.0;
priv->id = COMMAND_ID_NONE;
return;
}
......@@ -115,12 +118,14 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
if (r == 0) {
priv->cur_done = false;
priv->cur_start = false;
priv->id = priv->cur_command.id;
} else {
if (priv->cur_command.type ==
COMMAND_PTYPE_SETPOINT_TIME) {
priv->cmd_v = 0.0;
priv->cmd_x = priv->cur_command.value.f;
}
priv->id = COMMAND_ID_NONE;
}
}
if (!priv->cur_done) {
......@@ -308,6 +313,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x) },
{ "v", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v) },
{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x) },
{ "id", CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, id) },
{ NULL }
};
......@@ -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_v = 0.0;
spg->private->id = COMMAND_ID_NONE;
if (controller_block_interm_list_init(spg, interms))
goto err_private;
......
......@@ -9,6 +9,7 @@ blocks {
{ "test_output_float", "setpoint" }
{ "test_output_float", "x" }
{ "test_output_float", "v" }
{ "test_output_uint32", "id" }
{ "test_command", "command" }
}
......@@ -20,6 +21,7 @@ links {
{ "spg", "x", "x", "value", true }
{ "spg", "v", "v", "value", true }
{ "spg", "setpoint", "setpoint", "value", true }
{ "spg", "id", "id", "value", true }
}
set SETPOINT 4
......@@ -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 }
}
{ "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 {
float cur_a_out;
float cur_j_out;
uint32_t id;
struct command_entry cur_command;
bool cur_done;
......@@ -245,6 +246,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
priv->start_a = 0.0;
priv->start_j = 0.0;
priv->start_t = 0;
priv->id = COMMAND_ID_NONE;
goto set_output;
}
......@@ -256,12 +258,14 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
if (r == 0) {
priv->cur_done = false;
priv->cur_start = false;
priv->id = priv->cur_command.id;
} else {
if (priv->cur_command.type ==
COMMAND_PTYPE_SETPOINT_TIME) {
priv->cmd_v = 0.0;
priv->cmd_x = priv->cur_command.value.f;
}
priv->id = COMMAND_ID_NONE;
}
}
if (!priv->cur_done) {
......@@ -937,6 +941,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "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) },
{ "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 }
};
......@@ -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_j_out = 0.0;
spg->private->cmd_x_out = 0.0;
spg->private->id = COMMAND_ID_NONE;
if (controller_block_interm_list_init(spg, interms))
goto err_private;
......
......@@ -11,6 +11,7 @@ blocks {
{ "test_output_float", "v" }
{ "test_output_float", "a" }
{ "test_output_float", "j" }
{ "test_output_uint32", "id" }
{ "test_command", "command" }
}
......@@ -24,6 +25,7 @@ links {
{ "spg", "a", "a", "value", true }
{ "spg", "j", "j", "value", true }
{ "spg", "setpoint", "setpoint", "value", true }
{ "spg", "id", "id", "value", true }
}
set SETPOINT 4
......@@ -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 ,
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)
CTRL_TESTS += \
$(DIR)/block_gain.test.ctrl \
$(DIR)/block_limit.test.ctrl \
$(DIR)/block_matrix_2x2.test.ctrl \
$(DIR)/block_not.test.ctrl \
$(DIR)/block_rangecheck.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)
case COMMAND_VALUE_TYPE_FLOAT:
entry->value.f = va_arg(val, double);
break;
case COMMAND_VALUE_TYPE_BOOL:
entry->value.b = va_arg(val, int);
break;
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);
}
......
......@@ -53,7 +53,8 @@ static void calculate_test_input_bool(struct controller_block *block)
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);
}
}
......
......@@ -53,7 +53,8 @@ static void calculate_test_input_float(struct controller_block *block)
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);
}
}
......
/*
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... */
priv->values = calloc(nr, sizeof(uint32_t));
for (i = 0; i < nr; i++)
priv->values[i] = values[i];
priv->values_nr = nr;
log_send(LOG_T_INFO, "%s: %d test values", block->name, nr);
}
static struct controller_block_outterm_list outterms[] = {
{
.name = "value",
.type = CONTROLLER_BLOCK_TERM_UINT32,
.priv_offset = offsetof(struct controller_block_private, value)
},
{ NULL }
};
struct controller_block * block_test_input_uint32_create(char *name)
{
struct controller_block *block;
if (!(block = controller_block_alloc("test_input_uint32", name,
sizeof(struct controller_block_private))))
return NULL;
if (controller_block_outterm_list_init(block, outterms))
goto err_output;
block->calculate = calculate_test_input_uint32;
if (controller_block_param_list_init(block, params))
goto err_param;
block->param_set = param_set;
controller_block_add(block);
test_block_add(block);
return block;
err_param:
err_output:
controller_block_free(block);
return NULL;
}
......@@ -62,8 +62,9 @@ static void calculate_test_output_bool(struct controller_block *block)
}
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)) {
bool good = priv->values_ok == priv->values_cur;
test_block_done(block, good);
......
......@@ -69,7 +69,8 @@ static void calculate_test_output_float(struct controller_block *block)
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)) {
bool good = priv->values_ok == priv->values_cur;
test_block_done(block, good);
......@@ -119,7 +120,7 @@ struct controller_block * block_test_output_float_create(char *name)
if (!(block = controller_block_alloc("test_input_float", name,
sizeof(struct controller_block_private))))
return NULL;
if (controller_block_interm_list_init(block, interms))
goto err_input;
......
/*
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 <inttypes.h>
#include <controller/controller_block.h>
#include <test/test_block.h>
#include <log/log.h>
/*
inputs
nr name
-----------------
| |
----| 0 value |
| |
-----------------
*/