Commit 9165acfb authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

First set of readability and code improvements

-Safety blocks have some names changed
-Setpoint generator is redesigned.
 +No longer 'unitless' with 'd' as derivative, but physics names (x, v, a, j)
 +Removal of 'mirrored' code.
 +improved tracking at high speeds
 +Addition of 'precision' parameters to limit numerical noise
parent eaeb1f15
......@@ -39,9 +39,9 @@
| |
---| 3 safety |
| |
---| 4 safety_in_p |
---| 4 safety_in_positive |
| |
---| 5 safety_in_m |
---| 5 safety_in_negative |
| |
---| 6 enable |
| |
......@@ -67,8 +67,8 @@ struct controller_block_private {
float *position_in;
float *torque_in;
bool *safety_in;
bool *safety_in_p;
bool *safety_in_m;
bool *safety_in_positive;
bool *safety_in_negative;
float speed_out;
bool safe_out;
float torque_out;
......@@ -83,13 +83,14 @@ struct controller_block_private {
float safe_zone_min_speed;
float safe_zone_max_speed;
int err_max;
int err_min;
int warn_safe_max;
int warn_safe_min;
int err_hwsafe;
int err_hwsafe_p;
int err_hwsafe_m;
/* keep log state to prevent flooding */
bool err_max;
bool err_min;
bool warn_safe_max;
bool warn_safe_min;
bool err_hwsafe;
bool err_hwsafe_positive;
bool err_hwsafe_negative;
bool recover;
};
......@@ -97,7 +98,7 @@ struct controller_block_private {
static void calculate(struct controller_block *safety)
{
float out;
bool safe = 1;
bool safe = true;
bool enabled = *safety->private->enable_in;
out = *safety->private->speed_in;
......@@ -105,81 +106,81 @@ static void calculate(struct controller_block *safety)
if (*safety->private->position_in >= safety->private->position_max &&
out > 0.0) {
out = 0.0;
safe = 0;
safe = false;
if (!safety->private->err_max) {
safety->private->err_max = 1;
safety->private->err_max = true;
log_send(LOG_T_ERROR, "Azimuth position above maximum %e > %e",
*safety->private->position_in,
safety->private->position_max);
}
} else {
safety->private->err_max = 0;
safety->private->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 = 1;
safety->private->warn_safe_max = true;
log_send(LOG_T_WARNING, "Azimuth position in safe zone");
}
} else {
safety->private->warn_safe_max = 0;
safety->private->warn_safe_max = false;
}
if (*safety->private->position_in <= safety->private->position_min &&
out < 0.0) {
out = 0.0;
safe = 0;
safe = false;
if (!safety->private->err_min) {
safety->private->err_min = 1;
safety->private->err_min = true;
log_send(LOG_T_ERROR, "Azimuth position under minimum %e < %e",
*safety->private->position_in,
safety->private->position_min);
}
} else {
safety->private->err_min = 0;
safety->private->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 = 1;
safety->private->warn_safe_min = true;
log_send(LOG_T_ERROR, "Azimuth position in safe zone");
}
} else {
safety->private->warn_safe_min = 0;
safety->private->warn_safe_min = false;
}
if (*safety->private->safety_in == 0) {
safe = 0;
safe = false;
if (!safety->private->err_hwsafe) {
safety->private->err_hwsafe = 1;
safety->private->err_hwsafe = true;
log_send(LOG_T_ERROR, "Azimuth HW safety switch is open.");
}
} else {
safety->private->err_hwsafe = 0;
safety->private->err_hwsafe = false;
}
if (*safety->private->safety_in_p == 0) {
safe = 0;
if (!safety->private->err_hwsafe_p) {
safety->private->err_hwsafe_p = 1;
if (*safety->private->safety_in_positive == 0) {
safe = false;
if (!safety->private->err_hwsafe_positive) {
safety->private->err_hwsafe_positive = true;
log_send(LOG_T_ERROR, "Azimuth HW positive safety switch is open.");
}
} else {
safety->private->err_hwsafe_p = 0;
safety->private->err_hwsafe_positive = false;
}
if (*safety->private->safety_in_m == 0) {
safe = 0;
if (!safety->private->err_hwsafe_m) {
safety->private->err_hwsafe_m = 1;
if (*safety->private->safety_in_negative == 0) {
safe = false;
if (!safety->private->err_hwsafe_negative) {
safety->private->err_hwsafe_negative = true;
log_send(LOG_T_ERROR, "Azimuth HW negative safety switch is open.");
}
} else {
safety->private->err_hwsafe_m = 0;
safety->private->err_hwsafe_negative = false;
}
if (!safe) {
enabled = 0;
enabled = false;
}
if (!enabled && !safety->private->recover) {
......@@ -307,13 +308,13 @@ struct controller_block * block_dt_az_safety_create(char *name)
safety->private->safe_zone_max_speed = 0.0;
safety->private->emergency_torque = 0.0;
safety->private->err_max = 0;
safety->private->err_min = 0;
safety->private->warn_safe_max = 0;
safety->private->warn_safe_min = 0;
safety->private->err_hwsafe = 0;
safety->private->err_hwsafe_p = 0;
safety->private->err_hwsafe_m = 0;
safety->private->err_max = false;
safety->private->err_min = false;
safety->private->warn_safe_max = false;
safety->private->warn_safe_min = false;
safety->private->err_hwsafe = false;
safety->private->err_hwsafe_positive = false;
safety->private->err_hwsafe_negative = false;
safety->private->recover = false;
......@@ -337,13 +338,13 @@ struct controller_block * block_dt_az_safety_create(char *name)
safety->input[3].type = CONTROLLER_BLOCK_TERM_BOOL;
safety->input[3].value.b = &safety->private->safety_in;
safety->input[3].ghostof = NULL;
safety->input[4].name = "safety_in_p";
safety->input[4].name = "safety_in_positive";
safety->input[4].type = CONTROLLER_BLOCK_TERM_BOOL;
safety->input[4].value.b = &safety->private->safety_in_p;
safety->input[4].value.b = &safety->private->safety_in_positive;
safety->input[4].ghostof = NULL;
safety->input[5].name = "safety_in_m";
safety->input[5].name = "safety_in_negative";
safety->input[5].type = CONTROLLER_BLOCK_TERM_BOOL;
safety->input[5].value.b = &safety->private->safety_in_m;
safety->input[5].value.b = &safety->private->safety_in_negative;
safety->input[5].ghostof = NULL;
safety->input[6].name = "enable";
safety->input[6].type = CONTROLLER_BLOCK_TERM_BOOL;
......
......@@ -25,7 +25,7 @@ blocks {
{ "subtract", "elevation_error" }
{ "subtract", "elevation_torsion_error" }
{ "subtract", "elevation_torsion_setpoint_error" }
{ "add", "elevation_torsion_torque" }
{ "subtract", "elevation_torsion_torque" }
{ "filter_lp", "elevation_torsion_torque_lp" }
{ "pid_aw", "elevation_pid" }
{ "pid_aw", "elevation_torsion_pid" }
......@@ -52,10 +52,10 @@ links {
{ "azimuth_position_offset", "value", "azimuth_position_offset_sum", "in1" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_error", "negative" , true }
{ "azimuth_servo_state", "reset", "azimuth_spg", "reset" , false }
{ "azimuth_position_offset_sum", "out", "azimuth_spg", "cur0" , true }
{ "azimuth_spg", "spg0", "azimuth_servo_state", "spg0" , true }
{ "azimuth_spg", "spg1", "azimuth_servo_state", "spg1" , true }
{ "azimuth_spg", "spg2", "azimuth_servo_state", "spg2" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_spg", "reset_x" , true }
{ "azimuth_spg", "x", "azimuth_servo_state", "spg0" , true }
{ "azimuth_spg", "v", "azimuth_servo_state", "spg1" , true }
{ "azimuth_spg", "a", "azimuth_servo_state", "spg2" , true }
{ "azimuth_safety", "safe_out", "azimuth_servo_state", "safe" , false }
{ "azimuth_servo_state", "out0", "azimuth_error", "positive" , true }
{ "azimuth_position_offset_sum", "out", "azimuth_setpoint_error", "negative" , true }
......@@ -72,8 +72,8 @@ links {
{ "azimuth_position_offset_sum", "out", "azimuth_safety", "position_in" , true }
{ "azimuth_torque", "value", "azimuth_safety", "torque_in" , true }
{ "dt_az", "be1", "azimuth_safety", "safety_in" , true }
{ "dt_az", "be4", "azimuth_safety", "safety_in_p" , true }
{ "dt_az", "be2", "azimuth_safety", "safety_in_m" , true }
{ "dt_az", "be4", "azimuth_safety", "safety_in_positive", true }
{ "dt_az", "be2", "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_servo", "out", "dt_az", "speed" , true }
......@@ -83,10 +83,10 @@ links {
{ "false", "value", "dt_az", "ba2" , true }
{ "elevation_servo_state", "reset", "elevation_spg", "reset" , false }
{ "elevation_input_matrix", "out0", "elevation_spg", "cur0" , true }
{ "elevation_spg", "spg0", "elevation_servo_state", "spg0" , true }
{ "elevation_spg", "spg1", "elevation_servo_state", "spg1" , true }
{ "elevation_spg", "spg2", "elevation_servo_state", "spg2" , true }
{ "elevation_input_matrix", "out0", "elevation_spg", "reset_x" , true }
{ "elevation_spg", "x", "elevation_servo_state", "spg0" , true }
{ "elevation_spg", "v", "elevation_servo_state", "spg1" , true }
{ "elevation_spg", "a", "elevation_servo_state", "spg2" , true }
{ "elevation_safety", "safe_out", "elevation_servo_state", "safe" , false }
{ "elevation_servo_state", "out0", "elevation_error", "positive" , true }
{ "elevation_servo_state", "out1", "elevation_speed_ff", "in0" , true }
......@@ -108,9 +108,9 @@ links {
{ "elevation_servo_state", "enable", "elevation_jerk_limit", "enable" , true }
{ "elevation_speed_limit", "out", "elevation_jerk_limit", "in" , true }
{ "elevation_jerk_limit", "out", "elevation_output_matrix", "in0" , true }
{ "dt_el_r", "torque", "elevation_torsion_torque", "in0" , true }
{ "dt_el_l", "torque", "elevation_torsion_torque", "in1" , true }
{ "elevation_torsion_torque", "out", "elevation_torsion_torque_lp", "in" , true }
{ "dt_el_r", "torque", "elevation_torsion_torque", "positive" , true }
{ "dt_el_l", "torque", "elevation_torsion_torque", "negative" , true }
{ "elevation_torsion_torque", "difference", "elevation_torsion_torque_lp", "in" , true }
{ "elevation_position_offset_r_sum","out", "elevation_safety", "position_in_r" , true }
{ "elevation_position_offset_l_sum","out", "elevation_safety", "position_in_l" , true }
{ "elevation_input_matrix", "out1", "elevation_safety", "torsion_in" , true }
......@@ -119,8 +119,8 @@ links {
{ "elevation_torsion_torque_lp", "out", "elevation_safety", "torque_in" , true }
{ "elevation_torque_r", "value", "elevation_safety", "torque_in_r" , true }
{ "elevation_torque_l", "value", "elevation_safety", "torque_in_l" , true }
{ "dt_el_l", "be4", "elevation_safety", "safety_in_t" , true }
{ "dt_el_l", "be2", "elevation_safety", "safety_in_b" , true }
{ "dt_el_l", "be4", "elevation_safety", "safety_in_top", true }
{ "dt_el_l", "be2", "elevation_safety", "safety_in_bottom", true }
{ "dt_el_l", "be1", "elevation_safety", "safety_in_l" , true }
{ "dt_el_r", "be1", "elevation_safety", "safety_in_r" , true }
{ "elevation_servo_state", "enable", "elevation_safety", "enable" , true }
......@@ -136,8 +136,8 @@ links {
{ "false", "value", "dt_el_l", "ba2" , true }
{ "elevation_servo_state", "reset", "elevation_torsion_spg", "reset" , false }
{ "elevation_input_matrix", "out1", "elevation_torsion_spg", "cur0" , true }
{ "elevation_torsion_spg", "spg0", "elevation_torsion_error", "positive" , true }
{ "elevation_input_matrix", "out1", "elevation_torsion_spg", "reset_x" , true }
{ "elevation_torsion_spg", "x", "elevation_torsion_error", "positive" , true }
{ "elevation_input_matrix", "out1", "elevation_torsion_error", "negative" , true }
{ "elevation_torsion_spg", "setpoint", "elevation_torsion_setpoint_error", "positive" , true }
{ "elevation_input_matrix", "out1", "elevation_torsion_setpoint_error", "negative" , true }
......@@ -148,10 +148,10 @@ links {
}
traces {
{ "Azimuth_Spg0", "rad", "azimuth_spg", "spg0" }
{ "Azimuth_Spg1", "rad/s", "azimuth_spg", "spg1" }
{ "Azimuth_Spg2", "rad/s/s", "azimuth_spg", "spg2" }
{ "Azimuth_Spg3", "rad/s/s/s", "azimuth_spg", "spg3" }
{ "Azimuth_Spg0", "rad", "azimuth_spg", "x" }
{ "Azimuth_Spg1", "rad/s", "azimuth_spg", "v" }
{ "Azimuth_Spg2", "rad/s/s", "azimuth_spg", "a" }
{ "Azimuth_Spg3", "rad/s/s/s", "azimuth_spg", "j" }
{ "Azimuth_Setpoint", "rad", "azimuth_spg", "setpoint" }
{ "Azimuth_Error", "rad", "azimuth_error", "difference" }
{ "Azimuth_Setpoint_Error", "rad", "azimuth_setpoint_error", "difference" }
......@@ -172,10 +172,10 @@ traces {
{ "Elevation_Position", "rad", "elevation_input_matrix", "out0" }
{ "Elevation_Torsion", "rad", "elevation_input_matrix", "out1" }
{ "Elevation_Setpoint", "rad", "elevation_spg", "setpoint" }
{ "Elevation_Spg0", "rad", "elevation_spg", "spg0" }
{ "Elevation_Spg1", "rad/s", "elevation_spg", "spg1" }
{ "Elevation_Spg2", "rad/s", "elevation_spg", "spg2" }
{ "Elevation_Spg3", "rad/s", "elevation_spg", "spg3" }
{ "Elevation_Spg0", "rad", "elevation_spg", "x" }
{ "Elevation_Spg1", "rad/s", "elevation_spg", "v" }
{ "Elevation_Spg2", "rad/s", "elevation_spg", "a" }
{ "Elevation_Spg3", "rad/s", "elevation_spg", "j" }
{ "Elevation_Error", "rad", "elevation_error", "difference" }
{ "Elevation_Setpoint_Error", "rad", "elevation_setpoint_error", "difference" }
{ "Elevation_PID", "rad/s", "elevation_pid", "out" }
......@@ -200,15 +200,14 @@ traces {
params {
{ "azimuth_spg", "setpoint", (float) 0.0 }
{ "azimuth_spg", "t", (float) 0.004 }
{ "azimuth_spg", "max", (float) 4.712388980384689858 }
{ "azimuth_spg", "min", (float)-4.712388980384689858 }
{ "azimuth_spg", "max1d", (float) 0.01466076571675236845 }
{ "azimuth_spg", "min1d", (float)-0.01466076571675236845 }
{ "azimuth_spg", "max2d", (float) 0.0002 }
{ "azimuth_spg", "min2d", (float)-0.0002 }
{ "azimuth_spg", "min2d", (float)-0.0002 }
{ "azimuth_spg", "max3d", (float) 0.00001 }
{ "azimuth_spg", "min3d", (float)-0.00001 }
{ "azimuth_spg", "max_x", (float) 4.712388980384689858 }
{ "azimuth_spg", "min_x", (float)-4.712388980384689858 }
{ "azimuth_spg", "max_v", (float) 0.01466076571675236845 }
{ "azimuth_spg", "max_a", (float) 0.0002 }
{ "azimuth_spg", "max_j", (float) 0.00001 }
{ "azimuth_spg", "precision_x", (float) 0.000001 }
{ "azimuth_spg", "precision_a", (float) 0.000001 }
{ "azimuth_spg", "precision_v", (float) 0.000001 }
{ "azimuth_servo_state", "max0", (float) 4.712388980384689858 }
{ "azimuth_servo_state", "min0", (float)-4.712388980384689858 }
{ "azimuth_servo_state", "max1", (float) 0.01466076571675236845 }
......@@ -252,28 +251,28 @@ params {
{ "elevation_spg", "setpoint", (float) 0.0 }
{ "elevation_spg", "t", (float) 0.004 }
{ "elevation_spg", "max", (float) 1.570796326794896619 }
{ "elevation_spg", "min", (float)-0.0008726646259971647885 }
{ "elevation_spg", "max1d", (float) 0.004921828490624009407 }
{ "elevation_spg", "min1d", (float)-0.004921828490624009407 }
{ "elevation_spg", "max2d", (float) 0.0003 }
{ "elevation_spg", "min2d", (float)-0.0003 }
{ "elevation_spg", "max3d", (float) 0.000016 }
{ "elevation_spg", "min3d", (float)-0.000016 }
{ "elevation_spg", "max_x", (float) 1.570796326794896619 }
{ "elevation_spg", "min_x", (float)-0.0008726646259971647885 }
{ "elevation_spg", "max_v", (float) 0.004921828490624009407 }
{ "elevation_spg", "max_a", (float) 0.0003 }
{ "elevation_spg", "max_j", (float) 0.000016 }
{ "elevation_spg", "precision_x", (float) 0.000001 }
{ "elevation_spg", "precision_a", (float) 0.000001 }
{ "elevation_spg", "precision_v", (float) 0.000001 }
{ "elevation_servo_state", "max0", (float) 1.570796326794896619 }
{ "elevation_servo_state", "min0", (float)-0.0008726646259971647885 }
{ "elevation_servo_state", "max1", (float) 0.004921828490624009407 }
{ "elevation_servo_state", "max2", (float) 0.0003 }
{ "elevation_torsion_spg", "setpoint", (float) 0.0 }
{ "elevation_torsion_spg", "t", (float) 0.004 }
{ "elevation_torsion_spg", "max", (float) 0.01 }
{ "elevation_torsion_spg", "min", (float)-0.01 }
{ "elevation_torsion_spg", "max1d", (float) 0.0001047197551196597746 }
{ "elevation_torsion_spg", "min1d", (float)-0.0001047197551196597746 }
{ "elevation_torsion_spg", "max2d", (float) 0.00004 }
{ "elevation_torsion_spg", "min2d", (float)-0.00004 }
{ "elevation_torsion_spg", "max3d", (float) 0.00001 }
{ "elevation_torsion_spg", "min3d", (float)-0.00001 }
{ "elevation_torsion_spg", "max_x", (float) 0.01 }
{ "elevation_torsion_spg", "min_x", (float)-0.01 }
{ "elevation_torsion_spg", "max_v", (float) 0.0001047197551196597746 }
{ "elevation_torsion_spg", "max_a", (float) 0.00004 }
{ "elevation_torsion_spg", "max_j", (float) 0.00001 }
{ "elevation_torsion_spg", "precision_x", (float) 0.000001 }
{ "elevation_torsion_spg", "precision_a", (float) 0.000001 }
{ "elevation_torsion_spg", "precision_v", (float) 0.000001 }
{ "elevation_pid", "kp", (float) 0.80 }
{ "elevation_pid", "ki", (float) 0.001 }
{ "elevation_pid", "kd", (float) 0.001 }
......@@ -300,7 +299,7 @@ params {
{ "elevation_safety", "torsion_max", (float) 0.0002 }
{ "elevation_safety", "recover", (int) 0 }
{ "elevation_safety", "torque_max", (float) 1 }
{ "elevation_safety", "torque_max", (float) 20.0 }
{ "elevation_safety", "emergency_torque", (float) 3.0 }
{ "elevation_torque_r", "value", (float) 10.0 }
......
......@@ -52,9 +52,9 @@
| |
---| 9 safety_in_r |
| |
---| 10 safety_in_t |
---| 10 safety_in_top |
| |
---| 11 safety_in_b |
---| 11 safety_in_bottom |
| |
---| 10 enable |
| |
......@@ -86,8 +86,8 @@ struct controller_block_private {
float *torque_in_l;
bool *safety_in_l;
bool *safety_in_r;
bool *safety_in_t;
bool *safety_in_b;
bool *safety_in_top;
bool *safety_in_bottom;
float speed_out_r;
float speed_out_l;
bool safe_out;
......@@ -110,20 +110,22 @@ struct controller_block_private {
int recover;
int err_max_r;
int err_min_r;
int err_max_l;
int err_min_l;
int err_torsion;
int err_torsion_recover;
int warn_safe_max_l;
int warn_safe_min_l;
int warn_safe_max_r;
int warn_safe_min_r;
int err_hwsafe_r;
int err_hwsafe_l;
int err_hwsafe_t;
int err_hwsafe_b;
/* 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;
bool err_hwsafe_r;
bool err_hwsafe_l;
bool err_hwsafe_top;
bool err_hwsafe_bottom;
};
static void calculate(struct controller_block *safety)
......@@ -134,7 +136,7 @@ static void calculate(struct controller_block *safety)
bool safe;
bool enabled = *safety->private->enable_in;
safe = 1;
safe = true;
out0 = *safety->private->input_r;
out1 = -*safety->private->input_l;
position_in_r = *safety->private->position_in_r;
......@@ -144,206 +146,233 @@ static void calculate(struct controller_block *safety)
torque_in_r = *safety->private->torque_in_r;
torque_in_l = *safety->private->torque_in_l;
/* outside maximum range and not moving back */
if (position_in_r >= safety->private->position_max &&
out0 > 0.0) {
out0 = 0.0;
safe = 0;
safe = false;
if (!safety->private->err_max_r) {
safety->private->err_max_r = 1;
safety->private->err_max_r = true;
log_send(LOG_T_ERROR,
"Elevation position right %e above maximum %e",
position_in_r,
safety->private->position_max);
}
} else {
safety->private->err_max_r = 0;
safety->private->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 = 1;
safety->private->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);
}
} else {
safety->private->warn_safe_max_r = 0;
safety->private->warn_safe_max_r = false;
}
/* outside minimum and not moving back */
if (position_in_r <= safety->private->position_min &&
out0 < 0.0) {
out0 = 0.0;
safe = 0;
safe = false;
if (!safety->private->err_min_r) {
safety->private->err_min_r = 1;
safety->private->err_min_r = true;
log_send(LOG_T_ERROR,
"Elevation position right under minimum, %e < %e",
position_in_r, safety->private->position_min);
}
} else {
safety->private->err_min_r = 0;
safety->private->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 = 1;
safety->private->warn_safe_min_r = true;
log_send(LOG_T_WARNING,
"Elevation position right in safe zone");
}
} else {
safety->private->warn_safe_min_r = 0;
safety->private->warn_safe_min_r = false;
}
/* outside maximum range and not moving back */
if (position_in_l >= safety->private->position_max &&
out1 > 0.0) {
out1 = 0.0;
safe = 0;
safe = false;
if (!safety->private->err_max_l) {
safety->private->err_max_l = 1;
safety->private->err_max_l = true;
log_send(LOG_T_ERROR,
"Elevation left position %e above maximum %e",
position_in_l,
safety->private->position_max);
}
} else {
safety->private->err_max_l = 0;
safety->private->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 = 1;
safety->private->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);
}
} else {
safety->private->warn_safe_max_l = 0;
safety->private->warn_safe_max_l = false;
}
/* outside minimum and not moving back */
if (position_in_l <= safety->private->position_min &&
out1 < 0.0) {
out1 = 0.0;
safe = 0;
safe = false;
if (!safety->private->err_min_l) {
safety->private->err_min_l = 1;
safety->private->err_min_l = true;
log_send(LOG_T_ERROR,
"Elevation left position %e under minimum %e",
position_in_l, safety->private->position_min);
}
} else {
safety->private->err_min_l = 0;
safety->private->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 = 1;
safety->private->warn_safe_min_l = true;
log_send(LOG_T_WARNING,
"Elevation left position in safe zone");
}
} else {
safety->private->warn_safe_min_l = 0;
safety->private->warn_safe_min_l = false;
}
if (fabsf(torsion_in) >= safety->private->torsion_recover_max) {
out0 = 0.0;
out1 = 0.0;
safe = 0;
safe = false;
if (!safety->private->err_torsion_recover) {
safety->private->err_torsion_recover = 1;
safety->private->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 = 0;
safety->private->err_torsion_recover = false;
}