Add rangecheck block. (cherry pick from the corso2013 branch)

Add speed warning to azimuth controller.

controller/block/Makefile

@@ -34,6 +34,7 @@ BLOCKSRCS= \
 	block_quadrature_decoder.c \
 	block_quantize.c \
 	block_random.c \
+	block_rangecheck.c \
 	block_register.c \
 	block_setpoint_generator.c \
 	block_setreset.c \

controller/block/block_rangecheck.c

+/*
+	Copyright Jeroen Vreeken (jeroen@vreeken.net), 2013
+
+	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_block.h"
+
+/*
+       inputs      outputs
+       nr name     nr name
+
+     ----------------------
+     |                    |
+  ---| 0  in    0   valid |----
+     |	        1 invalid |
+     ----------------------
+
+     
+ */
+
+struct controller_block_private {
+	float *in;
+	bool valid;
+	bool invalid;
+
+	float min;
+	float max;
+};
+
+static void rangecheck_calculate(struct controller_block *check)
+{
+	struct controller_block_private *priv = check->private;
+	float in = *priv->in;
+	bool invalid;
+	
+	invalid = (in < priv->min) | (in > priv->max);
+	
+	priv->invalid = invalid;
+	priv->valid = !invalid;
+}
+
+static struct controller_block_param_list params[] = {
+	{ "max", false },
+	{ "min", false },
+	{ NULL },
+};
+
+static void param_get(struct controller_block *check, int param, void *val)
+{
+	switch (param) {
+		case 0:
+			*(float*)val = check->private->max;
+			break;
+		case 1:
+			*(float*)val = check->private->min;
+			break;
+	}
+}
+
+static void param_set(struct controller_block *check, int param, va_list val)
+{
+	switch (param) {
+		case 0:
+			check->private->max = va_arg(val, double);
+			break;
+		case 1:
+			check->private->min = va_arg(val, double);
+			break;
+	}
+}
+
+static struct controller_block_interm_list interms[] = {
+	{ "in",  CONTROLLER_BLOCK_TERM_FLOAT, 
+	    offsetof(struct controller_block_private, in)  },
+	{ NULL },
+};
+
+static struct controller_block_outterm_list outterms[] = {
+	{ "valid",  CONTROLLER_BLOCK_TERM_BOOL, 
+	    offsetof(struct controller_block_private, valid)  },
+	{ "invalid",  CONTROLLER_BLOCK_TERM_BOOL, 
+	    offsetof(struct controller_block_private, invalid)  },
+	{ NULL },
+};
+
+
+struct controller_block * block_rangecheck_create(char *name)
+{
+	struct controller_block *check;
+	
+	check = controller_block_alloc("rangecheck", name,
+	    sizeof(struct controller_block_private));
+	if (!check)
+		return NULL;
+
+	check->private->invalid = true;
+	check->private->valid = false;
+	check->private->max = 0.0;
+	check->private->min = 0.0;
+
+	if (controller_block_interm_list_init(check, interms))
+		goto err_block;
+
+	if (controller_block_outterm_list_init(check, outterms))
+		goto err_block;
+
+	check->calculate = rangecheck_calculate;
+
+	if (controller_block_param_list_init(check, params))
+		goto err_block;
+
+	check->param_get = param_get;
+	check->param_set = param_set;
+
+	controller_block_add(check);
+	return check;
+
+err_block:
+	controller_block_free(check);
+	return NULL;
+}

controller/dt_ctrl.ctrl

@@ -41,6 +41,7 @@ blocks {
 	{ "value",	        "azimuth_position_offset"  	   }
 	{ "add",  	        "azimuth_position_offset_sum"	   }
 	{ "gain", 	        "azimuth_position_gain"		   }
+	{ "rangecheck",		"azimuth_speed_warning"            }
 	
 	{ "matrix_2x2",	        "elevation_input_matrix"	   }
 	{ "setpoint_generator", "elevation_spg", 		"Elevation_Setpoint", "rad" }
@@ -103,7 +104,8 @@ links {
 	{ "azimuth_speed_servo",            "out",        "dt_az",                            "speed"        , true }
 	{ "azimuth_safety",                 "torque_out", "dt_az",                            "torque"       , true }
 	{ "azimuth_servo_state",            "enable",     "dt_az",                            "enable"       , true }
-	{ "false",                          "value",      "dt_az",                            "ba1"          , true }
+	{ "dt_az",                          "speed",      "azimuth_speed_warning",            "in"           , true }
+	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
 	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
 
 	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
@@ -257,6 +259,9 @@ params {
 							  (float) { 1.0, 2.0 }
 										}
 
+	{ "azimuth_speed_warning", "max",                 (double) rpm2rads(200.0)  }
+	{ "azimuth_speed_warning", "min",                 (double) rpm2rads(-200.0) }
+
 	# Due to high vibrations observed at high speed it was decided to limit
 	# the pid controller. At low speed the value is untouched, but the 
 	# output is limited when speed increases