Merge branch 'jeroen' into controller_bus

common/utils/Makefile

@@ -11,7 +11,7 @@ LIBUTILOBJS= $(UTILSRCS:.c=.lo)
 
 all: $(LIBUTILOBJS) weather_test libutils.la libutils.la_install
 
-libutils.la_LDFLAGS= -lm -rpath ${CURDIR}/../lib
+libutils.la_LDFLAGS= -lm -rpath ${CURDIR}/../lib -lrt
 libutils.la: $(LIBUTILOBJS)
 
 ${CURDIR}/../lib:

console/console/Makefile

@@ -8,7 +8,7 @@ CFLAGS= -Wall -O3 -I../../common/utils \
 	-I../../common/log \
 	-I../../include
 
-LDFLAGS+= -L../../common/lib/ -L../../lib/ -L./lib -lutils -ltrace -static -llog
+LDFLAGS+= -L../../common/lib/ -L../../lib/ -L./lib -lutils -ltrace -llog
 
 LIBNOVA=-lnova -L../../lib/
 

controller/block/Makefile

@@ -70,7 +70,7 @@ il2cdir:
 
 $(BLOCKSRCS): il2cdir
 
-libblock.la_LDFLAGS=-rpath ${CURDIR}/../lib
+libblock.la_LDFLAGS=-rpath ${CURDIR}/../lib -lm
 libblock.la: $(BLOCKS)
 
 libblock.la_install: libblock.la

controller/block/block_setpoint_generator.c

@@ -97,6 +97,10 @@ struct controller_block_private {
 	/* conversion factor for seconds/tick and its inverse */
 	float tick;	/* seconds per tick */
 	float freq;	/* ticks per second */
+	float freq2;
+	float freq3;
+	double t_max_a;
+	double v_delta_from_max_a;
 
 	/* parameters in real world format (time unit: second) */
 	float max_v_sec;
@@ -107,6 +111,13 @@ struct controller_block_private {
 	float precision_v_sec;
 	float precision_a_sec;
 
+	/* state at last change */
+	double start_x;
+	double start_v;
+	double start_a;
+	double start_j;
+	int start_t;
+
 	/* current internal state */
 	double cur_x;
 	double cur_v;
@@ -151,8 +162,8 @@ static double ticks_to_v(struct controller_block_private *priv,
 	double t;
 	
 	/* 	From a constant speed to a constant speed is done in two halves:
-		First halve (untill half the speed difference is reached is done
-		at max jerk, second half is done at -max jerk.
+		First halve (untill half the speed difference is reached) is 
+		done at max jerk, second half is done at -max jerk.
 		
 		1/2 v = 1/2 j t^2   ->   v = j t^2   ->  t = sqrt(v/j) 
 		
@@ -203,22 +214,36 @@ static double x_after_ticks(struct controller_block_private *priv,
 	return x_at_t;
 }
 
-static void calculate(struct controller_block *spg)
+static void setpoint_generator_calculate(struct controller_block *spg)
 {
 	struct controller_block_private *priv = spg->private;
+	double cur_x, cur_v;
+	double t_max_a;
 	bool ignore_x = false;
 	bool must_brake = false;
+	bool good_x;
+	bool good_v;
+
+	cur_x = priv->cur_x;
+	cur_v = priv->cur_v;
+
+	t_max_a = priv->t_max_a;
 
 	if (*priv->reset) {
 		priv->cmd_x = *priv->reset_x;
-		priv->cur_x = priv->cmd_x;
+		cur_x = priv->cmd_x;
 		priv->current_command.done = 1;
 		priv->current_command.type = BLOCK_SPG_SETPOINT;
 		priv->next_command.done = 1;
 		priv->cmd_v = 0.0;
-		priv->cur_v = 0.0;
+		cur_v = 0.0;
 		priv->cur_a = 0.0;
 		priv->cur_j = 0.0;
+		priv->start_x = cur_x;
+		priv->start_v = 0.0;
+		priv->start_a = 0.0;
+		priv->start_j = 0.0;
+		priv->start_t = 0;
 	}
 
 	if (priv->current_command.done) {
@@ -266,7 +291,7 @@ static void calculate(struct controller_block *spg)
 					} else {
 						priv->cmd_v =
 						    (priv->current_command.setpoint -
-						    priv->cmd_x) / t;
+						    priv->cmd_x) / (t+1);
 						priv->current_command.start = 1;
 					}
 				}
@@ -291,9 +316,7 @@ static void calculate(struct controller_block *spg)
 
 	if (priv->current_command.type == BLOCK_SPG_SPEED) {
 		ignore_x = true;
-		priv->cmd_x = priv->cur_x;
-	} else {
-		priv->cmd_x += priv->cmd_v;
+		priv->cmd_x = cur_x;
 	}
 	
 	if (priv->cmd_x > priv->max_x)
@@ -301,13 +324,20 @@ static void calculate(struct controller_block *spg)
 	if (priv->cmd_x < priv->min_x)
 		priv->cmd_x = priv->min_x;
 	
-	if (!almost_equal(priv->cur_v, priv->cmd_v) ||
-	    !almost_equal(priv->cur_x, priv->cmd_x)) {
-		double error_x = priv->cmd_x - priv->cur_x;
-		double error_v = priv->cmd_v - priv->cur_v;
+	good_x = almost_equal(cur_x, priv->cmd_x);
+	good_v = almost_equal(cur_v, priv->cmd_v);
+
+	if (!good_v || !good_x) {
+		double error_x;
+		double req_x_1;
+		double error_x_jpos;
+		double error_x_j0;
+		double error_x_jneg;
+		double error_v = priv->cmd_v - cur_v;
 		double x, v, a, j, t;
-		double req_x, req_v, t_max_a, v_delta_from_max_a;
+		double req_x, req_v, v_delta_from_max_a;
 		double error_v_after_a, error_x_at_v;
+		double j_from_pos;
 		bool state_at_max_a = false;
 		bool state_to_max_a = false;
 		
@@ -315,19 +345,34 @@ static void calculate(struct controller_block *spg)
 		   requested speed. 
 		 */
 		
-		x = priv->cur_x;
-		v = priv->cur_v;
+		x = cur_x;
+		v = cur_v;
 		a = priv->cur_a;
 		req_x = priv->cmd_x;
 		req_v = priv->cmd_v;
 		
-		/* Calculate delta v when comming from max_a */
-		t_max_a = ticks_to_a(priv, 0, priv->max_a);
-		v_delta_from_max_a = v_after_ticks(priv, 0, 0, priv->max_j, t_max_a);
+		req_x_1 = req_x + req_v;
+		
+		error_x_jpos = req_x_1 - x_after_ticks(priv, x, v, a, priv->max_j, 1);
+		error_x_j0 = req_x_1 - x_after_ticks(priv, x, v, a, 0, 1);
+		error_x_jneg = req_x_1 - x_after_ticks(priv, x, v, a, -priv->max_j, 1);
+		if (fabs(error_x_jpos) < fabs(error_x_jneg)) {
+			error_x = error_x_jpos;
+			j_from_pos = priv->max_j;
+		} else {
+			error_x = error_x_jneg;
+			j_from_pos = -priv->max_j;
+		}
+		if (fabs(error_x_j0) < fabs(error_x)) {
+			error_x = error_x_j0;
+			j_from_pos = 0;
+		}
+		
+		v_delta_from_max_a = priv->v_delta_from_max_a;
 
 		j = copysign(priv->max_j, error_v);
 			
-		if (fabs(a) > priv->max_j) {
+		if (fabs(a) >= priv->max_j) {
 			/* Not at constant velocity */
 		
 			if (signbit(a) != signbit(error_v)) {
@@ -362,6 +407,7 @@ static void calculate(struct controller_block *spg)
 					t_a_to_0 = ticks_to_a(priv, a, 0);
 				
 				} else if (fabs(a_peak) > priv->max_a) {
+					/* We are going to hit maximum acc */
 					a_peak = copysign(priv->max_a, a_peak);
 					
 					t = ticks_to_a(priv, 0, a_peak);
@@ -394,6 +440,7 @@ static void calculate(struct controller_block *spg)
 					if (t_to_max_a >= 1.0) {
 						state_to_max_a = true;
 						
+						/* accelerate to max a */
 						x = x_after_ticks(priv, x, v, a, j, t_to_max_a);
 						v = v_after_ticks(priv, v, a, j, t_to_max_a);
 						a = a_after_ticks(priv, a, j, t_to_max_a);
@@ -402,9 +449,9 @@ static void calculate(struct controller_block *spg)
 						state_at_max_a = true;
 					}
 				}
+				/* decellerate to a==0 */
 				x = x_after_ticks(priv, x, v, a, -j, t_a_to_0);
 				v = v_after_ticks(priv, v, a, -j, t_a_to_0);
-				a = a_after_ticks(priv, a, -j, t_a_to_0);
 				req_x = req_x + req_v * t_a_to_0;
 				a = 0;
 			}
@@ -427,7 +474,7 @@ static void calculate(struct controller_block *spg)
 			
 			x = x_after_ticks(priv, x, v, 0, j, t_max_a);
 			v = v_after_ticks(priv, v, 0, j, t_max_a);
-			a = a_after_ticks(priv, 0, j, t_max_a);
+			a = copysign(priv->max_a, j);
 			req_x = req_x + req_v * t_max_a;
 
 			t_at_max_a = (fabs(error_v_after_a) - v_delta_from_max_a*2) * priv->inv_max_a;
@@ -439,7 +486,6 @@ static void calculate(struct controller_block *spg)
 			x = x_after_ticks(priv, x, v, a, -j, t_max_a);
 			v = v_after_ticks(priv, v, a, -j, t_max_a);
 			req_x = req_x + req_v * t_max_a;
-			a = a_after_ticks(priv, a, -j, t_max_a);
 			a = 0;
 		} else {
 			/* Simple profile: triangle 
@@ -463,11 +509,13 @@ static void calculate(struct controller_block *spg)
 		
 		error_x_at_v = req_x - x;
 		
-		if (fabs(error_x_at_v) < fabs(error_x) || ignore_x) {
-			if (fabs(error_x_at_v) > req_v && !ignore_x &&
+		
+		if (fabs(error_x_at_v) < fabs(error_x) || 
+		    (signbit(error_x) != signbit(error_x_at_v) && !state_to_max_a && !state_at_max_a) ||
+		    ignore_x) {
+			if (!ignore_x &&
 			    signbit(error_x_at_v) == signbit(error_x) ) {
-				/* position got better, but not yet enough */
-				priv->cur_j = copysign(priv->max_j, error_x);
+				priv->cur_j = j_from_pos;
 			} else {
 				if (state_to_max_a) {
 					priv->cur_j = j;
@@ -481,17 +529,18 @@ static void calculate(struct controller_block *spg)
 			/* going to requested speed would make position error
 			   bigger, first go to position.
 			 */
-			priv->cur_j = copysign(priv->max_j, error_x);
+			priv->cur_j = j_from_pos;
 		}
 	}
 	
+	
 	/* When moving can we brake before the position limits? */
-	if (fabs(priv->cur_v) > 0.0) {
+	if (fabs(cur_v) > 0.0) {
 		double t, x, v, a, j;
 		bool done = false;
 		
-		x = priv->cur_x;
-		v = priv->cur_v;
+		x = cur_x;
+		v = cur_v;
 		a = priv->cur_a;
 
 		/* add 1 tick, we want to know if we are still safe untill the next
@@ -541,7 +590,7 @@ static void calculate(struct controller_block *spg)
 			/* Assuming we have a constant v,
 			   check if we have to start deceleration now. */
 
-			t = ticks_to_v(priv, priv->cur_v, 0);
+			t = ticks_to_v(priv, cur_v, 0);
 		
 			/* will we hit max a? */
 			if (fabs(a_after_ticks(priv, a, j, t/2)) > priv->max_a) {
@@ -549,7 +598,7 @@ static void calculate(struct controller_block *spg)
 			
 				double v_start_3, t_2;
 			
-				t = ticks_to_a(priv, 0, priv->max_a);
+				t = t_max_a;
 			
 				x = x_after_ticks(priv, x, v, a, j, t);
 				v = v_after_ticks(priv, v, a, j, t);
@@ -585,10 +634,11 @@ static void calculate(struct controller_block *spg)
 		}
 	}
 
+	/* If accelerating, can we decelerate before going beyond our max vel */
 	if (fabs(priv->cur_a) > 0.0) {
 		double t, v, a, j;
 		
-		v = priv->cur_v;
+		v = cur_v;
 		a = priv->cur_a;
 
 		/* add 1 tick, we want to know if we are still safe untill the next
@@ -615,58 +665,102 @@ static void calculate(struct controller_block *spg)
 		}
 	}
 
+
+	if (!ignore_x) {
+		priv->cmd_x += priv->cmd_v;
+	}
+
 	/* Is the difference between spg and command small enough?
 	   If so, make outputs equal to command */
 	if (!must_brake &&
-	    fabs(priv->cmd_x - priv->cur_x) < priv->precision_x &&
-	    fabs(priv->cmd_v - priv->cur_v) < priv->precision_v &&
+	    fabs(priv->cmd_x - cur_x) < priv->precision_x &&
+	    fabs(priv->cmd_v - cur_v) < priv->precision_v &&
 	    fabs(priv->cur_a) < priv->precision_a) {
 		priv->cur_j = 0.0;
 		priv->cur_a = 0.0;
-		priv->cur_v = priv->cmd_v;
-		priv->cur_x = priv->cmd_x;
+		cur_v = priv->cmd_v;
+		cur_x = priv->cmd_x;
 	}
 
-	priv->cur_a += priv->cur_j;
+	/* new jerk? */
+	if (!almost_equal(priv->cur_j, priv->start_j)) {
+		priv->start_j = priv->cur_j;
+		priv->start_a = priv->cur_a;
+		priv->start_v = cur_v;
+		priv->start_x = cur_x;
+		priv->start_t = 0;
+	}
+	priv->start_t++;
+	
+	priv->cur_a = a_after_ticks(priv, 
+	    priv->start_a, priv->start_j, priv->start_t);
+	cur_v = v_after_ticks(priv, 
+	    priv->start_v, priv->start_a, priv->start_j, priv->start_t);
+	cur_x = x_after_ticks(priv, 
+	    priv->start_x, priv->start_v, priv->start_a, priv->start_j, 
+	    priv->start_t);
+
 
 	if (fabs(priv->cur_a) > priv->max_a) {
 		priv->cur_a = copysign(priv->max_a, priv->cur_a);
 		priv->cur_j = 0;
-	}
 
-	priv->cur_v += priv->cur_a;
+		priv->start_j = priv->cur_j;
+		priv->start_a = priv->cur_a;
+		priv->start_v = cur_v;
+		priv->start_x = cur_x;
+		priv->start_t = 0;
+	}
 
-	if (fabs(priv->cur_v) >= priv->max_v) {
+	if (fabs(cur_v) >= priv->max_v) {
 		/* prevent further acceleration beyond max v */
-		if (signbit(priv->cur_v) == signbit(priv->cur_a)) {
+		if (signbit(cur_v) == signbit(priv->cur_a)) {
 			priv->cur_a = 0.0;
 		}
-		if (signbit(priv->cur_v) == signbit(priv->cur_j)) {
+		if (signbit(cur_v) == signbit(priv->cur_j)) {
 			priv->cur_j = 0.0;
 		}
-		priv->cur_v = copysign(priv->max_v, priv->cur_v);
+		cur_v = copysign(priv->max_v, cur_v);
+
+		priv->start_j = priv->cur_j;
+		priv->start_a = priv->cur_a;
+		priv->start_v = cur_v;
+		priv->start_x = cur_x;
+		priv->start_t = 0;
 	}
 	
-	priv->cur_x += priv->cur_v;
-	
-	if (priv->cur_x > priv->max_x) {
-		priv->cur_x = priv->max_x;
-		priv->cur_v = 0;
+	if (cur_x > priv->max_x) {
+		cur_x = priv->max_x;
+		cur_v = 0;
 		priv->cur_a = 0;
 		priv->cur_j = 0;
+
+		priv->start_j = priv->cur_j;
+		priv->start_a = priv->cur_a;
+		priv->start_v = cur_v;
+		priv->start_x = cur_x;
+		priv->start_t = 0;
 	}
-	if (priv->cur_x < priv->min_x) {
-		priv->cur_x = priv->min_x;
-		priv->cur_v = 0;
+	if (cur_x < priv->min_x) {
+		cur_x = priv->min_x;
+		cur_v = 0;
 		priv->cur_a = 0;
 		priv->cur_j = 0;
+
+		priv->start_j = priv->cur_j;
+		priv->start_a = priv->cur_a;
+		priv->start_v = cur_v;
+		priv->start_x = cur_x;
+		priv->start_t = 0;
 	}
 
-	priv->cur_x_out = priv->cur_x;
-	priv->cur_v_out = priv->cur_v * priv->freq;
-	priv->cur_a_out = priv->cur_a * priv->freq * priv->freq;
-	priv->cur_j_out = priv->cur_j * priv->freq * priv->freq * priv->freq;
+	priv->cur_x_out = cur_x;
+	priv->cur_v_out = cur_v * priv->freq;
+	priv->cur_a_out = priv->cur_a * priv->freq2;
+	priv->cur_j_out = priv->cur_j * priv->freq3;
 	priv->cmd_x_out = priv->cmd_x;
+	priv->cur_x = cur_x;
+	priv->cur_v = cur_v;
 }
 
 static bool block_setpoint_generator_queue_space(struct controller_block *spg)
@@ -763,6 +857,8 @@ static void param_get(struct controller_block *spg, int param, void *val)
 
 static void param_set(struct controller_block *spg, int param, va_list val)
 {
+	double t_max_a;
+	
 	switch (param) {
 		case 0:
 			spg->private->cmd_x = va_arg(val, double);
@@ -811,8 +907,16 @@ static void param_set(struct controller_block *spg, int param, va_list val)
 	spg->private->precision_a = spg->private->precision_a_sec * spg->private->tick * spg->private->tick;
 
 	spg->private->freq = 1.0 / spg->private->tick;
+	spg->private->freq2 = spg->private->freq * spg->private->freq;
+	spg->private->freq3 = spg->private->freq2 * spg->private->freq;
 	spg->private->inv_max_j = 1.0 / spg->private->max_j;
 	spg->private->inv_max_a = 1.0 / spg->private->max_a;
+
+	/* Calculate delta v when comming from max_a */
+	t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
+	spg->private->v_delta_from_max_a = 
+	    v_after_ticks(spg->private, 0, 0, spg->private->max_j, t_max_a);
+	spg->private->t_max_a = t_max_a;
 }
 
 static struct controller_block_interm_list interms[] = {
@@ -876,6 +980,8 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
 	spg->private->max_a_sec = 0.0;
 	spg->private->max_j_sec = 0.0;
 	spg->private->freq = 1.0;
+	spg->private->freq2 = 1.0;
+	spg->private->freq3 = 1.0;
 	spg->private->precision_x = 0.0;
 	spg->private->precision_v = 0.0;
 	spg->private->precision_a = 0.0;
@@ -887,6 +993,11 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
 	spg->private->cur_v = 0.0;
 	spg->private->cur_a = 0.0;
 	spg->private->cur_j = 0.0;
+	spg->private->start_x = 0.0;
+	spg->private->start_v = 0.0;
+	spg->private->start_a = 0.0;
+	spg->private->start_j = 0.0;
+	spg->private->start_t = 0;
 	spg->private->cur_x_out = 0.0;
 	spg->private->cur_v_out = 0.0;
 	spg->private->cur_a_out = 0.0;
@@ -899,7 +1010,7 @@ struct controller_block * block_setpoint_generator_create(char *name, va_list ap
 	if (controller_block_outterm_list_init(spg, outterms))
 		goto err_input;
 
-	spg->calculate = calculate;
+	spg->calculate = setpoint_generator_calculate;
 
 	if (controller_block_param_list_init(spg, params))
 		goto err_output;

controller/controller/Makefile

@@ -28,7 +28,7 @@ controller_load_parser.tab.lo: controller_load_parser.yy.c
 
 controller_load.lo: controller_load_parser.tab.h controller_load_parser.yy.h
 
-libcontroller.la_LDFLAGS=-rpath ${CURDIR}/../lib -lshell
+libcontroller.la_LDFLAGS=-rpath ${CURDIR}/../lib -lshell -ldl -lpthread -lrt
 libcontroller.la: $(CONTROLLER)
 
 libcontroller.la_install: libcontroller.la

controller/controller/controller_sample.c

@@ -29,6 +29,7 @@
 #include <string.h>
 #include <sys/mman.h>
 #include <errno.h>
+#include <signal.h>
 
 #include "controller_block.h"
 #include "controller_sample.h"
@@ -125,6 +126,34 @@ static ssize_t sample_timing_snprintf(struct sample_timing *st,
 }
 
 
+static int sample_timing_sane(struct timespec *t_exp, struct timespec *t_act)
+{
+	long s_diff;
+	long n_diff;
+	
+	s_diff = t_act->tv_sec - t_exp->tv_sec;
+	
+	if (s_diff < 0) {
+		return -1;
+	}
+	if (s_diff > 1) {
+		return 1;
+	}
+
+	n_diff = s_diff * 1000 * 1000 * 1000;
+	n_diff += t_act->tv_nsec - t_exp->tv_nsec;
+	if (n_diff > nsec_interval * 10) {
+		return 1;
+	}
+
+	return 0;
+}
+
+static void alarm_handler(int d)
+{
+	log_send(LOG_T_ERROR, "Sample has been inactive to long!");
+}
+
 /*
 	This thread should be realtime....
  */
@@ -142,6 +171,16 @@ static void *sample_thread(void *arg)
 	pthread_mutex_t lock;
 	pthread_cond_t cv;
 #endif
+	sigset_t sigset;
+	struct sigaction sigact;
+
+	sigemptyset (&sigset);
+	sigaddset(&sigset, SIGALRM);
+	sigprocmask(SIG_UNBLOCK, &sigset, NULL);
+	sigact.sa_handler = alarm_handler;
+	sigemptyset (&sigact.sa_mask);
+	sigact.sa_flags = 0;
+	sigaction(SIGALRM, &sigact, NULL);
 	
 	log_send(LOG_T_DEBUG, "Starting sample thread");
 	controller_sample_thread_running = true;
@@ -206,14 +245,26 @@ static void *sample_thread(void *arg)
 		
 		clock_gettime(clock, &t_end);
 		
-		sample_timing_add(&st_start, &t, &t_start);
-		sample_timing_add(&st_io, &t_start, &t_io);
-		sample_timing_add(&st_end, &t, &t_end);
-		sample_timing_add(&st_sample, &t_start, &t_end);
-		
 		if (sample_timing_diff_nsec(&t, &t_end) > nsec_interval) {
 			controller_sampleoverruns++;
 		}
+		if (sample_timing_sane(&t, &t_start)) {
+			log_send(LOG_T_ERROR, 
+			    "Time sanity check triggered: %lld.%09ld %lld.%09ld",
+			    (long long)t.tv_sec, t.tv_nsec, 
+			    (long long)t_start.tv_sec, t_start.tv_nsec);
+			t.tv_sec = t_start.tv_sec;
+			t.tv_nsec = (t_start.tv_nsec / nsec_interval) * nsec_interval;
+			log_send(LOG_T_WARNING,
+			    "Using current time for next sampe: %lld.%09ld",
+			    (long long)t.tv_sec, t.tv_nsec);
+		} else {
+			sample_timing_add(&st_start, &t, &t_start);
+			sample_timing_add(&st_io, &t_start, &t_io);
+			sample_timing_add(&st_end, &t, &t_end);
+			sample_timing_add(&st_sample, &t_start, &t_end);
+		}
+		alarm(1);
 	}
 	return NULL;
 }

controller/dt_ctrl.c

@@ -33,6 +33,7 @@
 #include <string.h>
 #include <unistd.h>
 #include <math.h>
+#include <signal.h>
 
 #include "dynarg.h"
 
@@ -53,7 +54,12 @@ int main(int argc, char **argv)
 {
 	char *ctrl_filename;
 	char *dot_filename;
-	
+	sigset_t sigset;
+
+	sigemptyset (&sigset);
+	sigaddset(&sigset, SIGALRM);
+	sigprocmask(SIG_BLOCK, &sigset, NULL);
+
 	log_server_start(CTRL_LOG_PORT, LOG_T_DEBUG, LOG_T_INFO);