Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Tammo Jan Dijkema
dt_ctrl
Commits
990cb146
Commit
990cb146
authored
Nov 26, 2014
by
Jeroen Vreeken
Browse files
Add id to setpoint generators
Lots of test updates
parent
4d0ee609
Changes
20
Show whitespace changes
Inline
Side-by-side
controller/block/block_limit.test.ctrl
0 → 100644
View file @
990cb146
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 }
}
}
controller/block/block_matrix_2x2.test.ctrl
0 → 100644
View file @
990cb146
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 }
}
}
controller/block/block_quantize.test.ctrl
0 → 100644
View file @
990cb146
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 }
}
}
controller/block/block_rangecheck.test.ctrl
0 → 100644
View file @
990cb146
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 }
}
}
controller/block/block_servo_state.c
View file @
990cb146
...
...
@@ -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
;
...
...
controller/block/block_servo_state.test.ctrl
0 → 100644
View file @
990cb146
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 } }
}
controller/block/block_setpoint_generator_1d.c
View file @
990cb146
...
...
@@ -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
}
};
...
...
@@ -345,6 +351,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
;
...
...
controller/block/block_setpoint_generator_1d.test.ctrl
View file @
990cb146
...
...
@@ -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}
}
}
controller/block/block_setpoint_generator_3d.c
View file @
990cb146
...
...
@@ -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
;
...
...
controller/block/block_setpoint_generator_3d.test.ctrl
View file @
990cb146
...
...
@@ -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}
}
}
controller/block/build.mk
View file @
990cb146
...
...
@@ -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
controller/test/block_test_command.c
View file @
990cb146
...
...
@@ -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
);
}
...
...
controller/test/block_test_input_bool.c
View file @
990cb146
...
...
@@ -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
);
}
}
...
...
controller/test/block_test_input_float.c
View file @
990cb146
...
...
@@ -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
);
}
}
...
...
controller/test/block_test_input_uint32.c
0 → 100644
View file @
990cb146
/*
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
;
}
controller/test/block_test_output_bool.c
View file @
990cb146
...
...
@@ -63,7 +63,8 @@ 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
);
...
...
controller/test/block_test_output_float.c
View file @
990cb146
...
...
@@ -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
);
...
...
controller/test/block_test_output_uint32.c
0 → 100644
View file @
990cb146
/*
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 |
| |
-----------------
*/
struct
controller_block_private
{
uint32_t
*
value
;
uint32_t
*
values
;
int
values_nr
;
int
values_cur
;