Generic helicopter mixer

fix code style

ROMFS/px4fmu_common/init.d/16001_helicopter

@@ -12,9 +12,9 @@ sh /etc/init.d/rc.mc_defaults
 # Configure as helicopter (number 4 defined in commander_helper.cpp)
 set MAV_TYPE 4
 
-set MIXER heli_120deg
+set MIXER blade130
 
-set PWM_OUT 1234
+#set PWM_OUT 1234
 
 if [ $AUTOCNF == yes ]
 then
@@ -39,7 +39,9 @@ then
 	param set MC_ACRO_R_MAX 360.0
 	param set MC_ACRO_P_MAX 360.0
 
+#	param set PWM_DISARMED 1500
 	param set PWM_MIN 1075
+#	param set PWM_MAX 1950
 
 	param set MPC_THR_MIN 0.06
 	param set MPC_MANTHR_MIN 0.06
@@ -48,3 +50,5 @@ then
 
 	param set CBRK_IO_SAFETY 22027
 fi
+
+set HIL no

ROMFS/px4fmu_common/mixers/blade130.main.mix

@@ -0,0 +1,16 @@
+Helicopter Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
+Blade 130x helicopter has longer servo arms left and right. The front servo arm is shortest and has normalized length 10000.
+==================================================
+
+H: 3
+T:      0   3000   6000   8000  10000
+P:    500   1500   2500   3500   4500
+# Swash plate servos:
+S:      0  10000  10000      0  -8000   8000
+S:    140  13054  10000      0  -8000   8000
+S:    220  13054  10000      0  -8000   8000
+
+# Tail servo:
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000

ROMFS/px4fmu_common/mixers/heli_120deg.main.mix

@@ -1,54 +0,0 @@
-Helicopter 120 degree Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
-Blade 130x helicopter has longer servo arms left and right. The front servo arm is shorter.
-Therefore it is not required to use the 0.866 factor.
-==================================================
-
-Output 0 - Left Servo Mixer
------------------
-Left Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) + Aileron (Roll - 0)
-
-M: 3
-O:       10000   10000      0 -10000  10000
-S: 3 5   10000   10000      0 -10000  10000
-S: 0 1  -10000  -10000      0 -10000  10000
-S: 0 0   10000   10000      0 -10000  10000
-
-
-Output 1 - Front Servo Mixer
-----------------
-
-Rear Servo = Collective (Thrust - 3) + Elevator (Pitch - 1)
-
-M: 2
-O:      10000  10000      0 -10000  10000
-S: 3 5  10000  10000      0 -10000  10000
-S: 0 1  10000  10000      0 -10000  10000
-
-
-Output 2 - Right Servo Mixer
-----------------
-Right Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) - Aileron (Roll - 0)
-
-M: 3
-O:       10000   10000      0 -10000  10000
-S: 3 5   10000   10000      0 -10000  10000
-S: 0 1  -10000  -10000      0 -10000  10000
-S: 0 0  -10000  -10000      0 -10000  10000
-
-
-Output 3 - Tail Servo Mixer
-----------------
-Tail Servo = Yaw (control index = 2)
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 2  10000  10000      0 -10000  10000
-
-
-Output 4 - Motor speed mixer
------------------
-This would be the motor speed control output from governor power demand- not sure what index to use here?
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 0 3      0  20000 -10000 -10000  10000

Tools/px_romfs_pruner.py

@@ -84,7 +84,7 @@ def main():
                 for line in f:
                     # handle mixer files differently than startup files
                     if file_path.endswith(".mix"):
-                        if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
+                        if line.startswith(("Z:", "M:", "R: ", "O:", "S:", "H:", "T:", "P:")):
                                             pruned_content += line
                     else:
                         if not line.isspace() \

src/modules/px4iofirmware/CMakeLists.txt

@@ -83,6 +83,7 @@ set(srcs
 	../systemlib/mixer/mixer.cpp
 	../systemlib/mixer/mixer_group.cpp
 	../systemlib/mixer/mixer_multirotor.cpp
+	../systemlib/mixer/mixer_helicopter.cpp
 	../systemlib/mixer/mixer_simple.cpp
 	../systemlib/pwm_limit/pwm_limit.c
 	../../lib/rc/st24.c

src/modules/systemlib/mixer/CMakeLists.txt

@@ -47,6 +47,7 @@ px4_add_module(
 		mixer_group.cpp
 		mixer_multirotor.cpp
 		mixer_simple.cpp
+		mixer_helicopter.cpp
 		mixer_load.c
 		mixer_multirotor.generated.h
 	DEPENDS

src/modules/systemlib/mixer/mixer.h

@@ -315,6 +315,20 @@ public:
 	 *
 	 * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
 	 *
+	 * Helicopter Mixer
+	 * ................
+	 *
+	 * The helicopter mixer includes throttle and pitch curves
+	 *
+	 * H: <swash plate servo count>
+	 * T: <0> <2500> <5000> <7500> <10000>
+	 * P: <-10000> <-5000> <0> <5000> <10000>
+	 *
+	 * The definition continues with <swash plate servo count> entries describing
+	 * the position of the servo, in the following form:
+	 *
+	 *   S: <angle (deg)> <normalized arm length> <scale> <offset> <lower limit> <upper limit>
+	 *
 	 * @param buf			The mixer configuration buffer.
 	 * @param buflen		The length of the buffer, updated to reflect
 	 *				bytes as they are consumed.
@@ -597,4 +611,77 @@ private:
 	MultirotorMixer operator=(const MultirotorMixer &);
 };
 
+/** helicopter swash servo mixer */
+struct mixer_heli_servo_s {
+	float angle;
+	float arm_length;
+	float scale;
+	float offset;
+	float min_output;
+	float max_output;
+};
+
+#define HELI_CURVES_NR_POINTS 5
+
+/** helicopter swash plate mixer */
+struct mixer_heli_s {
+	uint8_t				control_count;	/**< number of inputs */
+	float				throttle_curve[HELI_CURVES_NR_POINTS];
+	float				pitch_curve[HELI_CURVES_NR_POINTS];
+	struct mixer_heli_servo_s	servos[4];	/**< up to four inputs */
+};
+
+/**
+ * Generic helicopter mixer for helicopters with swash plate.
+ *
+ * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands
+ * for swash plate tilting and throttle- and pitch curves.
+ */
+class __EXPORT HelicopterMixer : public Mixer
+{
+public:
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param mixer_info		Pointer to heli mixer configuration
+	 */
+	HelicopterMixer(ControlCallback control_cb,
+			uintptr_t cb_handle,
+			mixer_heli_s *mixer_info);
+	~HelicopterMixer();
+
+	/**
+	 * Factory method.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new HelicopterMixer instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static HelicopterMixer		*from_text(Mixer::ControlCallback control_cb,
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
+	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
+	virtual void			groups_required(uint32_t &groups);
+
+private:
+	mixer_heli_s			_mixer_info;
+
+	/* do not allow to copy */
+	HelicopterMixer(const HelicopterMixer &);
+	HelicopterMixer operator=(const HelicopterMixer &);
+};
+
 #endif

src/modules/systemlib/mixer/mixer_group.cpp

@@ -197,6 +197,10 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 			m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
 			break;
 
+		case 'H':
+			m = HelicopterMixer::from_text(_control_cb, _cb_handle, p, resid);
+			break;
+
 		default:
 			/* it's probably junk or whitespace, skip a byte and retry */
 			buflen--;

src/modules/systemlib/mixer/mixer_helicopter.cpp

@@ -0,0 +1,283 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2016 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_helicopter.cpp
+ *
+ * Helicopter mixers.
+ */
+#include <px4_config.h>
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <math.h>
+
+#include <px4iofirmware/protocol.h>
+
+#include "mixer.h"
+
+#define debug(fmt, args...)	do { } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#include <debug.h>
+//#define debug(fmt, args...)	lowsyslog(fmt "\n", ##args)
+
+
+namespace
+{
+
+float constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+} // anonymous namespace
+
+HelicopterMixer::HelicopterMixer(ControlCallback control_cb,
+				 uintptr_t cb_handle,
+				 mixer_heli_s *mixer_info) :
+	Mixer(control_cb, cb_handle),
+	_mixer_info(*mixer_info)
+{
+
+}
+
+HelicopterMixer::~HelicopterMixer()
+{
+}
+
+HelicopterMixer *
+HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
+{
+	mixer_heli_s mixer_info;
+	unsigned swash_plate_servo_count = 0;
+	unsigned u[5];
+	int s[5];
+	int used;
+
+	/* 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]);
+			return nullptr;
+		}
+
+	}
+
+	if (sscanf(buf, "H: %u%n", &swash_plate_servo_count, &used) != 1) {
+		debug("helicopter parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	if (swash_plate_servo_count < 3 || swash_plate_servo_count > 4) {
+		debug("only supporting swash plate with 3 or 4 servos");
+		return nullptr;
+	}
+
+	if (used > (int)buflen) {
+		debug("OVERFLOW: helicopter spec used %d of %u", used, buflen);
+		return nullptr;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	buf = findtag(buf, buflen, 'T');
+
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("control parser failed finding tag, ret: '%s'", buf);
+		return nullptr;
+	}
+
+	if (sscanf(buf, "T: %u %u %u %u %u",
+		   &u[0], &u[1], &u[2], &u[3], &u[4]) != 5) {
+		debug("control parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
+		mixer_info.throttle_curve[i] = ((float) u[i]) / 10000.0f;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	buf = findtag(buf, buflen, 'P');
+
+	if ((buf == nullptr) || (buflen < 12)) {
+		debug("control parser failed finding tag, ret: '%s'", buf);
+		return nullptr;
+	}
+
+	if (sscanf(buf, "P: %d %d %d %d %d",
+		   &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
+		debug("control parse failed on '%s'", buf);
+		return nullptr;
+	}
+
+	for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
+		mixer_info.pitch_curve[i] = ((float) s[i]) / 10000.0f;
+	}
+
+	buf = skipline(buf, buflen);
+
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	mixer_info.control_count = swash_plate_servo_count;
+
+	/* Now loop through the servos */
+	for (unsigned i = 0; i < mixer_info.control_count; i++) {
+
+		buf = findtag(buf, buflen, 'S');
+
+		if ((buf == nullptr) || (buflen < 12)) {
+			debug("control parser failed finding tag, ret: '%s'", buf);
+			return nullptr;
+		}
+
+		if (sscanf(buf, "S: %u %u %d %d %d %d",
+			   &u[0],
+			   &u[1],
+			   &s[0],
+			   &s[1],
+			   &s[2],
+			   &s[3]) != 6) {
+			debug("control parse failed on '%s'", buf);
+			return nullptr;
+		}
+
+		mixer_info.servos[i].angle = ((float) u[0]) * M_PI_F / 180.0f;
+		mixer_info.servos[i].arm_length = ((float) u[1]) / 10000.0f;
+		mixer_info.servos[i].scale = ((float) s[0]) / 10000.0f;
+		mixer_info.servos[i].offset = ((float) s[1]) / 10000.0f;
+		mixer_info.servos[i].min_output = ((float) s[2]) / 10000.0f;
+		mixer_info.servos[i].max_output = ((float) s[3]) / 10000.0f;
+
+		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]);
+
+	HelicopterMixer *hm = new HelicopterMixer(
+		control_cb,
+		cb_handle,
+		&mixer_info);
+
+	if (hm != nullptr) {
+		debug("loaded heli mixer with %d swash plate input(s)", mixer_info.control_count);
+
+	} else {
+		debug("could not allocate memory for mixer");
+	}
+
+	return hm;
+}
+
+unsigned
+HelicopterMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
+{
+	/* Find index to use for curves */
+	float thrust_cmd = get_control(0, 3);
+	int idx = (thrust_cmd / 0.25f);
+
+	/* Make sure idx is in range */
+	if (idx < 0) { idx = 0; }
+
+	if (idx > HELI_CURVES_NR_POINTS - 1) { idx = HELI_CURVES_NR_POINTS - 1; }
+
+	/* Local throttle curve gradient and offset */
+	float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f;
+	float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);
+	float throttle = constrain((tg * thrust_cmd + to), 0.0f, 1.0f);
+
+	/* Local pitch curve gradient and offset */
+	float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f;
+	float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);
+	float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
+
+	float roll_cmd = get_control(0, 0);
+	float pitch_cmd = get_control(0, 1);
+
+	outputs[0] = throttle;
+
+	for (unsigned i = 0; i < _mixer_info.control_count; i++) {
+		outputs[i + 1] = collective_pitch
+				 + cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length
+				 - sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;
+		outputs[i + 1] *= _mixer_info.servos[i].scale;
+		outputs[i + 1] += _mixer_info.servos[i].offset;
+		outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);
+	}
+
+	return _mixer_info.control_count + 1;
+}
+
+void
+HelicopterMixer::groups_required(uint32_t &groups)
+{
+	/* XXX for now, hardcoded to indexes 0-3 in control group zero */
+	groups |= (1 << 0);
+}
+