Merge branch 'build' into beaglebone

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

+/*
+	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2014
+	Copyright Stichting C.A. Muller Radioastronomiestation, 2014
+
+	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/>.
+
+ */
+#define _ISOC99_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <float.h>
+
+#include <controller/controller_block.h>
+#include <controller/controller_command.h>
+#include <controller/controller_sample.h>
+#include <log/log.h>
+
+/*
+       inputs     outputs
+       nr name    nr name
+
+     --------------------------
+     |                        |
+ ----| 0  reset_x 0  x        |----
+     |                        |
+ ----| 1  reset   1  v        |----
+     |                        |
+     |            2  setpoint |----
+     |                        |
+     --------------------------
+
+	'setpoint' is the setpoint as the user requested it.
+	'x' is the output of the setpoint generator.
+	'v', is the first order derivative of 'x'.
+
+ */
+
+struct controller_block_private {
+	/*
+		In the setpoint generator blocks the following physics notations
+		will be used.
+	
+		x = position	
+		v = velocity (1st derivative of x)
+	*/
+
+
+	float *reset_x;
+	bool  *reset;
+	
+	/* beware: 'samples' is the time unit, not 'seconds', 
+	   all parameters and commands are scaled on block entry/exit */
+	float cmd_x;
+	float cmd_v;
+	float max_x;
+	float min_x;
+	float max_v;
+
+	/* 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;
+
+	/* current internal state */
+	float cur_x;
+	float cur_v;
+	
+	struct command_entry cur_command;
+	bool cur_done;
+	bool cur_start;
+	uint64_t command_t;
+
+	struct controller_command *command;
+};
+
+
+static void setpoint_generator_1d_calculate(struct controller_block *spg)
+{
+	struct controller_block_private *priv = spg->private;
+	float cur_x, cur_v;
+
+	cur_x = priv->cur_x;
+	cur_v = priv->cur_v;
+
+	if (*priv->reset) {
+		priv->cmd_x = *priv->reset_x;
+		priv->cur_x = priv->cmd_x;
+		priv->cur_done = true;
+		priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
+		priv->cmd_v = 0.0;
+		priv->cur_v = 0.0;
+		return;
+	}
+
+	if (priv->cur_done) {
+		int r;
+		
+		r = controller_command_queue_read(priv->command, &priv->cur_command);
+		if (r == 0) {
+			priv->cur_done = false;
+			priv->cur_start = false;
+		} else {
+			if (priv->cur_command.type ==
+			    COMMAND_PTYPE_SETPOINT_TIME) {
+				priv->cmd_v = 0.0;
+				priv->cmd_x = priv->cur_command.value.f;
+			}
+		}
+	}
+	if (!priv->cur_done) {
+		switch (priv->cur_command.type) {
+			case COMMAND_PTYPE_SETPOINT:
+				priv->cmd_x = priv->cur_command.value.f;
+				priv->cmd_v = 0.0;
+				priv->cur_done = true;
+				break;
+			case COMMAND_PTYPE_SPEED:
+				priv->cmd_v = priv->cur_command.value.f;
+				priv->cur_done = true;
+				break;
+			case COMMAND_PTYPE_SETPOINT_TIME:
+				if (!priv->cur_start) {
+					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)",
+						    spg->name, (long long)priv->cur_command.t.tv_sec, controller_time_seconds);
+						priv->cmd_x = priv->cur_command.value.f;
+						priv->cmd_v = 0.0;
+						priv->cur_done = true;
+					} else {
+						priv->cmd_v =
+						    (priv->cur_command.value.f -
+						    priv->cmd_x) / (t+1.0);
+						priv->cur_start = true;
+					}
+				}
+				
+				if (priv->command_t <= controller_time_nseconds) {
+					priv->cur_done = true;
+					priv->cmd_x =
+					   priv->cur_command.value.f -
+					   priv->cmd_v;
+				}
+				break;
+			default:
+				break;
+		}
+	}
+
+	if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
+		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;
+	} else {
+		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;
+			cur_v = 0;			
+		} else {
+			float v = copysign(priv->max_v, x_diff);
+
+			cur_x += v;
+			cur_v = v;
+		}
+	}
+	
+	priv->cur_x = cur_x;
+	priv->cur_v = cur_v * priv->freq;
+}
+
+static int block_setpoint_generator_command_filter(struct controller_command *command,
+    struct command_entry  *entry)
+{
+	struct controller_block_private *priv = command->block->private;
+	int r = 0;
+	
+	switch (entry->type) {
+		case COMMAND_PTYPE_SETPOINT_TIME:
+		case COMMAND_PTYPE_SETPOINT: {
+			float value = entry->value.f;
+			
+			if (value > priv->max_x)
+				value = priv->max_x;
+			if (value < priv->min_x)
+				value = priv->min_x;
+				
+			entry->value.f = value;
+			break;
+		}
+		case COMMAND_PTYPE_SPEED: {
+			float value = entry->value.f;
+			
+			if (fabs(value) > priv->max_v_sec)
+				value = copysign(priv->max_v_sec, value);
+			
+			entry->value.f = value;
+			
+			break;
+		}
+		default:
+			r = -1;
+			break;
+	}
+
+	return r;
+}
+
+static struct controller_block_param_list params[] = {
+	{ "setpoint",    true  },
+	{ "max_x",       true  },
+	{ "min_x",       true  },
+	{ "max_v",       true  },
+	{ NULL },
+};
+
+static void param_get(struct controller_block *spg, int param, void *val)
+{
+	switch (param) {
+		case 0:
+			*(float*)val =
+			    spg->private->cmd_x;
+			break;
+		case 1:
+			*(float*)val =
+			    spg->private->max_x;
+			break;
+		case 2:
+			*(float*)val =
+			    spg->private->min_x;
+			break;
+		case 3:
+			*(float*)val =
+			    spg->private->max_v_sec;
+			break;
+	}
+}
+
+static void param_set(struct controller_block *spg, int param, va_list val)
+{
+	switch (param) {
+		case 0:
+			spg->private->cmd_x = va_arg(val, double);
+			spg->private->cur_x = spg->private->cmd_x;
+			spg->private->cur_done = true;
+			spg->private->cmd_v = 0.0;
+			spg->private->cur_v = 0.0;
+			break;
+		case 1:
+			spg->private->max_x = va_arg(val, double);
+			break;
+		case 2:
+			spg->private->min_x = va_arg(val, double);
+			break;
+		case 3:
+			spg->private->max_v_sec = va_arg(val, double);
+			break;
+	}
+
+	/* Scale all settings to sample time unit */
+	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[] = {
+	{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
+	{ "reset",   CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, reset)   },
+	{ NULL }
+};
+
+static struct controller_block_outterm_list outterms[] = {
+	{ "x",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x) },
+	{ "v",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v) },
+	{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x) },
+	{ NULL }
+};
+
+
+struct controller_block * block_setpoint_generator_1d_create(char *name, va_list ap)
+{
+	struct controller_block *spg;
+	char *server_name;
+	char *spg_unit;
+	
+	server_name = va_arg(ap, char *);
+	spg_unit = va_arg(ap, char *);
+
+	spg = malloc(sizeof(struct controller_block));
+	if (!spg)
+		return NULL;
+
+	spg->type = "setpoint_generator_1d";
+	spg->name = malloc(strlen(name)+1);
+	if (!spg->name)
+		goto err_spg;
+	strcpy(spg->name, name);
+
+	spg->private = malloc(sizeof(struct controller_block_private));
+	if (!spg->private)
+		goto err_name;
+	spg->private->cmd_x = 0.0;
+	spg->private->cmd_v = 0.0;
+	spg->private->max_x = 0.0;
+	spg->private->min_x = 0.0;
+	spg->private->max_v = 0.0;
+	spg->private->max_v_sec = 0.0;
+	spg->private->freq = 1.0;
+
+	spg->private->cur_x = 0.0;
+	spg->private->cur_v = 0.0;
+
+	if (controller_block_interm_list_init(spg, interms))
+		goto err_private;
+
+	if (controller_block_outterm_list_init(spg, outterms))
+		goto err_input;
+
+	spg->calculate = setpoint_generator_1d_calculate;
+
+	if (controller_block_param_list_init(spg, params))
+		goto err_output;
+
+	spg->param_get = param_get;
+	spg->param_set = param_set;
+
+	spg->private->cur_done = true;
+	spg->private->cur_start = false;
+
+	controller_block_add(spg);
+
+	spg->private->command = controller_command_create(spg, 
+	    server_name, spg_unit);
+	spg->private->command->value_type = COMMAND_VALUE_TYPE_FLOAT;
+	spg->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;
+	spg->private->command->command_types[1] = COMMAND_PTYPE_SPEED;
+	spg->private->command->command_types[2] = COMMAND_PTYPE_SETPOINT_TIME;
+	spg->private->command->filter =
+	    block_setpoint_generator_command_filter;
+	
+	return spg;
+
+err_output:
+	free(spg->output);
+err_input:
+	free(spg->input);