New multirotor mixer; builds, not yet tested.

7 files changed, 361 insertions, 30 deletions

diff --git a/ROMFS/mixers/FMU_multirotor.mix b/ROMFS/mixers/FMU_multirotor.mix
index 362cfdbdf..285a40e1a 100644
--- a/ROMFS/mixers/FMU_multirotor.mix
+++ b/ROMFS/mixers/FMU_multirotor.mix
@@ -1,24 +1,7 @@
 Multirotor mixer for PX4FMU
 ===========================
 
-This file defines passthrough mixers suitable for driving ESCs over the full
-input range.
-
-Channel group 0, channels 0-3 values 0.0 - 1.0 are scaled to the full output range.
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 0      0  20000 -10000 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 1      0  20000 -10000 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 2      0  20000 -10000 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 3      0  20000 -10000 -10000  10000
+This file defines a single mixer for a quadrotor in the X configuration, with 10% contribution from 
+roll and pitch and 20% contribution from yaw and no deadband.
 
+R: 4x 1000 1000 2000 0
diff --git a/ROMFS/mixers/README b/ROMFS/mixers/README
index 3c740e37a..6649c53c2 100644
--- a/ROMFS/mixers/README
+++ b/ROMFS/mixers/README
@@ -125,4 +125,30 @@ operations, the values stored in the definition file are scaled by a factor of
 Multirotor Mixer
 ................
 
-The multirotor mixer is not yet defined.
+The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
+into a set of actuator outputs intended to drive motor speed controllers.
+
+The mixer definition is a single line of the form:
+
+R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
+
+The supported geometries include:
+
+  4x - quadrotor in X configuration
+  4+ - quadrotor in + configuration
+  6x - hexcopter in X configuration
+  6+ - hexcopter in + configuration
+  8x - octocopter in X configuration
+  8+ - octocopter in + configuration
+  
+Each of the roll, pitch and yaw scale values determine scaling of the roll,
+pitch and yaw controls relative to the thrust control.  Whilst the calculations
+are performed as floating-point operations, the values stored in the definition
+file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
+
+Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
+thrust input ranges from 0.0 to 1.0.  Output for each actuator is in the 
+range -1.0 to 1.0.
+
+In the case where an actuator saturates, all actuator values are rescaled so that 
+the saturating actuator is limited to 1.0.
diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp
index 49c8ed75a..b56226c03 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/apps/systemlib/mixer/mixer.cpp
@@ -60,6 +60,17 @@ Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
 }
 
 float
+Mixer::get_control(uint8_t group, uint8_t index)
+{
+	float	value;
+	
+	_control_cb(_cb_handle, group, index, value);
+
+	return value;
+}
+
+
+float
 Mixer::scale(const mixer_scaler_s &scaler, float input)
 {
 	float output;
diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h
index 882acb33e..94c179eba 100644
--- a/apps/systemlib/mixer/mixer.h
+++ b/apps/systemlib/mixer/mixer.h
@@ -185,6 +185,15 @@ protected:
 	uintptr_t			_cb_handle;
 
 	/**
+	 * Invoke the client callback to fetch a control value.
+	 *
+	 * @param group			Control group to fetch from.
+	 * @param index			Control index to fetch.
+	 * @return			The control value.
+	 */
+	float				get_control(uint8_t group, uint8_t index);
+
+	/**
 	 * Perform simpler linear scaling.
 	 *
 	 * @param scaler		The scaler configuration.
@@ -328,7 +337,7 @@ private:
 };
 
 /**
- * Multi-rotor mixer.
+ * Multi-rotor mixer for pre-defined vehicle geometries.
  *
  * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
  * a set of outputs based on the configured geometry.
@@ -336,22 +345,68 @@ private:
 class __EXPORT MultirotorMixer : public Mixer
 {
 public:
+	/**
+	 * Supported multirotor geometries.
+	 *
+	 * XXX add more
+	 */
 	enum Geometry {
-		MULTIROTOR_QUAD_PLUS,
-		MULTIROTOR_QUAD_X
-		/* XXX add more here */
+		QUAD_X = 0,	/**< quad in X configuration */
+		QUAD_PLUS,	/**< quad in + configuration */
+		HEX_X,		/**< hex in X configuration */
+		HEX_PLUS,	/**< hex in + configuration */
+		OCTA_X,
+		OCTA_PLUS,
+
+		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 */
 	};
 
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param geometry		The selected geometry.
+	 * @param roll_scale		Scaling factor applied to roll inputs
+	 *				compared to thrust.
+	 * @param pitch_scale		Scaling factor applied to pitch inputs
+	 *				compared to thrust.
+	 * @param yaw_wcale		Scaling factor applied to yaw inputs compared
+	 *				to thrust.
+	 * @param deadband		Minumum rotor control output value; usually
+	 *				tuned to ensure that rotors never stall at the
+	 * 				low end of their control range.
+	 */
 	MultirotorMixer(ControlCallback control_cb,
 			uintptr_t cb_handle,
-			Geometry geom);
+			Geometry geometry,
+			float roll_scale,
+			float pitch_scale,
+			float yaw_scale,
+			float deadband);
 	~MultirotorMixer();
 
 	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual void			groups_required(uint32_t &groups);
 
 private:
-	Geometry			_geometry;
+	float				_roll_scale;
+	float				_pitch_scale;
+	float				_yaw_scale;
+	float				_deadband;
+
+	unsigned			_rotor_count;
+	const Rotor			*_rotors;
+
 };
 
 #endif
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 3f954c129..bc944ccd9 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -210,6 +210,45 @@ fail:
 	return nullptr;
 }
 
+MultirotorMixer *
+mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf)
+{
+	MultirotorMixer::Geometry geometry;
+	char geomname[8];
+	int s[4];
+
+	if (sscanf(buf, "R: %7s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) {
+		debug("multirotor parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	if (!strcmp(geomname, "4+")) {
+		geometry = MultirotorMixer::QUAD_PLUS;
+	} else if (!strcmp(geomname, "4x")) {
+		geometry = MultirotorMixer::QUAD_X;
+	} else if (!strcmp(geomname, "6+")) {
+		geometry = MultirotorMixer::HEX_PLUS;
+	} else if (!strcmp(geomname, "6x")) {
+		geometry = MultirotorMixer::HEX_X;
+	} else if (!strcmp(geomname, "8+")) {
+		geometry = MultirotorMixer::OCTA_PLUS;
+	} else if (!strcmp(geomname, "8x")) {
+		geometry = MultirotorMixer::OCTA_X;
+	} else {
+		debug("unrecognised geometry '%s'", geomname);
+		return nullptr;
+	}
+
+	return new MultirotorMixer(
+		control_cb,
+		cb_handle,
+		geometry,
+		s[0] / 10000.0f,
+		s[1] / 10000.0f,
+		s[2] / 10000.0f,
+		s[3] / 10000.0f);
+}
+
 int
 mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer)
 {
@@ -239,6 +278,13 @@ mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer
 		return (mixer == nullptr) ? -1 : 1;
 	}
 
+	/* is it a multirotor mixer? */
+	if (buf[0] == 'R') {
+		debug("got a multirotor mixer");
+		mixer = mixer_load_multirotor(control_cb, cb_handle, buf);
+		return (mixer == nullptr) ? -1 : 1;
+	}
+
 	/* we don't recognise the mixer type */
 	debug("unrecognized mixer type '%c'", buf[0]);
 	return -1;
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index 713b1e25d..990c067fd 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -50,14 +50,100 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <math.h>
 
 #include "mixer.h"
 
+#define		CW		(-1.0f)
+#define		CCW		(1.0f)
+
+namespace 
+{
+
+/*
+ * 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_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_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_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+	&_config_quad_x[0],
+	&_config_quad_plus[0],
+	&_config_hex_x[0],
+	&_config_hex_plus[0],
+	&_config_octa_x[0],
+	&_config_octa_plus[0],
+};
+const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+	4, /* quad_x */
+	4, /* quad_plus */
+	6, /* hex_x */
+	6, /* hex_plus */
+	8, /* octa_x */
+	8, /* octa_plus */
+};
+
+}
+
 MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 				 uintptr_t cb_handle,
-				 MultirotorMixer::Geometry geom) :
+				 Geometry geometry,
+				 float roll_scale,
+				 float pitch_scale,
+				 float yaw_scale,
+				 float deadband) :
 	Mixer(control_cb, cb_handle),
-	_geometry(geom)
+	_roll_scale(roll_scale),
+	_pitch_scale(pitch_scale),
+	_yaw_scale(yaw_scale),
+	_deadband(-1.0 + deadband),	/* shift to output range here to avoid runtime calculation */
+	_rotor_count(_config_rotor_count[geometry]),
+	_rotors(_config_index[geometry])
 {
 }
 
@@ -68,7 +154,38 @@ MultirotorMixer::~MultirotorMixer()
 unsigned
 MultirotorMixer::mix(float *outputs, unsigned space)
 {
-	/* XXX implement this */
+	float		roll    = get_control(0, 0) * _roll_scale;
+	float		pitch   = get_control(0, 1) * _pitch_scale;
+	float		yaw     = get_control(0, 2) * _yaw_scale;
+	float		thrust  = get_control(0, 3);
+	float		max     = 0.0f;
+	float		fixup_scale;
+
+	/* perform initial mix pass yielding un-bounded outputs */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		float tmp = roll  * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale +
+			    yaw   * _rotors[i].yaw_scale +
+			    thrust;
+		if (tmp > max)
+			max = tmp;
+		outputs[i] = tmp;
+	}
+
+	/* scale values into the -1.0 - 1.0 range */
+	if (max > 1.0f) {
+		fixup_scale = 2.0f / max;
+	} else {
+		fixup_scale = 2.0f;
+	}
+	for (unsigned i = 0; i < _rotor_count; i++)
+		outputs[i] *= -1.0 + (outputs[i] * fixup_scale);
+
+	/* ensure outputs are out of the deadband */
+	for (unsigned i = 0; i < _rotor_count; i++)
+		if (outputs[i] < _deadband)
+			outputs[i] = _deadband;
+
 	return 0;
 }
 
diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables
new file mode 100755
index 000000000..3da1a11ba
--- /dev/null
+++ b/apps/systemlib/mixer/multi_tables
@@ -0,0 +1,93 @@
+#!/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 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 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 tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus}
+
+proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] $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::Geometry::MAX_GEOMETRY\] = {"
+foreach table $tables {
+	puts [format "\t&_config_%s\[0\]," $table]
+}
+puts "};"
+
+puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+foreach table $tables {
+	upvar #0 $table angles
+	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]
+}
+puts "};"