diff --git a/CMakeLists.txt b/CMakeLists.txt index 75fc8afb67..6c03aef707 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -420,6 +420,7 @@ if(BUILD_TESTING) DEPENDS px4 examples__dyn_hello + test_mixer_multirotor USES_TERMINAL COMMENT "Running tests" WORKING_DIRECTORY ${PX4_BINARY_DIR}) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 6454c9e40c..d72d5ee47d 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -57,6 +57,7 @@ add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL) add_subdirectory(led EXCLUDE_FROM_ALL) add_subdirectory(matrix EXCLUDE_FROM_ALL) add_subdirectory(mathlib EXCLUDE_FROM_ALL) +add_subdirectory(mixer) add_subdirectory(mixer_module EXCLUDE_FROM_ALL) add_subdirectory(motion_planning EXCLUDE_FROM_ALL) add_subdirectory(npfg EXCLUDE_FROM_ALL) diff --git a/src/lib/mixer/.gitignore b/src/lib/mixer/.gitignore new file mode 100644 index 0000000000..df7e57e698 --- /dev/null +++ b/src/lib/mixer/.gitignore @@ -0,0 +1,2 @@ + +/test_mixer_multirotor diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp new file mode 100644 index 0000000000..9e03335c12 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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_AllocatedActuatorMixer.cpp + * + * Mixer for allocated actuators. + * + * @author Julien Lecoeur + */ + +#include "AllocatedActuatorMixer.hpp" + +#include +#include +#include + +// #define debug(fmt, args...) do { } while(0) +#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +AllocatedActuatorMixer::AllocatedActuatorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + uint8_t index) : + Mixer(control_cb, cb_handle) +{ + if (index < 8) { + _control_group = 4; + _control_index = index; + + } else if (index < 16) { + _control_group = 5; + _control_index = index - 8; + + } else { + debug("'A:' invalid index"); + } +} + +unsigned AllocatedActuatorMixer::set_trim(float trim) +{ + return 1; +} + +unsigned AllocatedActuatorMixer::get_trim(float *trim) +{ + *trim = 0.0f; + return 1; +} + +int +AllocatedActuatorMixer::parse(const char *buf, unsigned &buflen, uint8_t &index) +{ + int ret; + int i; + + // enforce that the mixer ends with a new line + if (!string_well_formed(buf, buflen)) { + return -1; + } + + // parse line + if ((ret = sscanf(buf, "A: %d", &i)) != 1) { + debug("'A:' parser: failed on '%s'", buf); + return -1; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("'A:' parser: no line ending, line is incomplete"); + return -1; + } + + // check parsed index + if (i < 16) { + index = i; + + } else { + debug("'A:' parser: invalid index"); + return -1; + } + + return 0; +} + +AllocatedActuatorMixer * +AllocatedActuatorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen) +{ + uint8_t index; + + if (parse(buf, buflen, index) == 0) { + return new AllocatedActuatorMixer(control_cb, cb_handle, index); + + } else { + return nullptr; + } +} + +unsigned +AllocatedActuatorMixer::mix(float *outputs, unsigned space) +{ + if (space < 1) { + return 0; + } + + _control_cb(_cb_handle, + _control_group, + _control_index, + *outputs); + + return 1; +} + +void +AllocatedActuatorMixer::groups_required(uint32_t &groups) +{ + groups |= 1 << _control_group; +} diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp new file mode 100644 index 0000000000..b36cecfe0d --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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_AllocatedActuatorMixer.hpp + * + * Mixer for allocated actuators. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +/** + * Mixer for allocated actuators. + * + * Copies a single actuator to a single output. + */ +class AllocatedActuatorMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param index Actuator index (0..15) + */ + AllocatedActuatorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + uint8_t index); + virtual ~AllocatedActuatorMixer() = default; + + // no copy, assignment, move, move assignment + AllocatedActuatorMixer(const AllocatedActuatorMixer &) = delete; + AllocatedActuatorMixer &operator=(const AllocatedActuatorMixer &) = delete; + AllocatedActuatorMixer(AllocatedActuatorMixer &&) = delete; + AllocatedActuatorMixer &operator=(AllocatedActuatorMixer &&) = delete; + + /** + * Factory method with full external configuration. + * + * 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 AllocatedActuatorMixer instance, or nullptr + * if the text format is bad. + */ + static AllocatedActuatorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + void groups_required(uint32_t &groups) override; + unsigned set_trim(float trim) override; + unsigned get_trim(float *trim) override; + +private: + uint8_t _control_group; /**< group from which the input reads */ + uint8_t _control_index; /**< index within the control group */ + + static int parse(const char *buf, + unsigned &buflen, + uint8_t &index); +}; diff --git a/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt new file mode 100644 index 0000000000..0a8526545c --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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. +# +############################################################################ + +add_library(AllocatedActuatorMixer + AllocatedActuatorMixer.cpp + AllocatedActuatorMixer.hpp +) +target_link_libraries(AllocatedActuatorMixer PRIVATE MixerBase) +add_dependencies(AllocatedActuatorMixer prebuild_targets) diff --git a/src/lib/mixer/CMakeLists.txt b/src/lib/mixer/CMakeLists.txt new file mode 100644 index 0000000000..088e6fa366 --- /dev/null +++ b/src/lib/mixer/CMakeLists.txt @@ -0,0 +1,58 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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. +# +############################################################################ + +# required by other mixers +add_subdirectory(MixerBase) + +add_subdirectory(AllocatedActuatorMixer) +add_subdirectory(HelicopterMixer) +add_subdirectory(MultirotorMixer) +add_subdirectory(NullMixer) +add_subdirectory(SimpleMixer) + +add_library(mixer + MixerGroup.cpp + MixerGroup.hpp + load_mixer_file.cpp + mixer_load.h +) + +target_link_libraries(mixer + PRIVATE + AllocatedActuatorMixer + HelicopterMixer + MultirotorMixer + NullMixer + SimpleMixer + ) +add_dependencies(mixer prebuild_targets) diff --git a/src/lib/mixer/HelicopterMixer/CMakeLists.txt b/src/lib/mixer/HelicopterMixer/CMakeLists.txt new file mode 100644 index 0000000000..264f3335ef --- /dev/null +++ b/src/lib/mixer/HelicopterMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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. +# +############################################################################ + +add_library(HelicopterMixer + HelicopterMixer.cpp + HelicopterMixer.hpp +) +target_link_libraries(HelicopterMixer PRIVATE MixerBase) +add_dependencies(HelicopterMixer prebuild_targets) diff --git a/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp b/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp new file mode 100644 index 0000000000..e202367e97 --- /dev/null +++ b/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2017 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 "HelicopterMixer.hpp" + +#include +#include +#include + +#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) + +using math::constrain; + +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::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 a new line */ + if (!string_well_formed(buf, buflen)) { + 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) +{ + if (space < _mixer_info.control_count + 1u) { + return 0; + } + + /* 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; + + } else if (idx > HELI_CURVES_NR_POINTS - 2) { + /* We access idx + 1 below, so max legal index is (size - 2) */ + idx = HELI_CURVES_NR_POINTS - 2; + } + + /* 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(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.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; +} diff --git a/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp b/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp new file mode 100644 index 0000000000..ce50a8f58c --- /dev/null +++ b/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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. + * + ****************************************************************************/ + +#pragma once + +#include + +/** 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]; + 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 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); + virtual ~HelicopterMixer() = default; + + // no copy, assignment, move, move assignment + HelicopterMixer(const HelicopterMixer &) = delete; + HelicopterMixer &operator=(const HelicopterMixer &) = delete; + HelicopterMixer(HelicopterMixer &&) = delete; + HelicopterMixer &operator=(HelicopterMixer &&) = delete; + + /** + * 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); + + unsigned mix(float *outputs, unsigned space) override; + + void groups_required(uint32_t &groups) override { groups |= (1 << 0); } + + unsigned set_trim(float trim) override { return 4; } + unsigned get_trim(float *trim) override { return 4; } + +private: + mixer_heli_s _mixer_info; +}; diff --git a/src/lib/mixer/MixerBase/CMakeLists.txt b/src/lib/mixer/MixerBase/CMakeLists.txt new file mode 100644 index 0000000000..d4b1ec2915 --- /dev/null +++ b/src/lib/mixer/MixerBase/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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. +# +############################################################################ + +add_library(MixerBase + Mixer.cpp + Mixer.hpp +) +add_dependencies(MixerBase prebuild_targets) diff --git a/src/lib/mixer/MixerBase/Mixer.cpp b/src/lib/mixer/MixerBase/Mixer.cpp new file mode 100644 index 0000000000..b38b759a3f --- /dev/null +++ b/src/lib/mixer/MixerBase/Mixer.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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.cpp + * + * Programmable multi-channel mixer library. + */ + +#include "Mixer.hpp" + +#include +#include +#include + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +float +Mixer::get_control(uint8_t group, uint8_t index) +{ + float value; + + _control_cb(_cb_handle, group, index, value); + + return value; +} + +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) { + return buf; + } + + buf++; + buflen--; + } + + return nullptr; +} + +char +Mixer::findnexttag(const char *buf, unsigned buflen) +{ + while (buflen >= 2) { + if (isupper(buf[0]) && buf[1] == ':') { + return buf[0]; + } + + buf++; + buflen--; + } + + return 0; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + +bool +Mixer::string_well_formed(const char *buf, unsigned &buflen) +{ + /* enforce that the mixer ends with 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] == '\n' || buf[i] == '\r') { + /* found a line ending, so no split symbols / numbers. good. */ + return true; + } + + } + + debug("pre-parser rejected: No newline in buf"); + + return false; +} diff --git a/src/lib/mixer/MixerBase/Mixer.hpp b/src/lib/mixer/MixerBase/Mixer.hpp new file mode 100644 index 0000000000..e90290e143 --- /dev/null +++ b/src/lib/mixer/MixerBase/Mixer.hpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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.hpp + * + * Generic, programmable, procedural control signal mixers. + * + * This library implements a generic mixer interface that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * control value + * A mixer input value, typically provided by some controlling + * component of the system. + * + * control group + * A collection of controls provided by a single controlling component. + * + * actuator + * The mixer output value. + * + * + * Mixing basics + * ============= + * + * An actuator derives its value from the combination of one or more + * control values. Each of the control values is scaled according to + * the actuator's configuration and then combined to produce the + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing behaviour varies based on the specific mixer class; each + * mixer class describes its behaviour in more detail. + * + * + * Controls + * -------- + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. Some mixer classes have specific assumptions + * about the assignment of controls. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + +#pragma once + +#include +#include + +/** + * Abstract class defining a mixer mixing zero or more inputs to + * one or more outputs. + */ +class Mixer : public ListNode +{ +public: + enum class Airmode : int32_t { + disabled = 0, + roll_pitch = 1, + roll_pitch_yaw = 2 + }; + + /** + * Fetch a control value. + * + * @param handle Token passed when the callback is registered. + * @param control_group The group to fetch the control from. + * @param control_index The group-relative index to fetch the control from. + * @param control The returned control + * @return Zero if the value was fetched, nonzero otherwise. + */ + typedef int (* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control); + + /** + * Constructor. + * + * @param control_cb Callback invoked when reading controls. + */ + Mixer(ControlCallback control_cb, uintptr_t cb_handle) : _control_cb(control_cb), _cb_handle(cb_handle) {} + virtual ~Mixer() = default; + + // no copy, assignment, move, move assignment + Mixer(const Mixer &) = delete; + Mixer &operator=(const Mixer &) = delete; + Mixer(Mixer &&) = delete; + Mixer &operator=(Mixer &&) = delete; + + /** + * Perform the mixing function. + * + * @param outputs Array into which mixed output(s) should be placed. + * @param space The number of available entries in the output array; + * @return The number of entries in the output array that were populated. + */ + virtual unsigned mix(float *outputs, unsigned space) = 0; + + /** + * Get the saturation status. + * + * @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg. + */ + virtual uint16_t get_saturation_status() { return 0; } + + /** + * Analyses the mix configuration and updates a bitmask of groups + * that are required. + * + * @param groups A bitmask of groups (0-31) that the mixer requires. + */ + virtual void groups_required(uint32_t &groups) {}; + + /** + * @brief Empty method, only implemented for MultirotorMixer and MixerGroup class. + * + * @param[in] delta_out_max Maximum delta output. + * + */ + virtual void set_max_delta_out_once(float delta_out_max) {} + + /** + * @brief Set trim offset for this mixer + * + * @return the number of outputs this mixer feeds to + */ + virtual unsigned set_trim(float trim) { return 0; } + + /** + * @brief Get trim offset for this mixer + * + * @return the number of outputs this mixer feeds to + */ + virtual unsigned get_trim(float *trim) { return 0; } + + /* + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. + * + * @param[in] val The value + */ + virtual void set_thrust_factor(float val) {} + + /** + * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors. + * + * @param[in] airmode Select airmode type (0 = disabled, 1 = roll/pitch, 2 = roll/pitch/yaw) + */ + virtual void set_airmode(Airmode airmode) {}; + + virtual unsigned get_multirotor_count() { return 0; } + + virtual void set_dt_once(float dt) {} + +protected: + + /** client-supplied callback used when fetching control values */ + 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); + + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char *findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Find next tag and return it (0 is returned if no tag is found) + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + */ + static char findnexttag(const char *buf, unsigned buflen); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char *skipline(const char *buf, unsigned &buflen); + + /** + * Check wether the string is well formed and suitable for parsing + */ + static bool string_well_formed(const char *buf, unsigned &buflen); +}; diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp new file mode 100644 index 0000000000..3321b42253 --- /dev/null +++ b/src/lib/mixer/MixerGroup.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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_group.cpp + * + * Mixer collection. + */ + +#include "MixerGroup.hpp" + +#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp" +#include "HelicopterMixer/HelicopterMixer.hpp" +#include "MultirotorMixer/MultirotorMixer.hpp" +#include "NullMixer/NullMixer.hpp" +#include "SimpleMixer/SimpleMixer.hpp" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) syslog(fmt "\n", ##args) + +unsigned +MixerGroup::mix(float *outputs, unsigned space) +{ + unsigned index = 0; + + for (auto mixer : _mixers) { + index += mixer->mix(outputs + index, space - index); + + if (index >= space) { + break; + } + } + + return index; +} + +/* + * set_trims() has no effect except for the SimpleMixer implementation for which set_trim() + * always returns the value one. + * The only other existing implementation is MultirotorMixer, which ignores the trim value + * and returns _rotor_count. + */ +unsigned +MixerGroup::set_trims(int16_t *values, unsigned n) +{ + unsigned index = 0; + + for (auto mixer : _mixers) { + // convert from integer to float + // to be safe, clamp offset to range of [-500, 500] usec + float offset = math::constrain((float)values[index] / 10000, -1.0f, 1.0f); + + debug("set trim: %d, offset: %5.3f", values[index], (double)offset); + index += mixer->set_trim(offset); + + if (index >= n) { + break; + } + } + + return index; +} + +/* + * get_trims() has no effect except for the SimpleMixer implementation for which get_trim() + * always returns the value one and sets the trim value. + * The only other existing implementation is MultirotorMixer, which ignores the trim value + * and returns _rotor_count. + */ +unsigned +MixerGroup::get_trims(int16_t *values) +{ + unsigned index_mixer = 0; + unsigned index = 0; + + for (auto mixer : _mixers) { + float trim = 0; + index_mixer += mixer->get_trim(&trim); + + // MultirotorMixer returns the number of motors so we + // loop through index_mixer and set the same trim value for all motors + while (index < index_mixer) { + values[index] = trim * 10000; + index++; + } + } + + return index; +} + +void +MixerGroup::set_thrust_factor(float val) +{ + for (auto mixer : _mixers) { + mixer->set_thrust_factor(val); + } +} + +void +MixerGroup::set_airmode(Mixer::Airmode airmode) +{ + for (auto mixer : _mixers) { + mixer->set_airmode(airmode); + } +} + +unsigned +MixerGroup::get_multirotor_count() +{ + for (auto mixer : _mixers) { + unsigned rotor_count = mixer->get_multirotor_count(); + + if (rotor_count > 0) { + return rotor_count; + } + } + + return 0; +} + +uint16_t +MixerGroup::get_saturation_status() +{ + uint16_t sat = 0; + + for (auto mixer : _mixers) { + sat |= mixer->get_saturation_status(); + } + + return sat; +} + +void +MixerGroup::groups_required(uint32_t &groups) +{ + for (auto mixer : _mixers) { + mixer->groups_required(groups); + } +} + +int +MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + int ret = -1; + const char *end = buf + buflen; + + /* + * Loop until either we have emptied the buffer, or we have failed to + * allocate something when we expected to. + */ + while (buflen > 0) { + Mixer *m = nullptr; + const char *p = end - buflen; + unsigned resid = buflen; + + /* + * Use the next character as a hint to decide which mixer class to construct. + */ + switch (*p) { + case 'Z': + m = NullMixer::from_text(p, resid); + break; + + case 'A': + m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid); + break; + + case 'M': + m = SimpleMixer::from_text(control_cb, cb_handle, p, resid); + break; + + case 'R': + 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--; + continue; + } + + /* + * If we constructed something, add it to the group. + */ + if (m != nullptr) { + add_mixer(m); + + /* we constructed something */ + ret = 0; + + /* only adjust buflen if parsing was successful */ + buflen = resid; + debug("SUCCESS - buflen: %d", buflen); + + } else { + + /* + * There is data in the buffer that we expected to parse, but it didn't, + * so give up for now. + */ + break; + } + } + + /* nothing more in the buffer for us now */ + return ret; +} + +void MixerGroup::set_max_delta_out_once(float delta_out_max) +{ + for (auto mixer : _mixers) { + mixer->set_max_delta_out_once(delta_out_max); + } +} + +void +MixerGroup::set_dt_once(float dt) +{ + for (auto mixer : _mixers) { + mixer->set_dt_once(dt); + } +} diff --git a/src/lib/mixer/MixerGroup.hpp b/src/lib/mixer/MixerGroup.hpp new file mode 100644 index 0000000000..f5b7fedb50 --- /dev/null +++ b/src/lib/mixer/MixerGroup.hpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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. + * + ****************************************************************************/ + +#pragma once + +#include "MixerBase/Mixer.hpp" + +/** + * Group of mixers, built up from single mixers and processed + * in order when mixing. + */ +class MixerGroup +{ +public: + MixerGroup() = default; + + ~MixerGroup() + { + reset(); + } + + // no copy, assignment, move, move assignment + MixerGroup(const MixerGroup &) = delete; + MixerGroup &operator=(const MixerGroup &) = delete; + MixerGroup(MixerGroup &&) = delete; + MixerGroup &operator=(MixerGroup &&) = delete; + + unsigned mix(float *outputs, unsigned space); + + uint16_t get_saturation_status(); + + void groups_required(uint32_t &groups); + + /** + * Add a mixer to the group. + * + * @param mixer The mixer to be added. + */ + void add_mixer(Mixer *mixer) { _mixers.add(mixer); } + + /** + * Remove all the mixers from the group. + */ + void reset() { _mixers.clear(); } + + /** + * Count the mixers in the group. + */ + unsigned count() const { return _mixers.size(); } + + /** + * Adds mixers to the group based on a text description in a buffer. + * + * Mixer definitions begin with a single capital letter and a colon. + * The actual format of the mixer definition varies with the individual + * mixers; they are summarised here, but see ROMFS/mixers/README for + * more details. + * + * Null Mixer + * .......... + * + * The null mixer definition has the form: + * + * Z: + * + * Simple Mixer + * ............ + * + * A simple mixer definition begins with: + * + * M: + * O: <-ve scale> <+ve scale> + * + * The second line O: can be omitted. In that case 'O: 10000 10000 0 -10000 10000' is used. + * The definition continues with entries describing the control + * inputs and their scaling, in the form: + * + * S: <-ve scale> <+ve scale> + * + * Multirotor Mixer + * ................ + * + * The multirotor mixer definition is a single line of the form: + * + * 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. + * @return Zero on successful load, nonzero otherwise. + */ + int load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen); + + /** + * @brief Update slew rate parameter. This tells instances of the class MultirotorMixer + * the maximum allowed change of the output values per cycle. + * The value is only valid for one cycle, in order to have continuous + * slew rate limiting this function needs to be called before every call + * to mix(). + * + * @param[in] delta_out_max Maximum delta output. + * + */ + void set_max_delta_out_once(float delta_out_max); + + /* + * Invoke the set_offset method of each mixer in the group + * for each value in page r_page_servo_control_trim + */ + unsigned set_trims(int16_t *v, unsigned n); + unsigned get_trims(int16_t *values); + + /** + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. + * + * @param[in] val The value + */ + void set_thrust_factor(float val); + + void set_airmode(Mixer::Airmode airmode); + + unsigned get_multirotor_count(); + + void set_dt_once(float dt); + +private: + List _mixers; /**< linked list of mixers */ +}; diff --git a/src/lib/mixer/MultirotorMixer/CMakeLists.txt b/src/lib/mixer/MultirotorMixer/CMakeLists.txt new file mode 100644 index 0000000000..5ccc17be15 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/CMakeLists.txt @@ -0,0 +1,114 @@ +############################################################################ +# +# Copyright (c) 2015 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. +# +############################################################################ + +set(geometry_files + dodeca_bottom_cox.toml + dodeca_top_cox.toml + hex_cox.toml + hex_plus.toml + hex_t.toml + hex_x.toml + octa_cox.toml + octa_cox_wide.toml + octa_plus.toml + octa_x.toml + quad_deadcat.toml + quad_h.toml + quad_plus.toml + quad_s250aq.toml + quad_vtail.toml + quad_wide.toml + quad_x_cw.toml + quad_x.toml + quad_x_pusher.toml + quad_y.toml + tri_y.toml + twin_engine.toml +) + +set(geometries_list) +foreach(geom_file ${geometry_files}) + list(APPEND geometries_list ${CMAKE_CURRENT_SOURCE_DIR}/geometries/${geom_file}) +endforeach() + +set(MIXER_TOOLS ${CMAKE_CURRENT_SOURCE_DIR}/geometries/tools) + +# generate mixers and normalize +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h + COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py -f ${geometries_list} -o mixer_multirotor.generated.h + DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} + ) +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h + COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --normalize -f ${geometries_list} -o mixer_multirotor_normalized.generated.h + DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} + ) +add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h) + +# 6DOF mixers +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h + COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h + DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} + ) +add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h) + +add_library(MultirotorMixer + MultirotorMixer.cpp + MultirotorMixer.hpp + + ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h + ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h + ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h +) +target_link_libraries(MultirotorMixer PRIVATE MixerBase) +target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) +add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets) + +if(BUILD_TESTING) + + add_executable(test_mixer_multirotor + test_mixer_multirotor.cpp + MultirotorMixer.cpp + ) + target_compile_definitions(test_mixer_multirotor PRIVATE MIXER_MULTIROTOR_USE_MOCK_GEOMETRY) + target_compile_options(test_mixer_multirotor PRIVATE -Wno-unused-result) + target_link_libraries(test_mixer_multirotor PRIVATE MixerBase) + + add_test(NAME mixer_multirotor + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/mixer_multirotor.py --test --mixer-multirotor-binary $ + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + ) + +endif() diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp new file mode 100644 index 0000000000..92780e977c --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp @@ -0,0 +1,498 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 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_multirotor.cpp + * + * Multi-rotor mixers. + */ + +#include "MultirotorMixer.hpp" + +#include +#include +#include + +#include + +#ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY +enum class MultirotorGeometry : MultirotorGeometryUnderlyingType { + QUAD_X, + MAX_GEOMETRY +}; +namespace +{ +const MultirotorMixer::Rotor _config_quad_x[] = { + { -0.707107, 0.707107, 1.000000, 1.000000 }, + { 0.707107, -0.707107, 1.000000, 1.000000 }, + { 0.707107, 0.707107, -1.000000, 1.000000 }, + { -0.707107, -0.707107, -1.000000, 1.000000 }, +}; +const MultirotorMixer::Rotor *_config_index[] = { + &_config_quad_x[0] +}; +const unsigned _config_rotor_count[] = {4}; +const char *_config_key[] = {"4x"}; +} + +#else + +// This file is generated by the px_generate_mixers.py script which is invoked during the build process +// #include "mixer_multirotor.generated.h" +#include "mixer_multirotor_normalized.generated.h" + +#endif /* MIXER_MULTIROTOR_USE_MOCK_GEOMETRY */ + + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) syslog(fmt "\n", ##args) + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry) : + MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry]) +{ +} + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, + unsigned rotor_count) : + Mixer(control_cb, cb_handle), + _rotor_count(rotor_count), + _rotors(rotors), + _outputs_prev(new float[_rotor_count]), + _tmp_array(new float[_rotor_count]) +{ + for (unsigned i = 0; i < _rotor_count; ++i) { + _outputs_prev[i] = -1.f; + } +} + +MultirotorMixer::~MultirotorMixer() +{ + delete[] _outputs_prev; + delete[] _tmp_array; +} + +MultirotorMixer * +MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY; + char geomname[16]; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + if (sscanf(buf, "R: %15s", geomname) != 1) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + 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]); + + for (MultirotorGeometryUnderlyingType i = 0; i < (MultirotorGeometryUnderlyingType)MultirotorGeometry::MAX_GEOMETRY; + i++) { + if (!strcmp(geomname, _config_key[i])) { + geometry = (MultirotorGeometry)i; + break; + } + } + + if (geometry == MultirotorGeometry::MAX_GEOMETRY) { + debug("unrecognised geometry '%s'", geomname); + return nullptr; + } + + debug("adding multirotor mixer '%s'", geomname); + + return new MultirotorMixer(control_cb, cb_handle, geometry); +} + +float +MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, const float *outputs, + saturation_status &sat_status, float min_output, float max_output) const +{ + float k_min = 0.f; + float k_max = 0.f; + + for (unsigned i = 0; i < _rotor_count; i++) { + // Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway + if (fabsf(desaturation_vector[i]) < FLT_EPSILON) { + continue; + } + + if (outputs[i] < min_output) { + float k = (min_output - outputs[i]) / desaturation_vector[i]; + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + + sat_status.flags.motor_neg = true; + } + + if (outputs[i] > max_output) { + float k = (max_output - outputs[i]) / desaturation_vector[i]; + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + + sat_status.flags.motor_pos = true; + } + } + + // Reduce the saturation as much as possible + return k_min + k_max; +} + +void +MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *outputs, + saturation_status &sat_status, float min_output, float max_output, bool reduce_only) const +{ + float k1 = compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); + + if (reduce_only && k1 > 0.f) { + return; + } + + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += k1 * desaturation_vector[i]; + } + + // Compute the desaturation gain again based on the updated outputs. + // In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output. + // In that case adding 0.5 of the gain will equilibrate saturations. + float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); + + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += k2 * desaturation_vector[i]; + } +} + +void +MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs) +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + thrust * _rotors[i].thrust_scale; + + // Thrust will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].thrust_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + // Mix yaw independently + mix_yaw(yaw, outputs); +} + +void +MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs) +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust * _rotors[i].thrust_scale; + + // Thrust will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].thrust_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].yaw_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); +} + +void +MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs) +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + thrust * _rotors[i].thrust_scale; + + // Thrust will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].thrust_scale; + } + + // only reduce thrust + minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].roll_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].pitch_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + // Mix yaw independently + mix_yaw(yaw, outputs); +} + +void MultirotorMixer::mix_yaw(float yaw, float *outputs) +{ + // Add yaw to outputs + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += yaw * _rotors[i].yaw_scale; + + // Yaw will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].yaw_scale; + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f); + + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].thrust_scale; + } + + // reduce thrust only + minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true); +} + +unsigned +MultirotorMixer::mix(float *outputs, unsigned space) +{ + if (space < _rotor_count) { + return 0; + } + + float roll = math::constrain(get_control(0, 0), -1.0f, 1.0f); + float pitch = math::constrain(get_control(0, 1), -1.0f, 1.0f); + float yaw = math::constrain(get_control(0, 2), -1.0f, 1.0f); + float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f); + + // clean out class variable used to capture saturation + _saturation_status.value = 0; + + // Do the mixing using the strategy given by the current Airmode configuration + switch (_airmode) { + case Airmode::roll_pitch: + mix_airmode_rp(roll, pitch, yaw, thrust, outputs); + break; + + case Airmode::roll_pitch_yaw: + mix_airmode_rpy(roll, pitch, yaw, thrust, outputs); + break; + + case Airmode::disabled: + default: // just in case: default to disabled + mix_airmode_disabled(roll, pitch, yaw, thrust, outputs); + break; + } + + // Apply thrust model and scale outputs to range [idle_speed, 1]. + // At this point the outputs are expected to be in [0, 1], but they can be outside, for example + // if a roll command exceeds the motor band limit. + for (unsigned i = 0; i < _rotor_count; i++) { + // Implement simple model for static relationship between applied motor pwm and motor thrust + // model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2 + if (_thrust_factor > 0.0f) { + outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) * + (1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] / + _thrust_factor)); + } + + outputs[i] = math::constrain((2.f * outputs[i] - 1.f), -1.f, 1.f); + } + + // Slew rate limiting and saturation checking + for (unsigned i = 0; i < _rotor_count; i++) { + bool clipping_high = false; + bool clipping_low_roll_pitch = false; + bool clipping_low_yaw = false; + + // Check for saturation against static limits. + // We only check for low clipping if airmode is disabled (or yaw + // clipping if airmode==roll/pitch), since in all other cases thrust will + // be reduced or boosted and we can keep the integrators enabled, which + // leads to better tracking performance. + if (outputs[i] < -0.99f) { + if (_airmode == Airmode::disabled) { + clipping_low_roll_pitch = true; + clipping_low_yaw = true; + + } else if (_airmode == Airmode::roll_pitch) { + clipping_low_yaw = true; + } + } + + // check for saturation against slew rate limits + if (_delta_out_max > 0.0f) { + float delta_out = outputs[i] - _outputs_prev[i]; + + if (delta_out > _delta_out_max) { + outputs[i] = _outputs_prev[i] + _delta_out_max; + clipping_high = true; + + } else if (delta_out < -_delta_out_max) { + outputs[i] = _outputs_prev[i] - _delta_out_max; + clipping_low_roll_pitch = true; + clipping_low_yaw = true; + } + } + + _outputs_prev[i] = outputs[i]; + + // update the saturation status report + update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw); + } + + // this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen + _delta_out_max = 0.0f; + + return _rotor_count; +} + +/* + * This function update the control saturation status report using the following inputs: + * + * index: 0 based index identifying the motor that is saturating + * clipping_high: true if the motor demand is being limited in the positive direction + * clipping_low_roll_pitch: true if the motor demand is being limited in the negative direction (roll/pitch) + * clipping_low_yaw: true if the motor demand is being limited in the negative direction (yaw) +*/ +void +MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, + bool clipping_low_yaw) +{ + // The motor is saturated at the upper limit + // check which control axes and which directions are contributing + if (clipping_high) { + if (_rotors[index].roll_scale > 0.0f) { + // A positive change in roll will increase saturation + _saturation_status.flags.roll_pos = true; + + } else if (_rotors[index].roll_scale < 0.0f) { + // A negative change in roll will increase saturation + _saturation_status.flags.roll_neg = true; + } + + // check if the pitch input is saturating + if (_rotors[index].pitch_scale > 0.0f) { + // A positive change in pitch will increase saturation + _saturation_status.flags.pitch_pos = true; + + } else if (_rotors[index].pitch_scale < 0.0f) { + // A negative change in pitch will increase saturation + _saturation_status.flags.pitch_neg = true; + } + + // check if the yaw input is saturating + if (_rotors[index].yaw_scale > 0.0f) { + // A positive change in yaw will increase saturation + _saturation_status.flags.yaw_pos = true; + + } else if (_rotors[index].yaw_scale < 0.0f) { + // A negative change in yaw will increase saturation + _saturation_status.flags.yaw_neg = true; + } + + // A positive change in thrust will increase saturation + _saturation_status.flags.thrust_pos = true; + } + + // The motor is saturated at the lower limit + // check which control axes and which directions are contributing + if (clipping_low_roll_pitch) { + // check if the roll input is saturating + if (_rotors[index].roll_scale > 0.0f) { + // A negative change in roll will increase saturation + _saturation_status.flags.roll_neg = true; + + } else if (_rotors[index].roll_scale < 0.0f) { + // A positive change in roll will increase saturation + _saturation_status.flags.roll_pos = true; + } + + // check if the pitch input is saturating + if (_rotors[index].pitch_scale > 0.0f) { + // A negative change in pitch will increase saturation + _saturation_status.flags.pitch_neg = true; + + } else if (_rotors[index].pitch_scale < 0.0f) { + // A positive change in pitch will increase saturation + _saturation_status.flags.pitch_pos = true; + } + + // A negative change in thrust will increase saturation + _saturation_status.flags.thrust_neg = true; + } + + if (clipping_low_yaw) { + // check if the yaw input is saturating + if (_rotors[index].yaw_scale > 0.0f) { + // A negative change in yaw will increase saturation + _saturation_status.flags.yaw_neg = true; + + } else if (_rotors[index].yaw_scale < 0.0f) { + // A positive change in yaw will increase saturation + _saturation_status.flags.yaw_pos = true; + } + } + + _saturation_status.flags.valid = true; +} diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp new file mode 100644 index 0000000000..d8e7363417 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * Supported multirotor geometries. + * + * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h + */ +typedef uint8_t MultirotorGeometryUnderlyingType; +enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; + +/** + * 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. + */ +class MultirotorMixer : public Mixer +{ +public: + /** + + * 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 */ + float thrust_scale; /**< scales thrust 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 idle_speed Minimum 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, MultirotorGeometry geometry); + + /** + * Constructor (for testing). + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param rotors control allocation matrix + * @param rotor_count length of rotors array (= number of motors) + */ + MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count); + virtual ~MultirotorMixer(); + + // no copy, assignment, move, move assignment + MultirotorMixer(const MultirotorMixer &) = delete; + MultirotorMixer &operator=(const MultirotorMixer &) = delete; + MultirotorMixer(MultirotorMixer &&) = delete; + MultirotorMixer &operator=(MultirotorMixer &&) = delete; + + /** + * 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 MultirotorMixer instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + + uint16_t get_saturation_status() override { return _saturation_status.value; } + + void groups_required(uint32_t &groups) override { groups |= (1 << 0); } + + /** + * @brief Update slew rate parameter. This tells the multicopter mixer + * the maximum allowed change of the output values per cycle. + * The value is only valid for one cycle, in order to have continuous + * slew rate limiting this function needs to be called before every call + * to mix(). + * + * @param[in] delta_out_max Maximum delta output. + * + */ + void set_max_delta_out_once(float delta_out_max) override { _delta_out_max = delta_out_max; } + + unsigned set_trim(float trim) override { return _rotor_count; } + unsigned get_trim(float *trim) override { return _rotor_count; } + + /** + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. + * + * @param[in] val The value + */ + void set_thrust_factor(float val) override { _thrust_factor = math::constrain(val, 0.0f, 1.0f); } + + void set_airmode(Airmode airmode) override { _airmode = airmode; } + + unsigned get_multirotor_count() override { return _rotor_count; } + + union saturation_status { + struct { + uint16_t valid : 1; // 0 - true when the saturation status is used + uint16_t motor_pos : 1; // 1 - true when any motor has saturated in the positive direction + uint16_t motor_neg : 1; // 2 - true when any motor has saturated in the negative direction + uint16_t roll_pos : 1; // 3 - true when a positive roll demand change will increase saturation + uint16_t roll_neg : 1; // 4 - true when a negative roll demand change will increase saturation + uint16_t pitch_pos : 1; // 5 - true when a positive pitch demand change will increase saturation + uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation + uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation + uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation + uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation + uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation + } flags; + uint16_t value; + }; + +private: + /** + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. + * @see also minimize_saturation(). + * + * @return desaturation gain + */ + float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status, + float min_output, float max_output) const; + + /** + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param outputs output vector that is modified + * @param sat_status saturation status output + * @param min_output minimum desired value in outputs + * @param max_output maximum desired value in outputs + * @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector + */ + void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status, + float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const; + + /** + * Mix roll, pitch, yaw, thrust and set the outputs vector. + * + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + */ + inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs); + + /** + * Mix roll, pitch, yaw, thrust and set the outputs vector. + * + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. + */ + inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs); + + /** + * Mix roll, pitch, yaw, thrust and set the outputs vector. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mix_yaw() for the exact yaw behavior. + */ + inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs); + + /** + * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + * + * @param yaw demanded yaw + * @param outputs output vector that is updated + */ + inline void mix_yaw(float yaw, float *outputs); + + void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw); + + float _delta_out_max{0.0f}; + float _thrust_factor{0.0f}; + + Airmode _airmode{Airmode::disabled}; + + saturation_status _saturation_status{}; + + unsigned _rotor_count; + const Rotor *_rotors; + + float *_outputs_prev{nullptr}; + float *_tmp_array{nullptr}; +}; diff --git a/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml new file mode 100644 index 0000000000..7470bff67b --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml @@ -0,0 +1,40 @@ +# Generic Dodecacopter in X coax configuration, bottom half + +[info] +key = "6a" +description = "Generic Dodecacopter in X coax configuration, bottom half" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "mid_right_bottom" +position = [0.0, 1.0, 0.1] +direction = "CCW" + +[[rotors]] +name = "mid_left_bottom" +position = [0.0, -1.0, 0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.866025, -0.5, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_right_bottom" +position = [-0.866025, 0.5, 0.1] +direction = "CW" + +[[rotors]] +name = "front_right_bottom" +position = [0.866025, 0.5, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_left_bottom" +position = [-0.866025, -0.5, 0.1] +direction = "CCW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml new file mode 100644 index 0000000000..e875240bac --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml @@ -0,0 +1,40 @@ +# Generic Dodecacopter in X coax configuration, top half + +[info] +key = "6m" +description = "Generic Dodecacopter in X coax configuration, top half" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "mid_right_top" +position = [0.0, 1.0, -0.1] +direction = "CW" + +[[rotors]] +name = "mid_left_top" +position = [0.0, -1.0, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.866025, -0.5, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_right_top" +position = [-0.866025, 0.5, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_right_top" +position = [0.866025, 0.5, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_left_top" +position = [-0.866025, -0.5, -0.1] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml new file mode 100644 index 0000000000..a344c16e1f --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in coaxial configuration + +[info] +key = "6c" +description = "Generic Hexacopter in coaxial configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.5, 0.866, -0.1] +direction = "CW" + +[[rotors]] +name = "front_right_bottom" +position = [0.5, 0.866, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_top" +position = [-1.0, 0.0, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_bottom" +position = [-1.0, 0.0, 0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.5, -0.866, -0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.5, -0.866, 0.1] +direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml new file mode 100644 index 0000000000..6b86cb40b6 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in + configuration + +[info] +key = "6+" +description = "Generic Hexacopter in + configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front" +position = [1.0, 0.0, 0.0] +direction = "CW" + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.5, -0.866025, 0.0] +direction = "CW" + +[[rotors]] +name = "front_right" +position = [0.5, 0.866025, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.5, -0.866025, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.5, 0.866025, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml new file mode 100644 index 0000000000..fc8c32b964 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in T configuration + +[info] +key = "6t" +description = "Generic Hexacopter in T configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.729, 0.684, 0.1] +direction = "CW" + +[[rotors]] +name = "front_right_bottom" +position = [0.729, 0.684, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_top" +position = [-1.0, 0.0, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_bottom" +position = [-1.0, 0.0, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.729, -0.684, 0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.729, -0.684, -0.1] +direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml new file mode 100644 index 0000000000..a546a83f72 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in X configuration + +[info] +key = "6x" +description = "Generic Hexacopter in X configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "mid_right" +position = [0.0, 1.0, 0.0] +direction = "CW" + +[[rotors]] +name = "mid_left" +position = [0.0, -1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.866025, -0.5, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_right" +position = [-0.866025, 0.5, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_right" +position = [0.866025, 0.5, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.866025, -0.5, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml new file mode 100644 index 0000000000..6c42bfa1f7 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in coax configuration + +[info] +key = "8c" +description = "GenericOctacopter in coax configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.707107, 0.707107, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.707107, -0.707107, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_left_top" +position = [-0.707107, -0.707107, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_right_top" +position = [-0.707107, 0.707107, -0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.707107, -0.707107, 0.1] +direction = "CCW" + +[[rotors]] +name = "front_right_bottom" +position = [0.707107, 0.707107, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_right_bottom" +position = [-0.707107, 0.707107, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_left_bottom" +position = [-0.707107, -0.707107, 0.1] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml new file mode 100644 index 0000000000..785c6a4c0e --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in wide coax configuration + +[info] +key = "8cw" +description = "Generic Octacopter in wide coax configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.3746066, 0.927184, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.3746066, -0.927184, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_left_top" +position = [-0.62932, -0.777146, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_right_top" +position = [-0.62932, 0.777146, -0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.3746066, -0.927184, 0.1] +direction = "CCW" + +[[rotors]] +name = "front_right_bottom" +position = [0.3746066, 0.927184, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_right_bottom" +position = [-0.62932, 0.777146, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_left_bottom" +position = [-0.62932, -0.777146, 0.1] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml new file mode 100644 index 0000000000..7c13bc728d --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in + configuration + +[info] +key = "8+" +description = "Generic Octacopter in + configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front" +position = [1.0, 0.0, 0.0] +direction = "CW" + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] +direction = "CW" + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "left" +position = [0.0, -1.0, 0.0] +direction = "CW" + +[[rotors]] +name = "right" +position = [0.0, 1.0, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml new file mode 100644 index 0000000000..e85403b032 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in X configuration + +[info] +key = "8x" +description = "Generic Octacopter in X configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.9238795, 0.3826834, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_left" +position = [-0.9238795, -0.3826834, 0.0] +direction = "CW" + +[[rotors]] +name = "mid_front_right" +position = [0.3826834, 0.9238795, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.9238795, 0.3826834, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.9238795, -0.3826834, 0.0] +direction = "CCW" + +[[rotors]] +name = "mid_rear_left" +position = [-0.3826834, -0.9238795, 0.0] +direction = "CCW" + +[[rotors]] +name = "mid_front_left" +position = [0.3826834, -0.9238795, 0.0] +direction = "CW" + +[[rotors]] +name = "mid_rear_right" +position = [-0.3826834, 0.9238795, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml new file mode 100644 index 0000000000..1e7febaef4 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml @@ -0,0 +1,30 @@ +# SK450 DeadCat Quadcopter. +# Same geometry as quad_wide, except CG is located at intersection of rear arms, so front motors are more loaded. + +[info] +key = "4dc" +description = "SK450 DeadCat Quadcopter, CG at intersection of rear arms" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.01 + +[[rotors]] +name = "front_right" +position = [0.1155, 0.245, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.1875, -0.1875, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.1155, -0.245, 0.0] + +[[rotors]] +name = "rear_right" +position = [-0.1875, 0.1875, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml new file mode 100644 index 0000000000..e58b5a5ed6 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml @@ -0,0 +1,30 @@ +# Generic Quadcopter in H configuration + +[info] +key = "4h" +description = "Generic Quadcopter in H configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] +direction = "CCW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml new file mode 100644 index 0000000000..e28f8db7f0 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml @@ -0,0 +1,29 @@ +# Generic Quadcopter in + configuration + +[info] +key = "4+" +description = "Generic Quadcopter in + configuration" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "right" +position = [0.0, 1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "left" +position = [0.0, -1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "front" +position = [1.0, 0.0, 0.0] + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml new file mode 100644 index 0000000000..f6c360bc21 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml @@ -0,0 +1,30 @@ +# Spedix S250AQ Quadcopter +# Note: Rotor positions computed from specifications: length:210mm, width:260mm, wheel base:280mm + +[info] +key = "4s" +description = "Spedix S250AQ Quadcopter" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.01 + +[[rotors]] +name = "front_right" +position = [0.105, 0.130, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.105, -0.0552, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.105, -0.130, 0.0] + +[[rotors]] +name = "rear_right" +position = [-0.105, 0.0552, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml new file mode 100644 index 0000000000..3b934018db --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml @@ -0,0 +1,33 @@ +# Quadcopter in Y configuration with rear props tilted at 45 degrees + +[info] +key = "4vt" +description = "Quadcopter in Y configuration with rear props tilted at 45 degrees" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.2, 0.2, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_left" +position = [-0.3, -0.1, -0.1] +axis = [0.0, 0.707106, -0.707106] +direction = "CW" + +[[rotors]] +name = "front_left" +position = [0.2, -0.2, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.3, 0.1, -0.1] +axis = [0.0, -0.707106, -0.707106] +direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml new file mode 100644 index 0000000000..d2fde7861b --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml @@ -0,0 +1,30 @@ +# Generic Quadcopter in wide configuration + +[info] +key = "4w" +description = "Quadcopter in wide configuration. Same geometry as SK450 Deadcat except the CG is moved backward to load all motors equally" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.1515, 0.245, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.1515, -0.1875, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.1515, -0.245, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_right" +position = [-0.1515, 0.1875, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml new file mode 100644 index 0000000000..b4cf484c21 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml @@ -0,0 +1,29 @@ +# Generic Quadcopter in X configuration + +[info] +key = "4x" +description = "Generic Quadcopter in X configuration" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml new file mode 100644 index 0000000000..1af7882b23 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml @@ -0,0 +1,30 @@ +# Generic Quadcopter in X configuration +# with clock-wise motor numbering + +[info] +key = "4xcw" +description = "Quadcopter in X configuration with clock-wise motor numbering" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml new file mode 100644 index 0000000000..a63ff0652b --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml @@ -0,0 +1,37 @@ +# Quadcopter in X configuration, +# with added pusher motor in the back + +[info] +key = "4x1p" +description = "Quadcopter in X configuration, with added pusher motor in the back" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [1.0, 1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-1.0, -1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [1.0, -1.0, 0.0] + +[[rotors]] +name = "rear_right" +position = [-1.0, 1.0, 0.0] + +[[rotors]] +name = "pusher" +position = [-1.0, 0.0, 0.0] +axis = [1.0, 0.0, 0.0] +Ct = 2.0 +Cm = 0.0 diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml new file mode 100644 index 0000000000..b75756c8d9 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml @@ -0,0 +1,31 @@ +# Quadcopter in Y configuration with coax rear props + +[info] +key = "4y" +description = "Quadcopter in Y configuration with coax rear props" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.2, 0.2, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_top" +position = [-0.2, 0.0, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.2, -0.2, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_bottom" +position = [-0.2, 0.0, 0.1] +direction = "CW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py new file mode 100755 index 0000000000..932a265fb4 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py @@ -0,0 +1,398 @@ +#!/usr/bin/env python +############################################################################# +# +# Copyright (C) 2013-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. +# +############################################################################# + +""" +px_generate_mixers.py +Generates c/cpp header/source files for multirotor mixers +from geometry descriptions files (.toml format) +""" +import sys + +try: + import toml +except ImportError as e: + print("Failed to import toml: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user toml") + print("") + sys.exit(1) + +try: + import numpy as np +except ImportError as e: + print("Failed to import numpy: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user numpy") + print("") + sys.exit(1) + +__author__ = "Julien Lecoeur" +__copyright__ = "Copyright (C) 2013-2017 PX4 Development Team." +__license__ = "BSD" +__email__ = "julien.lecoeur@gmail.com" + + +def parse_geometry_toml(filename): + ''' + Parses toml geometry file and returns a dictionary with curated list of rotors + ''' + import os + + # Load toml file + d = toml.load(filename) + + # Check info section + if 'info' not in d: + raise AttributeError('{}: Error, missing info section'.format(filename)) + + # Check info section + for field in ['key', 'description']: + if field not in d['info']: + raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field)) + + # Use filename as mixer name + d['info']['name'] = os.path.basename(filename).split('.')[0].lower() + + # Store filename + d['info']['filename'] = filename + + # Check default rotor config + if 'rotor_default' in d: + default = d['rotor_default'] + else: + default = {} + + # Convert rotors + rotor_list = [] + if 'rotors' in d: + for r in d['rotors']: + # Make sure all fields are defined, fill missing with default + for field in ['name', 'position', 'axis', 'direction', 'Ct', 'Cm']: + if field not in r: + if field in default: + r[field] = default[field] + else: + raise AttributeError('{}: Error, unspecified field "{}" for rotor "{}"' + .format(filename, field, r['name'])) + + # Check direction field + r['direction'] = r['direction'].upper() + if r['direction'] not in ['CW', 'CCW']: + raise AttributeError('{}: Error, invalid direction value "{}" for rotor "{}"' + .format(filename, r['direction'], r['name'])) + + # Check vector3 fields + for field in ['position', 'axis']: + if len(r[field]) != 3: + raise AttributeError('{}: Error, field "{}" for rotor "{}"' + .format(filename, field, r['name']) + + ' must be an array of length 3') + + # Add rotor to list + rotor_list.append(r) + + # Clean dictionary + geometry = {'info': d['info'], + 'rotors': rotor_list} + + return geometry + +def torque_matrix(center, axis, dirs, Ct, Cm): + ''' + Compute torque generated by rotors + ''' + # normalize rotor axis + ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] + torque = Ct * np.cross(center, ax) - Cm * ax * dirs + return torque + +def geometry_to_torque_matrix(geometry): + ''' + Compute torque matrix Am and Bm from geometry dictionnary + Am is a 3xN matrix where N is the number of rotors + Each column is the torque generated by one rotor + ''' + Am = torque_matrix(center=np.array([rotor['position'] for rotor in geometry['rotors']]), + axis=np.array([rotor['axis'] for rotor in geometry['rotors']]), + dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0] + for rotor in geometry['rotors']]), + Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']]), + Cm=np.array([[rotor['Cm']] for rotor in geometry['rotors']])).T + return Am + +def thrust_matrix(axis, Ct): + ''' + Compute thrust generated by rotors + ''' + # Normalize rotor axis + ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] + thrust = Ct * ax + return thrust + +def geometry_to_thrust_matrix(geometry): + ''' + Compute thrust matrix At from geometry dictionnary + At is a 3xN matrix where N is the number of rotors + Each column is the thrust generated by one rotor + ''' + At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geometry['rotors']]), + Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T + + return At + +def geometry_to_mix(geometry): + ''' + Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary + + A is a 6xN matrix where N is the number of rotors + Each column is the torque and thrust generated by one rotor + + B is a Nx6 matrix where N is the number of rotors + Each column is the command to apply to the servos to get + roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust + ''' + # Combined torque & thrust matrix + At = geometry_to_thrust_matrix(geometry) + Am = geometry_to_torque_matrix(geometry) + A = np.vstack([Am, At]) + + # Mix matrix computed as pseudoinverse of A + B = np.linalg.pinv(A) + + return A, B + +def normalize_mix_px4(B): + ''' + Normalize mix for PX4 + This is for compatibility only and should ideally not be used + ''' + B_norm = np.linalg.norm(B, axis=0) + B_max = np.abs(B).max(axis=0) + B_sum = np.sum(B, axis=0) + + # Same scale on roll and pitch + B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0) + B_norm[1] = B_norm[0] + + # Scale yaw separately + B_norm[2] = B_max[2] + + # Same scale on x, y + B_norm[3] = max(B_max[3], B_max[4]) + B_norm[4] = B_norm[3] + + # Scale z thrust separately + B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5]) + + # Normalize + B_norm[np.abs(B_norm) < 1e-3] = 1 + B_px = (B / B_norm) + + return B_px + +def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False): + ''' + Generate C header file with same format as multi_tables.py + TODO: rewrite using templates (see generation of uORB headers) + ''' + from io import StringIO + buf = StringIO() + + # Print Header + buf.write(u"/*\n") + buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n") + buf.write(u"*/\n") + buf.write(u"\n") + buf.write(u"#ifndef _MIXER_MULTI_TABLES\n") + buf.write(u"#define _MIXER_MULTI_TABLES\n") + buf.write(u"\n") + + # Print enum + buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n") + for i, geometry in enumerate(geometries_list): + buf.write(u"\t{},{}// {} (text key {})\n".format( + geometry['info']['name'].upper(), ' ' * (max(0, 30 - len(geometry['info']['name']))), + geometry['info']['description'], geometry['info']['key'])) + buf.write(u"\n\tMAX_GEOMETRY\n") + buf.write(u"}; // enum class MultirotorGeometry\n\n") + + # Print mixer gains + buf.write(u"namespace {\n") + for geometry in geometries_list: + # Get desired mix matrix + if use_normalized_mix: + mix = geometry['mix']['B_px'] + else: + mix = geometry['mix']['B'] + + buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name'])) + + for row in mix: + if use_6dof: + # 6dof mixer + buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + row[0], row[1], row[2], + row[3], row[4], row[5])) + else: + # 4dof mixer + buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + row[0], row[1], row[2], + -row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly + + buf.write(u"};\n\n") + + # Print geometry indeces + buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n") + for geometry in geometries_list: + buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name'])) + buf.write(u"};\n\n") + + # Print geometry rotor counts + buf.write(u"static constexpr unsigned _config_rotor_count[] {\n") + for geometry in geometries_list: + buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name'])) + buf.write(u"};\n\n") + + # Print geometry key + buf.write(u"const char* _config_key[] {\n") + for geometry in geometries_list: + buf.write(u"\t\"{}\",\t/* {} */\n".format(geometry['info']['key'], geometry['info']['name'])) + buf.write(u"};\n\n") + + # Print footer + buf.write(u"} // anonymous namespace\n\n") + buf.write(u"#endif /* _MIXER_MULTI_TABLES */\n\n") + + return buf.getvalue() + + +if __name__ == '__main__': + import argparse + import glob + import os + + # Parse arguments + parser = argparse.ArgumentParser( + description='Convert geometry .toml files to mixer headers') + parser.add_argument('-d', dest='dir', + help='directory with geometry files') + parser.add_argument('-f', dest='files', + help="files to convert (use only without -d)", + nargs="+") + parser.add_argument('-o', dest='outputfile', + help='output header file') + parser.add_argument('--verbose', help='Print details on standard output', + action='store_true') + parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)', + action='store_true') + parser.add_argument('--sixdof', help='Use 6dof mixers', + action='store_true') + args = parser.parse_args() + + # Find toml files + if args.files is not None: + filenames = args.files + elif args.dir is not None: + filenames = glob.glob(os.path.join(args.dir, '*.toml')) + else: + parser.print_usage() + raise Exception("Missing input directory (-d) or list of geometry files (-f)") + + # List of geometries + geometries_list = [] + + for filename in filenames: + # Parse geometry file + geometry = parse_geometry_toml(filename) + + # Compute torque and thrust matrices + A, B = geometry_to_mix(geometry) + + # Normalize mixer + B_px = normalize_mix_px4(B) + + # Store matrices in geometry + geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px} + + # Add to list + geometries_list.append(geometry) + + if args.verbose: + print('\nFilename') + print(filename) + print('\nGeometry') + print(geometry) + print('\nA:') + print(A.round(2)) + print('\nB:') + print(B.round(2)) + print('\nNormalized Mix (as in PX4):') + print(B_px.round(2)) + print('\n-----------------------------') + + # Check that there are no duplicated mixer names or keys + for i in range(len(geometries_list)): + name_i = geometries_list[i]['info']['name'] + key_i = geometries_list[i]['info']['key'] + + for j in range(i + 1, len(geometries_list)): + name_j = geometries_list[j]['info']['name'] + key_j = geometries_list[j]['info']['key'] + + # Mixers cannot share the same name + if name_i == name_j: + raise ValueError('Duplicated mixer name "{}" in files {} and {}'.format( + name_i, + geometries_list[i]['info']['filename'], + geometries_list[j]['info']['filename'])) + + # Mixers cannot share the same key + if key_i == key_j: + raise ValueError('Duplicated mixer key "{}" for mixers "{}" and "{}"'.format( + key_i, name_i, name_j)) + + # Generate header file + header = generate_mixer_multirotor_header(geometries_list, + use_normalized_mix=args.normalize, + use_6dof=args.sixdof) + + if args.outputfile is not None: + # Write header file + with open(args.outputfile, 'w') as fd: + fd.write(header) + else: + # Print to standard output + print(header) diff --git a/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml b/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml new file mode 100644 index 0000000000..b41dbd8cbd --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml @@ -0,0 +1,23 @@ +# Tri Y + +[info] +key = "3y" +description = "Tri Y" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.0 +direction = "CW" + +[[rotors]] +name = "front_right" +position = [0.5, 0.866025, 0.0] + +[[rotors]] +name = "front_left" +position = [0.5, -0.866025, 0.0] + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml b/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml new file mode 100644 index 0000000000..6a547a1654 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml @@ -0,0 +1,19 @@ +# Twin engine + +[info] +key = "2-" +description = "Twin engine" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.0 +direction = "CW" + +[[rotors]] +name = "right" +position = [0.0, 1.0, 0.0] + +[[rotors]] +name = "left" +position = [0.0, -1.0, 0.0] \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/mixer_multirotor.py b/src/lib/mixer/MultirotorMixer/mixer_multirotor.py new file mode 100755 index 0000000000..b3badaade3 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/mixer_multirotor.py @@ -0,0 +1,386 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +Mixer multirotor test and prototyping script. + +Author: Mathieu Bresciani , Beat Kueng +Description: This script can be used to prototype new mixer algorithms and test +it against the C++ implementation. +""" + + +from __future__ import print_function + +from argparse import ArgumentParser +import numpy as np +import numpy.matlib +import subprocess + + +# -------------------------------------------------- +# mixing algorithms +# -------------------------------------------------- + +def compute_desaturation_gain(u, u_min, u_max, desaturation_vector): + """ + Computes the gain k by which desaturation_vector has to be multiplied + in order to unsaturate the output that has the greatest saturation + """ + d_u_sat_plus = u_max - u + d_u_sat_minus = u_min - u + k = np.zeros(u.size*2) + for i in range(u.size): + if abs(desaturation_vector[i]) < 0.000001: + # avoid division by zero + continue + + if d_u_sat_minus[i] > 0.0: + k[2*i] = d_u_sat_minus[i] / desaturation_vector[i] + if d_u_sat_plus[i] < 0.0: + k[2*i+1] = d_u_sat_plus[i] / desaturation_vector[i] + + k_min = min(k) + k_max = max(k) + + # Reduce the saturation as much as possible + k = k_min + k_max + return k + + +def minimize_sat(u, u_min, u_max, desaturation_vector): + """ + Minimize the saturation of the actuators by + adding or substracting a fraction of desaturation_vector. + desaturation_vector is the vector that added to the output u, + modifies the thrust or angular acceleration on a + specific axis. + For example, if desaturation_vector is given + to slide along the vertical thrust axis, the saturation will + be minimized by shifting the vertical thrust setpoint, + without changing the roll/pitch/yaw accelerations. + """ + k_1 = compute_desaturation_gain(u, u_min, u_max, desaturation_vector) + u_1 = u + k_1 * desaturation_vector # Try to unsaturate + + + # Compute the desaturation gain again based on the updated outputs. + # In most cases it will be zero. It won't be if max(outputs) - min(outputs) + # > max_output - min_output. + # In that case adding 0.5 of the gain will equilibrate saturations. + k_2 = compute_desaturation_gain(u_1, u_min, u_max, desaturation_vector) + + k_opt = k_1 + 0.5 * k_2 + + u_prime = u + k_opt * desaturation_vector + return u_prime + +def mix_yaw(m_sp, u, P, u_min, u_max): + """ + Mix yaw by adding it to an existing output vector u + + Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + some yaw control on the upper end. On the lower end thrust will never be increased, + but yaw is decreased as much as required. + """ + m_sp_yaw_only = np.matlib.zeros(m_sp.size).T + m_sp_yaw_only[2, 0] = m_sp[2, 0] + u_p = u + P * m_sp_yaw_only + + # Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + # and allow some yaw response at maximum thrust + u_r_dot = P[:,2] + u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot) + u_T = P[:, 3] + u_ppp = minimize_sat(u_pp, 0, u_max, u_T) + # reduce thrust only + if (u_ppp > (u_pp)).any(): + u_ppp = u_pp + return u_ppp + +def airmode_rp(m_sp, P, u_min, u_max): + """ + Mix roll, pitch, yaw and thrust. + + Desaturation behavior: airmode for roll/pitch: + thrust is increased/decreased as much as required to meet the demanded roll/pitch. + Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + """ + # Mix without yaw + m_sp_no_yaw = m_sp.copy() + m_sp_no_yaw[2, 0] = 0.0 + u = P * m_sp_no_yaw + + # Use thrust to unsaturate the outputs if needed + u_T = P[:, 3] + u_prime = minimize_sat(u, u_min, u_max, u_T) + + # Mix yaw axis independently + u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max) + + return (u, u_final) + + +def airmode_rpy(m_sp, P, u_min, u_max): + """ + Mix roll, pitch, yaw and thrust. + + Desaturation behavior: full airmode for roll/pitch/yaw: + thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw. + """ + # Mix with yaw + u = P * m_sp + + # Use thrust to unsaturate the outputs if needed + u_T = P[:, 3] + u_prime = minimize_sat(u, u_min, u_max, u_T) + + # Unsaturate yaw (in case upper and lower bounds are exceeded) + # to prioritize roll/pitch over yaw. + u_T = P[:, 2] + u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T) + + return (u, u_prime_yaw) + + +def normal_mode(m_sp, P, u_min, u_max): + """ + Mix roll, pitch, yaw and thrust. + + Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + Thrust can be reduced to unsaturate the upper side. + @see mix_yaw() for the exact yaw behavior. + """ + # Mix without yaw + m_sp_no_yaw = m_sp.copy() + m_sp_no_yaw[2, 0] = 0.0 + u = P * m_sp_no_yaw + + # Use thrust to unsaturate the outputs if needed + # by reducing the thrust only + u_T = P[:, 3] + u_prime = minimize_sat(u, u_min, u_max, u_T) + if (u_prime > (u)).any(): + u_prime = u + + # Reduce roll/pitch acceleration if needed to unsaturate + u_p_dot = P[:, 0] + u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot) + u_q_dot = P[:, 1] + u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot) + + # Mix yaw axis independently + u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max) + return (u, u_final) + + +# -------------------------------------------------- +# test cases +# -------------------------------------------------- + +# normalized control allocation test matrices (B_px from px_generate_mixers.py) +# quad_x +P1 = np.matrix([ + [-0.25, 0.25, 0.25, 1], + [-0.25, -0.25, -0.25, 1], + [0.25, -0.25, 0.25, 1], + [0.25, 0.25, -0.25, 1]]) +# quad_wide +P2 = np.matrix([ + [-0.5, 0.71, 0.77, 1. ], + [ 0.5, -0.71, 1., 1. ], + [ 0.5, 0.71, -0.77, 1. ], + [-0.5, -0.71, -1., 1. ]]) +# hex_x +P3 = np.matrix([ + [-1., 0., -1., 1. ], + [ 1., -0., 1., 1. ], + [ 0.5, 0.87, -1., 1. ], + [-0.5, -0.87, 1., 1. ], + [-0.5, 0.87, 1., 1. ], + [ 0.5, -0.87, -1., 1. ]]) +# hex_cox +P4 = np.matrix([ + [-0.87, 0.5, -1., 1. ], + [-0.87, 0.5, 1., 1. ], + [ 0., -1., -1., 1. ], + [ 0., -1., 1., 1. ], + [ 0.87, 0.5, -1., 1. ], + [ 0.87, 0.5, 1., 1. ]]) +# octa_plus +P5 = np.matrix([ + [-0., 1., -1., 1. ], + [ 0., -1., -1., 1. ], + [-0.71, 0.71, 1., 1. ], + [-0.71, -0.71, 1., 1. ], + [ 0.71, 0.71, 1., 1. ], + [ 0.71, -0.71, 1., 1. ], + [ 1., 0., -1., 1. ], + [-1., -0., -1., 1. ]]) + +P_tests = [ P1 ] + +test_cases_input = np.matrix([ + # desired accelerations (must be within [-1, 1]): + #roll pitch yaw thrust + [ 0.0, 0.0, 0.0, 0.0], + [-0.05, 0.0, 0.0, 0.0], + [ 0.05, -0.05, 0.0, 0.0], + [ 0.05, 0.05, -0.025, 0.0], + [ 0.0, 0.2, -0.025, 0.0], + [ 0.2, 0.05, 0.09, 0.0], + [-0.125, 0.02, 0.04, 0.0], + + # extreme cases + [ 1.0, 0.0, 0.0, 0.0], + [ 0.0, -1.0, 0.0, 0.0], + [ 0.0, 0.0, 1.0, 0.0], + [ 1.0, 1.0, -1.0, 0.0], + [-1.0, 0.9, -0.9, 0.0], + [-1.0, 0.9, 0.0, 0.0], + ]) + +# use the following thrust values for all test cases (must be within [0, 1]) +thrust_values = [0, 0.1, 0.45, 0.9, 1.0] +test_cases = np.zeros((test_cases_input.shape[0] * len(thrust_values), 4)) +for i in range(test_cases_input.shape[0]): + for k in range(len(thrust_values)): + test_case = test_cases_input[i] + test_case[0, 3] = thrust_values[k] + test_cases[i * len(thrust_values) + k, :] = test_case + + +def run_tests(mixer_cb, P, test_mixer_binary, test_index=None): + """ + Run all (or a specific) tests for a certain mixer method an control + allocation matrix P + """ + B = np.linalg.pinv(P) + proc = subprocess.Popen( + test_mixer_binary, + #'cat > /tmp/test_'+str(mode_idx), shell=True, # just to test the output + #stdout=subprocess.PIPE, + stdin=subprocess.PIPE) + proc.stdin.write("{:}\n".format(mode_idx).encode('utf-8')) # airmode + motor_count = P.shape[0] + proc.stdin.write("{:}\n".format(motor_count).encode('utf-8')) # motor count + # control allocation matrix + for row in P.getA(): + for col in row: + proc.stdin.write("{:.8f} ".format(col).encode('utf-8')) + proc.stdin.write("\n".encode('utf-8')) + proc.stdin.write("\n".encode('utf-8')) + + failed = False + try: + if test_index is None: + # go through all test cases + test_indices = range(test_cases.shape[0]) + else: + test_indices = [test_index] + for i in test_indices: + actuator_controls = test_cases[[i], :].T + proc.stdin.write("{:.8f} {:.8f} {:.8f} {:.8f}\n" + .format(actuator_controls[0, 0], actuator_controls[1, 0], + actuator_controls[2, 0], actuator_controls[3, 0]).encode('utf-8')) + + (u, u_new) = mixer_cb(actuator_controls, P, 0.0, 1.0) + # Saturate the outputs between 0 and 1 + u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T) + u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T) + # write expected outputs + for j in range(motor_count): + proc.stdin.write("{:.8f} ".format(u_new_sat[j, 0]).encode('utf-8')) + proc.stdin.write("\n".encode('utf-8')) + + proc.stdin.close() + except IOError as e: + failed = True + #result = proc.stdout.read() + proc.wait() + if proc.returncode != 0: failed = True + print("B:\n{}".format(B)) + print("P:\n{}".format(P)) + #print(result) + +parser = ArgumentParser(description=__doc__) +parser.add_argument('--test', action='store_true', default=False, help='Run tests') +parser.add_argument("--mixer-multirotor-binary", + help="select test_mixer_multirotor binary file name", + default='./test_mixer_multirotor') +parser.add_argument("--mode", "-m", dest="mode", + help="mixer mode: none, rp, rpy", default=None) +parser.add_argument("-i", dest="index", type=int, + help="Select a single test to run (starting at 1)", default=None) + +args = parser.parse_args() +mixer_mode = args.mode + +if args.test: + mixer_binary = args.mixer_multirotor_binary + test_index = args.index + if test_index is not None: test_index -= 1 + for mode_idx, (airmode, mixer_cb) in enumerate([ + ('none', normal_mode), + ('rp', airmode_rp), + ('rpy', airmode_rpy)]): + if mixer_mode is not None and mixer_mode != airmode: + continue + print('Testing mode: '+airmode) + for P in P_tests: + run_tests(mixer_cb, P, mixer_binary, test_index) + exit(1) + +# -------------------------------------------------- +# Prototyping and corner case testing playground +# -------------------------------------------------- + +# Compute the control allocation matrix +# u = P * m +P = P1 # normal quad +#P = P2 # wide quad + +# Normalized actuator effectiveness matrix using the pseudo inverse of P +# m = B * u +B = np.linalg.pinv(P) + +# Desired accelerations (actuator controls, in [-1, 1]) +p_dot_sp = 0.0 # roll acceleration (p is the roll rate) +q_dot_sp = 0.1 # pitch acceleration +r_dot_sp = 0.1 # yaw acceleration +T_sp = 0.0 # vertical thrust +m_sp = np.matrix([p_dot_sp, q_dot_sp, r_dot_sp, T_sp]).T # Vector of desired "accelerations" + +# Airmode type (none/rp/rpy) +airmode = mixer_mode +if airmode is None: airmode = "none" + +# Actuators output saturations +u_max = 1.0 +u_min = 0.0 + +if airmode == "none": + (u, u_new) = normal_mode(m_sp, P, u_min, u_max) +elif airmode == "rp": + (u, u_new) = airmode_rp(m_sp, P, u_min, u_max) +elif airmode == "rpy": + (u, u_new) = airmode_rpy(m_sp, P, u_min, u_max) +else: + u = 0.0 + u_new = 0.0 + +# Saturate the outputs between 0 and 1 +u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T) +u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T) + +np.set_printoptions(suppress=True) + +# Display some results +print("u = \n{}\n".format(u)) +print("u_new = \n{}\n".format(u_new)) +print("u_new_sat = \n{}\n".format(u_new_sat)) +print("Desired accelerations = \n{}\n".format(m_sp)) +# Compute back the allocated accelerations +m_new = B * u_new_sat +print("Allocated accelerations = \n{}\n".format(m_new)) diff --git a/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp b/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp new file mode 100644 index 0000000000..aebfe48304 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +/** + * testing binary that runs the multirotor mixer through test cases given + * via file or stdin and compares the mixer output against expected values. + */ + +#include "MultirotorMixer.hpp" + +#include +#include + +static const unsigned output_max = 16; +static float actuator_controls[output_max] {}; + +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + +static int +mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control) +{ + control = actuator_controls[control_index]; + return 0; +} + +int main(int argc, char *argv[]) +{ + FILE *file_in = stdin; + + if (argc > 1) { + file_in = fopen(argv[1], "r"); + } + + unsigned rotor_count = 0; + MultirotorMixer::Rotor rotors[output_max]; + float actuator_outputs[output_max]; + + // read airmode + int airmode; + fscanf(file_in, "%i", &airmode); + + // read the motor count & control allocation matrix + fscanf(file_in, "%i", &rotor_count); + + if (rotor_count > output_max) { + return -1; + } + + for (unsigned i = 0; i < rotor_count; ++i) { + fscanf(file_in, "%f %f %f %f", &rotors[i].roll_scale, &rotors[i].pitch_scale, + &rotors[i].yaw_scale, &rotors[i].thrust_scale); + } + + MultirotorMixer mixer(mixer_callback, 0, rotors, rotor_count); + mixer.set_airmode((Mixer::Airmode)airmode); + + int test_counter = 0; + int num_failed = 0; + + while (!feof(file_in)) { + + // read actuator controls + unsigned count = 0; + + while (count < 4 && fscanf(file_in, "%f", &actuator_controls[count]) == 1) { + ++count; + } + + if (count < 4) { + break; + } + + // do the mixing + if (mixer.mix(actuator_outputs, output_max) != rotor_count) { + return -1; + } + + // Account for MultirotorMixer outputing [-1,1] and python script [0,1] + for (unsigned i = 0; i < rotor_count; i++) { + actuator_outputs[i] = (actuator_outputs[i] + 1.f) * .5f; + } + + // read expected outputs + count = 0; + float expected_output[output_max]; + bool failed = false; + + while (count < rotor_count && fscanf(file_in, "%f", &expected_output[count]) == 1) { + if (fabsf(expected_output[count] - actuator_outputs[count]) > 0.00001f) { + failed = true; + } + + ++count; + } + + if (count < rotor_count) { + break; + } + + printf("EXPECT_EQ(allocate(%.3ff, %.3ff, %.3ff, -%.3ff), Vector4f(%.6ff, %.6ff, %.6ff, %.6ff)); // %i\n", + (double)actuator_controls[0], (double)actuator_controls[1], (double)actuator_controls[2], (double)actuator_controls[3], + (double)expected_output[0], (double)expected_output[1], (double)expected_output[2], (double)expected_output[3], + test_counter + 1); + + if (failed) { + printf("test %i failed:\n", test_counter + 1); + printf("control input : %.3f %.3f %.3f %.3f\n", (double)actuator_controls[0], (double)actuator_controls[1], + (double)actuator_controls[2], (double)actuator_controls[3]); + printf("mixer output : "); + + for (unsigned i = 0; i < rotor_count; ++i) { + printf("%.3f ", (double)actuator_outputs[i]); + } + + printf("\n"); + printf("expected output: "); + + for (unsigned i = 0; i < rotor_count; ++i) { + printf("%.3f ", (double)expected_output[i]); + } + + printf("\n"); + printf("diff : "); + + for (unsigned i = 0; i < rotor_count; ++i) { + printf("%.3f ", (double)(expected_output[i] - actuator_outputs[i])); + } + + printf("\n"); + ++num_failed; + } + + ++test_counter; + } + + printf("tested %i cases: %i success, %i failed\n", test_counter, + test_counter - num_failed, num_failed); + + + if (file_in != stdin) { + fclose(file_in); + } + + return num_failed > 0 ? -1 : 0; +} diff --git a/src/lib/mixer/NullMixer/CMakeLists.txt b/src/lib/mixer/NullMixer/CMakeLists.txt new file mode 100644 index 0000000000..6b2eeb3bbd --- /dev/null +++ b/src/lib/mixer/NullMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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. +# +############################################################################ + +add_library(NullMixer + NullMixer.cpp + NullMixer.hpp +) +target_link_libraries(NullMixer PRIVATE MixerBase) +add_dependencies(NullMixer prebuild_targets) diff --git a/src/lib/mixer/NullMixer/NullMixer.cpp b/src/lib/mixer/NullMixer/NullMixer.cpp new file mode 100644 index 0000000000..636cddd62d --- /dev/null +++ b/src/lib/mixer/NullMixer/NullMixer.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 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. + * + ****************************************************************************/ + +#include "NullMixer.hpp" + +#include +#include +#include + +unsigned +NullMixer::mix(float *outputs, unsigned space) +{ + if (space > 0) { + *outputs = NAN; + return 1; + } + + return 0; +} + +NullMixer * +NullMixer::from_text(const char *buf, unsigned &buflen) +{ + NullMixer *nm = nullptr; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { + nm = new NullMixer; + buflen -= 2; + } + + return nm; +} diff --git a/src/lib/mixer/NullMixer/NullMixer.hpp b/src/lib/mixer/NullMixer/NullMixer.hpp new file mode 100644 index 0000000000..4f098298b3 --- /dev/null +++ b/src/lib/mixer/NullMixer/NullMixer.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * Null mixer; returns zero. + * + * Used as a placeholder for output channels that are unassigned in groups. + */ +class NullMixer : public Mixer +{ +public: + NullMixer() : Mixer(nullptr, 0) {} + virtual ~NullMixer() = default; + + // no copy, assignment, move, move assignment + NullMixer(const NullMixer &) = delete; + NullMixer &operator=(const NullMixer &) = delete; + NullMixer(NullMixer &&) = delete; + NullMixer &operator=(NullMixer &&) = delete; + + /** + * 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 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 NullMixer instance, or nullptr + * if the text format is bad. + */ + static NullMixer *from_text(const char *buf, unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + + unsigned set_trim(float trim) override { return 1; } + unsigned get_trim(float *trim) override { return 1; } + +}; diff --git a/src/lib/mixer/SimpleMixer/CMakeLists.txt b/src/lib/mixer/SimpleMixer/CMakeLists.txt new file mode 100644 index 0000000000..ef01747d26 --- /dev/null +++ b/src/lib/mixer/SimpleMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 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. +# +############################################################################ + +add_library(SimpleMixer + SimpleMixer.cpp + SimpleMixer.hpp +) +target_link_libraries(SimpleMixer PRIVATE MixerBase) +add_dependencies(SimpleMixer prebuild_targets) diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.cpp b/src/lib/mixer/SimpleMixer/SimpleMixer.cpp new file mode 100644 index 0000000000..270b77ffcf --- /dev/null +++ b/src/lib/mixer/SimpleMixer/SimpleMixer.cpp @@ -0,0 +1,390 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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_simple.cpp + * + * Simple summing mixer. + */ + +#include "SimpleMixer.hpp" + +#include +#include + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _pinfo(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_pinfo != nullptr) { + free(_pinfo); + } +} + +unsigned SimpleMixer::set_trim(float trim) +{ + _pinfo->output_scaler.offset = trim; + return 1; +} + +unsigned SimpleMixer::get_trim(float *trim) +{ + *trim = _pinfo->output_scaler.offset; + return 1; +} + +void +SimpleMixer::set_dt_once(float dt) +{ + _dt = dt; +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time) +{ + int ret; + int s[6]; + int n = -1; + + buf = findtag(buf, buflen, 'O'); + + if ((buf == nullptr) || (buflen < 12)) { + debug("output parser failed finding tag, ret: '%s'", buf); + return -1; + } + + if ((ret = sscanf(buf, "O: %d %d %d %d %d %d %n", + &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &n)) < 5) { + debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); + return -1; + } + + // set slew rate limit to 0 if no 6th number is specified in mixer file + if (ret == 5) { + s[5] = 0; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } + + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + slew_rate_rise_time = s[5] / 10000.0f; + + return 0; +} + +int +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index) +{ + unsigned u[2]; + int s[5]; + + buf = findtag(buf, buflen, 'S'); + + if ((buf == nullptr) || (buflen < 16)) { + debug("control parser failed finding tag, ret: '%s'", buf); + return -1; + } + + if (sscanf(buf, "S: %u %u %d %d %d %d %d", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } + + control_group = u[0]; + control_index = u[1]; + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + unsigned inputs; + int used; + const char *end = buf + buflen; + char next_tag; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + /* get the base info for the mixer */ + if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { + debug("simple parse failed on '%s'", buf); + goto out; + } + + /* at least 1 input is required */ + if (inputs == 0) { + debug("simple parse got 0 inputs"); + goto out; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = inputs; + + /* find the next tag */ + next_tag = findnexttag(end - buflen, buflen); + + if (next_tag == 'S') { + /* No output scalers specified. Use default values. + * Corresponds to: + * O: 10000 10000 0 -10000 10000 0 + */ + mixinfo->output_scaler.negative_scale = 1.0f; + mixinfo->output_scaler.positive_scale = 1.0f; + mixinfo->output_scaler.offset = 0.f; + mixinfo->output_scaler.min_output = -1.0f; + mixinfo->output_scaler.max_output = 1.0f; + mixinfo->slew_rate_rise_time = 0.0f; + + } else { + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler, mixinfo->slew_rate_rise_time)) { + debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); + goto out; + } + } + + for (unsigned i = 0; i < inputs; i++) { + if (parse_control_scaler(end - buflen, buflen, + mixinfo->controls[i].scaler, + mixinfo->controls[i].control_group, + mixinfo->controls[i].control_index)) { + debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); + goto out; + } + } + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("loaded mixer with %d input(s)", inputs); + + } else { + debug("could not allocate memory for mixer"); + } + +out: + + if (mixinfo != nullptr) { + free(mixinfo); + } + + return sm; +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_pinfo == nullptr) { + return 0; + } + + if (space < 1) { + return 0; + } + + for (unsigned i = 0; i < _pinfo->control_count; i++) { + float input = 0.0f; + + _control_cb(_cb_handle, + _pinfo->controls[i].control_group, + _pinfo->controls[i].control_index, + input); + + sum += scale(_pinfo->controls[i].scaler, input); + } + + *outputs = scale(_pinfo->output_scaler, sum); + + if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) { + + // factor 2 is needed because actuator outputs are in the range [-1,1] + const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time; + + float delta_out = *outputs - _output_prev; + + if (delta_out > output_delta_max) { + *outputs = _output_prev + output_delta_max; + + } else if (delta_out < -output_delta_max) { + *outputs = _output_prev - output_delta_max; + } + + } + + // this will force the caller of the mixer to always supply dt values, otherwise no slew rate limiting will happen + _dt = 0.f; + + _output_prev = *outputs; + + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _pinfo->control_count; i++) { + groups |= 1 << _pinfo->controls[i].control_group; + } +} + +int +SimpleMixer::check() +{ + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_pinfo->control_count > 32) { + return -2; + } + + /* validate the output scaler */ + int ret = scale_check(_pinfo->output_scaler); + + if (ret != 0) { + return ret; + } + + /* validate input scalers */ + for (unsigned i = 0; i < _pinfo->control_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _pinfo->controls[i].control_group, + _pinfo->controls[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_pinfo->controls[i].scaler); + + if (ret != 0) { + return (10 * i + ret); + } + } + + return 0; +} + +float +SimpleMixer::scale(const mixer_scaler_s &scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler.negative_scale) + scaler.offset; + + } else { + output = (input * scaler.positive_scale) + scaler.offset; + } + + return math::constrain(output, scaler.min_output, scaler.max_output); +} + +int +SimpleMixer::scale_check(mixer_scaler_s &scaler) +{ + if (scaler.offset > 1.001f) { + return 1; + } + + if (scaler.offset < -1.001f) { + return 2; + } + + if (scaler.min_output > scaler.max_output) { + return 3; + } + + if (scaler.min_output < -1.001f) { + return 4; + } + + if (scaler.max_output > 1.001f) { + return 5; + } + + return 0; +} diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.hpp b/src/lib/mixer/SimpleMixer/SimpleMixer.hpp new file mode 100644 index 0000000000..d2811e6d59 --- /dev/null +++ b/src/lib/mixer/SimpleMixer/SimpleMixer.hpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 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. + * + ****************************************************************************/ + +#pragma once + +#include + +/** simple channel scaler */ +struct mixer_scaler_s { + float negative_scale{1.0f}; + float positive_scale{1.0f}; + float offset{0.0f}; + float min_output{-1.0f}; + float max_output{1.0f}; +}; + +/** mixer input */ +struct mixer_control_s { + uint8_t control_group; /**< group from which the input reads */ + uint8_t control_index; /**< index within the control group */ + mixer_scaler_s scaler; /**< scaling applied to the input before use */ +}; + +#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s)) + +/** simple mixer */ +struct mixer_simple_s { + uint8_t control_count; /**< number of inputs */ + mixer_scaler_s output_scaler; /**< scaling for the output */ + float slew_rate_rise_time{0.0f}; /**< output max rise time (slew rate limit)*/ + mixer_control_s controls[]; /**< actual size of the array is set by control_count */ +}; + +/** + * Simple summing mixer. + * + * Collects zero or more inputs and mixes them to a single output. + */ +class SimpleMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param mixinfo Mixer configuration. The pointer passed + * becomes the property of the mixer and + * will be freed when the mixer is deleted. + */ + SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo); + virtual ~SimpleMixer(); + + // no copy, assignment, move, move assignment + SimpleMixer(const SimpleMixer &) = delete; + SimpleMixer &operator=(const SimpleMixer &) = delete; + SimpleMixer(SimpleMixer &&) = delete; + SimpleMixer &operator=(SimpleMixer &&) = delete; + + /** + * Factory method with full external configuration. + * + * 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 SimpleMixer instance, or nullptr + * if the text format is bad. + */ + static SimpleMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + + void groups_required(uint32_t &groups) override; + + /** + * Check that the mixer configuration as loaded is sensible. + * + * Note that this function will call control_cb, but only cares about + * error returns, not the input value. + * + * @return Zero if the mixer makes sense, nonzero otherwise. + */ + int check(); + + unsigned set_trim(float trim) override; + unsigned get_trim(float *trim) override; + void set_dt_once(float dt) override; + +private: + + /** + * Perform simpler linear scaling. + * + * @param scaler The scaler configuration. + * @param input The value to be scaled. + * @return The scaled value. + */ + static float scale(const mixer_scaler_s &scaler, float input); + + /** + * Validate a scaler + * + * @param scaler The scaler to be validated. + * @return Zero if good, nonzero otherwise. + */ + static int scale_check(struct mixer_scaler_s &scaler); + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time); + static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index); + + float _output_prev{0.f}; + float _dt{0.f}; + + mixer_simple_s *_pinfo; + +}; diff --git a/src/lib/mixer/load_mixer_file.cpp b/src/lib/mixer/load_mixer_file.cpp new file mode 100644 index 0000000000..52fd84b6de --- /dev/null +++ b/src/lib/mixer/load_mixer_file.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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_load.c + * + * Programmable multi-channel mixer library. + */ + +#include +#include +#include + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf, unsigned maxlen) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + + if (fp == nullptr) { + printf("file not found\n"); + return -1; + } + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + + if (fgets(line, sizeof(line), fp) == nullptr) { + break; + } + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) { + continue; + } + + /* compact whitespace in the buffer */ + char *t, *f; + + for (f = line; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + + while (*t == ' ') { + t++; + } + + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + printf("line too long\n"); + fclose(fp); + return -1; + } + + /* add the line to the buffer */ + strcat(buf, line); + } + + fclose(fp); + return 0; +} diff --git a/src/lib/mixer/mixer_load.h b/src/lib/mixer/mixer_load.h new file mode 100644 index 0000000000..d61e93b4f8 --- /dev/null +++ b/src/lib/mixer/mixer_load.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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_load.h + * + */ + + +#ifndef _SYSTEMLIB_MIXER_LOAD_H +#define _SYSTEMLIB_MIXER_LOAD_H value + +#include + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); + +__END_DECLS + +#endif