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
a4feee76
Commit
a4feee76
authored
Mar 05, 2015
by
Jeroen Vreeken
Browse files
Remove now obsolete dt_elevation directory.
parent
d14ba627
Changes
3
Hide whitespace changes
Inline
Side-by-side
controller/build.mk
View file @
a4feee76
...
...
@@ -13,7 +13,6 @@ $(eval $(call SUBDIR,shell))
$(eval
$(call
SUBDIR,block))
$(eval
$(call
SUBDIR,trigger))
$(eval
$(call
SUBDIR,ec))
$(eval
$(call
SUBDIR,dt_elevation))
$(eval
$(call
SUBDIR,test))
ifneq
(,$(findstring arm,$(HW)))
...
...
controller/dt_elevation/build.mk
deleted
100644 → 0
View file @
d14ba627
DT_ELEVATION_TARGETS
+=
$(LIBDIR)
/libdt_elevation.la
DT_ELEVATION_SRCS
=
\
$(DIR)
/dt_el_safety.c
DT_ELEVATION_OBJS
:=
$(DT_ELEVATION_SRCS:.c=.lo)
$(DT_ELEVATION_OBJS)
:
CFLAGS += -O3 -Wall
$(LIBDIR)/libdt_elevation.la
:
libcontroller.la liblog.la
$(LIBDIR)/
libdt_elevation.la_LDFLAGS
+=
-lcontroller
-llog
$(LIBDIR)/libdt_elevation.la
:
$(DT_ELEVATION_OBJS)
$(LIB_LINK)
CTRL_BLOCKS
+=
dt_el_safety dt_el_stoeber_l_sim dt_el_stoeber_r_sim
CTRL_BLOCK_LIBS
+=
libdt_elevation.la
TARGETS
+=
$(DT_ELEVATION_TARGETS)
CLEAN
+=
$(DT_ELEVATION_TARGETS)
$(DT_ELEVATION_OBJS)
SRCS
+=
$(DT_ELEVATION_SRCS)
controller/dt_elevation/dt_el_safety.c
deleted
100644 → 0
View file @
d14ba627
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013
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
<math.h>
#include
<controller/controller_block.h>
#include
<controller/controller_command.h>
#include
<log/log.h>
/*
inputs outputs
nr name nr name
----------------------------------
| |
---| 0 speed_in_r 0 speed_out_r |----
| |
---| 1 speed_in_l 1 speed_out_l |----
| |
---| 2 position_in_r 2 safe_out |----
| |
---| 3 position_in_l |
| |
---| 4 torsion_in |
| |
---| 5 torque_in |
| |
---| 6 torque_in_r 3 torque_out_r |----
| |
---| 7 torque_in_l 4 torque_out_l |----
| |
---| 8 safety_in_top |
| |
---| 9 safety_in_bottom |
| |
---| 10 enable 5 recover |----
| |
----------------------------------
This is not a generic block.
It implements the safety checks specific for the dt elevation
control loop.
Checks:
+ Enabled
+ Direction limit if position beyond max/min
+ Speed limit if within safe-zone.
min safe_zone_min safe_zone_max max
|------------------|----------------------------|------------------|
safe_zone_min_speed normal operation safe_zone_max_speed
*/
struct
controller_block_private
{
float
*
input_r
;
float
*
input_l
;
float
*
position_in_r
;
float
*
position_in_l
;
float
*
torsion_in
;
float
*
torque_in
;
float
*
torque_in_r
;
float
*
torque_in_l
;
bool
*
safety_in_top
;
bool
*
safety_in_bottom
;
float
speed_out_r
;
float
speed_out_l
;
bool
safe_out
;
float
torque_out_r
;
float
torque_out_l
;
bool
*
enable_in
;
float
position_min
;
float
position_max
;
float
safe_zone_min
;
float
safe_zone_max
;
float
safe_zone_min_speed
;
float
safe_zone_max_speed
;
float
torsion_max
;
float
torsion_recover_max
;
float
torque_max
;
float
emergency_torque
;
bool
recover
;
/* flags to prevent log flooding */
bool
err_max_r
;
bool
err_min_r
;
bool
err_max_l
;
bool
err_min_l
;
bool
err_torque_max
;
bool
err_torsion
;
bool
err_torsion_recover
;
bool
warn_safe_max_l
;
bool
warn_safe_min_l
;
bool
warn_safe_max_r
;
bool
warn_safe_min_r
;
struct
controller_command
*
command
;
};
static
void
dt_el_safety_calculate
(
struct
controller_block
*
safety
)
{
struct
controller_block_private
*
priv
=
safety
->
private
;
float
out0
,
out1
;
float
position_in_r
,
position_in_l
;
float
torsion_in
,
torque_in
,
torque_in_r
,
torque_in_l
;
bool
safe
;
bool
enabled
=
*
priv
->
enable_in
;
struct
controller_command_entry
c_entry
;
if
(
!
controller_command_queue_read
(
priv
->
command
,
&
c_entry
))
{
bool
recover
=
c_entry
.
value
.
b
;
if
(
recover
&&
!
priv
->
recover
)
{
log_send
(
LOG_T_WARNING
,
"%s: Switching to recover mode, beware!"
,
safety
->
name
);
}
if
(
!
recover
&&
priv
->
recover
)
{
log_send
(
LOG_T_INFO
,
"%s: Switching back to normal mode"
,
safety
->
name
);
}
priv
->
recover
=
recover
;
}
safe
=
true
;
out0
=
*
priv
->
input_r
;
out1
=
-*
priv
->
input_l
;
position_in_r
=
*
priv
->
position_in_r
;
position_in_l
=
-*
priv
->
position_in_l
;
torsion_in
=
*
priv
->
torsion_in
;
torque_in
=
*
priv
->
torque_in
;
torque_in_r
=
*
priv
->
torque_in_r
;
torque_in_l
=
*
priv
->
torque_in_l
;
/* outside maximum range and not moving back */
if
(
position_in_r
>=
priv
->
position_max
&&
out0
>
0
.
0
)
{
out0
=
0
.
0
;
safe
=
false
;
if
(
!
priv
->
err_max_r
)
{
priv
->
err_max_r
=
true
;
log_send
(
LOG_T_ERROR
,
"Elevation position right %e above maximum %e"
,
position_in_r
,
priv
->
position_max
);
}
}
else
{
priv
->
err_max_r
=
false
;
}
/* inside safe zone and not moving at a safe speed */
if
(
position_in_r
>=
priv
->
safe_zone_max
&&
out0
>
priv
->
safe_zone_max_speed
)
{
out0
=
priv
->
safe_zone_max_speed
;
if
(
!
priv
->
warn_safe_max_r
)
{
priv
->
warn_safe_max_r
=
true
;
log_send
(
LOG_T_WARNING
,
"Elevation position right %e in safe zone %e"
,
position_in_r
,
priv
->
safe_zone_max
);
}
}
else
{
priv
->
warn_safe_max_r
=
false
;
}
/* outside minimum and not moving back */
if
(
position_in_r
<=
priv
->
position_min
&&
out0
<
0
.
0
)
{
out0
=
0
.
0
;
safe
=
false
;
if
(
!
priv
->
err_min_r
)
{
priv
->
err_min_r
=
true
;
log_send
(
LOG_T_ERROR
,
"Elevation position right under minimum, %e < %e"
,
position_in_r
,
priv
->
position_min
);
}
}
else
{
priv
->
err_min_r
=
false
;
}
/* inside safe zone and not moving back */
if
(
position_in_r
<=
priv
->
safe_zone_min
&&
out0
<
priv
->
safe_zone_min_speed
)
{
out0
=
priv
->
safe_zone_min_speed
;
if
(
!
priv
->
warn_safe_min_r
)
{
priv
->
warn_safe_min_r
=
true
;
log_send
(
LOG_T_WARNING
,
"Elevation position right in safe zone"
);
}
}
else
{
priv
->
warn_safe_min_r
=
false
;
}
/* outside maximum range and not moving back */
if
(
position_in_l
>=
priv
->
position_max
&&
out1
>
0
.
0
)
{
out1
=
0
.
0
;
safe
=
false
;
if
(
!
priv
->
err_max_l
)
{
priv
->
err_max_l
=
true
;
log_send
(
LOG_T_ERROR
,
"Elevation left position %e above maximum %e"
,
position_in_l
,
priv
->
position_max
);
}
}
else
{
priv
->
err_max_l
=
false
;
}
/* inside safe zone and not moving at a safe speed */
if
(
position_in_l
>=
priv
->
safe_zone_max
&&
out1
>
priv
->
safe_zone_max_speed
)
{
out1
=
priv
->
safe_zone_max_speed
;
if
(
!
priv
->
warn_safe_max_l
)
{
priv
->
warn_safe_max_l
=
true
;
log_send
(
LOG_T_WARNING
,
"Elevation left position %e in safe zone %e"
,
position_in_l
,
priv
->
safe_zone_max
);
}
}
else
{
priv
->
warn_safe_max_l
=
false
;
}
/* outside minimum and not moving back */
if
(
position_in_l
<=
priv
->
position_min
&&
out1
<
0
.
0
)
{
out1
=
0
.
0
;
safe
=
false
;
if
(
!
priv
->
err_min_l
)
{
priv
->
err_min_l
=
true
;
log_send
(
LOG_T_ERROR
,
"Elevation left position %e under minimum %e"
,
position_in_l
,
priv
->
position_min
);
}
}
else
{
priv
->
err_min_l
=
false
;
}
/* inside safe zone and not moving back */
if
(
position_in_l
<=
priv
->
safe_zone_min
&&
out1
<
priv
->
safe_zone_min_speed
)
{
out1
=
priv
->
safe_zone_min_speed
;
if
(
!
priv
->
warn_safe_min_l
)
{
priv
->
warn_safe_min_l
=
true
;
log_send
(
LOG_T_WARNING
,
"Elevation left position in safe zone"
);
}
}
else
{
priv
->
warn_safe_min_l
=
false
;
}
if
(
fabsf
(
torsion_in
)
>=
priv
->
torsion_recover_max
)
{
if
(
!
priv
->
recover
)
{
out0
=
0
.
0
;
out1
=
0
.
0
;
}
safe
=
false
;
if
(
!
priv
->
err_torsion_recover
)
{
priv
->
err_torsion_recover
=
true
;
log_send
(
LOG_T_ERROR
,
"Elevation torsion to big (%e) to recover, check calibration"
,
torsion_in
);
}
}
else
{
priv
->
err_torsion_recover
=
false
;
}
/* torsion to big */
if
(
fabsf
(
torsion_in
)
>=
priv
->
torsion_max
)
{
/* moving back? */
if
(
!
priv
->
recover
&&
position_in_r
>
position_in_l
)
{
if
(
out0
>
0
.
0
)
{
out0
=
0
.
0
;
safe
=
false
;
}
if
(
out1
<
0
.
0
)
{
out1
=
0
.
0
;
safe
=
false
;
}
}
if
(
!
priv
->
recover
&&
position_in_r
<
position_in_l
)
{
if
(
out0
<
0
.
0
)
{
out0
=
0
.
0
;
safe
=
false
;
}
if
(
out1
>
0
.
0
)
{
out1
=
0
.
0
;
safe
=
false
;
}
}
if
(
!
priv
->
err_torsion
)
{
priv
->
err_torsion
=
true
;
log_send
(
LOG_T_ERROR
,
"Elevation torsion (%e) to big"
,
torsion_in
);
}
}
else
{
priv
->
err_torsion
=
false
;
}
/* (filtered) total torque to big (elevation stuck?) */
if
(
fabs
(
torque_in
)
>
priv
->
torque_max
)
{
if
(
!
priv
->
err_torque_max
)
{
priv
->
err_torque_max
=
true
;
log_send
(
LOG_T_ERROR
,
"Elevation torque to big. %e > %e"
,
torque_in
,
priv
->
torque_max
);
}
safe
=
false
;
}
else
{
priv
->
err_torque_max
=
false
;
}
/* top safety switch */
if
(
!*
priv
->
safety_in_top
)
{
safe
=
false
;
}
/* bottom safety switch */
if
(
!*
priv
->
safety_in_bottom
)
{
safe
=
false
;
}
if
(
!
safe
)
enabled
=
false
;
if
(
!
enabled
&&
!
priv
->
recover
)
{
out0
=
0
.
0
;
out1
=
0
.
0
;
priv
->
torque_out_r
=
priv
->
emergency_torque
;
priv
->
torque_out_l
=
priv
->
emergency_torque
;
}
else
{
priv
->
torque_out_r
=
torque_in_r
;
priv
->
torque_out_l
=
torque_in_l
;
}
priv
->
speed_out_r
=
out0
;
priv
->
speed_out_l
=
-
out1
;
priv
->
safe_out
=
safe
||
priv
->
recover
;
}
static
int
param_set_position_min
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
position_min
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_position_max
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
position_max
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_zone_min
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
safe_zone_min
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_zone_max
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
safe_zone_max
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_zone_min_speed
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
safe_zone_min_speed
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_zone_max_speed
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
safe_zone_max_speed
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_torsion_max
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
torsion_max
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_torsion_recover_max
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
torsion_recover_max
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_recover
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
bool
recover
=
va_arg
(
val
,
int
);
if
(
recover
&&
!
safety
->
private
->
recover
)
{
log_send
(
LOG_T_WARNING
,
"%s: Switching to recover mode, beware!"
,
safety
->
name
);
}
if
(
!
recover
&&
safety
->
private
->
recover
)
{
log_send
(
LOG_T_INFO
,
"%s: Switching back to normal mode"
,
safety
->
name
);
}
safety
->
private
->
recover
=
recover
;
return
0
;
}
static
int
param_set_torque_max
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
torque_max
=
va_arg
(
val
,
double
);
return
0
;
}
static
int
param_set_emergency_torque
(
struct
controller_block
*
safety
,
char
*
param
,
int
argc
,
va_list
val
)
{
safety
->
private
->
emergency_torque
=
va_arg
(
val
,
double
);
return
0
;
}
static
struct
controller_block_param_list
params
[]
=
{
{
"position_min"
,
false
,
param_set_position_min
,
.
args
=
{
"double"
,
NULL
}
},
{
"position_max"
,
false
,
param_set_position_max
,
.
args
=
{
"double"
,
NULL
}
},
{
"safe_zone_min"
,
false
,
param_set_zone_min
,
.
args
=
{
"double"
,
NULL
}
},
{
"safe_zone_max"
,
false
,
param_set_zone_max
,
.
args
=
{
"double"
,
NULL
}
},
{
"safe_zone_min_speed"
,
false
,
param_set_zone_min_speed
,
.
args
=
{
"double"
,
NULL
}
},
{
"safe_zone_max_speed"
,
false
,
param_set_zone_max_speed
,
.
args
=
{
"double"
,
NULL
}
},
{
"torsion_max"
,
false
,
param_set_torsion_max
,
.
args
=
{
"double"
,
NULL
}
},
{
"torsion_recover_max"
,
false
,
param_set_torsion_recover_max
,
.
args
=
{
"double"
,
NULL
}
},
{
"recover"
,
false
,
param_set_recover
,
.
args
=
{
"int"
,
NULL
}
},
{
"torque_max"
,
false
,
param_set_torque_max
,
.
args
=
{
"double"
,
NULL
}},
{
"emergency_torque"
,
false
,
param_set_emergency_torque
,
.
args
=
{
"double"
,
NULL
}
},
{
NULL
},
};
static
struct
controller_block_outterm_list
outterms
[]
=
{
{
"speed_out_r"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
speed_out_r
)
},
{
"speed_out_l"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
speed_out_l
)
},
{
"safe_out"
,
CONTROLLER_BLOCK_TERM_BOOL
,
offsetof
(
struct
controller_block_private
,
safe_out
)
},
{
"torque_out_r"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
torque_out_r
)
},
{
"torque_out_l"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
torque_out_l
)
},
{
"recover"
,
CONTROLLER_BLOCK_TERM_BOOL
,
offsetof
(
struct
controller_block_private
,
recover
)
},
{
NULL
}
};
static
struct
controller_block_interm_list
interms
[]
=
{
{
"speed_in_r"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input_r
)
},
{
"speed_in_l"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
input_l
)
},
{
"position_in_r"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
position_in_r
)
},
{
"position_in_l"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
position_in_l
)
},
{
"torsion_in"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
torsion_in
)
},
{
"torque_in"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
torque_in
)
},
{
"torque_in_r"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
torque_in_r
)
},
{
"torque_in_l"
,
CONTROLLER_BLOCK_TERM_FLOAT
,
offsetof
(
struct
controller_block_private
,
torque_in_l
)
},
{
"safety_in_top"
,
CONTROLLER_BLOCK_TERM_BOOL
,
offsetof
(
struct
controller_block_private
,
safety_in_top
)
},
{
"safety_in_bottom"
,
CONTROLLER_BLOCK_TERM_BOOL
,
offsetof
(
struct
controller_block_private
,
safety_in_bottom
)},
{
"enable"
,
CONTROLLER_BLOCK_TERM_BOOL
,
offsetof
(
struct
controller_block_private
,
enable_in
)
},
{
NULL
}
};
static
struct
controller_block
*
block_dt_el_safety_create
(
char
*
name
,
int
argc
,
va_list
val
)
{
struct
controller_block
*
safety
;
safety
=
controller_block_alloc
(
"dt_al_safety"
,
name
,
sizeof
(
struct
controller_block_private
));
if
(
!
safety
)
return
NULL
;
safety
->
private
->
speed_out_r
=
0
.
0
;
safety
->
private
->
speed_out_l
=
0
.
0
;
safety
->
private
->
safe_out
=
0
.
0
;
safety
->
private
->
position_min
=
0
.
0
;
safety
->
private
->
position_max
=
0
.
0
;
safety
->
private
->
safe_zone_min
=
0
.
0
;
safety
->
private
->
safe_zone_max
=
0
.
0
;
safety
->
private
->
safe_zone_min_speed
=
0
.
0
;
safety
->
private
->
safe_zone_max_speed
=
0
.
0
;
safety
->
private
->
recover
=
0
;
safety
->
private
->
torsion_max
=
0
.
0
;
safety
->
private
->
torsion_recover_max
=
0
.
0
;
safety
->
private
->
torque_max
=
0
.
0
;
safety
->
private
->
torque_out_r
=
0
.
0
;
safety
->
private
->
torque_out_l
=
0
.
0
;
safety
->
private
->
emergency_torque
=
0
.
0
;
safety
->
private
->
err_max_r
=
false
;
safety
->
private
->
err_min_r
=
false
;
safety
->
private
->
err_max_l
=
false
;
safety
->
private
->
err_min_l
=
false
;
safety
->
private
->
err_torsion
=
false
;
safety
->
private
->
warn_safe_max_r
=
false
;
safety
->
private
->
warn_safe_min_r
=
false
;
safety
->
private
->
warn_safe_max_l
=
false
;
safety
->
private
->
warn_safe_min_l
=
false
;
safety
->
private
->
err_torsion_recover
=
false
;
if
(
controller_block_interm_list_init
(
safety
,
interms
))
goto
err_private
;
if
(
controller_block_outterm_list_init
(
safety
,
outterms
))
goto
err_input
;
safety
->
calculate
=
dt_el_safety_calculate
;
if
(
controller_block_param_list_add
(
safety
,
params
))
goto
err_output
;
if
(
controller_block_add
(
safety
)
!=
0
)
goto
err_add
;
safety
->
private
->
command
=
controller_command_create
(
safety
,
name
,
"Recover"
);
safety
->
private
->
command
->
value_type
=
COMMAND_VALUE_TYPE_BOOL
;
safety
->
private
->
command
->
command_types
[
0
]
=
COMMAND_PTYPE_SETPOINT
;
return
safety
;
err_add:
err_output:
free
(
safety
->
output
);
err_input:
free
(
safety
->
input
);
err_private:
free
(
safety
->
private
);
free
(
safety
->
name
);
free
(
safety
);
return
NULL
;
}
BLOCK_CREATE
(
dt_el_safety
)
=
{
.
create
=
block_dt_el_safety_create
,
.
args
=
{
NULL
},
};
Write
Preview