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

Finish moving all safety related stuff to network.

Remove azimuth safety block.
Remove now useless dt_azimuth directory.
parent c3abedd9
......@@ -424,7 +424,7 @@ var focusbox = new dt_ui_window_trace("focusbox", "Focusbox", [
var weather_url = "status.cgi";
var azimuth_enable_command = new controller_command("azimuth_servo_state", "bool");
var azimuth_recover_command = new controller_command("azimuth_safety", "bool");
var azimuth_recover_command = new controller_command("azimuth_recover", "bool");
var elevation_enable_command = new controller_command("elevation_servo_state", "bool");
var elevation_recover_command = new controller_command("elevation_safety", "bool");
......
......@@ -35,6 +35,8 @@
| |
---| 2 enable_neg |
| |
---| 3 enable |
| |
----------------------
*/
......@@ -43,6 +45,7 @@ struct controller_block_private {
float *in;
bool *enable_pos;
bool *enable_neg;
bool *enable;
float out;
};
......@@ -53,11 +56,14 @@ static void limit_switch_calculate(struct controller_block *limit)
float in = *priv->in;
bool pos = *priv->enable_pos;
bool neg = *priv->enable_neg;
bool en = *priv->enable;
out = 0.0;
if (in > 0.0 && pos)
out = in;
if (in < 0.0 && neg)
en |= (in > 0.0) & pos;
en |= (in < 0.0) & neg;
if (en)
out = in;
priv->out = out;
......@@ -67,6 +73,7 @@ static struct controller_block_interm_list interms[] = {
{ "in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, in) },
{ "enable_pos", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_pos) },
{ "enable_neg", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_neg) },
{ "enable", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable) },
{ NULL }
};
......
......@@ -9,6 +9,7 @@ blocks (10.0, 0.0) {
{ "test_input_float", "in" }
{ "test_input_bool", "enable_pos" }
{ "test_input_bool", "enable_neg" }
{ "test_input_bool", "enable" }
{ "test_output_float", "out" }
}
......@@ -17,37 +18,68 @@ links {
{ "in", "value", "limit", "in", true }
{ "enable_pos", "value", "limit", "enable_pos", true }
{ "enable_neg", "value", "limit", "enable_neg", true }
{ "enable", "value", "limit", "enable", true }
{ "limit", "out", "out", "value", true }
}
params {
{ "in", "value", 20, (float) {
{ "in", "value", 40, (float) {
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0 }
}
{ "enable_pos", "value", 20, (int) {
{ "enable_pos", "value", 40, (int) {
false, false, false, false, false,
false, false, false, false, false,
true, true, true, true, true,
true, true, true, true, true,
false, false, false, false, false,
false, false, false, false, false,
true, true, true, true, true,
true, true, true, true, true }
}
{ "enable_neg", "value", 20, (int) {
{ "enable_neg", "value", 40, (int) {
false, false, false, false, false,
true, true, true, true, true,
false, false, false, false, false,
true, true, true, true, true,
false, false, false, false, false,
true, true, true, true, true,
false, false, false, false, false,
true, true, true, true, true }
}
{ "enable", "value", 40, (int) {
false, false, false, false, false,
false, false, false, false, false,
false, false, false, false, false,
false, false, false, false, false,
true, true, true, true, true,
true, true, true, true, true,
true, true, true, true, true,
true, true, true, true, true }
}
{ "out", "value", 20,
{ "out", "value", 40,
(float) {
0.0, 0.0, 0.0, 0.0, 0.0,
0.0, -1.0, -20000.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0,
0.0, -1.0, -20000.0, 1.0, 20000.0
},
(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,
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,
......
......@@ -13,7 +13,6 @@ $(eval $(call SUBDIR,shell))
$(eval $(call SUBDIR,block))
$(eval $(call SUBDIR,trigger))
$(eval $(call SUBDIR,ec))
$(eval $(call SUBDIR,dt_azimuth))
$(eval $(call SUBDIR,dt_elevation))
$(eval $(call SUBDIR,test))
......
DT_AZIMUTH_TARGETS += $(LIBDIR)/libdt_azimuth.la
DT_AZIMUTH_SRCS = \
$(DIR)/dt_az_safety.c
DT_AZIMUTH_OBJS := $(DT_AZIMUTH_SRCS:.c=.lo)
$(DT_AZIMUTH_OBJS): CFLAGS += -O3 -Wall
$(LIBDIR)/libdt_azimuth.la: libcontroller.la liblog.la
$(LIBDIR)/libdt_azimuth.la_LDFLAGS += -lcontroller -llog
$(LIBDIR)/libdt_azimuth.la: $(DT_AZIMUTH_OBJS)
$(LIB_LINK)
CTRL_BLOCKS += dt_az_safety
CTRL_BLOCK_LIBS += libdt_azimuth.la
TARGETS += $(DT_AZIMUTH_TARGETS)
CLEAN += $(DT_AZIMUTH_TARGETS) $(DT_AZIMUTH_OBJS)
SRCS += $(DT_AZIMUTH_SRCS)
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008
Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008
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 <controller/controller_command.h>
#include <log/log.h>
/*
inputs outputs
nr name nr name
---------------------------------
| |
---| 0 speed_in 0 speed_out |----
| |
---| 1 position_in 1 safe_out |----
| |
---| 2 torque_in 2 torque_out |----
| |
---| 3 safety_in_positive |
| |
---| 4 safety_in_negative |
| |
---| 5 enable |
| |
---------------------------------
This is not a generic block.
It implements the safety checks specific for the dt azimuth
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 *speed_in;
float *position_in;
float *torque_in;
bool *safety_in_positive;
bool *safety_in_negative;
float speed_out;
bool safe_out;
float torque_out;
bool *enable_in;
float position_min;
float position_max;
float emergency_torque;
float safe_zone_min;
float safe_zone_max;
float safe_zone_min_speed;
float safe_zone_max_speed;
/* keep log state to prevent flooding */
bool warn_safe_max;
bool warn_safe_min;
bool recover;
struct controller_command *command;
};
static void dt_az_safety_calculate(struct controller_block *safety)
{
struct controller_block_private *priv = safety->private;
float out;
bool safe = true;
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;
}
out = *priv->speed_in;
if (*priv->position_in >= priv->position_max &&
out > 0.0) {
out = 0.0;
safe = false;
}
if (*priv->position_in >= priv->safe_zone_max &&
out > priv->safe_zone_max_speed) {
out = priv->safe_zone_max_speed;
if (!priv->warn_safe_max) {
priv->warn_safe_max = true;
log_send(LOG_T_WARNING, "Azimuth position in safe zone");
}
} else {
priv->warn_safe_max = false;
}
if (*priv->position_in <= priv->position_min &&
out < 0.0) {
out = 0.0;
safe = false;
}
if (*priv->position_in <= priv->safe_zone_min &&
out < priv->safe_zone_min_speed) {
out = priv->safe_zone_min_speed;
if (!priv->warn_safe_min) {
priv->warn_safe_min = true;
log_send(LOG_T_ERROR, "Azimuth position in safe zone");
}
} else {
priv->warn_safe_min = false;
}
if (*priv->safety_in_positive == 0) {
safe = false;
}
if (*priv->safety_in_negative == 0) {
safe = false;
}
if (!safe) {
enabled = false;
}
if (!enabled && !priv->recover) {
out = 0.0;
priv->torque_out = priv->emergency_torque;
} else {
priv->torque_out = *priv->torque_in;
}
priv->safe_out = safe || priv->recover;
priv->speed_out = out;
}
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_emergency_torque(struct controller_block *safety,
char *param, int argc, va_list val)
{
safety->private->emergency_torque = 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 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 } },
{ "emergency_torque", false, param_set_emergency_torque, .args = { "double", NULL } },
{ "recover", false, param_set_recover, .args = { "int", NULL } },
{ NULL },
};
static struct controller_block_interm_list interms[] = {
{ "speed_in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_in) },
{ "position_in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, position_in) },
{ "torque_in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_in) },
{ "safety_in_positive", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_positive) },
{ "safety_in_negative", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safety_in_negative) },
{ "enable", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_in) },
{ NULL }
};
static struct controller_block_outterm_list outterms[] = {
{ "speed_out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_out) },
{ "safe_out", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, safe_out) },
{ "torque_out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, torque_out) },
{ "recover", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, recover) },
{ NULL }
};
static struct controller_block * block_dt_az_safety_create(char *name, int argc, va_list val)
{
struct controller_block *safety;
safety = controller_block_alloc("dt_az_safety", name,
sizeof(struct controller_block_private));
if (!safety)
return NULL;
safety->private->speed_out = 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->emergency_torque = 0.0;
safety->private->warn_safe_max = false;
safety->private->warn_safe_min = false;
safety->private->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_az_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_az_safety) = {
.create = block_dt_az_safety_create,
.args = { NULL },
};
......@@ -43,7 +43,7 @@ blocks ($(frequency), $(delay)) {
{ "limit_var", "azimuth_pid_limit" }
{ "add", "azimuth_speed_ff" }
{ "limit", "azimuth_speed_limit" }
{ "dt_az_safety", "azimuth_safety" }
{ "limit_switch", "azimuth_range_limit" }
{ "gain", "azimuth_speed_servo" }
{ "value", "azimuth_torque" }
{ "value", "azimuth_position_offset" }
......@@ -56,7 +56,10 @@ blocks ($(frequency), $(delay)) {
{ "rangecheck", "azimuth_speed_range_negative" }
{ "rangecheck", "azimuth_speed_range_positive" }
{ "or2", "azimuth_position_range" }
{ "command_bool", "azimuth_recover" }
{ "and2", "azimuth_position_safe" }
{ "log", "azimuth_position_range_log" }
{ "log", "azimuth_position_safe_log" }
{ "matrix_2x2", "elevation_input_matrix" }
{ "setpoint_generator_3d", "elevation_spg", "Elevation_Setpoint", "rad" }
......@@ -113,10 +116,11 @@ links {
{ "azimuth_spg", "x", "azimuth_servo_state", "spg_x" , true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg_v" , true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg_a" , true }
{ "azimuth_safety", "safe_out", "azimuth_safe_and", "a" , false }
{ "azimuth_position_range", "q", "azimuth_safe_and", "a" , true }
{ "ethercat", "operational","azimuth_safe_and", "b" , true }
{ "azimuth_safe_and", "q", "azimuth_servo_state", "safe" , true }
{ "azimuth_safety", "recover", "azimuth_servo_state", "override" , false }
{ "azimuth_recover", "value", "azimuth_servo_state", "override" , true }
{ "azimuth_recover", "value", "azimuth_range_limit", "enable", true }
{ "azimuth_servo_state", "out_x", "azimuth_error", "positive" , true }
{ $<Azimuth_Position>, "azimuth_setpoint_error", "negative" , true }
{ "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true }
......@@ -128,15 +132,10 @@ links {
{ "azimuth_pid_limit", "out", "azimuth_pid_filter", "in" , true }
{ "azimuth_pid_filter", "out", "azimuth_speed_ff", "in1" , true }
{ "azimuth_speed_ff", "out", "azimuth_speed_limit", "in" , true }
{ "azimuth_speed_limit", "out", "azimuth_safety", "speed_in" , true }
{ $<Azimuth_Position>, "azimuth_safety", "position_in" , true }
{ "azimuth_torque", "value", "azimuth_safety", "torque_in" , true }
{ $<Azimuth_Drive_Safety_p270>, "azimuth_safety", "safety_in_positive", true }
{ $<Azimuth_Drive_Safety_m270>, "azimuth_safety", "safety_in_negative", true }
{ "azimuth_servo_state", "enable", "azimuth_safety", "enable" , true }
{ "azimuth_safety", "speed_out", "azimuth_speed_servo", "in" , true }
{ "azimuth_speed_limit", "out", "azimuth_range_limit", "in" , true }
{ "azimuth_range_limit", "out", "azimuth_speed_servo", "in" , true }
{ "azimuth_speed_servo", "out", "dt_az", "speed" , true }
{ "azimuth_safety", "torque_out", "dt_az", "torque" , true }
{ "azimuth_torque", "value", "dt_az", "torque" , true }
{ "azimuth_servo_state", "enable", "dt_az", "enable" , true }
{ "dt_az", "speed", "azimuth_speed_warning", "in" , true }
{ "azimuth_speed_warning", "invalid", "dt_az", "ba1" , true }
......@@ -144,11 +143,16 @@ links {
{ $<Azimuth_Drive_Safety_p270>, "azimuth_safety_hw_pos", "condition", true }
{ $<Azimuth_Drive_Safety_m270>, "azimuth_safety_hw_neg", "condition", true }
{ "azimuth_speed_limit", "out", "azimuth_speed_range_negative", "in", true }
{ "azimuth_speed_limit", "out", "azimuth_speed_range_positive", "in", true }
{ $<Azimuth_Position>, "azimuth_speed_range_negative", "in", true }
{ $<Azimuth_Position>, "azimuth_speed_range_positive", "in", true }
{ "azimuth_speed_range_positive", "valid", "azimuth_position_range", "a", true }
{ "azimuth_speed_range_negative", "valid", "azimuth_position_range", "b", true }
{ "azimuth_position_range", "q", "azimuth_position_range_log", "condition", true }
{ "azimuth_speed_range_positive", "valid", "azimuth_position_safe", "a", true }
{ "azimuth_speed_range_negative", "valid", "azimuth_position_safe", "b", true }
{ "azimuth_position_safe", "q", "azimuth_position_safe_log", "condition", true }
{ "azimuth_speed_range_positive", "valid", "azimuth_range_limit", "enable_pos", true }
{ "azimuth_speed_range_negative", "valid", "azimuth_range_limit", "enable_neg", true }
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
{ $<Elevation_Position>, "elevation_spg", "reset_x" , true }
......@@ -233,7 +237,7 @@ traces {
{ "Azimuth_Position", "rad", $<Azimuth_Position> }
{ "Azimuth_Speed", "rad/s", "dt_az", "speed" }
{ "Azimuth_Torque", "Nm", "dt_az", "torque" }
{ "Azimuth_Safe", "Boolean", "azimuth_safety", "safe_out" }
{ "Azimuth_Safe", "Boolean", "azimuth_position_range", "q" }
{ "Azimuth_Enabled", "Boolean", "dt_az", "enabled" }
{ "Azimuth_Drive_Safety", "Boolean", "dt_az", "be1" }
{ "Azimuth_Drive_Safety_p270", "Boolean", $<Azimuth_Drive_Safety_p270> }
......@@ -326,20 +330,6 @@ params {
# callibration value for position
{ "azimuth_position_offset","value", (float) 0.0 }
# positions were we go into safe behavior
# these must be outside the normal operating range
{ "azimuth_safety", "position_min", (float) deg2rad(-290.0) }
{ "azimuth_safety", "position_max", (float) deg2rad(290.0) }
# 'safe zone' is between the operating range and the absolute safety
# value above
{ "azimuth_safety", "safe_zone_min", (float) deg2rad(-280.0) }
{ "azimuth_safety", "safe_zone_max", (float) deg2rad(280.0) }
{ "azimuth_safety", "safe_zone_min_speed", (float) rpm2rads(-0.001) }
{ "azimuth_safety", "safe_zone_max_speed", (float) rpm2rads(0.001) }
{ "azimuth_safety", "emergency_torque", (float) 3.0 }
# we do not start in recovery mode
{ "azimuth_safety", "recover", (int) 0 }
{ "azimuth_safety_hw_pos", "msg_up", 2, "Azimuth HW positive safety switch is closed" }
{ "azimuth_safety_hw_pos", "msg_down", 0, "Azimuth HW positive safety switch is open" }
{ "azimuth_safety_hw_pos", "init", true }
......@@ -347,14 +337,22 @@ params {
{ "azimuth_safety_hw_neg", "msg_down", 0, "Azimuth HW negative safety switch is open" }
{ "azimuth_safety_hw_neg", "init", true }
# positions were we go into safe behavior
# these must be outside the normal operating range
# 'safe zone' is between the operating range and the absolute safety
# value above
{ "azimuth_speed_range_negative", "max", deg2rad(290.0) }
{ "azimuth_speed_range_negative", "min", deg2rad(-280.0) }
{ "azimuth_speed_range_negative", "max", deg2rad(-290.0) }
{ "azimuth_speed_range_negative", "min", deg2rad(280.0) }
{ "azimuth_speed_range_positive", "max", deg2rad(280.0) }
{ "azimuth_speed_range_positive", "min", deg2rad(-290.0) }
{ "azimuth_position_range_log", "msg_up", 1, "Azimuth position inside allowed range" }
{ "azimuth_position_range_log", "msg_down", 0, "Azimuth position outside allowed range" }
{ "azimuth_position_range_log", "init", true }
{ "azimuth_position_safe_log", "msg_up", 1, "Azimuth position left safe zone to normal operating range" }
{ "azimuth_position_safe_log", "msg_down", 1, "Azimuth position left normal operating range and is in safe zone" }
{ "azimuth_position_safe_log", "init", true }
{ "elevation_input_matrix", "constants", (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },