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