New multirotor mixer; builds, not yet tested.

apps/systemlib/mixer/mixer.cpp

@@ -59,6 +59,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)
 {

apps/systemlib/mixer/mixer.h

@@ -184,6 +184,15 @@ protected:
 	ControlCallback			_control_cb;
 	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.
 	 *
@@ -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

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;

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;
 }
 

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 "};"