Remove some code from elevation safety block to control file.

controller/dt_ctrl.ctrl

@@ -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.

controller/dt_elevation/dt_el_safety.c

@@ -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))