Skip to content

GitLab

Commit a4feee76 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Remove now obsolete dt_elevation directory.

parent d14ba627
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 },
};