Update test networks

Fix some setpoint generator quirks

controller/Makefile

@@ -3,3 +3,5 @@ all:
 
 $(MAKECMDGOALS):
 	@$(MAKE) --no-print-directory -C .. $(MAKECMDGOALS)
+
+.PHONY: test

controller/block/block_setpoint_generator_1d.c

@@ -139,8 +139,8 @@ static void setpoint_generator_1d_calculate(struct controller_block *spg)
 					uint64_t command_t;
 					int64_t t;
 					
-					command_t = priv->cur_command.t.tv_nsec +
-					    priv->cur_command.t.tv_sec * 1000000000;
+					command_t = (uint64_t)priv->cur_command.t.tv_nsec +
+					    (uint64_t)priv->cur_command.t.tv_sec * 1000000000;
 					    priv->command_t = command_t;
 					
 					t = (command_t - controller_time_nseconds) / priv->period_nsec;

controller/block/block_setpoint_generator_3d.c

@@ -26,6 +26,7 @@
 
 #include <controller/controller_block.h>
 #include <controller/controller_command.h>
+#include <controller/controller_sample.h>
 #include <log/log.h>
 
 /*
@@ -95,12 +96,12 @@ struct controller_block_private {
 	double precision_a;
 
 	/* 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;
+	uint64_t period_nsec;
 
 	/* parameters in real world format (time unit: second) */
 	float max_v_sec;
@@ -132,7 +133,7 @@ struct controller_block_private {
 	struct command_entry cur_command;
 	bool cur_done;
 	bool cur_start;
-	uint32_t cur_t_samplenr;
+	uint64_t command_t;
 
 	struct controller_command *command;
 };
@@ -244,6 +245,8 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
 		priv->start_a = 0.0;
 		priv->start_j = 0.0;
 		priv->start_t = 0;
+		
+		goto set_output;
 	}
 
 	if (priv->cur_done) {
@@ -262,8 +265,6 @@ static void setpoint_generator_3d_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;
@@ -271,24 +272,21 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
 				priv->cur_done = true;
 				break;
 			case COMMAND_PTYPE_SPEED:
-				priv->cmd_v = priv->cur_command.value.f * priv->tick;
+				priv->cmd_v = priv->cur_command.value.f * controller_sample_period();
 				priv->cur_done = true;
 				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);
-					priv->cur_t_samplenr = t_samplenr;
-
-					t = (double)(priv->cur_command.t.tv_sec
-					    - controller_time_seconds) * priv->freq +
-					    t_samplenr -
-					    (double)controller_time_samplenr;
+					uint64_t command_t;
+					int64_t t;
+					
+					command_t = (uint64_t)priv->cur_command.t.tv_nsec +
+					    (uint64_t)priv->cur_command.t.tv_sec * 1000000000;
+					    priv->command_t = command_t;
+					
+					t = (command_t - controller_time_nseconds) / priv->period_nsec;
 
-					if (t <= 0.0) {
+					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)",
@@ -299,13 +297,12 @@ static void setpoint_generator_3d_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 -
@@ -319,7 +316,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
 
 	if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
 		ignore_x = true;
-		priv->cmd_x = cur_x;
+		priv->cmd_x = cur_x + priv->cmd_v;
 	}
 	
 	if (priv->cmd_x > priv->max_x)
@@ -757,6 +754,7 @@ static void setpoint_generator_3d_calculate(struct controller_block *spg)
 		priv->start_t = 0;
 	}
 
+set_output:
 	priv->cur_x_out = cur_x;
 	priv->cur_v_out = cur_v * priv->freq;
 	priv->cur_a_out = priv->cur_a * priv->freq2;
@@ -825,8 +823,7 @@ static void param_get(struct controller_block *spg, int param, void *val)
 			    spg->private->cmd_x;
 			break;
 		case 1:
-			*(float*)val =
-			    spg->private->tick;
+			*(float*)val = controller_sample_period();
 			break;
 		case 2:
 			*(float*)val =
@@ -866,6 +863,7 @@ 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;
+	double tick = controller_sample_period();
 	
 	switch (param) {
 		case 0:
@@ -878,7 +876,6 @@ static void param_set(struct controller_block *spg, int param, va_list val)
 			spg->private->cur_j = 0.0;
 			break;
 		case 1:
-			spg->private->tick = va_arg(val, double);
 			break;
 		case 2:
 			spg->private->max_x = va_arg(val, double);
@@ -907,18 +904,19 @@ static void param_set(struct controller_block *spg, int param, va_list val)
 	}
 
 	/* Scale all settings to sample time unit */
-	spg->private->max_v = spg->private->max_v_sec * spg->private->tick;
-	spg->private->max_a = spg->private->max_a_sec * spg->private->tick * spg->private->tick;
-	spg->private->max_j = spg->private->max_j_sec * spg->private->tick * spg->private->tick * spg->private->tick;
+	spg->private->max_v = spg->private->max_v_sec * tick;
+	spg->private->max_a = spg->private->max_a_sec * tick * tick;
+	spg->private->max_j = spg->private->max_j_sec * tick * tick * tick;
 	spg->private->precision_x = spg->private->precision_x_sec;
-	spg->private->precision_v = spg->private->precision_v_sec * spg->private->tick;
-	spg->private->precision_a = spg->private->precision_a_sec * spg->private->tick * spg->private->tick;
+	spg->private->precision_v = spg->private->precision_v_sec * tick;
+	spg->private->precision_a = spg->private->precision_a_sec * tick * tick;
 
-	spg->private->freq = 1.0 / spg->private->tick;
+	spg->private->freq = 1.0 / 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;
+	spg->private->period_nsec = controller_sample_period() * 1000000000;
 
 	/* Calculate delta v when comming from max_a */
 	t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
@@ -967,7 +965,6 @@ struct controller_block * block_setpoint_generator_3d_create(char *name, va_list
 		goto err_name;
 	spg->private->cmd_x = 0.0;
 	spg->private->cmd_v = 0.0;
-	spg->private->tick = 1.0;
 	spg->private->max_x = 0.0;
 	spg->private->min_x = 0.0;
 	spg->private->max_v = 0.0;

controller/block/block_setpoint_generator_3d.test.ctrl

+frequency 10
+
+blocks {
+	{ "setpoint_generator_3d", "spg", "spg", "na" }
+
+	{ "test_input_bool",  "reset"   }
+	{ "test_input_float", "reset_x" }
+
+	{ "test_output_float", "setpoint" }
+	{ "test_output_float", "x"        }
+	{ "test_output_float", "v"        }
+	{ "test_output_float", "a"        }
+	{ "test_output_float", "j"        }
+
+	{ "test_command", "command" }
+}
+
+links {
+	{ "reset",   "value", "spg", "reset",   true }
+	{ "reset_x", "value", "spg", "reset_x", true }
+	
+	{ "spg", "x", "x", "value", true }
+	{ "spg", "v", "v", "value", true }
+	{ "spg", "a", "a", "value", true }
+	{ "spg", "j", "j", "value", true }
+	{ "spg", "setpoint", "setpoint", "value", true }
+}
+
+set SETPOINT		4
+set SPEED		5
+set SETPOINT_TIME 	6
+
+params {
+	{ "spg", "min_x", (float) -100.0 }
+	{ "spg", "max_x", (float)  200.0 }
+	{ "spg", "max_v", (float)  20.0  }
+	{ "spg", "max_a", (float)   2.0  }
+	{ "spg", "max_j", (float)   0.5  }
+
+	{ "command", "command", "spg", $[SETPOINT_TIME], 123,   80.0, 0, 500000000 }
+	{ "command", "command", "spg", $[SETPOINT],      124, -110.0 }
+	{ "command", "command", "spg", $[SETPOINT],      125,  300.0 }
+	{ "command", "command", "spg", $[SETPOINT],      126,   61.0 }
+	{ "command", "command", "spg", $[SPEED],         200,   11.0 }
+	{ "command", "command", "spg", $[SETPOINT_TIME], 127,  -95.0, 1, 100000000 }
+	{ "command", "command", "spg", $[SPEED],         201,  -33.0 }
+
+	{ "reset",   "value", 12, 
+		(int)   { true, false, false, false, false, false, false, false, false, false, 
+		          true, false	} }
+	{ "reset_x", "value", 11, 
+		(float) { 50.0, 33.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+		         -94.0  } }
+	
+	{ "setpoint", "value", 10,
+		(float) { 50.0, 56.0, 62.0, 68.0, 74.0, 80.0, -100.0, 200.0, 61.0, 51.1395 },
+		(float) {  0.0,  0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 0.0, 1e-7  }
+	}
+	{ "x", "value", 20,
+		(float) { 50.0, 50.00008, 50.00007, 50.002255, 50.00533, 50.01042, 50.01783, 50.02742, 50.0395, 50.05458,
+		         -94.0,         -9.400008e+01, -9.400066e+01, -9.400225e+01, -9.400533e+01,
+			 -9.401041e+01, -9.401800e+01, -9.402858e+01, -9.404266e+01, -9.406075e+01 },
+		(float) {  0.0,  1e-5, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3,
+		           0.0,  1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5, 1e-5  }
+	}
+	{ "v", "value", 10,
+		(float) {  0.0, 0.0025, 0.01, 0.0225, 0.04, 0.0625, 0.085, 0.1075, 0.135, 0.1675  },
+		(float) {  0.0,  1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7 }
+	}
+
+	{ "a", "value", 20,
+		(float) {  0.0,  0.05, 0.1, 0.15, 0.2, 0.25, 0.20, 0.25, 0.3, 0.35,
+		           0.0, -0.05,-0.1,-0.15,-0.2,-0.25,-0.30,-0.35,-0.4,-0.45},
+		(float) {  0.0,  1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7,
+		           0.0,  1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7, 1e-7 }
+	}
+
+	{ "j", "value", 20,
+		(float) {  0.0,  0.5, 0.5, 0.5, 0.5, 0.5, -0.5, 0.5, 0.5, 0.5,
+		           0.0, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5},
+		(float) {  0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ,
+		           0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0   }
+	}
+}

controller/block/build.mk

@@ -77,3 +77,10 @@ CTRL_BLOCK_LIBS += libblock.la
 TARGETS += $(BLOCK_TARGETS)
 CLEAN += $(BLOCK_TARGETS) $(BLOCK_OBJS)
 SRCS += $(BLOCK_SRCS)
+
+CTRL_TESTS += \
+	$(DIR)/block_gain.test.ctrl \
+	$(DIR)/block_not.test.ctrl \
+	$(DIR)/block_setpoint_generator_1d.test.ctrl \
+	$(DIR)/block_setpoint_generator_3d.test.ctrl
+

controller/controller/controller_sample.c

@@ -347,8 +347,8 @@ static void *sample_thread(void *arg)
 		
 		t.tv_nsec += nsec_interval;
 		tsnorm(&t);
-		
-		controller_time_nseconds = t.tv_sec * 1000000000;
+	
+		controller_time_nseconds = (uint64_t)t.tv_sec * 1000000000;
 		controller_time_nseconds += t.tv_nsec;
 		controller_time_seconds = t.tv_sec;
 		controller_time_samplenr = t.tv_nsec / nsec_interval;

controller/test/block_test_command.c

@@ -19,6 +19,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <inttypes.h>
 
 #include <controller/controller_block.h>
 #include <controller/controller_command.h>
@@ -42,6 +43,8 @@ static void calculate_test_command(struct controller_block *block)
 	if (!priv->start_sec) {
 		priv->start_sec = controller_time_seconds;
 		priv->start_nsec = controller_time_nseconds % 1000000000;
+		log_send(LOG_T_DEBUG, "%s: Start time: %" PRIu64 ".%09" PRIu32,
+		    block->name, priv->start_sec, priv->start_nsec);
 	}
 
 	if (priv->cur < priv->entries_nr) {
@@ -56,6 +59,8 @@ static void calculate_test_command(struct controller_block *block)
 			entry->t.tv_nsec += priv->start_nsec;
 		}
 		
+		log_send(LOG_T_DEBUG, "%s: Filter command %d and write to queue",
+		    command->name, priv->cur);
 		if (command->filter)
 			r = command->filter(command, entry);
 		if (r == 0)

controller/test/block_test_output_float.c

@@ -53,14 +53,15 @@ static void calculate_test_output_float(struct controller_block *block)
 		float value = *priv->value;
 		float value_good = priv->values[priv->values_cur];
 		float value_error = priv->errors[priv->values_cur];
+		float error = fabs(value - value_good);
 		
-		if (fabs(value - value_good) > value_error) {
+		if (error > value_error) {
 			log_send(LOG_T_ERROR,
-			    "%s: value %d does not match: %e (%a) != %e (%a) within %e",
+			    "%s: value %d does not match: %e (%a) != %e (%a) within %e, diff: %e",
 			    block->name, priv->values_cur,
 			    value, value,
 			    value_good, value_good, 
-			    value_error);
+			    value_error, error);
 		} else {
 			priv->values_ok++;
 		}

controller/test/build.mk

@@ -29,7 +29,5 @@ SRCS += $(TEST_BLOCK_SRCS)
 
 
 CTRL_TESTS += \
-	$(DIR)/block_gain.test.ctrl \
-	$(DIR)/block_not.test.ctrl \
-	$(DIR)/block_setpoint_generator_1d.test.ctrl
+	$(DIR)/test.ctrl
 

controller/test/test.ctrl

+frequency 100
+
+blocks {
+	{ "test_input_bool",   "input_bool"   }
+	{ "test_input_float",  "input_float"  }
+
+	{ "test_output_bool",  "output_bool"  }
+	{ "test_output_float", "output_float" }
+}
+
+links {
+	{ "input_bool", "value",  "output_bool", "value",  true }
+	{ "input_float", "value", "output_float", "value", true }
+}
+
+params {
+	{ "input_bool", "value", 4, (int) { true, false, true, false } }
+	{ "output_bool", "value", 4, (int) { true, false, true, false } }
+	{ "input_float", "value", 4, 
+		(float) { 12345678.9, -3.3333333, 0.0, 42.01e-20 },
+		(float) { 0.0, 0.0, 0.0, 0.0 } }
+	{ "output_float", "value", 4, 
+		(float) { 12345678.9, -3.3333333, 0.0, 42.01e-20 },
+		(float) { 0.0, 0.0, 0.0, 0.0 } }
+}