Checkpoint: more work in progress on mixer load/save

8 files changed, 158 insertions, 148 deletions

diff --git a/apps/drivers/drv_mixer.h b/apps/drivers/drv_mixer.h
index 10cda792a..f3a39e60c 100644
--- a/apps/drivers/drv_mixer.h
+++ b/apps/drivers/drv_mixer.h
@@ -80,11 +80,15 @@ struct MixInfo
  * is allocated following the MixInfo structure.  If the allocation
  * is too small, no mixer data is retured.  The control_count field in
  * the MixInfo.mixer structure is always updated.
+ *
+ * If no mixer is assigned for the given index, the ioctl returns ENOENT.
  */
 #define MIXERIOCGETMIXER(_mixer)	_MIXERIOC(0x20 + _mixer)
 
 /**
  * Copy a mixer from *(struct MixMixer *)arg to the device.
+ *
+ * If arg is zero, the mixer is deleted.
  */
 #define MIXERIOCSETMIXER(_mixer)	_MIXERIOC(0x40 + _mixer)
 
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index ff47261e7..8e468c012 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -110,13 +110,14 @@ FMUServo::FMUServo(Mode mode) :
 	_task_should_exit(false),
 	_armed(false)
 {
-	for (unsigned i = 0; i < 4; i++)
+	for (unsigned i = 0; i < _max_actuators; i++)
 		_mixer[i] = nullptr;
 }
 
 FMUServo::~FMUServo()
 {
 	if (_task != -1) {
+
 		/* task should wake up every 100ms or so at least */
 		_task_should_exit = true;
 
@@ -168,8 +169,6 @@ FMUServo::task_main_trampoline(int argc, char *argv[])
 void
 FMUServo::task_main()
 {
-	log("ready");
-
 	/* configure for PWM output */
 	switch (_mode) {
 	case MODE_2PWM:
@@ -217,7 +216,7 @@ FMUServo::task_main()
 
 		/* do we have a control update? */
 		if (fds[0].revents & POLLIN) {
-			struct actuator_controls ac;
+			struct actuator_controls_s ac;
 
 			/* get controls */
 			orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
@@ -239,7 +238,7 @@ FMUServo::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			struct actuator_armed aa;
+			struct actuator_armed_s aa;
 
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -329,6 +328,12 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MIXERIOCGETMIXER(0):
 		channel = cmd - MIXERIOCGETMIXER(0);
 
+		/* if no mixer is assigned, we return ENOENT */
+		if (_mixer[channel] == nullptr) {
+			ret = -ENOENT;
+			break;
+		}
+
 		/* caller's MixInfo */
 		mi = (struct MixInfo *)arg;
 
@@ -350,10 +355,14 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* FALLTHROUGH */
 	case MIXERIOCSETMIXER(1):
 	case MIXERIOCSETMIXER(0):
-		channel = cmd - MIXERIOCGETMIXER(0);
+		channel = cmd - MIXERIOCSETMIXER(0);
 
-		/* caller- supplied mixer */
+		/* get the caller-supplied mixer and check */
 		mm = (struct MixMixer *)arg;
+		if (mixer_check(mm, NUM_ACTUATOR_CONTROLS)) {
+			ret = -EINVAL;
+			break;
+		}
 
 		/* allocate local storage and copy from the caller*/
 		if (mm != nullptr) {
@@ -382,13 +391,13 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 namespace {
 
 enum PortMode {
+	PORT_MODE_UNSET = 0,
 	PORT_FULL_GPIO,
 	PORT_FULL_SERIAL,
 	PORT_FULL_PWM,
 	PORT_GPIO_AND_SERIAL,
 	PORT_PWM_AND_SERIAL,
 	PORT_PWM_AND_GPIO,
-	PORT_MODE_UNSET
 };
 
 PortMode g_port_mode;
@@ -476,7 +485,7 @@ fmu_new_mode(PortMode new_mode)
 
 } // namespace
 
-extern "C" int fmu_main(int argc, char *argv[]);
+extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
 
 int
 fmu_main(int argc, char *argv[])
diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile
index a83085bfc..b016ddc57 100644
--- a/apps/systemcmds/mixer/Makefile
+++ b/apps/systemcmds/mixer/Makefile
@@ -37,6 +37,6 @@
 
 APPNAME		 = mixer
 PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
+STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
index c5adf8dbf..bbbe67ff7 100644
--- a/apps/systemcmds/mixer/mixer.c
+++ b/apps/systemcmds/mixer/mixer.c
@@ -38,9 +38,9 @@
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>
-#include <getopt.h>
 #include <unistd.h>
 #include <fcntl.h>
+#include <errno.h>
 
 #include <systemlib/mixer.h>
 #include <drivers/drv_mixer.h>
@@ -48,80 +48,43 @@
 
 __EXPORT int mixer_main(int argc, char *argv[]);
 
-static void	usage(void);
+static void	usage(const char *reason);
 static void	load(const char *devname, const char *fname);
 static void	save(const char *devname, const char *fname);
 static void	show(const char *devname);
 
-enum Operation {
-	LOAD,
-	SAVE,
-	SHOW,
-	NONE
-};
-
 int
 mixer_main(int argc, char *argv[])
 {
-	const char *devname = NULL;
-	const char *fname = NULL;
-	int ch;
-	enum Operation todo = NONE;
-
 	if (argc < 2)
-		usage();
+		usage("missing command");
 	if (!strcmp(argv[1], "load")) {
-		todo = LOAD;
+		if (argc < 4)
+			usage("missing device or filename");
+		load(argv[2], argv[3]);
 	} else if (!strcmp(argv[1], "save")) {
-		todo = SAVE;
+		if (argc < 4)
+			usage("missing device or filename");
+		save(argv[2], argv[3]);
 	} else if (!strcmp(argv[1], "show")) {
-		todo = SHOW;
-	} else {
-		usage();
-	}
-
-	while ((ch = getopt(argc, argv, "d:f:")) != EOF) {
-		switch (ch) {
-		case 'd':
-			devname = optarg;
-			break;
-		case 'f':
-			fname = optarg;
-			break;
-		default:
-			usage();
-		}
-	}
+		if (argc < 3)
+			usage("missing device name");
+		show(argv[2]);
 
-	switch (todo) {
-	case LOAD:
-		if ((devname == NULL) || (fname == NULL))
-			usage();
-		load(devname, fname);
-		break;
-
-	case SAVE:
-		if ((devname == NULL) || (fname == NULL))
-			usage();
-		save(devname, fname);
-		break;
-
-	case SHOW:
-		if (devname == NULL)
-			usage();
-		show(devname);
-		break;
-	default:
-		exit(1);
+	} else {
+		usage("unrecognised command");
 	}
 	return 0;
 }
 
 static void
-usage(void)
+usage(const char *reason)
 {
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
 	fprintf(stderr, "usage:\n");
-	fprintf(stderr, "  mixer {load|save|show} -d <device> [-f <filename>]\n");
+	fprintf(stderr, "  mixer show <device>\n");
+	fprintf(stderr, "  mixer {load|save} <device> [<filename>]\n");
 	exit(1);
 }
 
@@ -132,7 +95,7 @@ load(const char *devname, const char *fname)
 	int		dev = -1;
 	unsigned	num_mixers = 0;
 	int		ret, result = 1;
-	struct MixMixer	*mixer;
+	struct MixMixer	*mixer = NULL;
 
 	/* open the device */
 	if ((dev = open(devname, 0)) < 0) {
@@ -156,29 +119,42 @@ load(const char *devname, const char *fname)
 	/* send mixers to the device */
 	for (unsigned i = 0; i < num_mixers; i++) {
 		ret = mixer_load(defs, &mixer);
-		if (ret < 0)
+		if (ret < 0) {
+			fprintf(stderr, "read for mixer %d failed\n", i);
 			goto out;
+		}
 
 		/* end of file? */
 		if (ret == 0)
 			break;
 
-		/* sanity check the mixer */
-		if (mixer_check(mixer, NUM_ACTUATOR_CONTROLS)) {
-			fprintf(stderr, "mixer %u fails sanity check\n", i);
-			goto out;
+		if (mixer != NULL) {
+			/* sanity check the mixer */
+			ret = mixer_check(mixer, NUM_ACTUATOR_CONTROLS);
+			if (ret != 0) {
+				fprintf(stderr, "mixer %u fails sanity check %d\n", i, ret);
+				goto out;
+			}
+
+			/* send the mixer to the device */
+			ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer);
+			if (ret < 0) {
+				fprintf(stderr, "mixer %d set failed\n", i);
+				goto out;
+			}
+
+			free(mixer);
+			mixer = NULL;
+		} else {
+			/* delete the mixer */
+			ret = ioctl(dev, MIXERIOCSETMIXER(i), 0);
+			if (ret < 0) {
+				fprintf(stderr, "mixer %d clear failed\n", i);
+				goto out;
+			}
 		}
-
-		/* send the mixer to the device */
-		ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer);
-		if (ret < 0)
-			goto out;
-
-		free(mixer);
-		mixer = NULL;
 	}
 
-	printf("mixer: loaded %d mixers to %s\n", num_mixers, devname);
 	result = 0;
 
 out:
@@ -201,18 +177,16 @@ getmixer(int dev, unsigned mixer_number, struct MixInfo **mip)
 
 	/* first-round initialisation */
 	if (mi == NULL) {
+		mi = (struct MixInfo *)malloc(MIXINFO_SIZE(0));
 		mi->num_controls = 0;
-		mi = (struct MixInfo *)malloc(MIXINFO_SIZE(mi->num_controls));
 	}
 
 	/* loop trying to get the next mixer until the buffer is big enough */
 	do {
 		/* try to get the mixer into the buffer as it stands */
 		ret = ioctl(dev, MIXERIOCGETMIXER(mixer_number), (unsigned long)mi);
-		if (ret < 0) {
-			fprintf(stderr, "MIXERIOCGETMIXER error\n");
+		if (ret < 0)
 			return -1;
-		}
 
 		/* did the mixer fit? */
 		if (mi->mixer.control_count <= mi->num_controls)
@@ -262,12 +236,16 @@ save(const char *devname, const char *fname)
 
 	/* get mixers from the device and save them */
 	for (unsigned i = 0; i < num_mixers; i++) {
+		struct MixMixer *mm;
 
 		ret = getmixer(dev, i, &mi);
-		if (ret < 0)
-			goto out;
-
-		ret = mixer_save(defs, &mi->mixer);
+		mm = &mi->mixer;
+		if (ret < 0) {
+			if (errno != ENOENT)
+				goto out;
+			mm = NULL;
+		}
+		ret = mixer_save(defs, mm);
 		if (ret < 0)
 			goto out;
 	}
@@ -289,7 +267,7 @@ out:
 static void
 show(const char *devname)
 {
-	struct MixInfo	*mi;
+	struct MixInfo	*mi = NULL;
 	int		dev = -1;
 	unsigned	num_mixers = 0;
 	int		ret;
@@ -311,19 +289,23 @@ show(const char *devname)
 	for (unsigned i = 0; i < num_mixers; i++) {
 
 		ret = getmixer(dev, i, &mi);
-		if (ret < 0)
-			goto out;
+		if (ret < 0) {
+			if (errno != ENOENT)
+				goto out;
+			continue;
+		}
 
-		printf("mixer %d : %d controls\n", i, mi->mixer.control_count);
-		printf("        -ve scale  +ve scale  offset  low limit  high limit");
-		printf("output  %f   %f   %f   %f   %f\n", 
+		printf("mixer %d:\n", i);
+		printf("        -ve scale  +ve scale    offset   low limit  high limit\n");
+		printf("output  %8.4f   %8.4f   %8.4f   %8.4f   %8.4f\n", 
 			mi->mixer.output_scaler.negative_scale,
 			mi->mixer.output_scaler.positive_scale,
 			mi->mixer.output_scaler.offset,
 			mi->mixer.output_scaler.lower_limit,
 			mi->mixer.output_scaler.upper_limit);
 		for (unsigned j = 0; j < mi->mixer.control_count; j++) {
-			printf("%d:     %f   %f   %f   %f   %f\n", 
+			printf("%d:      %8.4f   %8.4f   %8.4f   %8.4f   %8.4f\n", 
+				j,
 				mi->mixer.control_scaler[j].negative_scale,
 				mi->mixer.control_scaler[j].positive_scale,
 				mi->mixer.control_scaler[j].offset,
@@ -333,6 +315,8 @@ show(const char *devname)
 	}
 
 out:
+	printf("done\n");
+	usleep(100000);
 	/* free the mixinfo */
 	if (mi != NULL)
 		free(mi);
diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c
index cc7baebc2..94fabe87a 100644
--- a/apps/systemlib/mixer.c
+++ b/apps/systemlib/mixer.c
@@ -53,19 +53,19 @@ static int
 scale_check(struct MixScaler *scale)
 {
 	if (scale->offset > 1.0f)
-		return -1;
+		return 1;
 
-	if (scale->offset > 1.0f)
-		return -1;
+	if (scale->offset < -1.0f)
+		return 2;
 
 	if (scale->lower_limit > scale->upper_limit)
-		return -1;
+		return 3;
 
 	if (scale->lower_limit < -1.0f)
-		return -1;
+		return 4;
 
 	if (scale->upper_limit > 1.0f)
-		return -1;
+		return 5;
 
 	return 0;
 }
@@ -73,21 +73,25 @@ scale_check(struct MixScaler *scale)
 int
 mixer_check(struct MixMixer *mixer, unsigned control_count)
 {
+	int ret;
+
 	if (mixer->control_count < 1)
 		return -1;
 
 	if (mixer->control_count > control_count)
-		return -1;
+		return -2;
 
-	if (!scale_check(&mixer->output_scaler))
-		return -1;
+	ret = scale_check(&mixer->output_scaler);
+	if (ret != 0)
+		return ret;
 
 	for (unsigned i = 0; i < mixer->control_count; i++) {
 		if (mixer->control_scaler[i].control >= control_count)
-			return -1;
+			return -3;
 
-		if (!scale_check(&mixer->control_scaler[i]))
-			return -1;
+		ret = scale_check(&mixer->control_scaler[i]);
+		if (ret != 0)
+			return (10 * i + ret);
 	}
 
 	return 0;
@@ -148,6 +152,7 @@ mixer_getline(int fd, char *line, unsigned maxlen)
 		*line++ = c;
 	}
 	/* line too long */
+	puts("line too long");
 	return -1;
 }
 
@@ -184,32 +189,33 @@ mixer_load(int fd, struct MixMixer **mp)
 	if (sscanf(buf, "M: %u", &scalers) != 1)
 		goto out;
 
-	/* must have at least one scaler */
-	if (scalers < 1)
-		goto out;
-
-	/* allocate mixer */
-	scalers--;
-	mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
-
-	if (mixer == NULL)
-		goto out;
+	/* if there are scalers, load them */
+	if (scalers > 0) {
 
-	mixer->control_count = scalers;
+		/* allocate mixer */
+		scalers--;
+		mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
 
-	ret = mixer_getline(fd, buf, sizeof(buf));
+		if (mixer == NULL)
+			goto out;
 
-	if (ret < 1)
-		goto out;
+		mixer->control_count = scalers;
 
-	if (mixer_load_scaler(buf, &mixer->output_scaler))
-		goto out;
+		ret = mixer_getline(fd, buf, sizeof(buf));
 
-	for (unsigned i = 0; i < scalers; i++) {
-		if (mixer_getline(fd, buf, sizeof(buf)))
+		if (ret < 1)
 			goto out;
-		if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
+
+		if (mixer_load_scaler(buf, &mixer->output_scaler))
 			goto out;
+
+		for (unsigned i = 0; i < scalers; i++) {
+			ret = mixer_getline(fd, buf, sizeof(buf));
+			if (ret < 1)
+				goto out;
+			if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
+				goto out;
+		}
 	}
 
 	result = 1;
@@ -237,23 +243,25 @@ mixer_save(int fd, struct MixMixer *mixer)
 	int		len, ret;
 	
 	/* write the mixer header */
-	len = sprintf(buf, "M: %u\n", mixer->control_count);
+	len = sprintf(buf, "M: %u\n", (mixer != NULL) ? mixer->control_count : 0);
 	ret = write(fd, buf, len);
 	if (ret != len)
 		return -1;
 
-	/* write the output scaler */
-	len = mixer_save_scaler(buf, &mixer->output_scaler);
-	write(fd, buf, len);
-	if (ret != len)
-		return -1;
-
-	/* write the control scalers */
-	for (unsigned j = 0; j < mixer->control_count; j++) {
-		len = mixer_save_scaler(buf, &mixer->control_scaler[j]);
+	if (mixer != NULL) {
+		/* write the output scaler */
+		len = mixer_save_scaler(buf, &mixer->output_scaler);
 		write(fd, buf, len);
 		if (ret != len)
 			return -1;
+
+		/* write the control scalers */
+		for (unsigned j = 0; j < mixer->control_count; j++) {
+			len = mixer_save_scaler(buf, &mixer->control_scaler[j]);
+			write(fd, buf, len);
+			if (ret != len)
+				return -1;
+		}
 	}
 	return 0;
-}
+}
\ No newline at end of file
diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h
index cd110f01e..d65041ea7 100644
--- a/apps/systemlib/mixer.h
+++ b/apps/systemlib/mixer.h
@@ -161,8 +161,8 @@ __EXPORT float	mixer_mix(struct MixMixer *mixer, float *controls);
 /**
  * Check a mixer configuration for sanity.
  *
- * @param mixer		The mixer configuration to be checked.
- * @param control_count	The number of controls in the system.
+ * @param mixer			The mixer configuration to be checked.
+ * @param control_count		The number of controls in the system.
  * @return			Zero if the mixer configuration is sane,
  *				nonzero otherwise.
  */
@@ -177,17 +177,18 @@ __EXPORT int	mixer_check(struct MixMixer *mixer, unsigned control_count);
  * scaler in the file is always the output scaler, followed by the input
  * scalers.
  *
- * M: <control count + 1>
+ * M: <scaler count>
  * S: <control> <negative_scale> <positive_scale> <offset> <lower_limit> <upper_limit>
  * S: ...
  *
- * The <control> value for the output scaler is ignored.
+ * The <control> value for the output scaler is ignored by the mixer.
  *
  * Multiple mixer definitions may be stored in a single file; it is assumed that
- * the reader will know how many to expect and read accordingly. Mixers may be
- * 'skipped' in a file by  setting indicating that the mixer has only one scaler 
- * (the output scaler). This results in a mixer with zero controls, which will
- * always generate output corresponding to the output scaler offset.
+ * the reader will know how many to expect and read accordingly. 
+ *
+ * A mixer entry with a scaler count of zero indicates a disabled mixer. This
+ * will return NULL for the mixer when processed by this function, and will be
+ * generated by passing NULL as the mixer to mixer_save.
  *
  * @param fd			The file to read the definitions from.
  * @param mixer			Mixer is returned here.
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 8d84ff3c2..78708cdba 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -99,3 +99,7 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s);
+
+#include "topics/actuator_controls.h"
+ORB_DEFINE(actuator_controls, struct actuator_controls_s);
+ORB_DEFINE(actuator_armed, struct actuator_armed_s);
diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h
index 03c0c7b7d..5cdaf0a4e 100644
--- a/apps/uORB/topics/actuator_controls.h
+++ b/apps/uORB/topics/actuator_controls.h
@@ -47,14 +47,14 @@
 
 #define NUM_ACTUATOR_CONTROLS	16
 
-struct actuator_controls
+struct actuator_controls_s
 {
 	float	control[NUM_ACTUATOR_CONTROLS];
 };
 
 ORB_DECLARE(actuator_controls);
 
-struct actuator_armed
+struct actuator_armed_s
 {
 	bool	armed;
 };