Add log block for edge triggered logging.

Use log block for logging DT HW position switches.
Remove hard coded logging from safety block.
Leave the rest of the block intact for now.

common/log/log.c

@@ -483,7 +483,7 @@ void log_server_flush(void)
 		sem_post(&queue_wait);
 		sched_yield();
 		__sync_synchronize();
-	} while (queue_size);
+	} while (__sync_fetch_and_or(&queue_size, 0));
 }
 
 void log_string(char *str, size_t len, enum log_type type, char *fmt, ...)

controller/block/block_log.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 <log/log.h>
+#include <controller/controller_block.h>
+
+
+/*
+       inputs          
+       nr name         
+
+     -------------------
+  ---| 0  condition    |
+     -------------------
+ */
+
+struct controller_block_private {
+	bool *input;
+	
+	bool prev;
+	
+	enum log_type level_up;
+	enum log_type level_down;
+	char *msg_up;
+	char *msg_down;
+};
+
+static void block_calculate(struct controller_block *block)
+{
+	struct controller_block_private *priv = block->private;
+	bool input = *priv->input;
+	bool prev = priv->prev;
+
+	if (input && !prev) {
+		log_send(priv->level_up, "%s: %s", block->name, priv->msg_up);
+	}
+	if (!input && prev) {
+		log_send(priv->level_down, "%s: %s", block->name, priv->msg_down);
+	}
+
+	priv->prev = input;
+}
+
+static struct controller_block_interm_list interms[] = {
+	{ "condition", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, input) },
+	{ NULL }
+};
+
+static int param_msg_up(struct controller_block *block, char *param, 
+    int argc, va_list val)
+{
+	struct controller_block_private *priv = block->private;
+	enum log_type level = va_arg(val, int);
+	char *msg = va_arg(val, char *);
+	
+	free(priv->msg_up);
+	priv->msg_up = strdup(msg);
+	priv->level_up = level;
+	
+	return 0;
+}
+
+static int param_msg_down(struct controller_block *block, char *param, 
+    int argc, va_list val)
+{
+	struct controller_block_private *priv = block->private;
+	enum log_type level = va_arg(val, int);
+	char *msg = va_arg(val, char *);
+	
+	free(priv->msg_down);
+	priv->msg_down = strdup(msg);
+	priv->level_down = level;
+	
+	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 } },
+	{ NULL },
+};
+
+static struct controller_block * block_log_create(char *name, int argc, va_list val)
+{
+	struct controller_block *block;
+	
+	if (!(block = controller_block_alloc("log", name, sizeof(struct controller_block_private))))
+		return NULL;
+
+	if (controller_block_interm_list_init(block, interms))
+		goto err_block;
+		
+	if (controller_block_param_list_add(block, params))
+		goto err_block;
+
+	block->private->msg_up = strdup("Condition raised");
+	block->private->msg_down = strdup("Condition dropped");
+
+	block->calculate = block_calculate;
+
+	if (controller_block_add(block))
+		goto err_block;
+	
+	return block;
+
+err_block:
+	controller_block_free(block);
+	return NULL;
+}
+
+BLOCK_CREATE(log) = {
+	.create = block_log_create,
+	.args = { NULL },
+};

controller/block/block_log.test.ctrl

+trigger {
+	{ "immediate" }
+}
+
+blocks (10.0, 0.0) {
+	{ "log", "log_error_warning" }
+	{ "log", "log_info_debug" }
+
+	{ "test_input_bool", "condition" }
+}
+
+links {
+	{ "condition", "value", "log_error_warning", "condition", true }
+	{ "condition", "value", "log_info_debug", "condition",    true }
+}
+
+params {
+	{ "log_error_warning", "msg_up",   0, "Edge up (error)"     }
+	{ "log_error_warning", "msg_down", 1, "Edge down (warning)" }
+	{ "log_info_debug",    "msg_up",   2, "Edge up (info)"      }
+	{ "log_info_debug",    "msg_down", 3, "Edge down (debug)"   }
+
+	{ "condition", "value", 10, (int) {
+	    false, true,  true,  true,  true, 
+	    false, false, false, false, false
+	}}
+}

controller/block/build.mk

@@ -21,6 +21,7 @@ BLOCKS := \
 	limit_dyn \
 	limit_2nd \
 	limit_var \
+	log \
 	matrix_2x2 \
 	multiplexer \
 	pand \
@@ -91,6 +92,7 @@ CTRL_TESTS += \
 	$(DIR)/block_gain.test.ctrl \
 	$(DIR)/block_limit.test.ctrl \
 	$(DIR)/block_limit_var.test.ctrl \
+	$(DIR)/block_log.test.ctrl \
 	$(DIR)/block_matrix_2x2.test.ctrl \
 	$(DIR)/block_not.test.ctrl \
 	$(DIR)/block_oneshot.test.ctrl \

controller/dt_azimuth/dt_az_safety.c

@@ -85,8 +85,6 @@ struct controller_block_private {
 	bool err_min;
 	bool warn_safe_max;
 	bool warn_safe_min;
-	bool err_hwsafe_positive;
-	bool err_hwsafe_negative;
 
 	bool recover;
 
@@ -99,7 +97,7 @@ static void dt_az_safety_calculate(struct controller_block *safety)
 	float out;
 	bool safe = true;
 	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;
@@ -170,27 +168,9 @@ static void dt_az_safety_calculate(struct controller_block *safety)
 	
 	if (*priv->safety_in_positive == 0) {
 		safe = false;
-		if (!priv->err_hwsafe_positive) {
-			priv->err_hwsafe_positive = true;
-			log_send(LOG_T_ERROR, "Azimuth HW positive safety switch is open.");
-		}
-	} else {
-		if (priv->err_hwsafe_positive) {
-			priv->err_hwsafe_positive = false;
-			log_send(LOG_T_INFO, "Azimuth HW positive safety switch is closed.");
-		}
 	}
 	if (*priv->safety_in_negative == 0) {
 		safe = false;
-		if (!priv->err_hwsafe_negative) {
-			priv->err_hwsafe_negative = true;
-			log_send(LOG_T_ERROR, "Azimuth HW negative safety switch is open.");
-		}
-	} else {
-		if (priv->err_hwsafe_negative) {
-			priv->err_hwsafe_negative = false;
-			log_send(LOG_T_INFO, "Azimuth HW negative safety switch is closed.");
-		}
 	}
 
 	if (!safe) {
@@ -332,8 +312,6 @@ static struct controller_block * block_dt_az_safety_create(char *name, int argc,
 	safety->private->err_min = false;
 	safety->private->warn_safe_max = false;
 	safety->private->warn_safe_min = false;
-	safety->private->err_hwsafe_positive = false;
-	safety->private->err_hwsafe_negative = false;
 	
 	safety->private->recover = false;
 

controller/dt_ctrl.ctrl

@@ -50,6 +50,9 @@ blocks ($(frequency), $(delay)) {
 	{ "add",  	           "azimuth_position_offset_sum"   }
 	{ "gain", 	           "azimuth_position_gain"	   }
 	{ "rangecheck",		   "azimuth_speed_warning"         }
+
+	{ "log",                   "azimuth_safety_hw_pos" }
+	{ "log",                   "azimuth_safety_hw_neg" }
 	
 	{ "matrix_2x2",	           "elevation_input_matrix"	      }
 	{ "setpoint_generator_3d", "elevation_spg", 		"Elevation_Setpoint", "rad" }
@@ -132,6 +135,9 @@ links {
 	{ "azimuth_speed_warning",          "invalid",    "dt_az",                            "ba1"          , true }
 	{ "false",			    "value",      "dt_az",                            "ba2"          , true }
 
+	{ $<Azimuth_Drive_Safety_p270>,                   "azimuth_safety_hw_pos", "condition",                true }
+	{ $<Azimuth_Drive_Safety_m270>,                   "azimuth_safety_hw_neg", "condition",                true }
+
 	{ "elevation_servo_state",          "reset",      "elevation_spg",                    "reset"        , false }
 	{ $<Elevation_Position>,                          "elevation_spg",                    "reset_x"      , true }
 	{ "elevation_spg",                  "x",          "elevation_servo_state",            "spg_x"        , true }
@@ -318,6 +324,12 @@ params {
 	{ "azimuth_safety",        "emergency_torque",    (float) 3.0           }
 	# we do not start in recovery mode
 	{ "azimuth_safety",        "recover",             (int)   0  		}
+
+	{ "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_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"   }
+
 	
         { "elevation_input_matrix", "constants",          (float) { 0.5/$(elevation_gear), -0.5/$(elevation_gear) },
                                         	          (float) { 1.0/$(elevation_gear),  1.0/$(elevation_gear) } }