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

Move servo state and safety blocks to the command protocol

Various small fixes
Add command protocol to websocket
Add cross compiling with 'TARGET='
Fix some issues with ARM
parent cbe16b01
...@@ -22,6 +22,7 @@ ...@@ -22,6 +22,7 @@
#include <string.h> #include <string.h>
#include <controller/controller_block.h> #include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <dt_azimuth/dt_az_safety.h> #include <dt_azimuth/dt_az_safety.h>
#include <log/log.h> #include <log/log.h>
...@@ -89,85 +90,106 @@ struct controller_block_private { ...@@ -89,85 +90,106 @@ struct controller_block_private {
bool err_hwsafe_negative; bool err_hwsafe_negative;
bool recover; bool recover;
struct controller_command *command;
}; };
static void dt_az_safety_calculate(struct controller_block *safety) static void dt_az_safety_calculate(struct controller_block *safety)
{ {
struct controller_block_private *priv = safety->private;
float out; float out;
bool safe = true; bool safe = true;
bool enabled = *safety->private->enable_in; bool enabled = *priv->enable_in;
struct command_entry c_entry;
out = *safety->private->speed_in; if (!controller_command_queue_read(priv->command, &c_entry)) {
bool recover = c_entry.value.b;
if (*safety->private->position_in >= safety->private->position_max && 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) {
out = 0.0; out = 0.0;
safe = false; safe = false;
if (!safety->private->err_max) { if (!priv->err_max) {
safety->private->err_max = true; priv->err_max = true;
log_send(LOG_T_ERROR, "Azimuth position above maximum %e > %e", log_send(LOG_T_ERROR, "Azimuth position above maximum %e > %e",
*safety->private->position_in, *priv->position_in,
safety->private->position_max); priv->position_max);
} }
} else { } else {
safety->private->err_max = false; priv->err_max = false;
} }
if (*safety->private->position_in >= safety->private->safe_zone_max && if (*priv->position_in >= priv->safe_zone_max &&
out > safety->private->safe_zone_max_speed) { out > priv->safe_zone_max_speed) {
out = safety->private->safe_zone_max_speed; out = priv->safe_zone_max_speed;
if (!safety->private->warn_safe_max) { if (!priv->warn_safe_max) {
safety->private->warn_safe_max = true; priv->warn_safe_max = true;
log_send(LOG_T_WARNING, "Azimuth position in safe zone"); log_send(LOG_T_WARNING, "Azimuth position in safe zone");
} }
} else { } else {
safety->private->warn_safe_max = false; priv->warn_safe_max = false;
} }
if (*safety->private->position_in <= safety->private->position_min && if (*priv->position_in <= priv->position_min &&
out < 0.0) { out < 0.0) {
out = 0.0; out = 0.0;
safe = false; safe = false;
if (!safety->private->err_min) { if (!priv->err_min) {
safety->private->err_min = true; priv->err_min = true;
log_send(LOG_T_ERROR, "Azimuth position under minimum %e < %e", log_send(LOG_T_ERROR, "Azimuth position under minimum %e < %e",
*safety->private->position_in, *priv->position_in,
safety->private->position_min); priv->position_min);
} }
} else { } else {
safety->private->err_min = false; priv->err_min = false;
} }
if (*safety->private->position_in <= safety->private->safe_zone_min && if (*priv->position_in <= priv->safe_zone_min &&
out < safety->private->safe_zone_min_speed) { out < priv->safe_zone_min_speed) {
out = safety->private->safe_zone_min_speed; out = priv->safe_zone_min_speed;
if (!safety->private->warn_safe_min) { if (!priv->warn_safe_min) {
safety->private->warn_safe_min = true; priv->warn_safe_min = true;
log_send(LOG_T_ERROR, "Azimuth position in safe zone"); log_send(LOG_T_ERROR, "Azimuth position in safe zone");
} }
} else { } else {
safety->private->warn_safe_min = false; priv->warn_safe_min = false;
} }
if (*safety->private->safety_in_positive == 0) { if (*priv->safety_in_positive == 0) {
safe = false; safe = false;
if (!safety->private->err_hwsafe_positive) { if (!priv->err_hwsafe_positive) {
safety->private->err_hwsafe_positive = true; priv->err_hwsafe_positive = true;
log_send(LOG_T_ERROR, "Azimuth HW positive safety switch is open."); log_send(LOG_T_ERROR, "Azimuth HW positive safety switch is open.");
} }
} else { } else {
if (safety->private->err_hwsafe_positive) { if (priv->err_hwsafe_positive) {
safety->private->err_hwsafe_positive = false; priv->err_hwsafe_positive = false;
log_send(LOG_T_INFO, "Azimuth HW positive safety switch is closed."); log_send(LOG_T_INFO, "Azimuth HW positive safety switch is closed.");
} }
} }
if (*safety->private->safety_in_negative == 0) { if (*priv->safety_in_negative == 0) {
safe = false; safe = false;
if (!safety->private->err_hwsafe_negative) { if (!priv->err_hwsafe_negative) {
safety->private->err_hwsafe_negative = true; priv->err_hwsafe_negative = true;
log_send(LOG_T_ERROR, "Azimuth HW negative safety switch is open."); log_send(LOG_T_ERROR, "Azimuth HW negative safety switch is open.");
} }
} else { } else {
if (safety->private->err_hwsafe_negative) { if (priv->err_hwsafe_negative) {
safety->private->err_hwsafe_negative = false; priv->err_hwsafe_negative = false;
log_send(LOG_T_INFO, "Azimuth HW negative safety switch is closed."); log_send(LOG_T_INFO, "Azimuth HW negative safety switch is closed.");
} }
} }
...@@ -176,15 +198,15 @@ static void dt_az_safety_calculate(struct controller_block *safety) ...@@ -176,15 +198,15 @@ static void dt_az_safety_calculate(struct controller_block *safety)
enabled = false; enabled = false;
} }
if (!enabled && !safety->private->recover) { if (!enabled && !priv->recover) {
out = 0.0; out = 0.0;
safety->private->torque_out = safety->private->emergency_torque; priv->torque_out = priv->emergency_torque;
} else { } else {
safety->private->torque_out = *safety->private->torque_in; priv->torque_out = *priv->torque_in;
} }
safety->private->safe_out = safe; priv->safe_out = safe || priv->recover;
safety->private->speed_out = out; priv->speed_out = out;
} }
static struct controller_block_param_list params[] = { static struct controller_block_param_list params[] = {
...@@ -288,6 +310,7 @@ static struct controller_block_outterm_list outterms[] = { ...@@ -288,6 +310,7 @@ static struct controller_block_outterm_list outterms[] = {
{ "speed_out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, speed_out) }, { "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) }, { "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) }, { "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 } { NULL }
}; };
...@@ -343,6 +366,12 @@ struct controller_block * block_dt_az_safety_create(char *name) ...@@ -343,6 +366,12 @@ struct controller_block * block_dt_az_safety_create(char *name)
safety->param_set = param_set; safety->param_set = param_set;
controller_block_add(safety); controller_block_add(safety);
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; return safety;
err_output: err_output:
......
...@@ -46,6 +46,7 @@ static struct ghost_term ghost_outputs[] = { ...@@ -46,6 +46,7 @@ static struct ghost_term ghost_outputs[] = {
{ "be5", "dt_az_bex", "value" }, { "be5", "dt_az_bex", "value" },
{ "ae1", "dt_az_focusbox", "value" }, { "ae1", "dt_az_focusbox", "value" },
{ "external_enable", "dt_az_bex", "value" }, { "external_enable", "dt_az_bex", "value" },
{ NULL },
}; };
static struct ghost_term ghost_inputs[] = { static struct ghost_term ghost_inputs[] = {
......
...@@ -89,6 +89,7 @@ links { ...@@ -89,6 +89,7 @@ links {
{ "azimuth_safety", "safe_out", "azimuth_safe_and", "a" , false } { "azimuth_safety", "safe_out", "azimuth_safe_and", "a" , false }
{ "ethercat", "operational","azimuth_safe_and", "b" , true } { "ethercat", "operational","azimuth_safe_and", "b" , true }
{ "azimuth_safe_and", "q", "azimuth_servo_state", "safe" , true } { "azimuth_safe_and", "q", "azimuth_servo_state", "safe" , true }
{ "azimuth_safety", "recover", "azimuth_servo_state", "override" , false }
{ "azimuth_servo_state", "out_x", "azimuth_error", "positive" , true } { "azimuth_servo_state", "out_x", "azimuth_error", "positive" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true } { "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true }
{ "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true } { "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true }
...@@ -122,6 +123,7 @@ links { ...@@ -122,6 +123,7 @@ links {
{ "elevation_safety", "safe_out", "elevation_safe_and", "a" , false } { "elevation_safety", "safe_out", "elevation_safe_and", "a" , false }
{ "ethercat", "operational","elevation_safe_and", "b" , true } { "ethercat", "operational","elevation_safe_and", "b" , true }
{ "elevation_safe_and", "q", "elevation_servo_state", "safe" , true } { "elevation_safe_and", "q", "elevation_servo_state", "safe" , true }
{ "elevation_safety", "recover", "elevation_servo_state", "override" , false }
{ "elevation_servo_state", "out_x", "elevation_error", "positive" , true } { "elevation_servo_state", "out_x", "elevation_error", "positive" , true }
{ "elevation_servo_state", "out_v", "elevation_speed_ff", "in0" , true } { "elevation_servo_state", "out_v", "elevation_speed_ff", "in0" , true }
{ "dt_el_r", "position", "elevation_position_offset_r_sum", "in0" , true } { "dt_el_r", "position", "elevation_position_offset_r_sum", "in0" , true }
...@@ -368,11 +370,11 @@ params { ...@@ -368,11 +370,11 @@ params {
{ "elevation_position_offset_r","value", (float) 0.0 } { "elevation_position_offset_r","value", (float) 0.0 }
{ "elevation_position_offset_l","value", (float) 0.0 } { "elevation_position_offset_l","value", (float) 0.0 }
{ "elevation_safety", "position_min", (float) -78.19620464325844733 } { "elevation_safety", "position_min", (float) deg2rad(-0.1) * $(elevation_gear) }
{ "elevation_safety", "position_max", (float) 70767.56520214889483 } { "elevation_safety", "position_max", (float) deg2rad(90.5) * $(elevation_gear) }
{ "elevation_safety", "safe_zone_min", (float) -70.37658417893260259 } { "elevation_safety", "safe_zone_min", (float) deg2rad(-0.09) * $(elevation_gear) }
{ "elevation_safety", "safe_zone_max", (float) 70689.36899750563638 } { "elevation_safety", "safe_zone_max", (float) deg2rad(90.4) * $(elevation_gear) }
{ "elevation_safety", "torsion_recover_max", (float) 0.01745329251994329577 } { "elevation_safety", "torsion_recover_max", (float) deg2rad(1.0) }
} }
# Load file with calibration parameters. # Load file with calibration parameters.
......
...@@ -23,6 +23,7 @@ ...@@ -23,6 +23,7 @@
#include <math.h> #include <math.h>
#include <controller/controller_block.h> #include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <dt_elevation/dt_el_safety.h> #include <dt_elevation/dt_el_safety.h>
#include <log/log.h> #include <log/log.h>
...@@ -52,7 +53,7 @@ ...@@ -52,7 +53,7 @@
| | | |
---| 9 safety_in_bottom | ---| 9 safety_in_bottom |
| | | |
---| 10 enable | ---| 10 enable 5 recover |----
| | | |
---------------------------------- ----------------------------------
...@@ -102,7 +103,7 @@ struct controller_block_private { ...@@ -102,7 +103,7 @@ struct controller_block_private {
float torque_max; float torque_max;
float emergency_torque; float emergency_torque;
int recover; bool recover;
/* flags to prevent log flooding */ /* flags to prevent log flooding */
bool err_max_r; bool err_max_r;
...@@ -118,165 +119,186 @@ struct controller_block_private { ...@@ -118,165 +119,186 @@ struct controller_block_private {
bool warn_safe_min_r; bool warn_safe_min_r;
bool err_hwsafe_top; bool err_hwsafe_top;
bool err_hwsafe_bottom; bool err_hwsafe_bottom;
struct controller_command *command;
}; };
static void dt_el_safety_calculate(struct controller_block *safety) static void dt_el_safety_calculate(struct controller_block *safety)
{ {
struct controller_block_private *priv = safety->private;
float out0, out1; float out0, out1;
float position_in_r, position_in_l; float position_in_r, position_in_l;
float torsion_in, torque_in, torque_in_r, torque_in_l; float torsion_in, torque_in, torque_in_r, torque_in_l;
bool safe; bool safe;
bool enabled = *safety->private->enable_in; bool enabled = *priv->enable_in;
struct 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; safe = true;
out0 = *safety->private->input_r; out0 = *priv->input_r;
out1 = -*safety->private->input_l; out1 = -*priv->input_l;
position_in_r = *safety->private->position_in_r; position_in_r = *priv->position_in_r;
position_in_l = -*safety->private->position_in_l; position_in_l = -*priv->position_in_l;
torsion_in = *safety->private->torsion_in; torsion_in = *priv->torsion_in;
torque_in = *safety->private->torque_in; torque_in = *priv->torque_in;
torque_in_r = *safety->private->torque_in_r; torque_in_r = *priv->torque_in_r;
torque_in_l = *safety->private->torque_in_l; torque_in_l = *priv->torque_in_l;
/* outside maximum range and not moving back */ /* outside maximum range and not moving back */
if (position_in_r >= safety->private->position_max && if (position_in_r >= priv->position_max &&
out0 > 0.0) { out0 > 0.0) {
out0 = 0.0; out0 = 0.0;
safe = false; safe = false;
if (!safety->private->err_max_r) { if (!priv->err_max_r) {
safety->private->err_max_r = true; priv->err_max_r = true;
log_send(LOG_T_ERROR, log_send(LOG_T_ERROR,
"Elevation position right %e above maximum %e", "Elevation position right %e above maximum %e",
position_in_r, position_in_r,
safety->private->position_max); priv->position_max);
} }
} else { } else {
safety->private->err_max_r = false; priv->err_max_r = false;
} }
/* inside safe zone and not moving at a safe speed */ /* inside safe zone and not moving at a safe speed */
if (position_in_r >= safety->private->safe_zone_max && if (position_in_r >= priv->safe_zone_max &&
out0 > safety->private->safe_zone_max_speed) { out0 > priv->safe_zone_max_speed) {
out0 = safety->private->safe_zone_max_speed; out0 = priv->safe_zone_max_speed;
if (!safety->private->warn_safe_max_r) { if (!priv->warn_safe_max_r) {
safety->private->warn_safe_max_r = true; priv->warn_safe_max_r = true;
log_send(LOG_T_WARNING, log_send(LOG_T_WARNING,
"Elevation position right %e in safe zone %e", "Elevation position right %e in safe zone %e",
position_in_r, position_in_r,
safety->private->safe_zone_max); priv->safe_zone_max);
} }
} else { } else {
safety->private->warn_safe_max_r = false; priv->warn_safe_max_r = false;
} }
/* outside minimum and not moving back */ /* outside minimum and not moving back */
if (position_in_r <= safety->private->position_min && if (position_in_r <= priv->position_min &&
out0 < 0.0) { out0 < 0.0) {
out0 = 0.0; out0 = 0.0;
safe = false; safe = false;
if (!safety->private->err_min_r) { if (!priv->err_min_r) {
safety->private->err_min_r = true; priv->err_min_r = true;
log_send(LOG_T_ERROR, log_send(LOG_T_ERROR,
"Elevation position right under minimum, %e < %e", "Elevation position right under minimum, %e < %e",
position_in_r, safety->private->position_min); position_in_r, priv->position_min);
} }
} else { } else {
safety->private->err_min_r = false; priv->err_min_r = false;
} }
/* inside safe zone and not moving back */ /* inside safe zone and not moving back */
if (position_in_r <= safety->private->safe_zone_min && if (position_in_r <= priv->safe_zone_min &&
out0 < safety->private->safe_zone_min_speed) { out0 < priv->safe_zone_min_speed) {
out0 = safety->private->safe_zone_min_speed; out0 = priv->safe_zone_min_speed;
if (!safety->private->warn_safe_min_r) { if (!priv->warn_safe_min_r) {
safety->private->warn_safe_min_r = true; priv->warn_safe_min_r = true;
log_send(LOG_T_WARNING, log_send(LOG_T_WARNING,
"Elevation position right in safe zone"); "Elevation position right in safe zone");
} }
} else { } else {
safety->private->warn_safe_min_r = false; priv->warn_safe_min_r = false;
} }
/* outside maximum range and not moving back */ /* outside maximum range and not moving back */
if (position_in_l >= safety->private->position_max && if (position_in_l >= priv->position_max &&
out1 > 0.0) { out1 > 0.0) {
out1 = 0.0; out1 = 0.0;
safe = false; safe = false;
if (!safety->private->err_max_l) { if (!priv->err_max_l) {
safety->private->err_max_l = true; priv->err_max_l = true;
log_send(LOG_T_ERROR, log_send(LOG_T_ERROR,
"Elevation left position %e above maximum %e", "Elevation left position %e above maximum %e",
position_in_l, position_in_l,
safety->private->position_max); priv->position_max);
} }
} else { } else {
safety->private->err_max_l = false; priv->err_max_l = false;
} }
/* inside safe zone and not moving at a safe speed */ /* inside safe zone and not moving at a safe speed */
if (position_in_l >= safety->private->safe_zone_max && if (position_in_l >= priv->safe_zone_max &&
out1 > safety->private->safe_zone_max_speed) { out1 > priv->safe_zone_max_speed) {
out1 = safety->private->safe_zone_max_speed; out1 = priv->safe_zone_max_speed;
if (!safety->private->warn_safe_max_l) { if (!priv->warn_safe_max_l) {
safety->private->warn_safe_max_l = true; priv->warn_safe_max_l = true;
log_send(LOG_T_WARNING, log_send(LOG_T_WARNING,
"Elevation left position %e in safe zone %e", "Elevation left position %e in safe zone %e",
position_in_l, position_in_l,
safety->private->safe_zone_max); priv->safe_zone_max);
} }
} else { } else {
safety->private->warn_safe_max_l = false; priv->warn_safe_max_l = false;
} }
/* outside minimum and not moving back */ /* outside minimum and not moving back */
if (position_in_l <= safety->private->position_min && if (position_in_l <= priv->position_min &&
out1 < 0.0) { out1 < 0.0) {
out1 = 0.0; out1 = 0.0;
safe = false; safe = false;
if (!safety->private->err_min_l) { if (!priv->err_min_l) {
safety->private->err_min_l = true; priv->err_min_l = true;
log_send(LOG_T_ERROR, log_send(LOG_T_ERROR,
"Elevation left position %e under minimum %e", "Elevation left position %e under minimum %e",
position_in_l, safety->private->position_min); position_in_l, priv->position_min);
} }
} else { } else {
safety->private->err_min_l = false; priv->err_min_l = false;
} }
/* inside safe zone and not moving back */