Add BLOCK_CREATE macro for defining create function and parameter descriptions.

Als add parameter description and check to parameter lists.

common/log/log.c

@@ -465,3 +465,32 @@ void log_string(char *str, size_t len, enum log_type type, char *fmt, ...)
 	
 	va_end(ap);
 }
+
+void log_flush_stdout(void)
+{
+	while (msgs[msgs_rd].used) {
+		int i;
+		char time[100];
+		char *header;
+		struct tm gmt;
+			
+		__sync_synchronize();
+			
+		gmtime_r(&msgs[msgs_rd].t, &gmt);
+		timestamp(&gmt, time);
+
+		header = logtype(msgs[msgs_rd].type);
+			
+			
+		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);
+	}
+
+}

common/log/log.h

@@ -35,6 +35,8 @@ int log_server_start(int port, enum log_type console_level,
     enum log_type remote_level);
 void log_server_flush(void);
 
+void log_flush_stdout(void);
+
 int log_client_start(char *host, int port, enum log_type console_level,
     enum log_type remote_level, char *name);
 

controller/block/block_debug.c

@@ -61,7 +61,6 @@ struct controller_block * block_debug_create(char *name)
 	debug->output = NULL;
 	
 	debug->calculate = calculate;
-	debug->params = 0;
 
 	controller_block_add(debug);
 	return debug;

controller/block/block_decoder_uint32_bool.c

@@ -58,15 +58,11 @@ static struct controller_block_interm_list interms[] = {
 	{ NULL }
 };
 
-struct controller_block * block_decoder_uint32_bool_create(char *name, int argc, va_list ap)
+static struct controller_block * block_decoder_uint32_bool_create(char *name, int argc, va_list ap)
 {
 	struct controller_block *dec;
 	int n, i;
 	
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: create expects one argument", name);
-		return NULL;
-	}
 	n = va_arg(ap, int);
 
 	if (n < 1) {
@@ -99,8 +95,6 @@ struct controller_block * block_decoder_uint32_bool_create(char *name, int argc,
 
 	dec->calculate = calculate;
 
-	dec->params = 0;
-
 	controller_block_add(dec);
 	return dec;
 
@@ -112,3 +106,8 @@ err_out:
 	controller_block_free(dec);
 	return NULL;
 }
+
+BLOCK_CREATE(decoder_uint32) = {
+	.create = block_decoder_uint32_bool_create,
+	.args = { "int", NULL },
+};

controller/block/block_filter_iir.c

@@ -85,11 +85,6 @@ static int param_set(struct controller_block *iir, int argc, va_list ap)
 	float *poles;
 	float *zeros;
 			
-	if (argc != 4) {
-		log_send(LOG_T_ERROR, "%s: parameter expects 4 arguments: (int)len, (double)gain, (float[])poles, (float[])zeros", iir->name);
-		return -1;
-	}
-
 	len = va_arg(ap, int);
 	gain = va_arg(ap, double);
 	poles = va_arg(ap, float *);
@@ -129,7 +124,7 @@ static int param_set(struct controller_block *iir, int argc, va_list ap)
 }
 
 static struct controller_block_param_list params[] = {
-	{ "transfer", true, param_set },
+	{ "transfer", true, param_set, .args = { "int,double,float*,float*", NULL } },
 	{ NULL },
 };
 

controller/block/block_filter_lp.c

@@ -61,10 +61,6 @@ static int param_set(struct controller_block *lp, int argc, va_list val)
 {
 	double period = controller_time_period_get(lp->time);
 	
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", lp->name);
-		return -1;
-	}
 	lp->private->tau = va_arg(val, double);
 
 	lp->private->alpha = period / (period + lp->private->tau);
@@ -73,7 +69,7 @@ static int param_set(struct controller_block *lp, int argc, va_list val)
 }
 
 static struct controller_block_param_list params[] = {
-	{ "tau", true, param_set },
+	{ "tau", true, param_set, .args = { "double", NULL } },
 	{ NULL },
 };
 

controller/block/block_friction.c

@@ -101,10 +101,6 @@ static int param_set_friction_static(struct controller_block *friction, int argc
 {
 	struct controller_block_private *priv = friction->private;
 
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", friction->name);
-		return -1;
-	}
 	priv->friction_static = va_arg(val, double);
 	return 0;
 }
@@ -113,10 +109,6 @@ static int param_set_friction_dynamic(struct controller_block *friction, int arg
 {
 	struct controller_block_private *priv = friction->private;
 
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", friction->name);
-		return -1;
-	}
 	priv->friction_dynamic = va_arg(val, double);
 	return 0;
 }
@@ -125,10 +117,6 @@ static int param_set_deadzone_max(struct controller_block *friction, int argc, v
 {
 	struct controller_block_private *priv = friction->private;
 
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", friction->name);
-		return -1;
-	}
 	priv->deadzone_max = va_arg(val, double);
 	return 0;
 }
@@ -137,19 +125,15 @@ static int param_set_deadzone_min(struct controller_block *friction, int argc, v
 {
 	struct controller_block_private *priv = friction->private;
 
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", friction->name);
-		return -1;
-	}
 	priv->deadzone_min = va_arg(val, double);
 	return 0;
 }
 
 static struct controller_block_param_list params[] = {
-	{ "friction_static",  false, param_set_friction_static },
-	{ "friction_dynamic", false, param_set_friction_dynamic },
-	{ "deadzone_max",     false, param_set_deadzone_max },
-	{ "deadzone_min",     false, param_set_deadzone_min },
+	{ "friction_static",  false, param_set_friction_static, .args = { "double", NULL } },
+	{ "friction_dynamic", false, param_set_friction_dynamic, .args = { "double", NULL } },
+	{ "deadzone_max",     false, param_set_deadzone_max, .args = { "double", NULL } },
+	{ "deadzone_min",     false, param_set_deadzone_min, .args = { "double", NULL } },
 	{ NULL },
 };
 

controller/block/block_gain.c

@@ -51,16 +51,12 @@ static void gain_calculate(struct controller_block *gain)
 
 static int param_set(struct controller_block *gain, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", gain->name);
-		return -1;
-	}
 	gain->private->gain = va_arg(val, double);
 	return 0;
 }
 
 static struct controller_block_param_list params[] = {
-	{ "gain", false, param_set },
+	{ "gain", false, param_set, .args = { "double", NULL } },
 	{ NULL },
 };
 

controller/block/block_joystick.c

@@ -111,7 +111,7 @@ static enum controller_bus_state joystick_poll(struct controller_bus *bus)
 }
 
 
-struct controller_block * block_joystick_create(char *name, int argc, va_list ap)
+static struct controller_block * block_joystick_create(char *name, int argc, va_list ap)
 {
 	struct controller_block *block;
 	int fd;
@@ -119,10 +119,6 @@ struct controller_block * block_joystick_create(char *name, int argc, va_list ap
 	int nr_axes = 0, nr_buttons = 0;
 	int i, ret;
 
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: create expects one argument", name);
-		return NULL;
-	}
 	device = va_arg(ap, char *);
 
 	fd = joystick_open(device);
@@ -202,3 +198,8 @@ err_ioctl:
 	close(fd);
 	return NULL;
 }
+
+BLOCK_CREATE(joystick) = {
+	.create = block_joystick_create,
+	.args = { "char*", NULL },
+};

controller/block/block_limit.c

@@ -62,27 +62,19 @@ static void limit_calculate(struct controller_block *limit)
 
 static int param_set_min(struct controller_block *limit, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", limit->name);
-		return -1;
-	}
 	limit->private->min = va_arg(val, double);
 	return 0;
 }
 
 static int param_set_max(struct controller_block *limit, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", limit->name);
-		return -1;
-	}
 	limit->private->max = va_arg(val, double);
 	return 0;
 }
 
 static struct controller_block_param_list params[] = {
-	{ "min", false, param_set_min },
-	{ "max", false, param_set_max },
+	{ "min", false, param_set_min, .args = { "double", NULL } },
+	{ "max", false, param_set_max, .args = { "double", NULL } },
 	{ NULL },
 };
 

controller/block/block_limit_2nd.c

@@ -132,10 +132,6 @@ static void param_prep(struct controller_block *limit)
 
 static int param_set_min(struct controller_block *limit, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", limit->name);
-		return -1;
-	}
 	limit->private->mint = va_arg(val, double);
 	param_prep(limit);
 	return 0;
@@ -143,10 +139,6 @@ static int param_set_min(struct controller_block *limit, int argc, va_list val)
 
 static int param_set_max(struct controller_block *limit, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", limit->name);
-		return -1;
-	}
 	limit->private->maxt = va_arg(val, double);
 	param_prep(limit);
 	return 0;
@@ -154,10 +146,6 @@ static int param_set_max(struct controller_block *limit, int argc, va_list val)
 
 static int param_set_reset(struct controller_block *limit, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", limit->name);
-		return -1;
-	}
 	limit->private->der_last = 0;
 	limit->private->out = 0;
 	param_prep(limit);
@@ -165,9 +153,9 @@ static int param_set_reset(struct controller_block *limit, int argc, va_list val
 }
 	
 static struct controller_block_param_list params[] = {
-	{ "min",   true, param_set_min },
-	{ "max",   true, param_set_max },
-	{ "reset", true, param_set_reset },
+	{ "min",   true, param_set_min, .args = { "double", NULL } },
+	{ "max",   true, param_set_max, .args = { "double", NULL } },
+	{ "reset", true, param_set_reset, .args = { NULL } },
 	{ NULL },
 };
 

controller/block/block_limit_var.c

@@ -68,27 +68,19 @@ static void limit_var_calculate(struct controller_block *limit)
 
 static int param_set_offset(struct controller_block *limit, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", limit->name);
-		return -1;
-	}
 	limit->private->limit_off = va_arg(val, double);
 	return 0;
 }
 
 static int param_set_gain(struct controller_block *limit, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", limit->name);
-		return -1;
-	}
 	limit->private->limit_gain = va_arg(val, double);
 	return 0;
 }
 
 static struct controller_block_param_list params[] = {
-	{ "offset", false, param_set_offset },
-	{ "gain",   false, param_set_gain },
+	{ "offset", false, param_set_offset, .args = { "double", NULL } },
+	{ "gain",   false, param_set_gain, .args = { "double", NULL } },
 	{ NULL },
 };
 

controller/block/block_matrix_2x2.c

@@ -73,10 +73,6 @@ static int param_set(struct controller_block *matrix, int argc, va_list val)
 	struct controller_block_private *priv = matrix->private;
 	float *flt1, *flt2;
 	
-	if (argc != 2) {
-		log_send(LOG_T_ERROR, "%s: parameter expects 2 arguments", matrix->name);
-		return -1;
-	}
 	flt1 = va_arg(val, float *);
 	flt2 = va_arg(val, float *);
 	priv->param.c[0][0] = flt1[0];
@@ -88,7 +84,7 @@ static int param_set(struct controller_block *matrix, int argc, va_list val)
 }
 
 static struct controller_block_param_list params[] = {
-	{ "constants", true, param_set },
+	{ "constants", true, param_set, .args = { "float*,float*", NULL } },
 	{ NULL },
 };
 

controller/block/block_multiplexer.c

@@ -66,15 +66,11 @@ static struct controller_block_outterm_list outterms[] = {
 };
 
 
-struct controller_block * block_multiplexer_create(char *name, int argc, va_list ap)
+static struct controller_block * block_multiplexer_create(char *name, int argc, va_list ap)
 {
 	struct controller_block *mul;
 	int n, i;
 	
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: create expects one argument", name);
-		return NULL;
-	}
 	n = va_arg(ap, int);
 
 	if (!(mul = controller_block_alloc("multiplexer", name,
@@ -119,3 +115,8 @@ err_in:
 	controller_block_free(mul);
 	return NULL;
 }
+
+BLOCK_CREATE(multiplexer) = {
+	.create = block_multiplexer_create,
+	.args = { "int", NULL },
+};

controller/block/block_pid.c

@@ -115,10 +115,6 @@ static void param_prep(struct controller_block *pid)
 	
 static int param_set_kp(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->kp = va_arg(val, double);
 	param_prep(pid);
 	return 0;
@@ -126,10 +122,6 @@ static int param_set_kp(struct controller_block *pid, int argc, va_list val)
 
 static int param_set_ki(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->ki = va_arg(val, double);
 	param_prep(pid);
 	return 0;
@@ -137,10 +129,6 @@ static int param_set_ki(struct controller_block *pid, int argc, va_list val)
 
 static int param_set_kd(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->kd = va_arg(val, double);
 	param_prep(pid);
 	return 0;
@@ -155,10 +143,10 @@ static int param_set_reset(struct controller_block *pid, int argc, va_list val)
 }
 
 static struct controller_block_param_list params[] = {
-	{ "kp",    true, param_set_kp },
-	{ "ki",    true, param_set_ki },
-	{ "kd",    true, param_set_kd },
-	{ "reset", true, param_set_reset },
+	{ "kp",    true, param_set_kp, .args = { "double", NULL } },
+	{ "ki",    true, param_set_ki, .args = { "double", NULL } },
+	{ "kd",    true, param_set_kd, .args = { "double", NULL } },
+	{ "reset", true, param_set_reset, .args = { NULL } },
 	{ NULL },
 };
 

controller/block/block_pid_aw.c

@@ -120,10 +120,6 @@ static void param_prep(struct controller_block *pid)
 
 static int param_set_kp(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->kp = va_arg(val, double);
 	param_prep(pid);
 	return 0;
@@ -131,10 +127,6 @@ static int param_set_kp(struct controller_block *pid, int argc, va_list val)
 
 static int param_set_ki(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->ki = va_arg(val, double);
 	param_prep(pid);
 	return 0;
@@ -142,10 +134,6 @@ static int param_set_ki(struct controller_block *pid, int argc, va_list val)
 
 static int param_set_kd(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->kd = va_arg(val, double);
 	param_prep(pid);
 	return 0;
@@ -153,10 +141,6 @@ static int param_set_kd(struct controller_block *pid, int argc, va_list val)
 
 static int param_set_maxw(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->maxi = va_arg(val, double);
 
 	param_prep(pid);
@@ -165,10 +149,6 @@ static int param_set_maxw(struct controller_block *pid, int argc, va_list val)
 
 static int param_set_minw(struct controller_block *pid, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", pid->name);
-		return -1;
-	}
 	pid->private->mini = va_arg(val, double);
 
 	param_prep(pid);
@@ -188,12 +168,12 @@ static int param_set_reset(struct controller_block *pid, int argc, va_list val)
 }
 
 static struct controller_block_param_list params[] = {
-	{ "kp",    true, param_set_kp  },
-	{ "ki",    true, param_set_ki  },
-	{ "kd",    true, param_set_kd  },
-	{ "maxw",  false, param_set_maxw },
-	{ "minw",  false, param_set_minw },
-	{ "reset", true, param_set_reset },
+	{ "kp",    true, param_set_kp, .args = { "double", NULL } },
+	{ "ki",    true, param_set_ki, .args = { "double", NULL } },
+	{ "kd",    true, param_set_kd, .args = { "double", NULL } },
+	{ "maxw",  false, param_set_maxw, .args = { "double", NULL } },
+	{ "minw",  false, param_set_minw, .args = { "double", NULL } },
+	{ "reset", true, param_set_reset, .args = { NULL } },
 	{ NULL },
 };
 

controller/block/block_quantize.c

@@ -59,17 +59,13 @@ static void quantize_calculate(struct controller_block *quantize)
 
 static int param_set(struct controller_block *quantize, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", quantize->name);
-		return -1;
-	}
 	quantize->private->quantum = va_arg(val, double);
 	quantize->private->invquantum = 1.0 / quantize->private->quantum;
 	return 0;
 }
 
 static struct controller_block_param_list params[] = {
-	{ "quantum", true, param_set },
+	{ "quantum", true, param_set, .args = { "double", NULL } },
 	{ NULL },
 };
 

controller/block/block_rangecheck.c

@@ -59,27 +59,19 @@ static void rangecheck_calculate(struct controller_block *check)
 
 static int param_set_max(struct controller_block *check, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", check->name);
-		return -1;
-	}
 	check->private->max = va_arg(val, double);
 	return 0;
 }
 
 static int param_set_min(struct controller_block *block, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", block->name);
-		return -1;
-	}
 	block->private->min = va_arg(val, double);
 	return 0;
 }
 
 static struct controller_block_param_list params[] = {
-	{ "max", false, param_set_max },
-	{ "min", false, param_set_min },
+	{ "max", false, param_set_max, .args = { "double", NULL } },
+	{ "min", false, param_set_min, .args = { "double", NULL } },
 	{ NULL },
 };
 

controller/block/block_servo_state.c

@@ -228,60 +228,40 @@ static struct controller_block_outterm_list outterms[] = {
 
 static int param_set_min_x(struct controller_block *servo_state, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", servo_state->name);
-		return -1;
-	}
 	servo_state->private->min_x = va_arg(val, double);
 	return 0;
 }
 
 static int param_set_max_x(struct controller_block *servo_state, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", servo_state->name);
-		return -1;
-	}
 	servo_state->private->max_x = va_arg(val, double);
 	return 0;
 }
 
 static int param_set_max_v(struct controller_block *servo_state, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", servo_state->name);
-		return -1;
-	}
 	servo_state->private->max_v = va_arg(val, double);
 	return 0;
 }
 
 static int param_set_max_a(struct controller_block *servo_state, int argc, va_list val)
 {
-	if (argc != 1) {
-		log_send(LOG_T_ERROR, "%s: parameter expects one argument", servo_state->name);
-		return -1;
-	}
 	servo_state->private->max_a = va_arg(val, double);
 	return 0;
 }