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
87baa8ef
Commit
87baa8ef
authored
Nov 12, 2014
by
Jeroen Vreeken
Browse files
Rename 3d order SPG and add 1st order
parent
e4612fe1
Changes
5
Hide whitespace changes
Inline
Side-by-side
controller/block/block_setpoint_generator.h
deleted
100644 → 0
View file @
e4612fe1
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2009
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2009
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/>.
*/
#ifndef _INCLUDE_BLOCK_SETPOINT_GENERATOR_
#define _INCLUDE_BLOCK_SETPOINT_GENERATOR_
#include
"controller_block.h"
struct
controller_block
*
block_setpoint_generator_create
(
char
*
name
,
va_list
ap
);
#endif
/* _INCLUDE_BLOCK_SETPOINT_GENERATOR_ */
controller/block/block_setpoint_generator_1d.c
0 → 100644
View file @
87baa8ef
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2014
Copyright Stichting C.A. Muller Radioastronomiestation, 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/>.
*/
#define _ISOC99_SOURCE
#include
<stdio.h>
#include
<stdlib.h>
#include
<string.h>
#include
<math.h>
#include
<float.h>
#include
<controller/controller_block.h>
#include
<controller/controller_command.h>
#include
<controller/controller_sample.h>
#include
<log/log.h>
/*
inputs outputs
nr name nr name
--------------------------
| |
----| 0 reset_x 0 x |----
| |
----| 1 reset 1 v |----
| |
| 2 setpoint |----
| |
--------------------------
'setpoint' is the setpoint as the user requested it.
'x' is the output of the setpoint generator.
'v', is the first order derivative of 'x'.
*/
struct
controller_block_private
{
/*
In the setpoint generator blocks the following physics notations
will be used.
x = position
v = velocity (1st derivative of x)
*/
float
*
reset_x
;
bool
*
reset
;
/* beware: 'samples' is the time unit, not 'seconds',
all parameters and commands are scaled on block entry/exit */
float
cmd_x
;
float
cmd_v
;
float
max_x
;
float
min_x
;
float
max_v
;
/* conversion factor for seconds/tick and its inverse */
float
freq
;
/* ticks per second */
/* parameters in real world format (time unit: second) */
float
max_v_sec
;
/* current internal state */
float
cur_x
;
float
cur_v
;
struct
command_entry
cur_command
;
bool
cur_done
;
bool
cur_start
;
struct
controller_command
*
command
;
};
static
void
setpoint_generator_1d_calculate
(
struct
controller_block
*
spg
)
{
struct
controller_block_private
*
priv
=
spg
->
private
;
float
cur_x
,
cur_v
;
cur_x
=
priv
->
cur_x
;
cur_v
=
priv
->
cur_v
;
if
(
*
priv
->
reset
)
{
priv
->
cmd_x
=
*
priv
->
reset_x
;
cur_x
=
priv
->
cmd_x
;
priv
->
cur_done
=
true
;
priv
->
cur_command
.
type
=
COMMAND_PTYPE_SETPOINT
;
priv
->
cmd_v
=
0
.
0
;
cur_v
=
0
.
0
;
}
if
(
priv
->
cur_done
)
{
int
r
;
r
=
controller_command_queue_read
(
priv
->
command
,
&
priv
->
cur_command
);
if
(
r
==
0
)
{
priv
->
cur_done
=
false
;
priv
->
cur_start
=
false
;
}
else
{
if
(
priv
->
cur_command
.
type
==
COMMAND_PTYPE_SETPOINT_TIME
)
{
priv
->
cmd_v
=
0
.
0
;
priv
->
cmd_x
=
priv
->
cur_command
.
value
.
f
;
}
}
}
if
(
!
priv
->
cur_done
)
{
double
t
;
switch
(
priv
->
cur_command
.
type
)
{
case
COMMAND_PTYPE_SETPOINT
:
priv
->
cmd_x
=
priv
->
cur_command
.
value
.
f
;
priv
->
cmd_v
=
0
.
0
;
priv
->
cur_done
=
true
;
break
;
case
COMMAND_PTYPE_SPEED
:
priv
->
cmd_v
=
priv
->
cur_command
.
value
.
f
;
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
);
t
=
(
double
)(
priv
->
cur_command
.
t
.
tv_sec
-
controller_time_seconds
)
*
priv
->
freq
+
t_samplenr
-
(
double
)
controller_time_samplenr
;
if
(
t
<=
0
.
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)"
,
spg
->
name
,
(
long
long
)
priv
->
cur_command
.
t
.
tv_sec
,
controller_time_seconds
);
priv
->
cmd_x
=
priv
->
cur_command
.
value
.
f
;
priv
->
cmd_v
=
0
.
0
;
priv
->
cur_done
=
true
;
}
else
{
priv
->
cmd_v
=
(
priv
->
cur_command
.
value
.
f
-
priv
->
cmd_x
)
/
(
t
+
1
);
priv
->
cur_start
=
true
;
}
}
if
(
priv
->
cur_command
.
t
.
tv_sec
<=
controller_time_seconds
)
{
priv
->
cur_done
=
true
;
priv
->
cmd_x
=
priv
->
cur_command
.
value
.
f
-
priv
->
cmd_v
;
}
break
;
default:
break
;
}
}
if
(
fabs
(
priv
->
cmd_v
)
>=
priv
->
max_v
)
{
priv
->
cmd_v
=
copysign
(
priv
->
max_v
,
priv
->
cmd_v
);
}
if
(
priv
->
cur_command
.
type
==
COMMAND_PTYPE_SPEED
)
{
priv
->
cmd_x
=
cur_x
+
priv
->
cmd_v
*
controller_sample_period
();
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
;
cur_x
=
priv
->
cmd_x
;
cur_v
=
priv
->
cmd_v
;
}
else
{
float
x_diff
=
priv
->
cmd_x
-
cur_x
;
if
(
fabs
(
x_diff
)
<=
priv
->
max_v
)
{
cur_x
=
priv
->
cmd_x
;
cur_v
=
0
;
}
else
{
float
v
=
copysign
(
priv
->
max_v
,
x_diff
);
cur_x
+=
v
;
cur_v
=
v
;
}
}
priv
->
cur_x
=
cur_x
;
priv
->
cur_v
=
cur_v
*
priv
->
freq
;
}
static
int
block_setpoint_generator_command_filter
(
struct
controller_command
*
command
,
struct
command_entry
*
entry
)
{
struct
controller_block_private
*
priv
=
command
->
block
->
private
;
int
r
=
0
;
switch
(
entry
->
type
)
{
case
COMMAND_PTYPE_SETPOINT_TIME
:
case
COMMAND_PTYPE_SETPOINT
:
{
float
value
=
entry
->
value
.
f
;
if
(
value
>
priv
->
max_x
)
value
=
priv
->
max_x
;
if
(
value
<
priv
->
min_x
)
value
=
priv
->
min_x
;
entry
->
value
.
f
=
value
;
break
;
}
case
COMMAND_PTYPE_SPEED
:
{
float
value
=
entry
->
value
.
f
;
if
(
fabs
(
value
)
>
priv
->
max_v_sec
)
value
=
copysign
(
priv
->
max_v_sec
,
value
);
entry
->
value
.
f
=
value
;
break
;
}
default:
r
=
-
1
;
break
;
}
return
r
;
}
static
struct
controller_block_param_list
params
[]
=
{
{
"setpoint"
,
true
},
{
"max_x"
,
true
},
{
"min_x"
,
true
},
{
"max_v"
,
true
},
{
NULL
},
};
static
void
param_get
(
struct
controller_block
*
spg
,
int
param
,
void
*
val
)
{
switch
(
param
)
{
case
0
:
*
(
float
*
)
val
=
spg
->
private
->
cmd_x
;
break
;
case
1
:
*
(
float
*
)
val
=
spg
->
private
->
max_x
;
break
;
case
2
:
*
(
float
*
)
val
=
spg
->
private
->
min_x
;
break
;
case
3
:
*
(
float
*
)
val
=
spg
->
private
->
max_v_sec
;
break
;
}
}
static
void
param_set
(
struct
controller_block
*
spg
,
int
param
,
va_list
val
)
{
switch
(
param
)
{
case
0
:
spg
->
private
->
cmd_x
=
va_arg
(
val
,
double
);
spg
->
private
->
cur_x
=
spg
->
private
->
cmd_x
;
spg
->
private
->
cur_done
=
true
;
spg
->
private
->
cmd_v
=
0
.
0
;
spg
->
private
->
cur_v
=
0
.
0
;
break
;
case
1
:
spg
->
private
->
max_x
=
va_arg
(
val
,
double
);
break
;
case
2
:
spg
->
private
->
min_x
=
va_arg
(
val
,
double
);
break
;
case
3
:
spg
->
private
->
max_v_sec
=
va_arg
(
val
,
double
);
break
;
}
/* Scale all settings to sample time unit */
spg
->
private
->
max_v
=
spg
->
private
->
max_v_sec
*
controller_sample_period
();
spg
->
private
->
freq
=
1
.
0
/
controller_sample_period
();
}
static
struct
controller_block_interm_list
interms
[]
=
{
{
"reset_x"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
reset_x
)
},
{
"reset"
,
CONTROLLER_BLOCK_TERM_BOOL
,
offsetof
(
struct
controller_block_private
,
reset
)
},
{
NULL
}
};
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
)
},
{
NULL
}
};
struct
controller_block
*
block_setpoint_generator_1d_create
(
char
*
name
,
va_list
ap
)
{
struct
controller_block
*
spg
;
char
*
server_name
;
char
*
spg_unit
;
server_name
=
va_arg
(
ap
,
char
*
);
spg_unit
=
va_arg
(
ap
,
char
*
);
spg
=
malloc
(
sizeof
(
struct
controller_block
));
if
(
!
spg
)
return
NULL
;
spg
->
type
=
"setpoint_generator_1d"
;
spg
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
spg
->
name
)
goto
err_spg
;
strcpy
(
spg
->
name
,
name
);
spg
->
private
=
malloc
(
sizeof
(
struct
controller_block_private
));
if
(
!
spg
->
private
)
goto
err_name
;
spg
->
private
->
cmd_x
=
0
.
0
;
spg
->
private
->
cmd_v
=
0
.
0
;
spg
->
private
->
max_x
=
0
.
0
;
spg
->
private
->
min_x
=
0
.
0
;
spg
->
private
->
max_v
=
0
.
0
;
spg
->
private
->
max_v_sec
=
0
.
0
;
spg
->
private
->
freq
=
1
.
0
;
spg
->
private
->
cur_x
=
0
.
0
;
spg
->
private
->
cur_v
=
0
.
0
;
if
(
controller_block_interm_list_init
(
spg
,
interms
))
goto
err_private
;
if
(
controller_block_outterm_list_init
(
spg
,
outterms
))
goto
err_input
;
spg
->
calculate
=
setpoint_generator_1d_calculate
;
if
(
controller_block_param_list_init
(
spg
,
params
))
goto
err_output
;
spg
->
param_get
=
param_get
;
spg
->
param_set
=
param_set
;
spg
->
private
->
cur_done
=
true
;
spg
->
private
->
cur_start
=
false
;
controller_block_add
(
spg
);
spg
->
private
->
command
=
controller_command_create
(
spg
,
server_name
,
spg_unit
);
spg
->
private
->
command
->
value_type
=
COMMAND_VALUE_TYPE_FLOAT
;
spg
->
private
->
command
->
command_types
[
0
]
=
COMMAND_PTYPE_SETPOINT
;
spg
->
private
->
command
->
command_types
[
1
]
=
COMMAND_PTYPE_SPEED
;
spg
->
private
->
command
->
command_types
[
2
]
=
COMMAND_PTYPE_SETPOINT_TIME
;
spg
->
private
->
command
->
filter
=
block_setpoint_generator_command_filter
;
return
spg
;
err_output:
free
(
spg
->
output
);
err_input:
free
(
spg
->
input
);
err_private:
free
(
spg
->
private
);
err_name:
free
(
spg
->
name
);
err_spg:
free
(
spg
);
return
NULL
;
}
controller/block/block_setpoint_generator.c
→
controller/block/block_setpoint_generator
_3d
.c
View file @
87baa8ef
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013
, 2014
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013
This program is free software: you can redistribute it and/or modify
...
...
@@ -215,7 +215,7 @@ static double x_after_ticks(struct controller_block_private *priv,
return
x_at_t
;
}
static
void
setpoint_generator_calculate
(
struct
controller_block
*
spg
)
static
void
setpoint_generator_
3d_
calculate
(
struct
controller_block
*
spg
)
{
struct
controller_block_private
*
priv
=
spg
->
private
;
double
cur_x
,
cur_v
;
...
...
@@ -943,7 +943,7 @@ static struct controller_block_outterm_list outterms[] = {
};
struct
controller_block
*
block_setpoint_generator_create
(
char
*
name
,
va_list
ap
)
struct
controller_block
*
block_setpoint_generator_
3d_
create
(
char
*
name
,
va_list
ap
)
{
struct
controller_block
*
spg
;
char
*
server_name
;
...
...
@@ -956,7 +956,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
if
(
!
spg
)
return
NULL
;
spg
->
type
=
"setpoint_generator"
;
spg
->
type
=
"setpoint_generator
_3d
"
;
spg
->
name
=
malloc
(
strlen
(
name
)
+
1
);
if
(
!
spg
->
name
)
goto
err_spg
;
...
...
@@ -1009,7 +1009,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
if
(
controller_block_outterm_list_init
(
spg
,
outterms
))
goto
err_input
;
spg
->
calculate
=
setpoint_generator_calculate
;
spg
->
calculate
=
setpoint_generator_
3d_
calculate
;
if
(
controller_block_param_list_init
(
spg
,
params
))
goto
err_output
;
...
...
controller/block/build.mk
View file @
87baa8ef
...
...
@@ -27,7 +27,8 @@ BLOCKS := \
quantize
\
random
\
rangecheck
\
setpoint_generator
\
setpoint_generator_1d
\
setpoint_generator_3d
\
servo_state
\
sine
\
state_machine
\
...
...
controller/dt_ctrl.ctrl
View file @
87baa8ef
...
...
@@ -28,52 +28,52 @@ import "dt_ctrl_az_sim.ctrl"
import "dt_ctrl_ec_sim.ctrl"
blocks {
{ "setpoint_generator", "azimuth_spg",
"Azimuth_Setpoint", "rad" }
{ "servo_state", "azimuth_servo_state"
}
{ "and2", "azimuth_safe_and"
}
{ "subtract", "azimuth_setpoint_error" }
{ "subtract",
"azimuth_error"
}
{ "pid_aw", "azimuth_pid"
}
{ "filter_iir", "azimuth_pid_filter" }
{ "limit_var", "azimuth_pid_limit" }
{ "add", "azimuth_speed_ff" }
{ "limit", "azimuth_speed_limit"
}
{ "dt_az_safety", "azimuth_safety" }
{ "gain", "azimuth_speed_servo"
}
{ "value", "azimuth_torque" }
{ "value", "azimuth_position_offset" }
{ "add", "azimuth_position_offset_sum"
}
{ "gain", "azimuth_position_gain"
}
{ "rangecheck", "azimuth_speed_warning"
}
{ "setpoint_generator
_3d
", "azimuth_spg", "Azimuth_Setpoint", "rad" }
{ "servo_state",
"azimuth_servo_state" }
{ "and2",
"azimuth_safe_and" }
{ "subtract",
"azimuth_setpoint_error" }
{ "subtract",
"azimuth_error" }
{ "pid_aw",
"azimuth_pid" }
{ "filter_iir",
"azimuth_pid_filter" }
{ "limit_var",
"azimuth_pid_limit" }
{ "add",
"azimuth_speed_ff" }
{ "limit",
"azimuth_speed_limit" }
{ "dt_az_safety",
"azimuth_safety" }
{ "gain",
"azimuth_speed_servo" }
{ "value",
"azimuth_torque" }
{ "value",
"azimuth_position_offset" }
{ "add",
"azimuth_position_offset_sum" }
{ "gain",
"azimuth_position_gain" }
{ "rangecheck",
"azimuth_speed_warning" }
{ "matrix_2x2", "elevation_input_matrix" }
{ "setpoint_generator", "elevation_spg", "Elevation_Setpoint", "rad" }
{ "servo_state", "elevation_servo_state" }
{ "and2", "elevation_safe_and" }
{ "setpoint_generator", "elevation_torsion_spg", "Elevation_Torsion_Setpoint", "rad" }
{ "subtract", "elevation_setpoint_error" }
{ "subtract", "elevation_error" }
{ "subtract", "elevation_torsion_error" }
{ "subtract", "elevation_torsion_setpoint_error" }
{ "subtract", "elevation_torsion_torque" }
{ "filter_lp", "elevation_torsion_torque_lp"
}
{ "pid_aw", "elevation_pid"
}
{ "pid_aw", "elevation_torsion_pid"
}
{ "add", "elevation_speed_ff" }
{ "switch", "elevation_position_switch" }
{ "limit", "elevation_torsion_speed_limit"
}
{ "limit", "elevation_speed_limit"
}
{ "limit_2nd", "elevation_jerk_limit"
}
{ "dt_el_safety", "elevation_safety" }
{ "matrix_2x2", "elevation_output_matrix" }
{ "value", "elevation_torque_r" }
{ "value", "elevation_torque_l" }
{ "value", "elevation_position_offset_r" }
{ "value", "elevation_position_offset_l"
}
{ "add", "elevation_position_offset_r_sum" }
{ "add", "elevation_position_offset_l_sum" }
{ "matrix_2x2",
"elevation_input_matrix"
}
{ "setpoint_generator
_3d
", "elevation_spg", "Elevation_Setpoint", "rad" }
{ "servo_state",
"elevation_servo_state" }
{ "and2",
"elevation_safe_and" }
{ "setpoint_generator
_1d
", "elevation_torsion_spg", "Elevation_Torsion_Setpoint", "rad" }
{ "subtract",
"elevation_setpoint_error"
}
{ "subtract",
"elevation_error"
}
{ "subtract",
"elevation_torsion_error"
}
{ "subtract",
"elevation_torsion_setpoint_error" }
{ "subtract",
"elevation_torsion_torque"
}
{ "filter_lp",
"elevation_torsion_torque_lp"
}
{ "pid_aw",
"elevation_pid"
}
{ "pid_aw",
"elevation_torsion_pid"
}
{ "add",
"elevation_speed_ff"
}
{ "switch",
"elevation_position_switch" }
{ "limit",
"elevation_torsion_speed_limit"
}
{ "limit",
"elevation_speed_limit"
}
{ "limit_2nd",
"elevation_jerk_limit"
}
{ "dt_el_safety",
"elevation_safety"
}
{ "matrix_2x2",
"elevation_output_matrix"
}
{ "value",
"elevation_torque_r"
}
{ "value",
"elevation_torque_l"
}
{ "value",
"elevation_position_offset_r" }
{ "value",
"elevation_position_offset_l"
}
{ "add",
"elevation_position_offset_r_sum" }
{ "add",
"elevation_position_offset_l_sum" }
{ "value_bool", "false" }
{ "value_bool",
"false" }
}
links {
...
...
@@ -325,15 +325,9 @@ params {
{ "elevation_servo_state", "max_v", (float) rpm2rads(2100.0)/$(elevation_gear) }
{ "elevation_servo_state", "max_a", (float) 0.0003 }
{ "elevation_torsion_spg", "setpoint", (float) 0.0 }
{ "elevation_torsion_spg", "t", (float) 0.004 }
{ "elevation_torsion_spg", "max_x", (float) 0.00 }
{ "elevation_torsion_spg", "min_x", (float)-0.00 }
{ "elevation_torsion_spg", "max_v", (float) rpm2rads(0.001) }
{ "elevation_torsion_spg", "max_a", (float) 0.00004 }
{ "elevation_torsion_spg", "max_j", (float) 0.00001 }
{ "elevation_torsion_spg", "precision_x", (float) 0.000001 }
{ "elevation_torsion_spg", "precision_a", (float) 0.000001 }
{ "elevation_torsion_spg", "precision_v", (float) 0.000001 }
{ "elevation_pid", "kp", (float) 0.80 }
{ "elevation_pid", "ki", (float) 0.001 }
{ "elevation_pid", "kd", (float) 0.001 }
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment