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 @@
#include <string.h>
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <dt_azimuth/dt_az_safety.h>
#include <log/log.h>
......@@ -89,85 +90,106 @@ struct controller_block_private {
bool err_hwsafe_negative;
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 = *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;
safe = false;
if (!safety->private->err_max) {
safety->private->err_max = true;
if (!priv->err_max) {
priv->err_max = true;
log_send(LOG_T_ERROR, "Azimuth position above maximum %e > %e",
*safety->private->position_in,
safety->private->position_max);
*priv->position_in,
priv->position_max);
}
} else {
safety->private->err_max = false;
priv->err_max = false;
}
if (*safety->private->position_in >= safety->private->safe_zone_max &&
out > safety->private->safe_zone_max_speed) {
out = safety->private->safe_zone_max_speed;
if (!safety->private->warn_safe_max) {
safety->private->warn_safe_max = true;
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 {
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;
safe = false;
if (!safety->private->err_min) {
safety->private->err_min = true;
if (!priv->err_min) {
priv->err_min = true;
log_send(LOG_T_ERROR, "Azimuth position under minimum %e < %e",
*safety->private->position_in,
safety->private->position_min);
*priv->position_in,
priv->position_min);
}
} else {
safety->private->err_min = false;
priv->err_min = false;
}
if (*safety->private->position_in <= safety->private->safe_zone_min &&
out < safety->private->safe_zone_min_speed) {
out = safety->private->safe_zone_min_speed;
if (!safety->private->warn_safe_min) {
safety->private->warn_safe_min = true;
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 {
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;
if (!safety->private->err_hwsafe_positive) {
safety->private->err_hwsafe_positive = true;
if (!priv->err_hwsafe_positive) {
priv->err_hwsafe_positive = true;
log_send(LOG_T_ERROR, "Azimuth HW positive safety switch is open.");
}
} else {
if (safety->private->err_hwsafe_positive) {
safety->private->err_hwsafe_positive = false;
if (priv->err_hwsafe_positive) {
priv->err_hwsafe_positive = false;
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;
if (!safety->private->err_hwsafe_negative) {
safety->private->err_hwsafe_negative = true;
if (!priv->err_hwsafe_negative) {
priv->err_hwsafe_negative = true;
log_send(LOG_T_ERROR, "Azimuth HW negative safety switch is open.");
}
} else {
if (safety->private->err_hwsafe_negative) {
safety->private->err_hwsafe_negative = false;
if (priv->err_hwsafe_negative) {
priv->err_hwsafe_negative = false;
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)
enabled = false;
}
if (!enabled && !safety->private->recover) {
if (!enabled && !priv->recover) {
out = 0.0;
safety->private->torque_out = safety->private->emergency_torque;
priv->torque_out = priv->emergency_torque;
} else {
safety->private->torque_out = *safety->private->torque_in;
priv->torque_out = *priv->torque_in;
}
safety->private->safe_out = safe;
safety->private->speed_out = out;
priv->safe_out = safe || priv->recover;
priv->speed_out = out;
}
static struct controller_block_param_list params[] = {
......@@ -288,6 +310,7 @@ 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 }
};
......@@ -343,6 +366,12 @@ struct controller_block * block_dt_az_safety_create(char *name)
safety->param_set = param_set;
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;
err_output:
......
......@@ -46,6 +46,7 @@ static struct ghost_term ghost_outputs[] = {
{ "be5", "dt_az_bex", "value" },
{ "ae1", "dt_az_focusbox", "value" },
{ "external_enable", "dt_az_bex", "value" },
{ NULL },
};
static struct ghost_term ghost_inputs[] = {
......
......@@ -89,6 +89,7 @@ links {
{ "azimuth_safety", "safe_out", "azimuth_safe_and", "a" , false }
{ "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_servo_state", "out_x", "azimuth_error", "positive" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true }
{ "azimuth_spg", "setpoint", "azimuth_setpoint_error", "positive" , true }
......@@ -122,6 +123,7 @@ links {
{ "elevation_safety", "safe_out", "elevation_safe_and", "a" , false }
{ "ethercat", "operational","elevation_safe_and", "b" , 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_v", "elevation_speed_ff", "in0" , true }
{ "dt_el_r", "position", "elevation_position_offset_r_sum", "in0" , true }
......@@ -368,11 +370,11 @@ params {
{ "elevation_position_offset_r","value", (float) 0.0 }
{ "elevation_position_offset_l","value", (float) 0.0 }
{ "elevation_safety", "position_min", (float) -78.19620464325844733 }
{ "elevation_safety", "position_max", (float) 70767.56520214889483 }
{ "elevation_safety", "safe_zone_min", (float) -70.37658417893260259 }
{ "elevation_safety", "safe_zone_max", (float) 70689.36899750563638 }
{ "elevation_safety", "torsion_recover_max", (float) 0.01745329251994329577 }
{ "elevation_safety", "position_min", (float) deg2rad(-0.1) * $(elevation_gear) }
{ "elevation_safety", "position_max", (float) deg2rad(90.5) * $(elevation_gear) }
{ "elevation_safety", "safe_zone_min", (float) deg2rad(-0.09) * $(elevation_gear) }
{ "elevation_safety", "safe_zone_max", (float) deg2rad(90.4) * $(elevation_gear) }
{ "elevation_safety", "torsion_recover_max", (float) deg2rad(1.0) }
}
# Load file with calibration parameters.
......
......@@ -23,6 +23,7 @@
#include <math.h>
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <dt_elevation/dt_el_safety.h>
#include <log/log.h>
......@@ -52,7 +53,7 @@
| |
---| 9 safety_in_bottom |
| |
---| 10 enable |
---| 10 enable 5 recover |----
| |
----------------------------------
......@@ -102,7 +103,7 @@ struct controller_block_private {
float torque_max;
float emergency_torque;
int recover;
bool recover;
/* flags to prevent log flooding */
bool err_max_r;
......@@ -118,165 +119,186 @@ struct controller_block_private {
bool warn_safe_min_r;
bool err_hwsafe_top;
bool err_hwsafe_bottom;
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 = *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;
out0 = *safety->private->input_r;
out1 = -*safety->private->input_l;
position_in_r = *safety->private->position_in_r;
position_in_l = -*safety->private->position_in_l;
torsion_in = *safety->private->torsion_in;
torque_in = *safety->private->torque_in;
torque_in_r = *safety->private->torque_in_r;
torque_in_l = *safety->private->torque_in_l;
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 >= safety->private->position_max &&
if (position_in_r >= priv->position_max &&
out0 > 0.0) {
out0 = 0.0;
safe = false;
if (!safety->private->err_max_r) {
safety->private->err_max_r = true;
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,
safety->private->position_max);
priv->position_max);
}
} else {
safety->private->err_max_r = false;
priv->err_max_r = false;
}
/* inside safe zone and not moving at a safe speed */
if (position_in_r >= safety->private->safe_zone_max &&
out0 > safety->private->safe_zone_max_speed) {
out0 = safety->private->safe_zone_max_speed;
if (!safety->private->warn_safe_max_r) {
safety->private->warn_safe_max_r = true;
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,
safety->private->safe_zone_max);
priv->safe_zone_max);
}
} else {
safety->private->warn_safe_max_r = false;
priv->warn_safe_max_r = false;
}
/* 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;
safe = false;
if (!safety->private->err_min_r) {
safety->private->err_min_r = true;
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, safety->private->position_min);
position_in_r, priv->position_min);
}
} else {
safety->private->err_min_r = false;
priv->err_min_r = false;
}
/* inside safe zone and not moving back */
if (position_in_r <= safety->private->safe_zone_min &&
out0 < safety->private->safe_zone_min_speed) {
out0 = safety->private->safe_zone_min_speed;
if (!safety->private->warn_safe_min_r) {
safety->private->warn_safe_min_r = true;
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 {
safety->private->warn_safe_min_r = false;
priv->warn_safe_min_r = false;
}
/* 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;
safe = false;
if (!safety->private->err_max_l) {
safety->private->err_max_l = true;
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,
safety->private->position_max);
priv->position_max);
}
} else {
safety->private->err_max_l = false;
priv->err_max_l = false;
}
/* inside safe zone and not moving at a safe speed */
if (position_in_l >= safety->private->safe_zone_max &&
out1 > safety->private->safe_zone_max_speed) {
out1 = safety->private->safe_zone_max_speed;
if (!safety->private->warn_safe_max_l) {
safety->private->warn_safe_max_l = true;
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,
safety->private->safe_zone_max);
priv->safe_zone_max);
}
} else {
safety->private->warn_safe_max_l = false;
priv->warn_safe_max_l = false;
}
/* 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;
safe = false;
if (!safety->private->err_min_l) {
safety->private->err_min_l = true;
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, safety->private->position_min);
position_in_l, priv->position_min);
}
} else {
safety->private->err_min_l = false;
priv->err_min_l = false;
}
/* inside safe zone and not moving back */
if (position_in_l <= safety->private->safe_zone_min &&
out1 < safety->private->safe_zone_min_speed) {
out1 = safety->private->safe_zone_min_speed;
if (!safety->private->warn_safe_min_l) {
safety->private->warn_safe_min_l = true;
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 {
safety->private->warn_safe_min_l = false;
priv->warn_safe_min_l = false;
}
if (fabsf(torsion_in) >= safety->private->torsion_recover_max) {
if (!safety->private->recover) {
if (fabsf(torsion_in) >= priv->torsion_recover_max) {
if (!priv->recover) {
out0 = 0.0;
out1 = 0.0;
}
safe = false;
if (!safety->private->err_torsion_recover) {
safety->private->err_torsion_recover = true;
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 {
safety->private->err_torsion_recover = false;
priv->err_torsion_recover = false;
}
/* torsion to big */
if (fabsf(torsion_in) >= safety->private->torsion_max) {
if (fabsf(torsion_in) >= priv->torsion_max) {
/* moving back? */
if (!safety->private->recover && position_in_r > position_in_l) {
if (!priv->recover && position_in_r > position_in_l) {
if (out0 > 0.0) {
out0 = 0.0;
safe = false;
......@@ -286,7 +308,7 @@ static void dt_el_safety_calculate(struct controller_block *safety)
safe = false;
}
}
if (!safety->private->recover && position_in_r < position_in_l) {
if (!priv->recover && position_in_r < position_in_l) {
if (out0 < 0.0) {
out0 = 0.0;
safe = false;
......@@ -296,54 +318,54 @@ static void dt_el_safety_calculate(struct controller_block *safety)
safe = false;
}
}
if (!safety->private->err_torsion) {
safety->private->err_torsion = true;
if (!priv->err_torsion) {
priv->err_torsion = true;
log_send(LOG_T_ERROR, "Elevation torsion (%e) to big",
torsion_in);
}
} else {
safety->private->err_torsion = false;
priv->err_torsion = false;
}
/* (filtered) total torque to big (elevation stuck?) */
if (fabs(torque_in) > safety->private->torque_max) {
if (!safety->private->err_torque_max) {
safety->private->err_torque_max = true;
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, safety->private->torque_max);
torque_in, priv->torque_max);
}
safe = false;
} else {
safety->private->err_torque_max = false;
priv->err_torque_max = false;
}
/* top safety switch */
if (!*safety->private->safety_in_top) {
if (!*priv->safety_in_top) {
safe = false;
if (!safety->private->err_hwsafe_top) {
safety->private->err_hwsafe_top = true;
if (!priv->err_hwsafe_top) {
priv->err_hwsafe_top = true;
log_send(LOG_T_ERROR,
"Elevation HW top safety switch is open.");
}
} else {
if (safety->private->err_hwsafe_top) {
safety->private->err_hwsafe_top = false;
if (priv->err_hwsafe_top) {
priv->err_hwsafe_top = false;
log_send(LOG_T_INFO,
"Elevation HW top safety switch is closed.");
}
}
/* bottom safety switch */
if (!*safety->private->safety_in_bottom) {
if (!*priv->safety_in_bottom) {
safe = false;
if (!safety->private->err_hwsafe_bottom) {
safety->private->err_hwsafe_bottom = true;
if (!priv->err_hwsafe_bottom) {
priv->err_hwsafe_bottom = true;
log_send(LOG_T_ERROR,
"Elevation HW bottom safety switch is open.");
}
} else {
if (safety->private->err_hwsafe_bottom) {
safety->private->err_hwsafe_bottom = false;
if (priv->err_hwsafe_bottom) {
priv->err_hwsafe_bottom = false;
log_send(LOG_T_INFO,
"Elevation HW bottom safety switch is closed.");
}
......@@ -352,21 +374,21 @@ static void dt_el_safety_calculate(struct controller_block *safety)
if (!safe)
enabled = false;
if (!enabled && !safety->private->recover) {
if (!enabled && !priv->recover) {
out0 = 0.0;
out1 = 0.0;