Commit b3cf78ba authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

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.
parent 3abf6c50
......@@ -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, ...)
......
/*
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 },
};
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
}}
}
......@@ -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 \
......
......@@ -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;
......
......@@ -51,6 +51,9 @@ blocks ($(frequency), $(delay)) {
{ "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" }
{ "servo_state", "elevation_servo_state" }
......@@ -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 }
......@@ -319,6 +325,12 @@ params {
# 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) } }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment