From c5b890e87d545328734a815d90ce16d396630048 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 12 Oct 2013 20:17:59 +0200
Subject: [PATCH 01/13] Moved mixer file load / compression into mixer library.

---
 src/modules/systemlib/mixer/mixer.cpp         |  2 +
 src/modules/systemlib/mixer/mixer.h           |  2 +
 src/modules/systemlib/mixer/mixer_load.c      | 99 +++++++++++++++++++
 src/modules/systemlib/mixer/mixer_load.h      | 51 ++++++++++
 .../systemlib/mixer/mixer_multirotor.cpp      |  4 +-
 src/modules/systemlib/mixer/module.mk         |  3 +-
 src/systemcmds/mixer/{mixer.c => mixer.cpp}   | 55 ++---------
 src/systemcmds/mixer/module.mk                |  2 +-
 8 files changed, 167 insertions(+), 51 deletions(-)
 create mode 100644 src/modules/systemlib/mixer/mixer_load.c
 create mode 100644 src/modules/systemlib/mixer/mixer_load.h
 rename src/systemcmds/mixer/{mixer.c => mixer.cpp} (73%)

diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index b1bb1a66d7..b4b82c539f 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"
 
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index bbfa130a9d..6c322ff92e 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
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
new file mode 100644
index 0000000000..18c4e474a6
--- /dev/null
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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 <systemlib/err.h>
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "mixer_load.h"
+
+int load_mixer_file(const char *fname, char *buf)
+{
+	FILE		*fp;
+	char		line[120];
+
+	/* 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);
+	}
+
+	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 0000000000..b2327a4f72
--- /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);
+
+__END_DECLS
+
+#endif
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 2446cc3fbe..89afc272c0 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 */
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
index 4d45e1c506..fc7485e202 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
diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp
similarity index 73%
rename from src/systemcmds/mixer/mixer.c
rename to src/systemcmds/mixer/mixer.cpp
index e642ed0676..b1ebebbb44 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]);
 
 	/* 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 d5a3f9f77b..cdbff75f04 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
 

From 7d6a4ece6b1875bab62c4dbcb95fcc9fa5661674 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 12 Oct 2013 20:19:09 +0200
Subject: [PATCH 02/13] Added mixer test

---
 src/systemcmds/tests/module.mk      |  1 +
 src/systemcmds/tests/test_mixer.cpp | 74 +++++++++++++++++++++++++++++
 src/systemcmds/tests/tests.h        |  5 ++
 src/systemcmds/tests/tests_main.c   |  1 +
 4 files changed, 81 insertions(+)
 create mode 100644 src/systemcmds/tests/test_mixer.cpp

diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 754d3a0da9..6729ce4ae3 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 0000000000..acb7bd88f6
--- /dev/null
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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 <time.h>
+
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+
+#include "tests.h"
+
+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]);
+
+	warnx("loaded: %s", &buf[0]);
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index c02ea6808f..c483108cf0 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 9f8c5c9eae..9eb9c632ec 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}
 };

From 42b75ae8963b2f711a72ac1cb6cfd1b44bd826b2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 12 Oct 2013 20:19:34 +0200
Subject: [PATCH 03/13] Added host-building mixer test

---
 Tools/tests-host/.gitignore         |  2 ++
 Tools/tests-host/Makefile           | 36 +++++++++++++++++++++++++++++
 Tools/tests-host/arch/board/board.h |  0
 Tools/tests-host/mixer_test.cpp     | 12 ++++++++++
 Tools/tests-host/nuttx/config.h     |  0
 5 files changed, 50 insertions(+)
 create mode 100644 Tools/tests-host/.gitignore
 create mode 100644 Tools/tests-host/Makefile
 create mode 100644 Tools/tests-host/arch/board/board.h
 create mode 100644 Tools/tests-host/mixer_test.cpp
 create mode 100644 Tools/tests-host/nuttx/config.h

diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore
new file mode 100644
index 0000000000..61e0915515
--- /dev/null
+++ b/Tools/tests-host/.gitignore
@@ -0,0 +1,2 @@
+./obj/*
+mixer_test
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
new file mode 100644
index 0000000000..c603b2aa2a
--- /dev/null
+++ b/Tools/tests-host/Makefile
@@ -0,0 +1,36 @@
+
+CC=g++
+CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
+
+ODIR=obj
+LDIR =../lib
+
+LIBS=-lm
+
+#_DEPS = test.h
+#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
+
+_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o
+OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
+
+#$(DEPS)
+$(ODIR)/%.o: %.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/systemcmds/tests/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+#
+mixer_test: $(OBJ)
+	g++ -o $@ $^ $(CFLAGS) $(LIBS)
+
+.PHONY: clean
+
+clean:
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
\ No newline at end of file
diff --git a/Tools/tests-host/arch/board/board.h b/Tools/tests-host/arch/board/board.h
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
new file mode 100644
index 0000000000..5d92040f11
--- /dev/null
+++ b/Tools/tests-host/mixer_test.cpp
@@ -0,0 +1,12 @@
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+
+extern int test_mixer(int argc, char *argv[]);
+
+int main(int argc, char *argv[]) {
+	warnx("Host execution started");
+
+	char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"};
+
+	test_mixer(2, args);
+}
\ No newline at end of file
diff --git a/Tools/tests-host/nuttx/config.h b/Tools/tests-host/nuttx/config.h
new file mode 100644
index 0000000000..e69de29bb2

From 1dc9569e31717aefab8e05b858122f433dab1698 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 13 Oct 2013 11:44:26 +0200
Subject: [PATCH 04/13] Fixed mixer chunk load and line ending detection for
 good.

---
 src/modules/systemlib/mixer/mixer.cpp         |  27 ++++
 src/modules/systemlib/mixer/mixer.h           |  23 ++++
 src/modules/systemlib/mixer/mixer_group.cpp   |  15 ++
 src/modules/systemlib/mixer/mixer_load.c      |  15 +-
 src/modules/systemlib/mixer/mixer_load.h      |   2 +-
 .../systemlib/mixer/mixer_multirotor.cpp      |  10 +-
 src/modules/systemlib/mixer/mixer_simple.cpp  |  62 +++------
 src/systemcmds/mixer/mixer.cpp                |   2 +-
 src/systemcmds/tests/test_mixer.cpp           | 129 +++++++++++++++++-
 9 files changed, 229 insertions(+), 56 deletions(-)

diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index b4b82c539f..cce46bf5fc 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -116,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 6c322ff92e..723bf9f3b7 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -212,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:
 };
 
@@ -240,6 +258,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.
 	 *
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 1dbc512cdb..3ed99fba09 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
index 18c4e474a6..a55ddf8a35 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -38,22 +38,22 @@
  */
 
 #include <nuttx/config.h>
-#include <systemlib/err.h>
 #include <string.h>
 #include <stdio.h>
 #include <ctype.h>
 
 #include "mixer_load.h"
 
-int load_mixer_file(const char *fname, char *buf)
+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)
-		err(1, "can't open %s", fname);
+	if (fp == NULL) {
+		return 1;
+	}
 
 	/* read valid lines from the file into a buffer */
 	buf[0] = '\0';
@@ -70,7 +70,7 @@ int load_mixer_file(const char *fname, char *buf)
 
 		/* compact whitespace in the buffer */
 		char *t, *f;
-		for (f = buf; *f != '\0'; f++) {
+		for (f = line; *f != '\0'; f++) {
 			/* scan for space characters */
 			if (*f == ' ') {
 				/* look for additional spaces */
@@ -87,8 +87,9 @@ int load_mixer_file(const char *fname, char *buf)
 		}
 
 		/* if the line is too long to fit in the buffer, bail */
-		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
-			break;
+		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
+			return 1;
+		}
 
 		/* add the line to the buffer */
 		strcat(buf, line);
diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h
index b2327a4f72..4b7091d5b2 100644
--- a/src/modules/systemlib/mixer/mixer_load.h
+++ b/src/modules/systemlib/mixer/mixer_load.h
@@ -44,7 +44,7 @@
 
 __BEGIN_DECLS
 
-__EXPORT int load_mixer_file(const char *fname, char *buf);
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
 
 __END_DECLS
 
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 89afc272c0..b89f341b60 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -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 c8434f991b..c3985b5de6 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/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp
index b1ebebbb44..6da39d371b 100644
--- a/src/systemcmds/mixer/mixer.cpp
+++ b/src/systemcmds/mixer/mixer.cpp
@@ -102,7 +102,7 @@ load(const char *devname, const char *fname)
 	if (ioctl(dev, MIXERIOCRESET, 0))
 		err(1, "can't reset mixers on %s", devname);
 
-	load_mixer_file(fname, &buf[0]);
+	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/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index acb7bd88f6..4da86042d2 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -46,6 +46,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
+#include <string.h>
 #include <time.h>
 
 #include <systemlib/err.h>
@@ -53,6 +54,11 @@
 
 #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");
@@ -66,9 +72,128 @@ int test_mixer(int argc, char *argv[])
 
 	char		buf[2048];
 
-	load_mixer_file(filename, &buf[0]);
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	unsigned loaded = strlen(buf);
 
-	warnx("loaded: %s", &buf[0]);
+	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;
 }

From 5671df0af4e258ef0a83377cdbd50e59734aa00b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 13 Oct 2013 11:44:42 +0200
Subject: [PATCH 05/13] Improved mixer tests

---
 Tools/tests-host/Makefile       | 7 +++++--
 Tools/tests-host/mixer_test.cpp | 8 ++++----
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
index c603b2aa2a..97410ff47c 100644
--- a/Tools/tests-host/Makefile
+++ b/Tools/tests-host/Makefile
@@ -10,14 +10,14 @@ LIBS=-lm
 #_DEPS = test.h
 #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
 
-_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o
+_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o
 OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
 
 #$(DEPS)
 $(ODIR)/%.o: %.cpp
 	$(CC) -c -o $@ $< $(CFLAGS)
 
-$(ODIR)/%.o: ../../src/systemcmds/tests/%.c
+$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
 	$(CC) -c -o $@ $< $(CFLAGS)
 
 $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
@@ -26,6 +26,9 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
 $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
 	$(CC) -c -o $@ $< $(CFLAGS)
 
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
 #
 mixer_test: $(OBJ)
 	g++ -o $@ $^ $(CFLAGS) $(LIBS)
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
index 5d92040f11..042322aadc 100644
--- a/Tools/tests-host/mixer_test.cpp
+++ b/Tools/tests-host/mixer_test.cpp
@@ -1,12 +1,12 @@
 #include <systemlib/mixer/mixer.h>
 #include <systemlib/err.h>
-
-extern int test_mixer(int argc, char *argv[]);
+#include "../../src/systemcmds/tests/tests.h"
 
 int main(int argc, char *argv[]) {
 	warnx("Host execution started");
 
-	char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"};
+	char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix",
+				 "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
 
-	test_mixer(2, args);
+	test_mixer(3, args);
 }
\ No newline at end of file

From af7288ed936d642f3141a3e8792799909d351885 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 14 Oct 2013 09:56:57 +0200
Subject: [PATCH 06/13] Enable position control for Easystar type planes

---
 ROMFS/px4fmu_common/init.d/100_mpx_easystar | 3 +--
 ROMFS/px4fmu_common/init.d/101_hk_bixler    | 3 +--
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
index 828540ad90..1aa1ef494e 100644
--- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
@@ -98,8 +98,7 @@ else
 	mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
 fi
 fw_att_control start
-# Not ready yet for prime-time
-#fw_pos_control_l1 start
+fw_pos_control_l1 start
 
 if [ $EXIT_ON_END == yes ]
 then
diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler
index 5a9cb2171a..4d8a24c4ae 100644
--- a/ROMFS/px4fmu_common/init.d/101_hk_bixler
+++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler
@@ -91,8 +91,7 @@ att_pos_estimator_ekf start
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
 fw_att_control start
-# Not ready yet for prime-time
-#fw_pos_control_l1 start
+fw_pos_control_l1 start
 
 if [ $EXIT_ON_END == yes ]
 then

From 57b8dee7092eb84e8f4f31dcee85736eedb2ca8f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 14 Oct 2013 13:41:37 +0200
Subject: [PATCH 07/13] Bring back proper log conversion copy operation

---
 src/modules/sdlog2/sdlog2.c | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ec8033202d..080aa550cb 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -583,6 +583,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
+	const char *converter_in = "/etc/logging/conv.zip";
+	char* converter_out = malloc(120);
+	sprintf(converter_out, "%s/conv.zip", folder_path);
+
+	if (file_copy(converter_in, converter_out)) {
+		errx(1, "unable to copy conversion scripts, exiting.");
+	}
+	free(converter_out);
+
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -1251,7 +1260,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return ret;
+	return OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)

From c6b58491bbd7390650723c65b4d4e9ec0922c8de Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 14 Oct 2013 22:18:44 +0200
Subject: [PATCH 08/13] Work queue in RGB driver without work_cancel()

---
 src/drivers/rgbled/rgbled.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index fedff769b8..aeb7e2f787 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -248,6 +248,11 @@ RGBLED::led_trampoline(void *arg)
 void
 RGBLED::led()
 {
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
 	switch (_mode) {
 	case RGBLED_MODE_BLINK_SLOW:
 	case RGBLED_MODE_BLINK_NORMAL:
@@ -471,11 +476,6 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
 		}
 
-		/* if it should stop, then cancel the workq */
-		if (!should_run && _running) {
-			_running = false;
-			work_cancel(LPWORK, &_work);
-		}
 	}
 }
 

From fbe595a591734bffa95d28125b8e0bda117d7314 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 14 Oct 2013 23:10:12 +0200
Subject: [PATCH 09/13] Fixed some stupid compile errors

---
 src/drivers/rgbled/rgbled.cpp | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index aeb7e2f787..ea87b37d94 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -103,6 +103,7 @@ private:
 
 	bool			_running;
 	int			_led_interval;
+	bool			_should_run;
 	int			_counter;
 
 	void 			set_color(rgbled_color_t ledcolor);
@@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) :
 	_brightness(1.0f),
 	_running(false),
 	_led_interval(0),
+	_should_run(false),
 	_counter(0)
 {
 	memset(&_work, 0, sizeof(_work));
@@ -414,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode)
 {
 	if (mode != _mode) {
 		_mode = mode;
-		bool should_run = false;
 
 		switch (mode) {
 		case RGBLED_MODE_OFF:
+			_should_run = false;
 			send_led_enable(false);
 			break;
 
@@ -428,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_SLOW:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 2000;
 			_brightness = 1.0f;
@@ -436,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_NORMAL:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 500;
 			_brightness = 1.0f;
@@ -444,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_FAST:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 100;
 			_brightness = 1.0f;
@@ -452,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BREATHE:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 25;
 			send_led_enable(true);
 			break;
 
 		case RGBLED_MODE_PATTERN:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_brightness = 1.0f;
 			send_led_enable(true);
@@ -471,7 +473,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 		}
 
 		/* if it should run now, start the workq */
-		if (should_run && !_running) {
+		if (_should_run && !_running) {
 			_running = true;
 			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
 		}

From 39336fd58533c85ef6bad93b80dd51e62b6eba3a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Oct 2013 10:46:28 +0200
Subject: [PATCH 10/13] Better defaults for RC SCALE

---
 ROMFS/px4fmu_common/init.d/1000_rc_fw.hil   | 2 ++
 ROMFS/px4fmu_common/init.d/100_mpx_easystar | 2 ++
 ROMFS/px4fmu_common/init.d/101_hk_bixler    | 2 ++
 ROMFS/px4fmu_common/init.d/31_io_phantom    | 2 ++
 4 files changed, 8 insertions(+)

diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
index 6e29bd6f80..5e4028bbb8 100644
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
@@ -33,6 +33,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 
 	param set SYS_AUTOCONFIG 0
 	param save
diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
index 1aa1ef494e..4f843e9aa1 100644
--- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler
index 4d8a24c4ae..cef86c34d0 100644
--- a/ROMFS/px4fmu_common/init.d/101_hk_bixler
+++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler
@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
index 8fe94452f8..6528337454 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 17
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save

From 8ed0796448e49ae34dbfdf31c056e402834c0b8f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Oct 2013 22:17:53 +0200
Subject: [PATCH 11/13] L1 control diagram illustrating corner case

---
 Documentation/l1_control.odg | Bin 0 -> 11753 bytes
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 Documentation/l1_control.odg

diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg
new file mode 100644
index 0000000000000000000000000000000000000000..69910c677459aae4664a08b1bbeb6aaed48721ad
GIT binary patch
literal 11753
zcmeIYcT`hb6E+?}5RhIK&`_n9&^yvadPjN)0YXV23B3u@K|llnkuJSS?;uK15Ru+N
zdM_eH`fuUAzIyNLeSi1+{{CjIvsQA>%rmoRpG@|%_h_nMUL^+rZ~%Z%Jyp3hOzt)g
z003}#;Q#;tN0=iN?coFkJ2^SPAYe2M0q1drTX7@6C>V+x;RJ<SAs{Y}P&k^~1_^eu
zg+Wl7e*yf0r^#Y`6#%%rz5_zhU~YFu2MK9+M+Z7rC=vxjz$F<#+`J5QP&fo(1%um2
zGU(|la)~fVOW{c%tgT@XsMrrLT$lEA?v4&{l-M7SBpF<gaIx?4h{3^*P?Q)NBKB)w
zV!vvOeRuT79d`#9++LEw7L9fi<Kc00bK`d7=SCoHctE0}qCDT9GW@A(1^JJjlMB+}
zyOtG%2kHR596E{z#0}#4p?ewOzYXeA_t%h|kWds9j{ctKe~a;}&97Jp1mZtIU*_YF
zbbSw<kC#_~=a26{qDDEw9R97xOI<Dq!qEwghFLoNC8uCG%<<pCT<U%w>90}SAg!z%
z{uB2vlfuW#%g<xOV+BToxm;mTH^v{4|1pmLJzCMfjn)bYcKe?eAYPtJ9R|8T(8Ydv
zQTZ68e!P{J(<deIeG(x^==ZU3S%J||DL!5hKNpCX3&f`j;uGWL6B7`X;Q5W7gcU^W
zHwFR%Vjy7&o*xhY@Bp=fp<!?vE-M%0cTZB9x_lyPg2Fm~BLCO(znVcjAP!KJ6yM*p
z|GfQ&t_>6pMS{@?q?9Vm5((A#{@df>=N4j814A@)49(p|gy?>GWbTAS*g+v^9w?tE
zm=7et#b;>=wG!sz;uYcpSwV%kz@ox}d?FAo5o=x{A*(-K|Mc>Q*Z=&sQI{`13JrtM
zeNPp_@)Da1f^dPOB^mgI7<i;4{+!1@Gymh;e-OS;$R7m=2M6bSMFadI_y7O^1w~(`
z5p`LFri^A#b3(+v+vZQc>}B}SEh%k1=k=$Nm3>RAY)dy{HWS$Nkov*;lLe8}!}v&L
z(N|#_9VzNxV}R9rK7~VehXb+Q8l_jnJ+9ER?M4lV6pORGmfT}AN59BjO?Tb$UORUR
z`zpGhzL`aZ^;%CYK-)7^qHmziq14>%#l0ONQe|lJQ-GDRha<i}BB!cGsfMh4|EYoE
z5vub6J4LmraMb)jtxLG&tPnRh_vFH+wJqyiJJw9nM-e3O-m#qhnBLV))-KPz18;e6
z{&7`99!^l>gVMCR^%mhQ=-mfe-2Bx`U}C#un++Os67@%!yw)4j1ta>ms?RJtUCm>@
zeU4O|5`?^XibmDQ7ObL$zf|BRZ$Z~S>_=1U5ho^!DFc%QU1-%teKUw#6q26`Zcm7s
z=Lrz5<I?Hir}U^);q9w*#Sat<8WpR!0hb$IDe)R4eJ=L=S|o71J!sI)`E99FKkEVY
zIpvCF63lo0?Su7%%x_f>0|mQ$+_C%;uFq}s++SMxMqbE3DJQY#_95>6$~~w0JKWT+
zso(T8{aAJUHeUA<-z45bF9#~KK9VMW=jP*$znAgCiB!Gs)j}%=NxVGpDb^%OS-Oe7
zl4Wx=YlulWW9=&VfXdyDZtrc^gN{5REy0MJ75An|xA<cuDWB>E&DzTvTp+pLS!PC4
z7g~A2ZxFDu;B~yy51Uau_Fuc%ywRpxI>Bv|Rv1o^O~c2)VS42HB`h%7#$X($3|SAK
zE&Upm!UaiTq&O-y<d@yPd3vbkJQGsinc9zKq2G=@4FTTVN;Z_5kyONQYoO@7J;Su#
zTTi_Gtcw}X2hG{rb2U*N_L*iU3xfzsis!as@HsrAvE!b2U2oo@M38~Y{J<o;N$?%|
z6&_sk*Ni0yN{Y6HKrP_HjW))Pv1AK2p~uD_2vy^_+2uEa)nidyhi;fY(RQV1TFj9+
znaJ`*dl>9gx`{td_<Sd2cG8UP@CgoX-LUXlb<hQH2eUds7HGTa`ECdK?pkqmh){qa
z=s5a>6(B<%jwf2;4`FCV8b-bTvTttKIbt^qiVOQJC?Gt!QZ-3qnna!1$MI>%j{%!V
zLhcrMndtnis}0?@%ece4+jKG%2##W)JOkV0%z*h?5WTUaGVzG~llkir90hx_xpyd2
zZ!3v5MFai9E3ZiBUM*3z|CZmDbgOU}6kuD%a(pAvEPr)UO>j}m#XPg5*kEbwi5Xd4
z<<cIPC|I2#M_^MP+8w6&RSwLu<Ti%QYCU-9-{NR~lsa+m`Z%lHm)z!EuDA8kPiA{%
zHfM~w$QW41?8~$Bu;5P(UZqb8q!*FEYyF&j$0QDYpD;)bSq#%RzxsO0rImVCXEd(g
z-Am`Lu_g&rn$*$Kt0~D)rDin<?Zq!xorRN`kZjk5bF2w<w!+acQq${&tMYeNFbg=L
ziZTHcOcB&k_n%MUhRC#(7T~7vs<J(@I}^x`A4TxxslOavZF@!;c7fBT^wIVD07&2E
zn!F-L>Nm3sws@^)Zo5n|nHJcxBNq>?%!s)fa3NfblmUU=%H&t|m`9jmeIszP>_#@B
z^>VnJp&2DFmLN{1WJ*?5Gk3=2$==rH1aX(Y<E$WucI0z&(`iTI=Ghvs`63YBvSy#V
zk97{OP1~>*iWHJX^X4t_y|@c(?w+_J9n@T+QICPGgCpyjuD@t=G<iL@;V_K=K7G-J
zzQER0xrRfm5kHQH1pqiQ0s#M9L$3n>00;t(zFhLTJ34HdP9P`Vk@)l)sDdl<_s45L
z;A3O5KGSWes*1lRzS!kisO{k_Q>!_Ob>%8R0p`aLebnNu?1@c<aTQmVl(}1AgOn|^
z!G=25OC7?UGh;T*4MCGFD8$`a#v8`R!U{_`)+!e<@ICuhVaJg`<>#3k3|_#p@m(u^
zCZk<o_B?JU>a`f#NA}ga@~)>9gIt+&oX!a{&ibA<FiNJ!Lk|p2R1fq_*}59s%j<Nv
zj1pQMMTZPm4jiTmS0z;zORCZu*7G}LsqSt*4_VcnpJ32`ORi~d4ZrE=;Y_3SetWsg
z<$<+LeY06<mEcgncLt=>8O`Q+>%geVA-+?)`*4qrP~IjeO0Y<{zB?VAd(47v;ASpd
z)Qiyw?%<k|(XEhRy^94mU|OmSt`}-Lt$(2Uu_U?uc4CvgaM}wS?YcDf!L(HN!R>MG
zsa;PEZ^tu9oW<@Fx8%mI1f3Jodda*-iLN^{*4a~48UvLsdN*_*nbxb|za1>>y|uZY
zcPtj&MYo;jm9>^i<Ha?%ilnkl4{`aV`H1Gp?j{}SU2-q{P_r=O#Wyn(d(ty4C+!V{
z&$bjr#fpj)m9^^MV<%bOY=4>ssP~E;R?B>!PlvuH$3pe~6_s<&vdpt3C{4PPC&@#z
zdl7vub&-cUi7`S25tD~*1p(r^*TgXSv=EsVXOH^})#t-j9hh@o9?RX)&NsfUNboA-
zddf3(Iz8ba4)e+?K~;k=^Y#o3-rAl32~q~(XVx}Nl%5pKcyqzl<L7RIn5hNb80CbD
z2G3JmicYZD&8OAv)g$u;iE4}|EvZUbZR<GPffpIqli1hzb<^u}KcaB8xSxl_gro7A
znm(6w=~|WKcCl-lG!_?xc`FQUzgb1K_rB6L$1%Mj=;oAP&~3NGjiE4usDF1YCt|Xv
zgt4^lMO~e{KrC>u>s};mrpG56RwJav|C;q(H!CZ#F6|1ph98jyekYr54+8&4N#>1k
z=zaVnvzN{bZY3MT6g&P@C7vlueZc(C$7>yZ7<8wOdU0CrHO20XjXOei-8jJpn)bLB
zLkg)SL+f-#CUOh1!rV-%p4W0gpWY2Tyfam(DQ*xefScnF^_<&|z>1La2rgzcmr%L!
zytOeK$$R(=e(n_`7qTNzQO0aS?I!qe#;v>XA(~-NH7|=7tJWJ7t<^0+1_!U|Eg$O{
zJ)5Hkhp-a9DMxYKQa0P~58Wa${W=!>M&SM!wNSAg>jf=g!fRUud6%~!zE%o-JU*|f
zqO!fkph$$h+MOvPb5}p_fx;P#QWK>`mwI+A93mRU!`mFoT|}Y)KEB|Um!L|7ZDZp?
zO{$1Ulh)^k_NxA-m*E`CUA<bQ-V7bNX`gjIN6bi(oD4ip_G9?S0wvsmV0<u_edzbN
zdy>FQOUll0+jzeD#m-po*$PP7oK`pxh&@1tlf)s{kDb@lPe8KAm2SH=eZ38T>-HI8
z>Q2a6*TIQtF!9Z*vTw%3Ggld~9d&5(>v2dEZ9QWU>v^o6)DLt$V@U3Pe)1w*!_9N&
zrR3Bq!!f4OYbFk_=Fsi}V|jSF2CKg9+~;aajYF56$!|x1a|b$aTC1Ac#e`%F;I%Bv
z!I`b0%FqJAZA&c6whctQO+>CFWpBzGHHl5Hb`&o_T16sFB5l!EhnB)+aRa<V$YB~J
zQj0rR|Jtm0PJ~s9nS3O}C;QkiOt-AnspNFK<pKs;7wDH!9}gAP+c5KgS)8G-XPnb|
zcAdP-eCwG3Y!dK_U0z1RJ1;`JB2~Pazkx9C&WZjUpKI>nYkvuh5C)W_tpLt|gza4s
z$-tF9;y2Wy%7Gsk8LZNnWH)SaNO~lc1B<fg<uyowz7pQ*ak-mW^j4{?8sbXwD*_(W
zG)j*`eICe9k&N_?A5U+7HgbwTcH0c`oH?8;Hd!KRXc-F7*mvNTns*Iyu2^Cgj#&@N
z&%{U8Ihu6);>o&EL?}n<+CD$f$NH8<IRdA|r-_^B+Vs>#_-x8ChE0h(C*n=_>pdv8
zsRo1UrW_Uouj|&D(sH1L*$M{kdKlG1A2#cRDJBKo8bIT35O<l;=ToS#ji`1SGbmMX
zTErRjoH7lpd*Q~~JKt*$5!J89?>bd?<y1F5O?*T9?k+aNDBs%1c!WYDo`n$&GnAfm
zSSXO1f42BYb4)i)?4ZXbLZF{p*K$)`U<HSMHrXHqaln%$q4D*jwgm!G@JwjkZ?oTQ
zsM8c16jSJIhda=2)mMUB&1$u4G3>>(HEY_t+|%HH9(2zA)gy+WH<pf)@$i03@hPv@
z5m?63B8g6DS>q{PLPpSYlUjVoZ>_awD|^~l@^e9(J&(I%GS@oErJLS`D+pM`ikL3I
z8%AFoYu&)A`=Ikk=kaLH`#S%dQ&h*@uV}ni2lVkmcZ63?y_ks(&`Eb3grW5Kg`<)N
z=MHoVz>3!sl;w>3>9@xE1`bC*g^9wIYBss$0?Z&EBb~)jHE9eC^T7_I8UAQKxRoQ#
z`yzL)UC3n2_L^ET(r)n6v{2-mZEm@!>AR$wdmZkA@U0EBq2vbd?U$;#kbvoXiA3nA
zL#DpK>1!}1u9k%9b&LkggGXpPPwW|Eog3D^CIjGoFB>TNZnY|ZHVam4z<uw+H(4r`
zCSj%tlE}mASOfe)(ytHeZnusuXho_Kppwh{J8rF$6gh?yuxbsnNjVN%CyG#m^5sM`
z0(*)iD-J%4^V>_mMIHFCSY|~|eS91GRq*3mvGP(E>S{@i?d6c-n{#NKazVI=xt)SE
z?SV&>kikjac<<1;z>;mqGc(7zd-sKJ9q(%&OZX?V`Q+>+_1CaA)ek%5t4$eYMbCeu
ze_NCA&?%)SSNt9WXN^X*?D+l*mYuHm1GnZ|2U3aM-C5k1%y!;g-JyLGyfaokg!PIj
zjcaxS?GZmMWIMdAuykktqf2K>v|-)1jA=qj>Yibz;mN%mK_65n4_`K~e}AXgE6vv=
zy)c*3$^c!YC*5=EVi|8k<e5A5dnoKBCf>-6RNO)ZBKu%b!<LDUoH?SBS5v54ujLGx
zo3Ix9`Jj>QTIFPf2QgOl?U}E>90--sYSd!+-;1@Ao_VrltH%<(H(Z>%`)SR~)@=%P
zJUuiWU3bVwv=&)%0nQn-EXUdRPcomy_~Na({0R?KO*fyYN}}E}$~@<B)7^}Jh5DmC
z_SfgFFdj@y1etb%YjTPZqSwBOAJN+=`SqY%x(M~KVtwpJAMM+7JR8VY=afekE^_A;
zxW_Krn;*Yc)=l^B`<5J<cStzC=vV9M7}}*iLS8UWdYjl>rrPGdv{<&rL`LQ9H>;Kx
zL?h<x0}g=a#^Q0R74=>Zb+^2u9k1Y}jxW;t$q#`9twkRx2U?Fh6s_H$cFmK?54)+_
zeIvK298)li@2S>XSN!mHTCB0>P*K`&lJ=|bMoFt|4eeArpvAA)ll)3J3FPw^wHylW
zeBcn(169O8EGfoi$k%Y4iCI*uaNgWN-B_d5!nRAK=N@Fk!C%UwwR_S-k`>zeumf9h
zvYZ%v_sdRH(qjOQJV7w0HC>d4s%Uu%_^v#;S$`Pr2f^e+jo6)|cWq1Wt_po2e*JMQ
z8BccjQ4B0vG<-mMSujU6E@Fgd+ZKzM3dqPkO`;)Y^o6_)>p?HJKa{#Xt1XEB6Sf=Y
zea@{!_2_Eao{T%W0g9X-+YKTB*|vcjOYCrd42J+Olb|pqe|v{?_RgzF&=Ux;yc(HS
z1Y@fjzLllRhb7ALS60IEd6+G7_IDL|<FX?S$^D-O?B2CzOeS97#jX!HVCCEFO2vfX
z(l9^?V{TMNb6F)v#gW|^9(_2e8Xh|P&NY<LHyudxAo#PIN<D5HenKmh%;*Xc?+)+^
zk4RJyPK7g{gnYmOq+rlBy=QAhP|QW&JAR_?Bg2--WMIs|T6WUNJEBn`wrwudu)%`8
z8?~+}iK2MU-Q;2MB}pN|Igze#Nt09ifzHnqla*35*K|-jksuM9!m$)O<b%1}s;`Me
z@sE<WfjT{-#LquKmO;x0QWA_;mt=6-NO<jW{M(hVktls17Qowv^SklRx_8eBvn;uq
zO{tEw_N9{~YmdWA2gkjW6Ka`Sa#gl{A`+UH{a!YHlfGmXg*i&Lx9|age&i2U@nf%f
z@t&bYDmDOcdEEd20J^p=j+SsR%mKxN{_&C932qansjftTM~w#n00@-t$!h}ufGPms
z3IGQK004N(q(=e(*idD88C~}mn^Q1kvLPTy%OrlPswqdrKD?q?fI^!bTuZk!`vPbh
z6$!1H$?ar%iD>b=;I?>OlLZ9i<`9wk#H>7$2QnSg^<4{-y0<5j`Qw)4^z`%_Sod+m
zRC|H(ZnSxX6t-YU-D&-P@i1SHaeem)_sC-@?ENUHpyUGd&2B<g^zA7v#5CVv&z@HU
zu}{njBnK$%i$<KDpPigXTj?CT%T$C-lPgly()y_CMV+n02va4NpB}A3OawU^#YPu*
zzxwXz?tv`d3uvBM)P^Lu#uH^kTPf2HjV5H7sWDBR9Jyb(Uu>VPj~Jki=BzSSAfMh4
zgf8(CF>AAvv0WG&Xusej_i@Nhl@6x-cC=sdLbjl^A)XwP>wl0D?c04QWh6Ht9uyZw
z?x!!H;@g+A*9XZm9muFswI8<?^TxXz|G1E3$xw=HeM4=o*i2zGBrqjz_nBS9OQUVt
zhVZ^lUqy3w*H82J_sWW=8DEm=*xr^D50^SG8}3jNahh(R8ZgT7uze|lcJ1r*F2-3(
zQGFlMN0=tNS9UL@J(l0GsarUgcp}$esDY+`O6C^z=SB|WJIHUYg;Gm2)OA~%#e`DQ
z(uMwMySs<X(N3a1AA~)zONNbdCZ#r;wq|#z>-WAyJ25odPdf>1w|J3T9V5n;p@3tN
zC(}=Er&#A`K=@Z6QIf(mN;THO%~f5DgPq;OVtztirw5IBIzUGcGOO7cpR|bE`VyE$
z(GZ9FzE^XS>sg&DUZ8=hEmVYvev5|B9?gBL0~L2}Ptg>qdwU>TTfC$)l(?ttMdjX;
zqH=m<pP-5YkkKgNL5qL~zG<4zw+CaZdm#88-@2Jxw>fK+EO0=N?EUhxjr$1pHqlpG
zDDA$E-}H%!sr@eN9%w=`Cq(Obw0H4Nv@CFdi*`!KR8TZU>Ht;uKI>Dbd^bxpM~Bu5
z<QkP}h3?*$5k-wJ#pcUpf<C8163#8d?fVANS)Y`k5%yScVx^4e`|GHO!_<R?omwkd
z*@u<^1|dhc`IQzod|q7K$}=u$;Fwt7@F5Rrwx3XW<dX}_a`l(uc0T?5%$a<7d!E$q
zOLDQ!^N-U?>mFm`q`+;Uc}ufCFoP}2cLbK@>I9s^B$^`VAGT4d4lX{m*<U(1kq$#h
zI1WawW}UxS&HTD>J)v-<PT*Qjc$P=agyhDwk_*D?@XOk{oz$SI+HK>;w=*kBMQ*?e
zu3Nl*oTn8+%c)BXWv8j;o&fRFO#ad8=tdAo4ppq{Q}%V?OU%%H$3-c9qRhghB^Iw{
zw++-Vn_A(%mh`ew1C9OEMV^}92>(3mv%|irI>Y18I~N_BTP=qdbN<fcg&%+oXR`%|
z$<s$%?}6ed-Dd}7y+C-4#?JbdG!;=)mdNS1)ja*1a{Nk%wQEMHv`WtBQg#`UkA3H}
zqEPsvUce!n8)(FK)Z{MP%cFS>@sFT|4IlC^@rzT;1|I#%qIOg(S$xi~t0#y(a=Q0G
zQ;qU(LR6&22u=)TX}JzJ=7-o`dZ;JncpYecFzrs1FoI7gfsI`V+_oT@o{O0;OOg-!
zCJWw`Qjsf_CLe;IqB4>XL0zdTu3x3@L^Z{C$0Q$WwaGgqMb-NOgI2-*N_!xA;jG&u
zorDG!lkew`^G0owR>FJ|d2_rhf!0y0w?{(W&of7_KT7zA%JrLK_Wy4EDC_@MnK&i7
zc~#xHXf(?8@<>5hL0!H?)*|4q$DO|%HUNMC06-1_w}#pLd18a&<AFJXZJ;O~Ss2<8
z?1bX^XJhK$7&{@HT$~)CaF_o>J2V2}-~@(49sUpPF3)3JP?lii|FEO|U7SdS4HAm_
z|FOLM%l~_NE+hTl{(fh5nUbGYzdQJ0`He3y1Oj#V<G70l;(|o}xuJ;(xLmbd&g13s
z;!^&3l71JAwF3f-hW<#|?}E8p$e_?34$wc&pp1K>=4(lsm)Y>Sgzgbm`fjAc#0Q#2
zH!RDpxMH_{+^CaLiEF=gpPaQ|D%Ic}eF*I?rN^U2)tE-5aOqe!<MPGG2Cshc*OqMs
zq=izH(&b&xg=Zud9FQ9F3Leh$K?ARoV;GijxrT+Od4RrmN2Y~qT0Gtk#C2FD?bLo$
zrq^PwiPR<I<u1r3nRc&YeMW0gX+$Z)GNj$1ViubTGO8a}k&M6D$3tUZ5^rj+e0#wV
zl08w@UigLI9y%gM;?6&8xQdZe;?dPfEv<{6P1`QPQ})u!OoG?QyYUMjTX*(0LMVCF
zT4>R7GbEz@%}D?7Z0!(-eQTvY=}91o;Dg1GflmlDis6glMO?|cw$?HSgQDCcW+}rV
z?HD{=I+OT>d9#Ia^zOQ^q*%k&2qtgmP0FmVsvK@Q&}Wev@ek(4mkg~j*S$7durkhP
zCemCzn0L!iY*U1~W$BA6ypOBxdqokZdf3(#_HJ2pi6={AebcmhGdS#FPJ{DVYanaq
z2$9~CyO>R%t`W#@>2=jOFULKOqDEhfy-I}^*t>s7rx%3b$NPrEJSO(~7)ERqi)kXW
z9UJ3QQJNflW=Gu-24zpZ)|4coy(`I7Q#$2GUF$EfJITOh2{_M<G?HwY6TG6S1N}?U
z<D1v>%fwmR>Lt6h&8Wg2T*IXEU=2&ZADAR$>^l8vHn8CNdnqTW(+!TB+RFsI1(Lfj
zAVRzFWd0%A>>5N|&8v}iC5Ai4+g&L%+s5=F@>kj_b$KtINKd-AG9}8lT2UrCTRf8p
z1d$h%apFmNH2at{V_n?S3@H5IvpVJA>PCg-HuF{_e{jb7_=(Dy6P4-A6vUZ|YfA$1
z>{Ef2dqm`sgC#!84i#_GDPf!+LU0qzeUFpISQEGPosWDZxxIq@?A-oH6diKUmf1OD
zivmpS?Zci4w3QhTjRncDUMExH<9@hf1rU!O`ivXx2iE>vIqzDURy_SO3!RhPDo~3z
z-*~;MkeVRBT1If5I6<_8EOzesSoi0)$m|=V&R+AySls;7UXM$B6tf?uurmcMXyx0N
zg^A8PnSWao)AZZ;zy>XfZ)5XFG4psD$0NMM-ll@$XBE-z70<oT`6{)DKb|swD%6R~
zL(h_eSPe(GATj>tL6Ok|s<I+M<`uUYcZwbr1xZChVfFz10E=1|{|jBNhfnh6@wh!$
zmvI|YahVJrvo1Q>=P1aM3m4K<`bHq-XDW3*u=fXGsa{LpEf2^$oqc8GLNOv!>7qbv
zB1+aQ<B5!TNiNvOyVh!5=NqA7b~cOo$xP5gN$6>sN7Qxx(XiJmS%Pd28E1JZ@0Vt2
zY2TD1Z4J&0E=|s7KgPWqwv;^x!KYfME(NoK?t_TS$Uk?xNgt@;D$xe-pFiYJ+zr)u
zY*|&z^q$DnenI@&ErLZzmW4!>3OvQpDy>}6vI%~5rfD6eW=eU(<Ho#Cq1cLFA&6F{
zu%AYbqi4mJym&=pyU}#Z<-p@9dMig*KHHt4mEp_BT#OKwS;-fDZKzeS9GRMGc$|7!
zu&7L{L9KsH@Ic;TMU+HvN**3X#hDo1E4AoE4p>^*T#--!m~w!$$`SMlr>izSt+pNk
zAn6l)*;neq467eH$!rTN=pfngM>0p2#PLU2v}fxkOiA5j%pbh6as8^PO;j7_-C!~w
z`dnO1T^r(7Xf^|>B)V?}ucqlulU}_~R)iCFC!ScWTnY7~1~L9FTRncog*Kd_TB-KE
zEE0M>uPOJ7;}i3VK$O|s!i(znF#j87{x40}4Yo*XG>T6ek8bPloC%pbFQv#e1Ych^
zIF9`cjM&4c&Fp<GlNM^Lkbawq0EdStW2~JRPc5}YByfpJe&tw!0qp_`%v?QHVAxx;
z#OX0(k}Bm1>Ak+Vx*kC<;FlrMR7)qz^ECJEXG>z!Ge43l#%hXWX<heQj?Z6DYHk+k
zz0L`L#T<Ccfjx99FJHSJ;^=P~;<C=j*kLAoIGCpxwYC(hb*l}16&^)jn*ID$@c``;
zS9$Azk#+pBV58QE)bn>Wnsyc4uCj0JCvZWKo(%>AGC_9%=A<uO&P~|qfvfcUM@?lb
zLfaKLHJEgZaPUTD-BL`7JUzKCjylcH&w5)V6-wJJX7OAUjbXw#EX|B~Cn;0|-N*S+
zumF~AXRQ<mxM?i~Kd~hTkPvxSA@|DrZv13#q>g;@^*5}_UdWJ{!)*1mfGM4Zvqf}}
zI{MMKH3QOag80`BOH`55Ihq5g_RO;)z^zIQsTza;K`FM8lK**WuHfML4)SIG-m$yR
z`;C&;)cy~elXbS#itef+T#J(f;cIceaYCOih((F(>~bX4S8|bW1rGMqZ*JC)t7ya=
zTA%nnOW;U#NE;ZKcVqD~YsSEk6lWh&R%5&Aa9`RKE$7DhwNSTa+sid^X?kxF+oMCF
z+>6KoKEgPg-Y**1@<~3mxA(pkh4wdE5W#skNo`Aw#F#V4mnsR<eFG`K%%iloVx#G$
zg5xUPiyo#^n|Ck!Rz7U;;f`>{#z@3~Q@W)CmgKsZS>LGX-q?c>WksjVt+&0soGj@z
zwjZ<TMY5bE6qg+%v9oM43YIbRjoXExhv5|q-Yd7m^qUP=mLv&?*}+dc(;{ruo%AAd
zM@vH#mKE-IHxm(hZemO6I1N{4py~GN)BA_BAtS77^8ml?0fm!1sbcN4%8on|0g+N%
zOZV$oCN}Zsr$RfP=%|;|uR&$P4ut8i4gq$G``(nbdHkaScd~-mc8dK1Fj!4KO)0Ie
zshIfZM?{mgJ0Y9Zx-eraFC+l@RF#RfkFMd?zWLHwr3ow@&o**?lBe^<ihhNNJAq=M
zHdc(erh!O@o>lPWad(5wAxe?V<)m-k-+Ouc#x0$FgSQ2sg}l!jp}~s)g(9Qwndr9<
zEg$LKaMNG)`r0=T#o>BX*KM|}qq@LZ(t_zhq|$N$?4LZRx?Ck)>C)b)z3d$1|FKH?
z-Fk-t005{d=*n;@t1I$2g5fZ0DC+y!aj{M-{4E`R(;)}LmVa?BF7qb1zd8K`p!k9u
z7av1M&`+8)-!Ug&Cu`pta?)fYIk3_DQJ*FBy}QF0haZ?C{UD_^1DX~Ref)USExChD
zK$0moA)OtSSOV)t%CGA2DQ6;Mm|&}z7RxTK51IT9x|3F)b6s^9gX>Iv^Mqt@e|2tj
zIUE}oV->4boy4q@6kmxffZ>1QqX-Z*(t?q(Y<%NP<Z;=hFK`kN1C=R;U}`rOxrZct
zR<&d=#ZbGe>wQn-+VE}I`2JXJ2*lMqFH$Xlo1REyk{+Z^K$mmO{>&}9=_c}pqpER6
zCH8gZeXSMR`I+Ri==Iiy`!`u|C1^?#a5&C#``%=23D1oHr+hFklUe{+>?FMc08C?E
zCiMyiIpCk$Gyf_-nl=A!_4gjkpQ3fy;`wJs<`1hMQ}}B;z-8mqPwBt<*EXuZTmHJ`
zciArRQz-HN(me3P@Sk=Be~JU;KkpCz9p$Ih9~|tb+`aYRl;gh`*<UmM(-`obll>H*
z-+=wf(Ecq>)o*ZqWo-WzXY)5WzcRRg#<}EhKZW`Be<$g$jP9S2E(y_3G5-zHPloqz
zQR07t@{{rXf%4;>z4ZNqwq0_*pVIvslz(7=|LcAF>i`*m|DCFz{O_-!{aPz88Q@Pb
s1pdkge|Y~fq+iRyWhMP7_gMd-u4<}aVPC2d1Bd}aR{?+&wo9Y`4}A~OqW}N^

literal 0
HcmV?d00001


From 4532eca4ef6f6170765062ccd2ca7f29814e0b3a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Oct 2013 22:55:16 +0200
Subject: [PATCH 12/13] Covered corner case in L1 controller not adressed so
 far in existing concepts

---
 src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 33 +++++++++++++++++-------
 1 file changed, 23 insertions(+), 10 deletions(-)

diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index c5c0c7a3c1..daf136d495 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
-	float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
+	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	
+	/* calculate angle of airplane position vector relative to line) */
+
+	// XXX this could probably also be based solely on the dot product
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
 
 	/* extension from [2], fly directly to A */
 	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
@@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
 
-// XXX this can be useful as last-resort guard, but is currently not needed
-#if 0
-	} else if (absf(bearing_wp_b) > math::radians(80.0f)) {
-		/* extension, fly back to waypoint */
+	/*
+	 * If the AB vector and the vector from B to airplane point in the same
+	 * direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
+	 */
+	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
+		/*
+		 * Extension, fly back to waypoint.
+		 * 
+		 * This corner case is possible if the system was following
+		 * the AB line from waypoint A to waypoint B, then is
+		 * switched to manual mode (or otherwise misses the waypoint)
+		 * and behind the waypoint continues to follow the AB line.
+		 */
 
 		/* calculate eta to fly to waypoint B */
 		
 		/* velocity across / orthogonal to line */
-		xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
+		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
 		/* velocity along line */
-		ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
+		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = bearing_wp_b;
-#endif
+		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+
 	} else {
 
 		/* calculate eta to fly along the line between A and B */

From 3ca94e7943fc76053114ab97d4b63dc6902fd5bf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 17 Oct 2013 13:38:21 +0200
Subject: [PATCH 13/13] Prevent warnings by data conversion

---
 mavlink/include/mavlink/v1.0/mavlink_conversions.h | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h
index d893635770..51afac87c3 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h
@@ -9,6 +9,10 @@
 #endif
 #include <math.h>
 
+#ifndef M_PI_2
+    #define M_PI_2 ((float)asin(1))
+#endif
+
 /**
  * @file mavlink_conversions.h
  *
@@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo
     float phi, theta, psi;
     theta = asin(-dcm[2][0]);
 
-    if (fabs(theta - M_PI_2) < 1.0e-3f) {
+    if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
         phi = 0.0f;
-        psi = (atan2(dcm[1][2] - dcm[0][1],
+        psi = (atan2f(dcm[1][2] - dcm[0][1],
                 dcm[0][2] + dcm[1][1]) + phi);
 
-    } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
+    } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
         phi = 0.0f;
         psi = atan2f(dcm[1][2] - dcm[0][1],
                   dcm[0][2] + dcm[1][1] - phi);
@@ -128,4 +132,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo
     dcm[2][2] = cosPhi * cosThe;
 }
 
-#endif
\ No newline at end of file
+#endif