From a7c8d77453bf05235f809280d261659602e513fc Mon Sep 17 00:00:00 2001 From: Bart Slinger Date: Wed, 24 Aug 2016 16:32:03 +0200 Subject: [PATCH] Generic helicopter mixer fix code style --- ROMFS/px4fmu_common/init.d/16001_helicopter | 8 +- ROMFS/px4fmu_common/mixers/blade130.main.mix | 16 + .../px4fmu_common/mixers/heli_120deg.main.mix | 54 ---- Tools/px_romfs_pruner.py | 2 +- src/modules/px4iofirmware/CMakeLists.txt | 1 + src/modules/systemlib/mixer/CMakeLists.txt | 1 + src/modules/systemlib/mixer/mixer.h | 87 ++++++ src/modules/systemlib/mixer/mixer_group.cpp | 4 + .../systemlib/mixer/mixer_helicopter.cpp | 283 ++++++++++++++++++ 9 files changed, 399 insertions(+), 57 deletions(-) create mode 100644 ROMFS/px4fmu_common/mixers/blade130.main.mix delete mode 100644 ROMFS/px4fmu_common/mixers/heli_120deg.main.mix create mode 100644 src/modules/systemlib/mixer/mixer_helicopter.cpp diff --git a/ROMFS/px4fmu_common/init.d/16001_helicopter b/ROMFS/px4fmu_common/init.d/16001_helicopter index fba8f9ebf0..18efea586f 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 0000000000..d03f19faf5 --- /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 a5e57e5952..0000000000 --- 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 d6481e03ed..bfe5b392ff 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 a0a52ac9cf..754bf715a3 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 e9abd7a2a0..daa2bcd282 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 0b433071d0..166d1718b5 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -315,6 +315,20 @@ public: * * R: * + * Helicopter Mixer + * ................ + * + * The helicopter mixer includes throttle and pitch curves + * + * H: + * T: <0> <2500> <5000> <7500> <10000> + * P: <-10000> <-5000> <0> <5000> <10000> + * + * The definition continues with entries describing + * the position of the servo, in the following form: + * + * S: + * * @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 252e513899..693ef31b40 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 0000000000..d16d897171 --- /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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#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); +} +