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

Remove some code from elevation safety block to control file.

parent b3cf78ba
......@@ -81,6 +81,9 @@ blocks ($(frequency), $(delay)) {
{ "add", "elevation_position_offset_r_sum" }
{ "add", "elevation_position_offset_l_sum" }
{ "log", "elevation_safety_hw_top" }
{ "log", "elevation_safety_hw_bottom" }
{ "command_bool", "focusbox_plus" }
{ "command_bool", "focusbox_min" }
......@@ -202,6 +205,9 @@ links {
{ "elevation_servo_state", "enable", "elevation_torsion_pid", "enable" , true }
{ "elevation_torsion_pid", "out", "elevation_torsion_speed_limit", "in" , true }
{ "elevation_torsion_speed_limit", "out", "elevation_output_matrix", "in1" , true }
{ $<Elevation_Top_Safe>, "elevation_safety_hw_top", "condition", true }
{ $<Elevation_Bottom_Safe>, "elevation_safety_hw_bottom", "condition", true }
}
traces {
......@@ -391,6 +397,11 @@ params {
{ "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) }
{ "elevation_safety_hw_top", "msg_up", 2, "Elevation HW top safety switch is closed" }
{ "elevation_safety_hw_top", "msg_down", 0, "Elevation HW top safety switch is open" }
{ "elevation_safety_hw_bottom", "msg_up", 2, "Elevation HW bottom safety switch is closed" }
{ "elevation_safety_hw_bottom", "msg_down", 0, "Elevation HW bottom safety switch is open" }
}
# Load file with calibration parameters.
......
......@@ -116,8 +116,6 @@ struct controller_block_private {
bool warn_safe_min_l;
bool warn_safe_max_r;
bool warn_safe_min_r;
bool err_hwsafe_top;
bool err_hwsafe_bottom;
struct controller_command *command;
};
......@@ -130,7 +128,7 @@ static void dt_el_safety_calculate(struct controller_block *safety)
float torsion_in, torque_in, torque_in_r, torque_in_l;
bool safe;
bool enabled = *priv->enable_in;
struct command_entry c_entry;
struct controller_command_entry c_entry;
if (!controller_command_queue_read(priv->command, &c_entry)) {
bool recover = c_entry.value.b;
......@@ -341,33 +339,11 @@ static void dt_el_safety_calculate(struct controller_block *safety)
/* top safety switch */
if (!*priv->safety_in_top) {
safe = false;
if (!priv->err_hwsafe_top) {
priv->err_hwsafe_top = true;
log_send(LOG_T_ERROR,
"Elevation HW top safety switch is open.");
}
} else {
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 (!*priv->safety_in_bottom) {
safe = false;
if (!priv->err_hwsafe_bottom) {
priv->err_hwsafe_bottom = true;
log_send(LOG_T_ERROR,
"Elevation HW bottom safety switch is open.");
}
} else {
if (priv->err_hwsafe_bottom) {
priv->err_hwsafe_bottom = false;
log_send(LOG_T_INFO,
"Elevation HW bottom safety switch is closed.");
}
}
if (!safe)
......@@ -557,8 +533,6 @@ static struct controller_block * block_dt_el_safety_create(char *name, int argc,
safety->private->warn_safe_min_r = false;
safety->private->warn_safe_max_l = false;
safety->private->warn_safe_min_l = false;
safety->private->err_hwsafe_top = false;
safety->private->err_hwsafe_bottom = false;
safety->private->err_torsion_recover = false;
if (controller_block_interm_list_init(safety, interms))
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment