Merge remote-tracking branch 'px4/master' into pwm_ioctls

Conflicts:
	src/drivers/px4io/px4io.cpp

6 files changed, 215 insertions, 48 deletions

diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp
index e642ed067..6da39d371 100644
--- a/src/systemcmds/mixer/mixer.c
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -47,10 +47,15 @@
 #include <nuttx/compiler.h>
 
 #include <systemlib/err.h>
-#include <drivers/drv_mixer.h>
+#include <systemlib/mixer/mixer.h>
 #include <uORB/topics/actuator_controls.h>
 
-__EXPORT int mixer_main(int argc, char *argv[]);
+/**
+ * Mixer utility for loading mixer files to devices
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mixer_main(int argc, char *argv[]);
 
 static void	usage(const char *reason) noreturn_function;
 static void	load(const char *devname, const char *fname) noreturn_function;
@@ -87,8 +92,6 @@ static void
 load(const char *devname, const char *fname)
 {
 	int		dev;
-	FILE		*fp;
-	char		line[120];
 	char		buf[2048];
 
 	/* open the device */
@@ -99,49 +102,7 @@ load(const char *devname, const char *fname)
 	if (ioctl(dev, MIXERIOCRESET, 0))
 		err(1, "can't reset mixers on %s", devname);
 
-	/* open the mixer definition file */
-	fp = fopen(fname, "r");
-	if (fp == NULL)
-		err(1, "can't open %s", fname);
-
-	/* read valid lines from the file into a buffer */
-	buf[0] = '\0';
-	for (;;) {
-
-		/* get a line, bail on error/EOF */
-		line[0] = '\0';
-		if (fgets(line, sizeof(line), fp) == NULL)
-			break;
-
-		/* if the line doesn't look like a mixer definition line, skip it */
-		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
-			continue;
-
-		/* compact whitespace in the buffer */
-		char *t, *f;
-		for (f = buf; *f != '\0'; f++) {
-			/* scan for space characters */
-			if (*f == ' ') {
-				/* look for additional spaces */
-				t = f + 1;
-				while (*t == ' ')
-					t++;
-				if (*t == '\0') {
-					/* strip trailing whitespace */
-					*f = '\0';
-				} else if (t > (f + 1)) {
-					memmove(f + 1, t, strlen(t) + 1);
-				}
-			}
-		}
-
-		/* if the line is too long to fit in the buffer, bail */
-		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
-			break;
-
-		/* add the line to the buffer */
-		strcat(buf, line);
-	}
+	load_mixer_file(fname, &buf[0], sizeof(buf));
 
 	/* XXX pass the buffer to the device */
 	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk
index d5a3f9f77..cdbff75f0 100644
--- a/src/systemcmds/mixer/module.mk
+++ b/src/systemcmds/mixer/module.mk
@@ -36,7 +36,7 @@
 #
 
 MODULE_COMMAND	 = mixer
-SRCS		 = mixer.c
+SRCS		 = mixer.cpp
 
 MODULE_STACKSIZE = 4096
 
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 754d3a0da..6729ce4ae 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -23,6 +23,7 @@ SRCS			 = test_adc.c \
 			   test_uart_console.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
+			   test_mixer.cpp \
 			   tests_file.c \
 			   tests_main.c \
 			   tests_param.c
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
new file mode 100644
index 000000000..4da86042d
--- /dev/null
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_mixer.hpp
+ *
+ * Mixer load test
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <time.h>
+
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+
+#include "tests.h"
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+int test_mixer(int argc, char *argv[])
+{
+	warnx("testing mixer");
+
+	char *filename = "/etc/mixers/IO_pass.mix";
+
+	if (argc > 1)
+		filename = argv[1];
+
+	warnx("loading: %s", filename);
+
+	char		buf[2048];
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	unsigned loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	/* load the mixer in chunks, like
+	 * in the case of a remote load,
+	 * e.g. on PX4IO.
+	 */
+
+	unsigned nused = 0;
+
+	const unsigned chunk_size = 64;
+
+	MixerGroup mixer_group(mixer_callback, 0);
+
+	/* load at once test */
+	unsigned xx = loaded;
+	mixer_group.load_from_buf(&buf[0], xx);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 8)
+		return 1;
+
+	unsigned empty_load = 2;
+	char empty_buf[2];
+	empty_buf[0] = ' ';
+	empty_buf[1] = '\0';
+	mixer_group.reset();
+	mixer_group.load_from_buf(&empty_buf[0], empty_load);
+	warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+	if (empty_load != 0)
+		return 1;
+
+	/* FIRST mark the mixer as invalid */
+	bool mixer_ok = false;
+	/* THEN actually delete it */
+	mixer_group.reset();
+	char mixer_text[256];		/* large enough for one mixer */
+	unsigned mixer_text_length = 0;
+
+	unsigned transmitted = 0;
+
+	warnx("transmitted: %d, loaded: %d", transmitted, loaded);
+
+	while (transmitted < loaded) {
+
+		unsigned	text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
+
+		/* check for overflow - this would be really fatal */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+			bool mixer_ok = false;
+			return 1;
+		}
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+
+			/* only set mixer ok if no residual is left over */
+			if (resid == 0) {
+				mixer_ok = true;
+			} else {
+				/* not yet reached the end of the mixer, set as not ok */
+				mixer_ok = false;
+			}
+
+			warnx("used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		transmitted += text_length;
+	}
+
+	warnx("chunked load: loaded %u mixers", mixer_group.count());
+
+	if (mixer_group.count() != 8)
+		return 1;
+
+	/* load multirotor at once test */
+	mixer_group.reset();
+
+	if (argc > 2)
+		filename = argv[2];
+	else
+		filename = "/etc/mixers/FMU_quad_w.mix";
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	unsigned mc_loaded = loaded;
+	mixer_group.load_from_buf(&buf[0], mc_loaded);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 8)
+		return 1;
+}
+
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
+{
+	if (control_group != 0)
+		return -1;
+
+	control = 0.0f;
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index c02ea6808..c483108cf 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -76,6 +76,8 @@
  * Public Function Prototypes
  ****************************************************************************/
 
+__BEGIN_DECLS
+
 extern int	test_sensors(int argc, char *argv[]);
 extern int	test_gpio(int argc, char *argv[]);
 extern int	test_hrt(int argc, char *argv[]);
@@ -98,5 +100,8 @@ extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
+extern int	test_mixer(int argc, char *argv[]);
+
+__END_DECLS
 
 #endif /* __APPS_PX4_TESTS_H */
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 9f8c5c9ea..9eb9c632e 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -110,6 +110,7 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };