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

Conflicts:
	src/drivers/px4io/px4io.cpp

8 files changed, 251 insertions, 48 deletions

diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index b1bb1a66d..cce46bf5f 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -50,6 +50,8 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <ctype.h>
+#include <systemlib/err.h>
 
 #include "mixer.h"
 
@@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler)
 	return 0;
 }
 
+const char *
+Mixer::findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
+	}
+	return nullptr;
+}
+
+const char *
+Mixer::skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
+		/* skip up to it AND one beyond - could be on the NUL symbol now */
+		buflen -= (p - buf) + 1;
+		return p + 1;
+	}
+
+	return nullptr;
+}
+
 /****************************************************************************/
 
 NullMixer::NullMixer() :
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index bbfa130a9..723bf9f3b 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -128,7 +128,9 @@
 #ifndef _SYSTEMLIB_MIXER_MIXER_H
 #define _SYSTEMLIB_MIXER_MIXER_H value
 
+#include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+#include "mixer_load.h"
 
 /**
  * Abstract class defining a mixer mixing zero or more inputs to
@@ -210,6 +212,24 @@ protected:
 	 */
 	static int			scale_check(struct mixer_scaler_s &scaler);
 
+	/**
+	 * Find a tag
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @param tag			character to search for.
+	 */
+	static const char *		findtag(const char *buf, unsigned &buflen, char tag);
+
+	/**
+	 * Skip a line
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @return			0 / OK if a line could be skipped, 1 else
+	 */
+	static const char *		skipline(const char *buf, unsigned &buflen);
+
 private:
 };
 
@@ -239,6 +259,11 @@ public:
 	void				reset();
 
 	/**
+	 * Count the mixers in the group.
+	 */
+	unsigned			count();
+
+	/**
 	 * Adds mixers to the group based on a text description in a buffer.
 	 *
 	 * Mixer definitions begin with a single capital letter and a colon.
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 1dbc512cd..3ed99fba0 100644
--- a/src/modules/systemlib/mixer/mixer_group.cpp
+++ b/src/modules/systemlib/mixer/mixer_group.cpp
@@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space)
 	return index;
 }
 
+unsigned
+MixerGroup::count()
+{
+	Mixer	*mixer = _first;
+	unsigned index = 0;
+
+	while ((mixer != nullptr)) {
+		mixer = mixer->_next;
+		index++;
+	}
+
+	return index;
+}
+
 void
 MixerGroup::groups_required(uint32_t &groups)
 {
@@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 
 			/* only adjust buflen if parsing was successful */
 			buflen = resid;
+			debug("SUCCESS - buflen: %d", buflen);
 		} else {
 
 			/*
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
new file mode 100644
index 000000000..a55ddf8a3
--- /dev/null
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer_load.c
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include <nuttx/config.h>
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "mixer_load.h"
+
+int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
+{
+	FILE		*fp;
+	char		line[120];
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+	if (fp == NULL) {
+		return 1;
+	}
+
+	/* 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 = line; *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) >= maxlen) {
+			return 1;
+		}
+
+		/* add the line to the buffer */
+		strcat(buf, line);
+	}
+
+	return 0;
+}
+
diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h
new file mode 100644
index 000000000..4b7091d5b
--- /dev/null
+++ b/src/modules/systemlib/mixer/mixer_load.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 mixer_load.h
+ *
+ */
+
+
+#ifndef _SYSTEMLIB_MIXER_LOAD_H
+#define _SYSTEMLIB_MIXER_LOAD_H value
+
+#include <nuttx/config.h>
+
+__BEGIN_DECLS
+
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 2446cc3fb..b89f341b6 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
 	{  1.000000,  0.000000, -1.00 },
 	{ -1.000000,  0.000000, -1.00 },
 };
-const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
 	&_config_quad_v[0],
@@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 };
-const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */
 	4, /* quad_plus */
 	4, /* quad_v */
@@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	}
 
 	if (used > (int)buflen) {
-		debug("multirotor spec used %d of %u", used, buflen);
+		debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
 		return nullptr;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
 
 	if (!strcmp(geomname, "4+")) {
 		geometry = MultirotorMixer::QUAD_PLUS;
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index c8434f991..c3985b5de 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -55,7 +55,7 @@
 #include "mixer.h"
 
 #define debug(fmt, args...)	do { } while(0)
-// #define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
 SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
@@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer()
 		free(_info);
 }
 
-static const char *
-findtag(const char *buf, unsigned &buflen, char tag)
-{
-	while (buflen >= 2) {
-		if ((buf[0] == tag) && (buf[1] == ':'))
-			return buf;
-		buf++;
-		buflen--;
-	}
-	return nullptr;
-}
-
-static void
-skipline(const char *buf, unsigned &buflen)
-{
-	const char *p;
-
-	/* if we can find a CR or NL in the buffer, skip up to it */
-	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
-		buflen -= (p - buf);
-}
-
 int
 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
 {
@@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	scaler.negative_scale	= s[0] / 10000.0f;
 	scaler.positive_scale	= s[1] / 10000.0f;
@@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 
 	buf = findtag(buf, buflen, 'S');
 	if ((buf == nullptr) || (buflen < 16)) {
-		debug("contorl parser failed finding tag, ret: '%s'", buf);
+		debug("control parser failed finding tag, ret: '%s'", buf);
 		return -1;
 	}
 
@@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 		debug("control parse failed on '%s'", buf);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	control_group		= u[0];
 	control_index		= u[1];
@@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	int used;
 	const char *end = buf + buflen;
 
-	/* enforce that the mixer ends with space or a new line */
-	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
-			continue;
-
-		/* require a space or newline at the end of the buffer, fail on printable chars */
-		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
-			/* found a line ending or space, so no split symbols / numbers. good. */
-			break;
-		} else {
-			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
-			goto out;
-		}
-
-	}
-
 	/* get the base info for the mixer */
 	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
 		debug("simple parse failed on '%s'", buf);
 		goto out;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		goto out;
+	}
 
 	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
 
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
index 4d45e1c50..fc7485e20 100644
--- a/src/modules/systemlib/mixer/module.mk
+++ b/src/modules/systemlib/mixer/module.mk
@@ -39,4 +39,5 @@ LIBNAME	= mixerlib
 SRCS		= mixer.cpp \
 		  mixer_group.cpp \
 		  mixer_multirotor.cpp \
-		  mixer_simple.cpp
+		  mixer_simple.cpp \
+		  mixer_load.c