diff --git a/ROMFS/px4fmu_common/init.d/16001_helicopter b/ROMFS/px4fmu_common/init.d/16001_helicopter
index fba8f9ebf03d11b92fd9bb18f6f0d330b69fb921..18efea586f0b7f4d327de24dfd01e02761b7a0ad 100644
--- a/ROMFS/px4fmu_common/init.d/16001_helicopter
+++ b/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
diff --git a/ROMFS/px4fmu_common/mixers/blade130.main.mix b/ROMFS/px4fmu_common/mixers/blade130.main.mix
new file mode 100644
index 0000000000000000000000000000000000000000..d03f19faf5ff6756bcef7e20ad755aa02d183571
--- /dev/null
+++ b/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
diff --git a/ROMFS/px4fmu_common/mixers/heli_120deg.main.mix b/ROMFS/px4fmu_common/mixers/heli_120deg.main.mix
deleted file mode 100644
index a5e57e595225c0dd58d530aaecf79fbf9f1c36a0..0000000000000000000000000000000000000000
--- a/ROMFS/px4fmu_common/mixers/heli_120deg.main.mix
+++ /dev/null
@@ -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
diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py
index d6481e03ed80fc6e22e2a0bbe1b25b136f829cb3..bfe5b392ffd4bd42230abf16ad14625d40435730 100644
--- a/Tools/px_romfs_pruner.py
+++ b/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() \
diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt
index a0a52ac9cfcd70ca6b02c4cf004ace241465dc17..754bf715a3ed11c47022a32926450273837924eb 100644
--- a/src/modules/px4iofirmware/CMakeLists.txt
+++ b/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
diff --git a/src/modules/systemlib/mixer/CMakeLists.txt b/src/modules/systemlib/mixer/CMakeLists.txt
index e9abd7a2a07f0734b5a0d4c70beaab0d0c669b93..daa2bcd282d8e1410d23b65d0ad9a04ad953c619 100644
--- a/src/modules/systemlib/mixer/CMakeLists.txt
+++ b/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
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 0b433071d035a7c13d64d085c679fedbf19e4ada..166d1718b56400af65a1db3d3d76e7a18fe6f837 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/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
diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 252e51389990705d2f3eb63efe5e782db6e0c78f..693ef31b40a8150d01d1210fadff9134450cc544 100644
--- a/src/modules/systemlib/mixer/mixer_group.cpp
+++ b/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--;
diff --git a/src/modules/systemlib/mixer/mixer_helicopter.cpp b/src/modules/systemlib/mixer/mixer_helicopter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d16d8971714c279bb91903a0dc1f63da4fc258b3
--- /dev/null
+++ b/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);
+}
+