Merge remote-tracking branch 'upstream/master'

15 files changed, 510 insertions, 302 deletions

diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index 998b5ac7d..a1a8e7ea8 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -154,6 +154,7 @@ warn(const char *fmt, ...)
 
 	va_start(args, fmt);
 	vwarn(fmt, args);
+	va_end(args);
 }
 
 void
@@ -169,6 +170,7 @@ warnc(int errcode, const char *fmt, ...)
 
 	va_start(args, fmt);
 	vwarnc(errcode, fmt, args);
+	va_end(args);
 }
 
 void
@@ -184,6 +186,7 @@ warnx(const char *fmt, ...)
 
 	va_start(args, fmt);
 	vwarnx(fmt, args);
+	va_end(args);
 }
 
 void
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
index 4bcf95784..24f4e4207 100644
--- a/src/modules/systemlib/mcu_version.c
+++ b/src/modules/systemlib/mcu_version.c
@@ -47,7 +47,8 @@
 #ifdef CONFIG_ARCH_CHIP_STM32
 #include <up_arch.h>
 
-#define DBGMCU_IDCODE	0xE0042000
+#define DBGMCU_IDCODE	0xE0042000  //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code)
+#define UNIQUE_ID    	0x1FFF7A10  //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register)
 
 #define STM32F40x_41x	0x413
 #define STM32F42x_43x	0x419
@@ -57,7 +58,13 @@
 
 #endif
 
-
+/** Copy the 96bit MCU Unique ID into the provided pointer */
+void mcu_unique_id(uint32_t *uid_96_bit)
+{
+	uid_96_bit[0] = getreg32(UNIQUE_ID);
+	uid_96_bit[1] = getreg32(UNIQUE_ID+4);
+	uid_96_bit[2] = getreg32(UNIQUE_ID+8);
+}
 
 int mcu_version(char* rev, char** revstr)
 {
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
index 1b3d0aba9..c8a0bf5cd 100644
--- a/src/modules/systemlib/mcu_version.h
+++ b/src/modules/systemlib/mcu_version.h
@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <stdint.h>
+
 /* magic numbers from reference manual */
 enum MCU_REV {
 	MCU_REV_STM32F4_REV_A = 0x1000,
@@ -42,6 +44,15 @@ enum MCU_REV {
 	MCU_REV_STM32F4_REV_3 = 0x2001
 };
 
+
+/**
+ * Reports the microcontroller unique id.
+ *
+ * This ID is guaranteed to be unique for every mcu. 
+ * @param uid_96_bit A uint32_t[3] array to copy the data to.
+ */
+__EXPORT void mcu_unique_id(uint32_t *uid_96_bit);
+
 /**
  * Reports the microcontroller version of the main CPU.
  *
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 17989558e..67ef521b4 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -442,6 +442,14 @@ private:
 };
 
 /**
+ * Supported multirotor geometries.
+ *
+ * Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h
+ */
+typedef unsigned int MultirotorGeometryUnderlyingType;
+enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
+
+/**
  * Multi-rotor mixer for pre-defined vehicle geometries.
  *
  * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
@@ -451,32 +459,14 @@ class __EXPORT MultirotorMixer : public Mixer
 {
 public:
 	/**
-	 * Supported multirotor geometries.
-	 *
-	 * XXX add more
-	 */
-	enum Geometry {
-		QUAD_X = 0,	/**< quad in X configuration */
-		QUAD_PLUS,	/**< quad in + configuration */
-		QUAD_V,		/**< quad in V configuration */
-		QUAD_WIDE,	/**< quad in wide configuration */
-		HEX_X,		/**< hex in X configuration */
-		HEX_PLUS,	/**< hex in + configuration */
-		HEX_COX,
-		OCTA_X,
-		OCTA_PLUS,
-		OCTA_COX,
-
-		MAX_GEOMETRY
-	};
 
-	/**
 	 * Precalculated rotor mix.
 	 */
 	struct Rotor {
 		float	roll_scale;	/**< scales roll for this rotor */
 		float	pitch_scale;	/**< scales pitch for this rotor */
 		float	yaw_scale;	/**< scales yaw for this rotor */
+		float	out_scale;	/**< scales total out for this rotor */
 	};
 
 	/**
@@ -497,7 +487,7 @@ public:
 	 */
 	MultirotorMixer(ControlCallback control_cb,
 			uintptr_t cb_handle,
-			Geometry geometry,
+			MultirotorGeometry geometry,
 			float roll_scale,
 			float pitch_scale,
 			float yaw_scale,
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 57e17b67d..2ab5b5e8e 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -55,6 +55,9 @@
 
 #include "mixer.h"
 
+// This file is generated by the multi_tables script which is invoked during the build process
+#include "mixer_multirotor.generated.h"
+
 #define debug(fmt, args...)	do { } while(0)
 //#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 //#include <debug.h>
@@ -73,117 +76,11 @@ float constrain(float val, float min, float max)
 	return (val < min) ? min : ((val > max) ? max : val);
 }
 
-/*
- * These tables automatically generated by multi_tables - do not edit.
- */
-const MultirotorMixer::Rotor _config_quad_x[] = {
-	{ -0.707107,  0.707107,  1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{  0.707107,  0.707107, -1.00 },
-	{ -0.707107, -0.707107, -1.00 },
-};
-const MultirotorMixer::Rotor _config_quad_plus[] = {
-	{ -1.000000,  0.000000,  1.00 },
-	{  1.000000,  0.000000,  1.00 },
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-};
-const MultirotorMixer::Rotor _config_quad_v[] = {
-	{ -0.927184,  0.374607,  1.00 },
-	{  0.694658, -0.719340,  1.00 },
-	{  0.927184,  0.374607, -1.00 },
-	{ -0.694658, -0.719340, -1.00 },
-};
-const MultirotorMixer::Rotor _config_quad_wide[] = {
-	{ -0.927184,  0.374607,  1.00 },
-	{  0.777146, -0.629320,  1.00 },
-	{  0.927184,  0.374607, -1.00 },
-	{ -0.777146, -0.629320, -1.00 },
-};
-const MultirotorMixer::Rotor _config_hex_x[] = {
-	{ -1.000000,  0.000000, -1.00 },
-	{  1.000000,  0.000000,  1.00 },
-	{  0.500000,  0.866025, -1.00 },
-	{ -0.500000, -0.866025,  1.00 },
-	{ -0.500000,  0.866025,  1.00 },
-	{  0.500000, -0.866025, -1.00 },
-};
-const MultirotorMixer::Rotor _config_hex_plus[] = {
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000,  1.00 },
-	{  0.866025, -0.500000, -1.00 },
-	{ -0.866025,  0.500000,  1.00 },
-	{  0.866025,  0.500000,  1.00 },
-	{ -0.866025, -0.500000, -1.00 },
-};
-const MultirotorMixer::Rotor _config_hex_cox[] = {
-	{ -0.866025,  0.500000, -1.00 },
-	{ -0.866025,  0.500000,  1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-	{ -0.000000, -1.000000,  1.00 },
-	{  0.866025,  0.500000, -1.00 },
-	{  0.866025,  0.500000,  1.00 },
-};
-const MultirotorMixer::Rotor _config_octa_x[] = {
-	{ -0.382683,  0.923880, -1.00 },
-	{  0.382683, -0.923880, -1.00 },
-	{ -0.923880,  0.382683,  1.00 },
-	{ -0.382683, -0.923880,  1.00 },
-	{  0.382683,  0.923880,  1.00 },
-	{  0.923880, -0.382683,  1.00 },
-	{  0.923880,  0.382683, -1.00 },
-	{ -0.923880, -0.382683, -1.00 },
-};
-const MultirotorMixer::Rotor _config_octa_plus[] = {
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-	{ -0.707107,  0.707107,  1.00 },
-	{ -0.707107, -0.707107,  1.00 },
-	{  0.707107,  0.707107,  1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{  1.000000,  0.000000, -1.00 },
-	{ -1.000000,  0.000000, -1.00 },
-};
-const MultirotorMixer::Rotor _config_octa_cox[] = {
-	{ -0.707107,  0.707107,  1.00 },
-	{  0.707107,  0.707107, -1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{ -0.707107, -0.707107, -1.00 },
-	{  0.707107,  0.707107,  1.00 },
-	{ -0.707107,  0.707107, -1.00 },
-	{ -0.707107, -0.707107,  1.00 },
-	{  0.707107, -0.707107, -1.00 },
-};
-const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
-	&_config_quad_x[0],
-	&_config_quad_plus[0],
-	&_config_quad_v[0],
-	&_config_quad_wide[0],
-	&_config_hex_x[0],
-	&_config_hex_plus[0],
-	&_config_hex_cox[0],
-	&_config_octa_x[0],
-	&_config_octa_plus[0],
-	&_config_octa_cox[0],
-};
-const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
-	4, /* quad_x */
-	4, /* quad_plus */
-	4, /* quad_v */
-	4, /* quad_wide */
-	6, /* hex_x */
-	6, /* hex_plus */
-	6, /* hex_cox */
-	8, /* octa_x */
-	8, /* octa_plus */
-	8, /* octa_cox */
-};
-
-}
+} // anonymous namespace
 
 MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 				 uintptr_t cb_handle,
-				 Geometry geometry,
+				 MultirotorGeometry geometry,
 				 float roll_scale,
 				 float pitch_scale,
 				 float yaw_scale,
@@ -193,8 +90,9 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 	_pitch_scale(pitch_scale),
 	_yaw_scale(yaw_scale),
 	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
-	_rotor_count(_config_rotor_count[geometry]),
-	_rotors(_config_index[geometry])
+	_limits_pub(),
+	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
+	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry])
 {
 }
 
@@ -205,7 +103,7 @@ MultirotorMixer::~MultirotorMixer()
 MultirotorMixer *
 MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
 {
-	MultirotorMixer::Geometry geometry;
+	MultirotorGeometry geometry;
 	char geomname[8];
 	int s[4];
 	int used;
@@ -245,35 +143,40 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
 
 	if (!strcmp(geomname, "4+")) {
-		geometry = MultirotorMixer::QUAD_PLUS;
+		geometry = MultirotorGeometry::QUAD_PLUS;
 
 	} else if (!strcmp(geomname, "4x")) {
-		geometry = MultirotorMixer::QUAD_X;
+		geometry = MultirotorGeometry::QUAD_X;
 
 	} else if (!strcmp(geomname, "4v")) {
-		geometry = MultirotorMixer::QUAD_V;
+		geometry = MultirotorGeometry::QUAD_V;
 
 	} else if (!strcmp(geomname, "4w")) {
-		geometry = MultirotorMixer::QUAD_WIDE;
+		geometry = MultirotorGeometry::QUAD_WIDE;
+
+	} else if (!strcmp(geomname, "4dc")) {
+		geometry = MultirotorGeometry::QUAD_DEADCAT;
 
 	} else if (!strcmp(geomname, "6+")) {
-		geometry = MultirotorMixer::HEX_PLUS;
+		geometry = MultirotorGeometry::HEX_PLUS;
 
 	} else if (!strcmp(geomname, "6x")) {
-		geometry = MultirotorMixer::HEX_X;
+		geometry = MultirotorGeometry::HEX_X;
 
 	} else if (!strcmp(geomname, "6c")) {
-		geometry = MultirotorMixer::HEX_COX;
+		geometry = MultirotorGeometry::HEX_COX;
 
 	} else if (!strcmp(geomname, "8+")) {
-		geometry = MultirotorMixer::OCTA_PLUS;
+		geometry = MultirotorGeometry::OCTA_PLUS;
 
 	} else if (!strcmp(geomname, "8x")) {
-		geometry = MultirotorMixer::OCTA_X;
+		geometry = MultirotorGeometry::OCTA_X;
 		
 	} else if (!strcmp(geomname, "8c")) {
-		geometry = MultirotorMixer::OCTA_COX;
+		geometry = MultirotorGeometry::OCTA_COX;
 
+	} else if (!strcmp(geomname, "2-")) {
+		geometry = MultirotorGeometry::TWIN_ENGINE;
 	} else {
 		debug("unrecognised geometry '%s'", geomname);
 		return nullptr;
@@ -314,6 +217,8 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 			    pitch * _rotors[i].pitch_scale +
 			    thrust;
 
+		out *= _rotors[i].out_scale;
+
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
 			yaw = -out / _rotors[i].yaw_scale;
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
index fc7485e20..3fd07f5ba 100644
--- a/src/modules/systemlib/mixer/module.mk
+++ b/src/modules/systemlib/mixer/module.mk
@@ -31,13 +31,17 @@
 #
 ############################################################################
 
+
 #
 # mixer library
 #
 LIBNAME	= mixerlib
-
+	
 SRCS		= mixer.cpp \
 		  mixer_group.cpp \
 		  mixer_multirotor.cpp \
 		  mixer_simple.cpp \
 		  mixer_load.c
+
+SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
+include $(SELF_DIR)multi_tables.mk
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
deleted file mode 100755
index b5698036e..000000000
--- a/src/modules/systemlib/mixer/multi_tables
+++ /dev/null
@@ -1,127 +0,0 @@
-#!/usr/bin/tclsh
-#
-# Generate multirotor mixer scale tables compatible with the ArduCopter layout
-#
-
-proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) }
-proc rcos {a} { expr cos([rad $a])}
-
-set quad_x {
-	  45 CCW
-	-135 CCW
-	 -45 CW
-	 135 CW
-}
-
-set quad_plus {
-	  90 CCW
-	 -90 CCW
-	   0 CW
-	 180 CW
-}
-
-set quad_v {
-	  68 CCW
-	-136 CCW
-	 -68 CW
-	 136 CW
-}
-
-set quad_wide {
-	  68 CCW
-	-129 CCW
-	 -68 CW
-	 129 CW
-}
-
-set hex_x {
-	  90 CW
-	 -90 CCW
-	 -30 CW
-	 150 CCW
-	  30 CCW
-	-150 CW
-}
-
-set hex_plus {
-	   0 CW
-	 180 CCW
-	-120 CW
-	  60 CCW
-	 -60 CCW
-	 120 CW
-}
-
-set hex_cox {
-	  60 CW
-	  60 CCW
-	 180 CW
-	 180 CCW
-	 -60 CW
-	 -60 CCW
-}
-
-set octa_x {
-	  22.5 CW
-	-157.5 CW
-	  67.5 CCW
-	 157.5 CCW
-	 -22.5 CCW
-	-112.5 CCW
-	 -67.5 CW
-	 112.5 CW
-}
-
-set octa_plus {
-	   0 CW
-	 180 CW
-	  45 CCW
-	 135 CCW
-	 -45 CCW
-	-135 CCW
-	 -90 CW
-	  90 CW
-}
-
-set octa_cox {
-	  45 CCW
-	 -45 CW
-	-135 CCW
-	 135 CW
-	 -45 CCW
-	  45 CW
-	 135 CCW
-	-135 CW
-}
-
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
-
-proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
-
-foreach table $tables {
-	puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table]
-
-	upvar #0 $table angles
-	foreach {angle dir} $angles {
-		if {$dir == "CW"} {
-			set dd 1.0
-		} else {
-			set dd -1.0
-		}
-		factors $angle $dd
-	}
-	puts "};"
-}
-
-puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {"
-foreach table $tables {
-	puts [format "\t&_config_%s\[0\]," $table]
-}
-puts "};"
-
-puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {"
-foreach table $tables {
-	upvar #0 $table angles
-	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]
-}
-puts "};"
diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk
new file mode 100644
index 000000000..c537c83a4
--- /dev/null
+++ b/src/modules/systemlib/mixer/multi_tables.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2014 Anton Matosov <anton.matosov@gmail.com>. 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.
+#
+############################################################################
+
+
+SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
+MULTI_TABLES := $(SELF_DIR)multi_tables.py
+
+# Add explicit dependency, as implicit one doesn't work often.
+$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h
+
+$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES)
+	$(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h
diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py
new file mode 100755
index 000000000..ba59e0536
--- /dev/null
+++ b/src/modules/systemlib/mixer/multi_tables.py
@@ -0,0 +1,213 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (c) 2013, 2014 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.
+#
+############################################################################
+
+#
+# Generate multirotor mixer scale tables compatible with the ArduCopter layout
+#
+
+# for python2.7 compatibility
+from __future__ import  print_function
+
+import math
+
+print("/*")
+print("* This file is automatically generated by multi_tables - do not edit.")
+print("*/")
+print("")
+print("#ifndef _MIXER_MULTI_TABLES")
+print("#define _MIXER_MULTI_TABLES")
+print("")
+
+def rcos(angleInRadians):
+  return math.cos(math.radians(angleInRadians))
+
+CCW = 1.0
+CW = -CCW
+
+quad_x = [
+    [  45, CCW],
+    [-135, CCW],
+    [-45,  CW],
+    [135,  CW],
+]
+
+quad_plus = [
+    [  90, CCW],
+    [ -90, CCW],
+    [   0, CW],
+    [ 180, CW],
+]
+
+quad_deadcat = [
+    [  63, CCW, 1.0],
+    [-135, CCW, 0.964],
+    [ -63, CW, 1.0],
+    [ 135, CW, 0.964],
+]
+
+quad_v = [
+    [  18.8, 0.4242],
+    [ -18.8, 1.0],
+    [ -18.8, -0.4242],
+    [  18.8, -1.0],
+]
+
+quad_wide = [
+    [   68, CCW],
+    [ -129, CCW],
+    [  -68, CW],
+    [  129, CW],
+]
+
+hex_x = [
+    [  90, CW],
+    [ -90, CCW],
+    [ -30, CW],
+    [ 150, CCW],
+    [  30, CCW],
+    [-150, CW],
+]
+
+hex_plus = [
+    [   0, CW],
+    [ 180, CCW],
+    [-120, CW],
+    [  60, CCW],
+    [ -60, CCW],
+    [ 120, CW],
+]
+
+hex_cox = [
+    [  60, CW],
+    [  60, CCW],
+    [ 180, CW],
+    [ 180, CCW],
+    [ -60, CW],
+    [ -60, CCW],
+]
+
+octa_x = [
+    [  22.5, CW],
+    [-157.5, CW],
+    [  67.5, CCW],
+    [ 157.5, CCW],
+    [ -22.5, CCW],
+    [-112.5, CCW],
+    [ -67.5, CW],
+    [ 112.5, CW],
+]
+
+octa_plus = [
+    [   0, CW],
+    [ 180, CW],
+    [  45, CCW],
+    [ 135, CCW],
+    [ -45, CCW],
+    [-135, CCW],
+    [ -90, CW],
+    [  90, CW],
+]
+
+octa_cox = [
+    [  45, CCW],
+    [ -45, CW],
+    [-135, CCW],
+    [ 135, CW],
+    [ -45, CCW],
+    [  45, CW],
+    [ 135, CCW],
+    [-135, CW],
+]
+
+twin_engine = [
+    [ 90, 0.0],
+    [-90, 0.0],
+]
+
+
+tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine]
+
+def variableName(variable):
+    for variableName, value in list(globals().items()):
+        if value is variable:
+            return variableName
+
+def unpackScales(scalesList):
+    if len(scalesList) == 2:
+        scalesList += [1.0] #Add thrust scale
+    return scalesList
+
+def printEnum():
+    print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {")
+    for table in tables:
+        print("\t{},".format(variableName(table).upper()))
+
+    print("\n\tMAX_GEOMETRY")
+    print("}; // enum class MultirotorGeometry\n")
+
+def printScaleTables():
+    for table in tables:
+        print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)))
+        for row in table:
+            angle, yawScale, thrustScale = unpackScales(row)
+            rollScale = rcos(angle + 90)
+            pitchScale = rcos(angle)
+            print("\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale, thrustScale))
+        print("};\n")
+
+def printScaleTablesIndex():
+    print("const MultirotorMixer::Rotor *_config_index[] = {")
+    for table in tables:
+        print("\t&_config_{}[0],".format(variableName(table)))
+    print("};\n")
+
+
+def printScaleTablesCounts():
+    print("const unsigned _config_rotor_count[] = {")
+    for table in tables:
+        print("\t{}, /* {} */".format(len(table), variableName(table)))
+    print("};\n")
+
+
+
+printEnum()
+
+print("namespace {")
+printScaleTables()
+printScaleTablesIndex()
+printScaleTablesCounts()
+
+print("} // anonymous namespace\n")
+print("#endif /* _MIXER_MULTI_TABLES */")
+print("")
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index fe8b7e75a..f4dff2838 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -57,3 +57,5 @@ SRCS		 = err.c \
 		   mcu_version.c
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS	= -Wno-sign-compare
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index d6d8284d2..950577f00 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -39,9 +39,10 @@
 
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
 #include <sys/queue.h>
 #include <drivers/drv_hrt.h>
-
+#include <math.h>
 #include "perf_counter.h"
 
 /**
@@ -67,10 +68,13 @@ struct perf_ctr_count {
 struct perf_ctr_elapsed {
 	struct perf_ctr_header	hdr;
 	uint64_t		event_count;
+	uint64_t		event_overruns;
 	uint64_t		time_start;
 	uint64_t		time_total;
 	uint64_t		time_least;
 	uint64_t		time_most;
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -84,7 +88,8 @@ struct perf_ctr_interval {
 	uint64_t		time_last;
 	uint64_t		time_least;
 	uint64_t		time_most;
-
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -109,6 +114,7 @@ perf_alloc(enum perf_counter_type type, const char *name)
 
 	case PC_INTERVAL:
 		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+
 		break;
 
 	default:
@@ -124,6 +130,28 @@ perf_alloc(enum perf_counter_type type, const char *name)
 	return ctr;
 }
 
+perf_counter_t
+perf_alloc_once(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		if (!strcmp(handle->name, name)) {
+			if (type == handle->type) {
+				/* they are the same counter */
+				return handle;
+			} else {
+				/* same name but different type, assuming this is an error and not intended */
+				return NULL;
+			}
+		}
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	/* if the execution reaches here, no existing counter of that name was found */
+	return perf_alloc(type, name);
+}
+
 void
 perf_free(perf_counter_t handle)
 {
@@ -156,15 +184,23 @@ perf_count(perf_counter_t handle)
 		case 1:
 			pci->time_least = now - pci->time_last;
 			pci->time_most = now - pci->time_last;
+			pci->mean = pci->time_least / 1e6f;
+			pci->M2 = 0;
 			break;
 		default: {
-			hrt_abstime interval = now - pci->time_last;
-			if (interval < pci->time_least)
-				pci->time_least = interval;
-			if (interval > pci->time_most)
-				pci->time_most = interval;
-			break;
-		}
+				hrt_abstime interval = now - pci->time_last;
+				if (interval < pci->time_least)
+					pci->time_least = interval;
+				if (interval > pci->time_most)
+					pci->time_most = interval;
+				// maintain mean and variance of interval in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = interval / 1e6f;
+				float delta_intvl = dt - pci->mean;
+				pci->mean += delta_intvl / pci->event_count;
+				pci->M2 += delta_intvl * (dt - pci->mean);
+				break;
+			}
 		}
 		pci->time_last = now;
 		pci->event_count++;
@@ -203,17 +239,72 @@ perf_end(perf_counter_t handle)
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
 			if (pce->time_start != 0) {
-				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
+				int64_t elapsed = hrt_absolute_time() - pce->time_start;
+
+				if (elapsed < 0) {
+					pce->event_overruns++;
+				} else {
+
+					pce->event_count++;
+					pce->time_total += elapsed;
+
+					if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
+						pce->time_least = elapsed;
+
+					if (pce->time_most < (uint64_t)elapsed)
+						pce->time_most = elapsed;
+
+					// maintain mean and variance of the elapsed time in seconds
+					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+					float dt = elapsed / 1e6f;
+					float delta_intvl = dt - pce->mean;
+					pce->mean += delta_intvl / pce->event_count;
+					pce->M2 += delta_intvl * (dt - pce->mean);
+
+					pce->time_start = 0;
+				}
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+#include <systemlib/err.h>
+
+void
+perf_set(perf_counter_t handle, int64_t elapsed)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			if (elapsed < 0) {
+				pce->event_overruns++;
+			} else {
 
 				pce->event_count++;
 				pce->time_total += elapsed;
 
-				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+				if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
 					pce->time_least = elapsed;
 
-				if (pce->time_most < elapsed)
+				if (pce->time_most < (uint64_t)elapsed)
 					pce->time_most = elapsed;
 
+				// maintain mean and variance of the elapsed time in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = elapsed / 1e6f;
+				float delta_intvl = dt - pce->mean;
+				pce->mean += delta_intvl / pce->event_count;
+				pce->M2 += delta_intvl * (dt - pce->mean);
+
 				pce->time_start = 0;
 			}
 		}
@@ -300,26 +391,31 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
-
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
-		       handle->name,
-		       pce->event_count,
-		       pce->time_total,
-		       pce->time_total / pce->event_count,
-		       pce->time_least,
-		       pce->time_most);
+		float rms = sqrtf(pce->M2 / (pce->event_count-1));
+
+		dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+			handle->name,
+			pce->event_count,
+			pce->event_overruns,
+			pce->time_total,
+			pce->time_total / pce->event_count,
+			pce->time_least,
+			pce->time_most,
+			(double)(1e6f * rms));
 		break;
 	}
 
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
-
-		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
-		       handle->name,
-		       pci->event_count,
-		       (pci->time_last - pci->time_first) / pci->event_count,
-		       pci->time_least,
-		       pci->time_most);
+		float rms = sqrtf(pci->M2 / (pci->event_count-1));
+
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+			handle->name,
+			pci->event_count,
+			(pci->time_last - pci->time_first) / pci->event_count,
+			pci->time_least,
+			pci->time_most,
+			(double)(1e6f * rms));
 		break;
 	}
 
@@ -365,6 +461,21 @@ perf_print_all(int fd)
 	}
 }
 
+extern const uint16_t latency_bucket_count;
+extern uint32_t latency_counters[];
+extern const uint16_t latency_buckets[];
+
+void
+perf_print_latency(int fd)
+{
+	dprintf(fd, "bucket : events\n");
+	for (int i = 0; i < latency_bucket_count; i++) {
+		printf("  %4i : %i\n", latency_buckets[i], latency_counters[i]);
+	}
+	// print the overflow bucket value
+	dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]);
+}
+
 void
 perf_reset_all(void)
 {
@@ -374,4 +485,7 @@ perf_reset_all(void)
 		perf_reset(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
+	for (int i = 0; i <= latency_bucket_count; i++) {
+		latency_counters[i] = 0;
+	}
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 668d9dfdf..0c1243de3 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -56,7 +56,7 @@ typedef struct perf_ctr_header	*perf_counter_t;
 __BEGIN_DECLS
 
 /**
- * Create a new counter.
+ * Create a new local counter.
  *
  * @param type			The type of the new counter.
  * @param name			The counter name.
@@ -66,6 +66,16 @@ __BEGIN_DECLS
 __EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
 
 /**
+ * Get the reference to an existing counter or create a new one if it does not exist.
+ *
+ * @param type			The type of the counter.
+ * @param name			The counter name.
+ * @return			Handle for the counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc_once(enum perf_counter_type type, const char *name);
+
+/**
  * Free a counter.
  *
  * @param handle		The performance counter's handle.
@@ -94,7 +104,7 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
- * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * If a call is made without a corresponding perf_begin call, or if perf_cancel
  * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
@@ -102,6 +112,18 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
 /**
+ * Register a measurement
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresponding perf_begin call. It sets the
+ * value provided as argument as a new measurement.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param elapsed		The time elapsed. Negative values lead to incrementing the overrun counter.
+ */
+__EXPORT extern void		perf_set(perf_counter_t handle, int64_t elapsed);
+
+/**
  * Cancel a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
@@ -143,6 +165,13 @@ __EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
 __EXPORT extern void		perf_print_all(int fd);
 
 /**
+ * Print hrt latency counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+__EXPORT extern void		perf_print_latency(int fd);
+
+/**
  * Reset all of the performance counters.
  */
 __EXPORT extern void		perf_reset_all(void);
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 702e435ac..a0988035c 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -82,3 +82,17 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
+
+/**
+* Companion computer interface
+*
+* Configures the baud rate of the companion computer interface.
+* Set to zero to disable, set to 921600 to enable.
+* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for
+* other baud rates.
+*
+* @min 0
+* @max 921600
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_COMPANION, 0);
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 90d8dd77c..82183b0d7 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
 	kill(tcb->pid, SIGUSR1);
 }
 
-int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[])
 {
 	int pid;
 
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 3728f2067..2f24215a9 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -41,6 +41,7 @@
 #define SYSTEMLIB_H_
 #include <float.h>
 #include <stdint.h>
+#include <sched.h>
 
 __BEGIN_DECLS
 
@@ -63,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name,
 			int scheduler,
 			int stack_size,
 			main_t entry,
-			const char *argv[]);
+			char * const argv[]);
 
 enum MULT_PORTS {
 	MULT_0_US2_RXTX = 0,