Add limit_switch block. It limits directions based on the inputs.

Add range checkers to take over azimuth position range checks.
Add test for new block.
Remove some more logs from the az_safety block.

controller/block/block_limit_switch.c

+/*
+	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2015
+
+	This program is free software: you can redistribute it and/or modify
+	it under the terms of the GNU General Public License as published by
+	the Free Software Foundation, either version 3 of the License, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <controller/controller_block.h>
+#include <log/log.h>
+
+/*
+       inputs      outputs
+       nr name     nr name
+
+     ----------------------
+     |                    |
+  ---| 0  in       0  out |----
+     |	                  |
+  ---| 1  enable_pos      |
+     |	                  |
+  ---| 2  enable_neg      |
+     |	                  |
+     ----------------------
+     
+ */
+
+struct controller_block_private {
+	float *in;
+	bool *enable_pos;
+	bool *enable_neg;
+	float out;
+};
+
+static void limit_switch_calculate(struct controller_block *limit)
+{
+	struct controller_block_private *priv = limit->private;
+	float out;
+	float in = *priv->in;
+	bool pos = *priv->enable_pos;
+	bool neg = *priv->enable_neg;
+
+	out = 0.0;
+	if (in > 0.0 && pos)
+		out = in;
+	if (in < 0.0 && neg)
+		out = in;
+
+	priv->out = out;
+}
+
+static struct controller_block_interm_list interms[] = {
+	{ "in", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, in) },
+	{ "enable_pos", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_pos) },
+	{ "enable_neg", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, enable_neg) },
+	{ NULL }
+};
+
+static struct controller_block_outterm_list outterms[] = {
+	{ "out", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, out) },
+	{ NULL }
+};
+
+
+static struct controller_block * block_limit_switch_create(char *name, int argc, va_list val)
+{
+	struct controller_block *limit;
+	
+	if (!(limit = controller_block_alloc("limit_switch", name, sizeof(struct controller_block_private))))
+		return NULL;
+
+	limit->private->out = 0.0;
+
+	if (controller_block_interm_list_init(limit, interms))
+		goto err_block;
+
+	if (controller_block_outterm_list_init(limit, outterms))
+		goto err_block;
+	
+	limit->calculate = limit_switch_calculate;
+
+	if (controller_block_add(limit))
+		goto err_block;
+
+	return limit;
+
+err_block:
+	controller_block_free(limit);
+	return NULL;
+}
+
+BLOCK_CREATE(limit_switch) = {
+	.create = block_limit_switch_create,
+	.args = { NULL },
+};

controller/block/block_limit_switch.test.ctrl

+
+trigger {
+	{ "immediate" }
+}
+
+blocks (10.0, 0.0) {
+	{ "limit_switch", "limit" }
+	
+	{ "test_input_float", "in" }
+	{ "test_input_bool", "enable_pos" }
+	{ "test_input_bool", "enable_neg" }
+	
+	{ "test_output_float", "out"   }
+}
+
+links {
+	{ "in", "value",         "limit", "in",         true }
+	{ "enable_pos", "value", "limit", "enable_pos", true }
+	{ "enable_neg", "value", "limit", "enable_neg", true }
+	{ "limit", "out",        "out", "value",        true }
+}
+
+params {
+	{ "in", "value", 20, (float) { 
+	    0.0, -1.0, -20000.0, 1.0, 20000.0,
+	    0.0, -1.0, -20000.0, 1.0, 20000.0,
+	    0.0, -1.0, -20000.0, 1.0, 20000.0,
+	    0.0, -1.0, -20000.0, 1.0, 20000.0 } 
+	}
+	{ "enable_pos", "value", 20, (int) { 
+	    false, false, false, false, false,
+	    false, false, false, false, false,
+	    true,  true,  true,  true,  true,
+	    true,  true,  true,  true,  true }
+	}
+	{ "enable_neg", "value", 20, (int) { 
+	    false, false, false, false, false,
+	    true,  true,  true,  true,  true,
+	    false, false, false, false, false,
+	    true,  true,  true,  true,  true }
+	}
+
+	{ "out", "value", 20, 
+		(float) { 
+		    0.0, 0.0, 0.0, 0.0, 0.0, 
+		    0.0, -1.0, -20000.0, 0.0, 0.0, 
+		    0.0, 0.0, 0.0, 1.0, 20000.0, 
+		    0.0, -1.0, -20000.0, 1.0, 20000.0
+		},
+		(float) { 
+		    0.0, 0.0, 0.0, 0.0, 0.0, 
+		    0.0, 0.0, 0.0, 0.0, 0.0, 
+		    0.0, 0.0, 0.0, 0.0, 0.0, 
+		    0.0, 0.0, 0.0, 0.0, 0.0
+		}
+	}
+}

controller/block/block_log.c

@@ -93,9 +93,20 @@ static int param_msg_down(struct controller_block *block, char *param,
 	return 0;
 }
 
+static int param_init(struct controller_block *block, char *param,
+    int argc, va_list val)
+{
+	struct controller_block_private *priv = block->private;
+	
+	priv->prev = va_arg(val, int);
+	
+	return 0;
+}
+
 static struct controller_block_param_list params[] = {
 	{ "msg_up", false, param_msg_up, .args = { "int,char*", NULL } },
 	{ "msg_down", false, param_msg_down, .args = { "int,char*", NULL } },
+	{ "init", true, param_init, .args = { "int", NULL } },
 	{ NULL },
 };
 

controller/block/build.mk

@@ -20,6 +20,7 @@ BLOCKS := \
 	limit \
 	limit_dyn \
 	limit_2nd \
+	limit_switch \
 	limit_var \
 	log \
 	matrix_2x2 \
@@ -91,6 +92,7 @@ CTRL_TESTS += \
 	$(DIR)/block_command_float.test.ctrl \
 	$(DIR)/block_gain.test.ctrl \
 	$(DIR)/block_limit.test.ctrl \
+	$(DIR)/block_limit_switch.test.ctrl \
 	$(DIR)/block_limit_var.test.ctrl \
 	$(DIR)/block_log.test.ctrl \
 	$(DIR)/block_matrix_2x2.test.ctrl \

controller/dt_azimuth/dt_az_safety.c

@@ -81,8 +81,6 @@ struct controller_block_private {
 	float safe_zone_max_speed;
 
 	/* keep log state to prevent flooding */
-	bool err_max;
-	bool err_min;
 	bool warn_safe_max;
 	bool warn_safe_min;
 
@@ -122,14 +120,6 @@ static void dt_az_safety_calculate(struct controller_block *safety)
 	    out > 0.0) {
 		out = 0.0;
 		safe = false;
-		if (!priv->err_max) {
-			priv->err_max = true;
-			log_send(LOG_T_ERROR, "Azimuth position above maximum %e > %e",
-			    *priv->position_in,
-			    priv->position_max);
-		}
-	} else {
-		priv->err_max = false;
 	}
 	if (*priv->position_in >= priv->safe_zone_max &&
 	    out > priv->safe_zone_max_speed) {
@@ -146,14 +136,6 @@ static void dt_az_safety_calculate(struct controller_block *safety)
 	    out < 0.0) {
 		out = 0.0;
 		safe = false;
-		if (!priv->err_min) {
-			priv->err_min = true;
-			log_send(LOG_T_ERROR, "Azimuth position under minimum %e < %e",
-			    *priv->position_in,
-			    priv->position_min);
-		}
-	} else {
-		priv->err_min = false;
 	}
 	if (*priv->position_in <= priv->safe_zone_min &&
 	    out < priv->safe_zone_min_speed) {
@@ -308,8 +290,6 @@ static struct controller_block * block_dt_az_safety_create(char *name, int argc,
 	safety->private->safe_zone_max_speed = 0.0;
 	safety->private->emergency_torque = 0.0;
 
-	safety->private->err_max = false;
-	safety->private->err_min = false;
 	safety->private->warn_safe_max = false;
 	safety->private->warn_safe_min = false;
 	

controller/dt_ctrl.ctrl

@@ -53,7 +53,11 @@ blocks ($(frequency), $(delay)) {
 
 	{ "log",                   "azimuth_safety_hw_pos" }
 	{ "log",                   "azimuth_safety_hw_neg" }
-	
+	{ "rangecheck",            "azimuth_speed_range_negative" }
+	{ "rangecheck",            "azimuth_speed_range_positive" }
+	{ "or2",                   "azimuth_position_range"       }
+	{ "log",                   "azimuth_position_range_log"   }
+		
 	{ "matrix_2x2",	           "elevation_input_matrix"	      }
 	{ "setpoint_generator_3d", "elevation_spg", 		"Elevation_Setpoint", "rad" }
 	{ "servo_state",           "elevation_servo_state"            }
@@ -140,6 +144,11 @@ links {
 
 	{ $<Azimuth_Drive_Safety_p270>,                   "azimuth_safety_hw_pos", "condition",                true }
 	{ $<Azimuth_Drive_Safety_m270>,                   "azimuth_safety_hw_neg", "condition",                true }
+	{ "azimuth_speed_limit", "out",                   "azimuth_speed_range_negative", "in",                true }
+	{ "azimuth_speed_limit", "out",                   "azimuth_speed_range_positive", "in",                true }
+	{ "azimuth_speed_range_positive", "valid",        "azimuth_position_range", "a",                       true }
+	{ "azimuth_speed_range_negative", "valid",        "azimuth_position_range", "b",                       true }
+	{ "azimuth_position_range", "q",                  "azimuth_position_range_log", "condition",           true }
 
 	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
 	{ $<Elevation_Position>,                          "elevation_spg",                    "reset_x"      , true }
@@ -333,9 +342,19 @@ params {
 
 	{ "azimuth_safety_hw_pos", "msg_up",   2, "Azimuth HW positive safety switch is closed" }
 	{ "azimuth_safety_hw_pos", "msg_down", 0, "Azimuth HW positive safety switch is open"   }
+	{ "azimuth_safety_hw_pos", "init",     true }
 	{ "azimuth_safety_hw_neg", "msg_up",   2, "Azimuth HW negative safety switch is closed" }
 	{ "azimuth_safety_hw_neg", "msg_down", 0, "Azimuth HW negative safety switch is open"   }
+	{ "azimuth_safety_hw_neg", "init",     true }
+
+	{ "azimuth_speed_range_negative", "max", deg2rad(290.0)  }
+	{ "azimuth_speed_range_negative", "min", deg2rad(-280.0) }
+	{ "azimuth_speed_range_negative", "max", deg2rad(-290.0) }
+	{ "azimuth_speed_range_negative", "min", deg2rad(280.0)  }
 
+	{ "azimuth_position_range_log", "msg_up",   1, "Azimuth position inside allowed range"  }
+	{ "azimuth_position_range_log", "msg_down", 0, "Azimuth position outside allowed range" }
+	{ "azimuth_position_range_log", "init",     true }
 	
         { "elevation_input_matrix", "constants",          (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
                                         	          (float) { 1.0/$(elevation_gear),  1.0/$(elevation_gear) } }
@@ -400,8 +419,10 @@ params {
 
 	{ "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_top",    "init",     true }
 	{ "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"   }
+	{ "elevation_safety_hw_bottom", "init",     true }
 }
 
 # Load file with calibration parameters.