Lots of small fixes

New 1d setpoint generator for simple control
Added test blocks
Added tests for some blocks
Removed broken iir_test

Makefile

@@ -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)

common/log/log.c

 /*
-	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, ...)

common/utils/build.mk

@@ -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

controller/Makefile

 all:
 	@$(MAKE) --no-print-directory -C .. targets_controller
+
+$(MAKECMDGOALS):
+	@$(MAKE) --no-print-directory -C .. $(MAKECMDGOALS)

controller/block/Makefile

 all:
 	@$(MAKE) --no-print-directory -C ../.. targets_controller/block
+
+$(MAKECMDGOALS):
+	@$(MAKE) --no-print-directory -C ../.. $(MAKECMDGOALS)

controller/block/block_setpoint_generator_1d.c

@@ -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[] = {

controller/block/build.mk

@@ -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)

controller/build.mk

@@ -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)

controller/controller/build.mk

@@ -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)
 

controller/controller/controller_block_trace.c

@@ -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;
 		
 	}
 }

controller/controller/controller_command.c

@@ -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;
 	}
 }
 

controller/controller/controller_command.h

@@ -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();
+