Commit 98c41253 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Lots of small fixes

New 1d setpoint generator for simple control
Added test blocks
Added tests for some blocks
Removed broken iir_test
parent 87baa8ef
......@@ -120,6 +120,7 @@ TARGETS:=
include $(DIR)/$1/build.mk
targets_$$(DIR): $$(TARGETS)
$$(TARGETS): $(DIR)/$1/build.mk
TARGETS:= $$($(DIR)_TMPTARGETS) $$(TARGETS)
......@@ -127,6 +128,10 @@ DIR := $(DIR)
endef
CFLAGS := -pthread -Icommon -Icommon/include -Icontroller -Iconsole/console -Iinclude
LDFLAGS := -pthread -Lcommon/lib -Lcontroller/lib -Lconsole/console/lib -Llib -Wl,--as-needed
VPATH += common/lib controller/lib console/console/lib lib
TARGETS:=
$(eval $(call SUBDIR,common))
......@@ -140,9 +145,6 @@ TARGETS:=
$(eval $(call SUBDIR,console))
console: $(TARGETS)
CFLAGS := -Icommon -Icommon/include -Icontroller -Iconsole/console -Iinclude
LDFLAGS := -Lcommon/lib -Lcontroller/lib -Lconsole/console/lib -Llib -Wl,--as-needed
VPATH += common/lib controller/lib console/console/lib lib
SRCS_TMP:=$(SRCS:.il=.il2c.d)
......
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2013
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2008, 2013, 2014
Copyright Stichting C.A. Muller Radioastronomiestation, 2008, 2013
This program is free software: you can redistribute it and/or modify
......@@ -35,6 +35,7 @@
#include <limits.h>
#include <stdarg.h>
#include <stdbool.h>
#include <sched.h>
#include <utils/tcp_connect.h>
......@@ -57,7 +58,7 @@ struct log_client {
#define LOG_MAX_LEN 400
struct log_msg {
int used;
volatile int used;
time_t t;
int type;
char msg[LOG_MAX_LEN];
......@@ -68,6 +69,7 @@ static struct log_client clients[LOG_MAX_CLIENTS];
static struct log_msg msgs[LOG_MAX_QUEUE];
static int msgs_wr = 0;
static int msgs_rd = 0;
static int queue_size = 0;
static int lost_counter = 0;
static sem_t queue_wait;
......@@ -75,44 +77,59 @@ static sem_t queue_wait;
static enum log_type log_level_console;
static enum log_type log_level_remote;
static pthread_mutex_t log_mutex;
void log_send(enum log_type type, char *fmt, ...)
{
int val;
va_list ap;
int msg;
bool log_lost = false;
if (log_level_console < type && log_level_remote < type)
return;
va_start(ap, fmt);
pthread_mutex_lock(&log_mutex);
msg = __sync_fetch_and_add(&msgs_wr, 1);
msg %= LOG_MAX_QUEUE;
if (msgs[msgs_wr].used == 0) {
msgs[msgs_wr].t = time(NULL);
msgs[msgs_wr].type = type;
vsnprintf(msgs[msgs_wr].msg, LOG_MAX_LEN, fmt, ap);
msgs[msgs_wr].used = 1;
msgs_wr++;
msgs_wr %= LOG_MAX_QUEUE;
if (msgs[msg].used == 0) {
int lost;
lost = __sync_fetch_and_and(&lost_counter, 0);
if (lost) {
__sync_fetch_and_add(&queue_size, 1);
msgs[msg].t = time(NULL);
msgs[msg].type = LOG_T_ERROR;
snprintf(msgs[msg].msg, LOG_MAX_LEN,
"Lost %d messages", lost);
__sync_synchronize();
msgs[msg].used = 1;
/* Now try to get another entry for the real msg */
msg = __sync_fetch_and_add(&msgs_wr, 1);
msg %= LOG_MAX_QUEUE;
}
if (msgs[msg].used == 0) {
__sync_fetch_and_add(&queue_size, 1);
msgs[msg].t = time(NULL);
msgs[msg].type = type;
vsnprintf(msgs[msg].msg, LOG_MAX_LEN, fmt, ap);
__sync_synchronize();
msgs[msg].used = 1;
} else {
log_lost = true;
}
} else {
lost_counter++;
log_lost = true;
}
/* If we lost some in the past check if we can log now... */
if (lost_counter && msgs[msgs_wr].used == 0) {
msgs[msgs_wr].t = time(NULL);
msgs[msgs_wr].type = LOG_T_WARNING;
snprintf(msgs[msgs_wr].msg, LOG_MAX_LEN,
"Lost %d messages", lost_counter);
msgs[msgs_wr].used = 1;
msgs_wr++;
msgs_wr %= LOG_MAX_QUEUE;
lost_counter = 0;
if (log_lost) {
__sync_fetch_and_add(&lost_counter, 1);
}
pthread_mutex_unlock(&log_mutex);
va_end(ap);
sem_getvalue(&queue_wait, &val);
......@@ -228,6 +245,8 @@ static void *log_server(void *arg)
char *header;
struct tm gmt;
__sync_synchronize();
gmtime_r(&msgs[msgs_rd].t, &gmt);
timestamp(&gmt, time);
......@@ -260,10 +279,13 @@ static void *log_server(void *arg)
if (msgs[msgs_rd].type <= log_level_console)
printf("%s %s%s\n", time, header, msgs[msgs_rd].msg);
__sync_synchronize();
msgs[msgs_rd].used = 0;
msgs_rd++;
msgs_rd %= LOG_MAX_QUEUE;
__sync_fetch_and_sub(&queue_size, 1);
}
}
......@@ -320,6 +342,8 @@ static void *log_client(void *arg)
char *header;
struct tm gmt;
__sync_synchronize();
gmtime_r(&msgs[msgs_rd].t, &gmt);
timestamp(&gmt, time);
......@@ -356,9 +380,12 @@ static void *log_client(void *arg)
if (msgs[msgs_rd].type <= log_level_console)
printf("%s %s%s\n", time, header, msgs[msgs_rd].msg);
__sync_synchronize();
msgs[msgs_rd].used = 0;
msgs_rd++;
msgs_rd %= LOG_MAX_QUEUE;
__sync_fetch_and_sub(&queue_size, 1);
}
}
......@@ -406,10 +433,11 @@ int log_client_start(char *host, int port, enum log_type console_level,
void log_server_flush(void)
{
sleep(1);
do {
sleep(1);
} while (msgs[msgs_rd].used);
sem_post(&queue_wait);
sched_yield();
__sync_synchronize();
} while (queue_size);
}
void log_string(char *str, size_t len, enum log_type type, char *fmt, ...)
......
......@@ -6,7 +6,8 @@ TARGETS += $(DIR)/weather_test
WEATHER_SRCS := $(DIR)/weather_test.c $(DIR)/weather.c $(DIR)/tcp_connect.c
WEATHER_OBJS := $(WEATHER_SRCS:.c=.o)
$(DIR)/weather_test_LDFLAGS += -lpthread
$(DIR)/weather_test_CFLAGS += -pthread
$(DIR)/weather_test_LDFLAGS += -pthread
$(DIR)/weather_test: $(WEATHER_OBJS)
ARCHSRCS := $(DIR)/tcp_connect.c \
......@@ -21,8 +22,8 @@ ARCHSRCS := $(DIR)/tcp_connect.c \
ARCHOBJS := $(ARCHSRCS:.c=.lo)
LU_CFLAGS := -Wall -g -fPIC
LU_LDFLAGS := -lpthread -lrt -lm
LU_CFLAGS := -Wall -g -fPIC -pthread
LU_LDFLAGS := -lrt -lm
LU_CFLAGS += `pkg-config --cflags glib-2.0`
LU_LDFLAGS += -lglib-2.0
......
all:
@$(MAKE) --no-print-directory -C .. targets_controller
$(MAKECMDGOALS):
@$(MAKE) --no-print-directory -C .. $(MAKECMDGOALS)
all:
@$(MAKE) --no-print-directory -C ../.. targets_controller/block
$(MAKECMDGOALS):
@$(MAKE) --no-print-directory -C ../.. $(MAKECMDGOALS)
......@@ -72,6 +72,7 @@ struct controller_block_private {
/* conversion factor for seconds/tick and its inverse */
float freq; /* ticks per second */
uint64_t period_nsec;
/* parameters in real world format (time unit: second) */
float max_v_sec;
......@@ -83,6 +84,7 @@ struct controller_block_private {
struct command_entry cur_command;
bool cur_done;
bool cur_start;
uint64_t command_t;
struct controller_command *command;
};
......@@ -98,11 +100,12 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
if (*priv->reset) {
priv->cmd_x = *priv->reset_x;
cur_x = priv->cmd_x;
priv->cur_x = priv->cmd_x;
priv->cur_done = true;
priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
priv->cmd_v = 0.0;
cur_v = 0.0;
priv->cur_v = 0.0;
return;
}
if (priv->cur_done) {
......@@ -121,8 +124,6 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
}
}
if (!priv->cur_done) {
double t;
switch (priv->cur_command.type) {
case COMMAND_PTYPE_SETPOINT:
priv->cmd_x = priv->cur_command.value.f;
......@@ -135,18 +136,16 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
break;
case COMMAND_PTYPE_SETPOINT_TIME:
if (!priv->cur_start) {
double t_samplenr;
t_samplenr = floor(
((double)priv->cur_command.t.tv_nsec * priv->freq) /
1000000.0);
t = (double)(priv->cur_command.t.tv_sec
- controller_time_seconds) * priv->freq +
t_samplenr -
(double)controller_time_samplenr;
if (t <= 0.0) {
uint64_t command_t;
int64_t t;
command_t = priv->cur_command.t.tv_nsec +
priv->cur_command.t.tv_sec * 1000000000;
priv->command_t = command_t;
t = (command_t - controller_time_nseconds) / priv->period_nsec;
if (t < 0) {
/* Command is to old, pretend it
is a setpoint without time */
log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%lld < %d)",
......@@ -157,13 +156,12 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
} else {
priv->cmd_v =
(priv->cur_command.value.f -
priv->cmd_x) / (t+1);
priv->cmd_x) / (t+1.0);
priv->cur_start = true;
}
}
if (priv->cur_command.t.tv_sec <=
controller_time_seconds) {
if (priv->command_t <= controller_time_nseconds) {
priv->cur_done = true;
priv->cmd_x =
priv->cur_command.value.f -
......@@ -175,20 +173,20 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
}
}
if (fabs(priv->cmd_v) >= priv->max_v) {
priv->cmd_v = copysign(priv->max_v, priv->cmd_v);
}
if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
priv->cmd_x = cur_x + priv->cmd_v * controller_sample_period();
cur_v = priv->cmd_v * controller_sample_period();
priv->cmd_x = cur_x + cur_v;
if (priv->cmd_x > priv->max_x)
priv->cmd_x = priv->max_x;
if (priv->cmd_x < priv->min_x)
priv->cmd_x = priv->min_x;
cur_x = priv->cmd_x;
cur_v = priv->cmd_v;
} else {
float x_diff = priv->cmd_x - cur_x;
float x_diff;
priv->cmd_x += priv->cmd_v;
x_diff = priv->cmd_x - cur_x;
if (fabs(x_diff) <= priv->max_v) {
cur_x = priv->cmd_x;
......@@ -297,6 +295,7 @@ static void param_set(struct controller_block *spg, int param, va_list val)
spg->private->max_v = spg->private->max_v_sec * controller_sample_period();
spg->private->freq = 1.0 / controller_sample_period();
spg->private->period_nsec = controller_sample_period() * 1000000000;
}
static struct controller_block_interm_list interms[] = {
......
......@@ -54,9 +54,12 @@ BLOCKS+= \
joystick
endif
BLOCK_SRCS := \
$(addsuffix .c,$(addprefix $(DIR)/block_,$(BLOCKS))) \
$(addsuffix .il,$(addprefix $(DIR)/block_,$(BLOCKS_IL)))
BLOCK_SRCS_IL := $(addsuffix .il,$(addprefix $(DIR)/block_,$(BLOCKS_IL)))
$(BLOCK_SRCS_IL): $(IL2C)
BLOCK_SRCS := $(addsuffix .c,$(addprefix $(DIR)/block_,$(BLOCKS))) \
$(BLOCK_SRCS_IL)
BLOCK_OBJSC := $(BLOCK_SRCS:.c=.lo)
BLOCK_OBJS := $(BLOCK_OBJSC:.il=.il2c.lo)
......
......@@ -3,6 +3,7 @@ CLEAN += $(DIR)/lib/.libs
CTRL_BLOCKS :=
CTRL_BLOCK_LIBS :=
CTRL_TESTS :=
$(eval $(call SUBDIR,controller))
$(eval $(call SUBDIR,shell))
......@@ -27,8 +28,8 @@ $(DIR)/dt_ctrl: \
liblog.la \
libshell.la \
$(CTRL_BLOCK_LIBS)
$(DIR)/dt_ctrl: CFLAGS += -Wall -O3
$(DIR)/dt_ctrl: LDFLAGS += \
$(DIR)/dt_ctrl_CFLAGS += -Wall -O3
$(DIR)/dt_ctrl_LDFLAGS += \
-Wl,-E \
-lcontroller \
-llog \
......@@ -36,6 +37,20 @@ $(DIR)/dt_ctrl: LDFLAGS += \
$(BLOCKS)
$(DIR)/dt_ctrl: $(DT_CTRL_OBJS)
test: dt_ctrl_test
DT_CTRL_TESTS := $(CTRL_TESTS)
DT_CTRL := $(DIR)/dt_ctrl
define DT_CTRl_TEST
$(DT_CTRL) $1
endef
dt_ctrl_test: $(DIR)/dt_ctrl
dt_ctrl_test:
echo tests: $(DT_CTRL_TESTS)
$(foreach TESTITEM,$(DT_CTRL_TESTS),$(call DT_CTRl_TEST,$(TESTITEM)))
SRCS += $(DT_CTRL_SRCS)
TARGETS += $(DT_CTRL_TARGETS)
......
......@@ -17,9 +17,9 @@ CONTROLLER_SRCS= \
CONTROLLER_OBJS := $(CONTROLLER_SRCS:.c=.lo)
$(CONTROLLER_OBJS): CFLAGS += -O3 -Wall
$(CONTROLLER_OBJS): CFLAGS += -O3 -Wall -pthread
$(LIBDIR)/libcontroller.la: libshell.la libcommand.la libtrace.la
$(LIBDIR)/libcontroller.la_LDFLAGS += -lshell -ldl -lpthread -lrt -lcommand -ltrace
$(LIBDIR)/libcontroller.la_LDFLAGS += -lshell -ldl -pthread -lrt -lcommand -ltrace
$(LIBDIR)/libcontroller.la: $(CONTROLLER_OBJS)
$(LIB_LINK)
......
......@@ -153,52 +153,56 @@ void controller_block_trace(void)
}
for (i = 0; i < nr_traces; i++) {
traces[i]->wr_pos++;
if (traces[i]->wr_pos == traces[i]->len)
traces[i]->wr_pos = 0;
size_t wr_pos = traces[i]->wr_pos;
if (controller_time_samplenr == 0)
{
traces[i]->timestamp_pos = traces[i]->wr_pos;
traces[i]->timestamp_pos = wr_pos;
traces[i]->timestamp = controller_time_seconds;
} else if (traces[i]->timestamp_pos == traces[i]->wr_pos) {
} else if (traces[i]->timestamp_pos == wr_pos) {
traces[i]->timestamp_pos = traces[i]->len;
}
switch (traces[i]->type) {
case CONTROLLER_BLOCK_TERM_BOOL:
traces[i]->buffer[traces[i]->wr_pos].b =
traces[i]->buffer[wr_pos].b =
*(bool*)traces[i]->ptr;
break;
case CONTROLLER_BLOCK_TERM_FLOAT:
traces[i]->buffer[traces[i]->wr_pos].f =
traces[i]->buffer[wr_pos].f =
*(float*)traces[i]->ptr;
break;
case CONTROLLER_BLOCK_TERM_UINT32:
traces[i]->buffer[traces[i]->wr_pos].u32 =
traces[i]->buffer[wr_pos].u32 =
*(uint32_t*)traces[i]->ptr;
break;
case CONTROLLER_BLOCK_TERM_UINT16:
traces[i]->buffer[traces[i]->wr_pos].u16 =
traces[i]->buffer[wr_pos].u16 =
*(uint16_t*)traces[i]->ptr;
break;
case CONTROLLER_BLOCK_TERM_UINT8:
traces[i]->buffer[traces[i]->wr_pos].u8 =
traces[i]->buffer[wr_pos].u8 =
*(uint8_t*)traces[i]->ptr;
break;
case CONTROLLER_BLOCK_TERM_SINT32:
traces[i]->buffer[traces[i]->wr_pos].s32 =
traces[i]->buffer[wr_pos].s32 =
*(int32_t*)traces[i]->ptr;
break;
case CONTROLLER_BLOCK_TERM_SINT16:
traces[i]->buffer[traces[i]->wr_pos].s16 =
traces[i]->buffer[wr_pos].s16 =
*(int16_t*)traces[i]->ptr;
break;
case CONTROLLER_BLOCK_TERM_SINT8:
traces[i]->buffer[traces[i]->wr_pos].s8 =
traces[i]->buffer[wr_pos].s8 =
*(int8_t*)traces[i]->ptr;
break;
default:
traces[i]->buffer[traces[i]->wr_pos].s32 = -1;
traces[i]->buffer[wr_pos].s32 = -1;
}
wr_pos++;
if (wr_pos == traces[i]->len)
wr_pos = 0;
__sync_synchronize();
traces[i]->wr_pos = wr_pos;
}
}
......
......@@ -97,6 +97,18 @@ void controller_command_destroy(struct controller_command *command)
}
struct controller_command *controller_command_find_by_name(char *name)
{
int i;
for (i = 0; i < command_list_nr; i++) {
if (!strcmp(name, command_list[i]->name)) {
return command_list[i];
}
}
return NULL;
}
struct command_hdl {
bool free;
......@@ -125,34 +137,24 @@ static void handler_close(struct command *command)
static void handler_name(struct command *command, char *name)
{
struct command_hdl *hdl = command->private;
int i;
bool found = false;
struct controller_command *cc;
for (i = 0; i < command_list_nr; i++) {
if (!strcmp(name, command_list[i]->name)) {
found = true;
hdl->ccommand = command_list[i];
command->type = command_list[i]->value_type;
break;
}
}
if (!found) {
cc = controller_command_find_by_name(name);
if (cc) {
hdl->ccommand = cc;
command->type = cc->value_type;
log_send(LOG_T_DEBUG, "Handling commands for %s", name);
} else {
log_send(LOG_T_DEBUG, "Command server %s does not exist", name);
command_close(command);
hdl->free = true;
} else {
log_send(LOG_T_DEBUG, "Handling commands for %s", name);
}
}
static void handler_entry(struct command *command, struct command_entry *entry)
{
struct command_hdl *hdl = command->private;
int qw = hdl->ccommand->queue_w;
int qwn = (qw + 1) % CONTROLLER_COMMAND_QUEUE_DEPTH;
int qr = hdl->ccommand->queue_r;
if (hdl->ccommand->filter) {
if (hdl->ccommand->filter(hdl->ccommand, entry)) {
log_send(LOG_T_WARNING,
......@@ -161,15 +163,11 @@ static void handler_entry(struct command *command, struct command_entry *entry)
return;
}
}
if (qwn == qr) {
if (controller_command_queue_write(hdl->ccommand, entry)) {
log_send(LOG_T_WARNING,
"%s: Command queue full, dropping command",
hdl->ccommand->name);
} else {
memcpy(&hdl->ccommand->queue[qw], entry,
sizeof(struct command_entry));
hdl->ccommand->queue_w = qwn;
}
}
......
......@@ -36,7 +36,6 @@ struct controller_command {
/* returns zero if command ok. */
int (*filter)(struct controller_command *command, struct command_entry *entry);
struct command_entry queue[CONTROLLER_COMMAND_QUEUE_DEPTH];
int queue_w;
int queue_r;
......@@ -46,6 +45,7 @@ struct controller_command *controller_command_create(
struct controller_block *block, char *name, char *unit);
void controller_command_destroy(struct controller_command *command);
struct controller_command *controller_command_find_by_name(char *name);
void controller_command_server_start(int portnr, int max);
......@@ -57,10 +57,13 @@ static inline int controller_command_queue_read(struct controller_command *comma
int qw = command->queue_w;
if (qr != qw) {
__sync_synchronize();
memcpy(entry, &command->queue[qr], sizeof(struct command_entry));
qr++;
qr %= CONTROLLER_COMMAND_QUEUE_DEPTH;
__sync_synchronize();
command->queue_r = qr;
return 0;
}
......@@ -68,4 +71,27 @@ static inline int controller_command_queue_read(struct controller_command *comma
return -1;
}
static inline int controller_command_queue_write(
struct controller_command *command,
struct command_entry *entry)
{
int qw = command->queue_w;
int qwn = (qw + 1) % CONTROLLER_COMMAND_QUEUE_DEPTH;
int qr;
qr = command->queue_r;
__sync_synchronize();
if (qwn == qr) {