diff --git a/src/lib/mixer/CMakeLists.txt b/src/lib/mixer/CMakeLists.txt index c0932bf654..8ac1ec6827 100644 --- a/src/lib/mixer/CMakeLists.txt +++ b/src/lib/mixer/CMakeLists.txt @@ -84,12 +84,20 @@ add_custom_command( add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h) add_library(mixer - mixer.cpp - mixer_group.cpp - mixer_helicopter.cpp - mixer_load.c - mixer_multirotor.cpp - mixer_simple.cpp + HelicopterMixer.cpp + HelicopterMixer.hpp + Mixer.cpp + Mixer.hpp + MixerGroup.cpp + MixerGroup.hpp + MultirotorMixer.cpp + MultirotorMixer.hpp + NullMixer.cpp + NullMixer.hpp + SimpleMixer.cpp + SimpleMixer.hpp + + load_mixer_file.cpp ) target_include_directories(mixer PRIVATE ${PX4_BINARY_DIR}/src/lib/mixer) @@ -100,8 +108,8 @@ if(BUILD_TESTING) add_executable(test_mixer_multirotor test_mixer_multirotor.cpp - mixer_multirotor.cpp - mixer.cpp + MultirotorMixer.cpp + Mixer.cpp ) target_compile_definitions(test_mixer_multirotor PRIVATE MIXER_MULTIROTOR_USE_MOCK_GEOMETRY) target_compile_options(test_mixer_multirotor PRIVATE -Wno-unused-result) diff --git a/src/lib/mixer/mixer_helicopter.cpp b/src/lib/mixer/HelicopterMixer.cpp similarity index 96% rename from src/lib/mixer/mixer_helicopter.cpp rename to src/lib/mixer/HelicopterMixer.cpp index 88b1f96f49..f331f926a2 100644 --- a/src/lib/mixer/mixer_helicopter.cpp +++ b/src/lib/mixer/HelicopterMixer.cpp @@ -39,6 +39,8 @@ #include "mixer.h" +#include "HelicopterMixer.hpp" + #include #include #include @@ -50,9 +52,7 @@ using math::constrain; -HelicopterMixer::HelicopterMixer(ControlCallback control_cb, - uintptr_t cb_handle, - mixer_heli_s *mixer_info) : +HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s *mixer_info) : Mixer(control_cb, cb_handle), _mixer_info(*mixer_info) { @@ -243,10 +243,3 @@ HelicopterMixer::mix(float *outputs, unsigned space) return _mixer_info.control_count + 1; } - -void -HelicopterMixer::groups_required(uint32_t &groups) -{ - /* XXX for now, hardcoded to indexes 0-3 in control group zero */ - groups |= (1 << 0); -} diff --git a/src/lib/mixer/HelicopterMixer.hpp b/src/lib/mixer/HelicopterMixer.hpp new file mode 100644 index 0000000000..70b6192251 --- /dev/null +++ b/src/lib/mixer/HelicopterMixer.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. + * + ****************************************************************************/ + +#pragma once + +/** helicopter swash servo mixer */ +struct mixer_heli_servo_s { + float angle; + float arm_length; + float scale; + float offset; + float min_output; + float max_output; +}; + +#define HELI_CURVES_NR_POINTS 5 + +/** helicopter swash plate mixer */ +struct mixer_heli_s { + uint8_t control_count; /**< number of inputs */ + float throttle_curve[HELI_CURVES_NR_POINTS]; + float pitch_curve[HELI_CURVES_NR_POINTS]; + struct mixer_heli_servo_s servos[4]; /**< up to four inputs */ +}; + +/** + * Generic helicopter mixer for helicopters with swash plate. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands + * for swash plate tilting and throttle- and pitch curves. + */ +class 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; + + /** + * 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/mixer.cpp b/src/lib/mixer/Mixer.cpp similarity index 82% rename from src/lib/mixer/mixer.cpp rename to src/lib/mixer/Mixer.cpp index 2907fa09f9..64a23762cf 100644 --- a/src/lib/mixer/mixer.cpp +++ b/src/lib/mixer/Mixer.cpp @@ -39,6 +39,8 @@ #include "mixer.h" +#include "Mixer.hpp" + #include #include #include @@ -47,7 +49,6 @@ //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : - _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { @@ -56,14 +57,13 @@ Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : float Mixer::get_control(uint8_t group, uint8_t index) { - float value; + float value; _control_cb(_cb_handle, group, index, value); return value; } - float Mixer::scale(const mixer_scaler_s &scaler, float input) { @@ -76,14 +76,7 @@ Mixer::scale(const mixer_scaler_s &scaler, float input) output = (input * scaler.positive_scale) + scaler.offset; } - if (output > scaler.max_output) { - output = scaler.max_output; - - } else if (output < scaler.min_output) { - output = scaler.min_output; - } - - return output; + return math::constrain(output, scaler.min_output, scaler.max_output); } int @@ -178,51 +171,3 @@ Mixer::string_well_formed(const char *buf, unsigned &buflen) return false; } - -/****************************************************************************/ - -NullMixer::NullMixer() : - Mixer(nullptr, 0) -{ -} - -unsigned -NullMixer::mix(float *outputs, unsigned space) -{ - if (space > 0) { - *outputs = NAN; - return 1; - } - - return 0; -} - -uint16_t -NullMixer::get_saturation_status() -{ - return 0; -} - -void -NullMixer::groups_required(uint32_t &groups) -{ - -} - -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/Mixer.hpp b/src/lib/mixer/Mixer.hpp new file mode 100644 index 0000000000..95e548fdfb --- /dev/null +++ b/src/lib/mixer/Mixer.hpp @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * 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 + +/** simple channel scaler */ +struct mixer_scaler_s { + float negative_scale; + float positive_scale; + float offset; + float min_output; + float max_output; +}; + +/** + * Abstract class defining a mixer mixing zero or more inputs to + * one or more outputs. + */ +class Mixer +{ +public: + enum class Airmode : int32_t { + disabled = 0, + roll_pitch = 1, + roll_pitch_yaw = 2 + }; + + /** next mixer in a list */ + Mixer *_next{nullptr}; + + /** + * 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); + virtual ~Mixer() = default; + + /** + * 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; } + +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); + + /** + * 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); + + /** + * 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/mixer_group.cpp b/src/lib/mixer/MixerGroup.cpp similarity index 92% rename from src/lib/mixer/mixer_group.cpp rename to src/lib/mixer/MixerGroup.cpp index ea7448c7a3..727119d400 100644 --- a/src/lib/mixer/mixer_group.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -53,14 +53,15 @@ #include "mixer.h" +#include "MixerGroup.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) MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : - Mixer(control_cb, cb_handle), - _first(nullptr) + Mixer(control_cb, cb_handle) { } @@ -72,9 +73,7 @@ MixerGroup::~MixerGroup() void MixerGroup::add_mixer(Mixer *mixer) { - Mixer **mpp; - - mpp = &_first; + Mixer **mpp = &_first; while (*mpp != nullptr) { mpp = &((*mpp)->_next); @@ -105,7 +104,7 @@ MixerGroup::reset() unsigned MixerGroup::mix(float *outputs, unsigned space) { - Mixer *mixer = _first; + Mixer *mixer = _first; unsigned index = 0; while ((mixer != nullptr) && (index < space)) { @@ -125,7 +124,7 @@ MixerGroup::mix(float *outputs, unsigned space) unsigned MixerGroup::set_trims(int16_t *values, unsigned n) { - Mixer *mixer = _first; + Mixer *mixer = _first; unsigned index = 0; while ((mixer != nullptr) && (index < n)) { @@ -135,7 +134,7 @@ MixerGroup::set_trims(int16_t *values, unsigned n) /* to be safe, clamp offset to range of [-500, 500] usec */ if (offset < -1.0f) { offset = -1.0f; } - if (offset > 1.0f) { offset = 1.0f; } + if (offset > 1.0f) { offset = 1.0f; } debug("set trim: %d, offset: %5.3f", values[index], (double)offset); index += mixer->set_trim(offset); @@ -154,7 +153,7 @@ MixerGroup::set_trims(int16_t *values, unsigned n) unsigned MixerGroup::get_trims(int16_t *values) { - Mixer *mixer = _first; + Mixer *mixer = _first; unsigned index_mixer = 0; unsigned index = 0; float trim; @@ -179,7 +178,7 @@ MixerGroup::get_trims(int16_t *values) void MixerGroup::set_thrust_factor(float val) { - Mixer *mixer = _first; + Mixer *mixer = _first; while (mixer != nullptr) { mixer->set_thrust_factor(val); @@ -190,7 +189,7 @@ MixerGroup::set_thrust_factor(float val) void MixerGroup::set_airmode(Airmode airmode) { - Mixer *mixer = _first; + Mixer *mixer = _first; while (mixer != nullptr) { mixer->set_airmode(airmode); @@ -201,25 +200,25 @@ MixerGroup::set_airmode(Airmode airmode) unsigned MixerGroup::get_multirotor_count() { - Mixer *mixer = _first; - unsigned rotor_count = 0; + Mixer *mixer = _first; while (mixer != nullptr) { + unsigned rotor_count = mixer->get_multirotor_count(); - rotor_count = mixer->get_multirotor_count(); + if (rotor_count > 0) { + return rotor_count; + } - if (rotor_count > 0) { break; } - - mixer = mixer -> _next; + mixer = mixer->_next; } - return rotor_count; + return 0; } uint16_t MixerGroup::get_saturation_status() { - Mixer *mixer = _first; + Mixer *mixer = _first; uint16_t sat = 0; while (mixer != nullptr) { @@ -233,7 +232,7 @@ MixerGroup::get_saturation_status() unsigned MixerGroup::count() { - Mixer *mixer = _first; + Mixer *mixer = _first; unsigned index = 0; while (mixer != nullptr) { @@ -247,7 +246,7 @@ MixerGroup::count() void MixerGroup::groups_required(uint32_t &groups) { - Mixer *mixer = _first; + Mixer *mixer = _first; while (mixer != nullptr) { mixer->groups_required(groups); @@ -325,7 +324,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) void MixerGroup::set_max_delta_out_once(float delta_out_max) { - Mixer *mixer = _first; + Mixer *mixer = _first; while (mixer != nullptr) { mixer->set_max_delta_out_once(delta_out_max); diff --git a/src/lib/mixer/MixerGroup.hpp b/src/lib/mixer/MixerGroup.hpp new file mode 100644 index 0000000000..31510d2b5f --- /dev/null +++ b/src/lib/mixer/MixerGroup.hpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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 + +/** + * Group of mixers, built up from single mixers and processed + * in order when mixing. + */ +class MixerGroup : public Mixer +{ +public: + MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); + virtual ~MixerGroup(); + + unsigned mix(float *outputs, unsigned space) override; + uint16_t get_saturation_status() override; + void groups_required(uint32_t &groups) override; + + /** + * Add a mixer to the group. + * + * @param mixer The mixer to be added. + */ + void add_mixer(Mixer *mixer); + + /** + * Remove all the mixers from the group. + */ + void reset(); + + /** + * Count the mixers in the group. + */ + unsigned count(); + + /** + * 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(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) override; + + /* + * 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) override; + + void set_airmode(Airmode airmode) override; + + unsigned get_multirotor_count() override; + +private: + Mixer *_first{nullptr}; /**< linked list of mixers */ +}; diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/MultirotorMixer.cpp similarity index 92% rename from src/lib/mixer/mixer_multirotor.cpp rename to src/lib/mixer/MultirotorMixer.cpp index 8559d01823..6c0393dffd 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/MultirotorMixer.cpp @@ -45,6 +45,8 @@ #include +#include "MultirotorMixer.hpp" + #ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY enum class MultirotorGeometry : MultirotorGeometryUnderlyingType { QUAD_X, @@ -91,9 +93,6 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ - _delta_out_max(0.0f), - _thrust_factor(0.0f), - _airmode(Airmode::disabled), _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]), _outputs_prev(new float[_rotor_count]), @@ -103,18 +102,8 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _outputs_prev[i] = _idle_speed; } } -MultirotorMixer::MultirotorMixer(ControlCallback control_cb, - uintptr_t cb_handle, - Rotor *rotors, - unsigned rotor_count) : +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, Rotor *rotors, unsigned rotor_count) : Mixer(control_cb, cb_handle), - _roll_scale(1.f), - _pitch_scale(1.f), - _yaw_scale(1.f), - _idle_speed(0.f), - _delta_out_max(0.0f), - _thrust_factor(0.0f), - _airmode(Airmode::disabled), _rotor_count(rotor_count), _rotors(rotors), _outputs_prev(new float[_rotor_count]), @@ -188,7 +177,8 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl s[3] / 10000.0f); } -float MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, const float *outputs, +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; @@ -225,9 +215,9 @@ float MultirotorMixer::compute_desaturation_gain(const float *desaturation_vecto 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 +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); @@ -249,7 +239,8 @@ void MultirotorMixer::minimize_saturation(const float *desaturation_vector, floa } } -void MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs) +void +MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs) { // Airmode for roll and pitch, but not yaw @@ -269,7 +260,8 @@ void MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float t mix_yaw(yaw, outputs); } -void MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs) +void +MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs) { // Airmode for roll, pitch and yaw @@ -295,7 +287,8 @@ void MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float minimize_saturation(_tmp_array, outputs, _saturation_status); } -void MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs) +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 @@ -492,7 +485,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo // A positive change in thrust will increase saturation _saturation_status.flags.thrust_pos = true; - } // The motor is saturated at the lower limit @@ -536,21 +528,3 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo _saturation_status.flags.valid = true; } - -void -MultirotorMixer::set_airmode(Airmode airmode) -{ - _airmode = airmode; -} - -void -MultirotorMixer::groups_required(uint32_t &groups) -{ - /* XXX for now, hardcoded to indexes 0-3 in control group zero */ - groups |= (1 << 0); -} - -uint16_t MultirotorMixer::get_saturation_status() -{ - return _saturation_status.value; -} diff --git a/src/lib/mixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer.hpp new file mode 100644 index 0000000000..8e0a83814b --- /dev/null +++ b/src/lib/mixer/MultirotorMixer.hpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * 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 + +/** + * Supported multirotor geometries. + * + * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h + */ +typedef unsigned int 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, + float roll_scale, + float pitch_scale, + float yaw_scale, + float idle_speed); + + /** + * 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, Rotor *rotors, unsigned rotor_count); + virtual ~MultirotorMixer(); + + /** + * 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 _roll_scale{1.0f}; + float _pitch_scale{1.0f}; + float _yaw_scale{1.0f}; + float _idle_speed{0.0f}; + 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/NullMixer.cpp b/src/lib/mixer/NullMixer.cpp new file mode 100644 index 0000000000..8fe7e13c8c --- /dev/null +++ b/src/lib/mixer/NullMixer.cpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 "mixer.h" + +#include +#include +#include + +NullMixer::NullMixer() : + Mixer(nullptr, 0) +{ +} + +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.hpp b/src/lib/mixer/NullMixer.hpp new file mode 100644 index 0000000000..6901d6f9b0 --- /dev/null +++ b/src/lib/mixer/NullMixer.hpp @@ -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. + * + ****************************************************************************/ + +#pragma once + +/** + * Null mixer; returns zero. + * + * Used as a placeholder for output channels that are unassigned in groups. + */ +class NullMixer : public Mixer +{ +public: + NullMixer(); + virtual ~NullMixer() = default; + + /** + * 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/mixer_simple.cpp b/src/lib/mixer/SimpleMixer.cpp similarity index 97% rename from src/lib/mixer/mixer_simple.cpp rename to src/lib/mixer/SimpleMixer.cpp index 5fa4a21f05..f7bc47ad94 100644 --- a/src/lib/mixer/mixer_simple.cpp +++ b/src/lib/mixer/SimpleMixer.cpp @@ -44,9 +44,7 @@ #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) : +SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) : Mixer(control_cb, cb_handle), _pinfo(mixinfo) { @@ -306,7 +304,7 @@ out: unsigned SimpleMixer::mix(float *outputs, unsigned space) { - float sum = 0.0f; + float sum = 0.0f; if (_pinfo == nullptr) { return 0; @@ -331,12 +329,6 @@ SimpleMixer::mix(float *outputs, unsigned space) return 1; } -uint16_t -SimpleMixer::get_saturation_status() -{ - return 0; -} - void SimpleMixer::groups_required(uint32_t &groups) { @@ -348,7 +340,6 @@ SimpleMixer::groups_required(uint32_t &groups) int SimpleMixer::check() { - int ret; float junk; /* sanity that presumes that a mixer includes a control no more than once */ @@ -358,7 +349,7 @@ SimpleMixer::check() } /* validate the output scaler */ - ret = scale_check(_pinfo->output_scaler); + int ret = scale_check(_pinfo->output_scaler); if (ret != 0) { return ret; diff --git a/src/lib/mixer/SimpleMixer.hpp b/src/lib/mixer/SimpleMixer.hpp new file mode 100644 index 0000000000..d6867d3dd0 --- /dev/null +++ b/src/lib/mixer/SimpleMixer.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 + +/** 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 */ + struct 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 */ + struct mixer_scaler_s output_scaler; /**< scaling for the output */ + struct mixer_control_s controls[0]; /**< 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(); + + /** + * 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); + + /** + * Factory method for PWM/PPM input to internal float representation. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param input The control index used when fetching the input. + * @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out) + * @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out) + * @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out) + * @return A new SimpleMixer instance, or nullptr if one could not be + * allocated. + */ + static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, + uint16_t mid, uint16_t max); + + 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; + +private: + mixer_simple_s *_pinfo{nullptr}; + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); + static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index); +}; diff --git a/src/lib/mixer/mixer_load.c b/src/lib/mixer/load_mixer_file.cpp similarity index 100% rename from src/lib/mixer/mixer_load.c rename to src/lib/mixer/load_mixer_file.cpp diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 9c5feda690..e8b0d3d643 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -130,790 +130,9 @@ #include #include -/** simple channel scaler */ -struct mixer_scaler_s { - float negative_scale; - float positive_scale; - float offset; - float min_output; - float max_output; -}; - -/** 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 */ - struct mixer_scaler_s scaler; /**< scaling applied to the input before use */ -}; - -/** simple mixer */ -struct mixer_simple_s { - uint8_t control_count; /**< number of inputs */ - struct mixer_scaler_s output_scaler; /**< scaling for the output */ - struct mixer_control_s controls[0]; /**< actual size of the array is set by control_count */ -}; - -#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s)) - - -/** - * Abstract class defining a mixer mixing zero or more inputs to - * one or more outputs. - */ -class Mixer -{ -public: - enum class Airmode : int32_t { - disabled = 0, - roll_pitch = 1, - roll_pitch_yaw = 2 - }; - - /** next mixer in a list */ - Mixer *_next; - - /** - * 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); - virtual ~Mixer() = default; - - /** - * 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(void) = 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) = 0; - - /** - * @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) = 0; - - /** - * @brief Get trim offset for this mixer - * - * @return the number of outputs this mixer feeds to - */ - virtual unsigned get_trim(float *trim) = 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;} - -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); - - /** - * 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); - - /** - * 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); - -private: - - /* do not allow to copy due to pointer data members */ - Mixer(const Mixer &); - Mixer &operator=(const Mixer &); -}; - -/** - * Group of mixers, built up from single mixers and processed - * in order when mixing. - */ -class MixerGroup : public Mixer -{ -public: - MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); - ~MixerGroup(); - - unsigned mix(float *outputs, unsigned space) override; - uint16_t get_saturation_status(void) override; - void groups_required(uint32_t &groups) override; - - /** - * Add a mixer to the group. - * - * @param mixer The mixer to be added. - */ - void add_mixer(Mixer *mixer); - - /** - * Remove all the mixers from the group. - */ - void reset(); - - /** - * Count the mixers in the group. - */ - unsigned count(); - - /** - * 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(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) override; - - /* - * 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 set_trim(float trim) override - { - return 0; - } - - unsigned get_trims(int16_t *values); - - unsigned get_trim(float *trim) override - { - return 0; - } - - /** - * @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; - - void set_airmode(Airmode airmode) override; - - unsigned get_multirotor_count() override; - -private: - Mixer *_first; /**< linked list of mixers */ - - /* do not allow to copy due to pointer data members */ - MixerGroup(const MixerGroup &); - MixerGroup operator=(const MixerGroup &); -}; - -/** - * Null mixer; returns zero. - * - * Used as a placeholder for output channels that are unassigned in groups. - */ -class NullMixer : public Mixer -{ -public: - NullMixer(); - ~NullMixer() = default; - - /** - * 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; - uint16_t get_saturation_status(void) override; - void groups_required(uint32_t &groups) override; - unsigned set_trim(float trim) override - { - return 1; - } - - unsigned get_trim(float *trim) override - { - return 1; - } - -}; - -/** - * 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); - ~SimpleMixer(); - - /** - * 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); - - /** - * Factory method for PWM/PPM input to internal float representation. - * - * @param control_cb The callback to invoke when fetching a - * control value. - * @param cb_handle Handle passed to the control callback. - * @param input The control index used when fetching the input. - * @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out) - * @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out) - * @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out) - * @return A new SimpleMixer instance, or nullptr if one could not be - * allocated. - */ - static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, - uint16_t mid, uint16_t max); - - unsigned mix(float *outputs, unsigned space) override; - uint16_t get_saturation_status(void) 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; - -protected: - -private: - mixer_simple_s *_pinfo; - - static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); - static int parse_control_scaler(const char *buf, - unsigned &buflen, - mixer_scaler_s &scaler, - uint8_t &control_group, - uint8_t &control_index); - - /* do not allow to copy due to ptr data members */ - SimpleMixer(const SimpleMixer &); - SimpleMixer operator=(const SimpleMixer &); -}; - -/** - * Supported multirotor geometries. - * - * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h - */ -typedef unsigned int 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, - float roll_scale, - float pitch_scale, - float yaw_scale, - float idle_speed); - - /** - * 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, - Rotor *rotors, - unsigned rotor_count); - - ~MultirotorMixer(); - - /** - * 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(void) override; - void groups_required(uint32_t &groups) override; - - /** - * @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; - - 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 _roll_scale; - float _pitch_scale; - float _yaw_scale; - float _idle_speed; - float _delta_out_max; - float _thrust_factor; - - Airmode _airmode; - - saturation_status _saturation_status; - - unsigned _rotor_count; - const Rotor *_rotors; - - float *_outputs_prev = nullptr; - float *_tmp_array = nullptr; - - /* do not allow to copy due to ptr data members */ - MultirotorMixer(const MultirotorMixer &); - MultirotorMixer operator=(const MultirotorMixer &); -}; - -/** helicopter swash servo mixer */ -struct mixer_heli_servo_s { - float angle; - float arm_length; - float scale; - float offset; - float min_output; - float max_output; -}; - -#define HELI_CURVES_NR_POINTS 5 - -/** helicopter swash plate mixer */ -struct mixer_heli_s { - uint8_t control_count; /**< number of inputs */ - float throttle_curve[HELI_CURVES_NR_POINTS]; - float pitch_curve[HELI_CURVES_NR_POINTS]; - struct mixer_heli_servo_s servos[4]; /**< up to four inputs */ -}; - -/** - * Generic helicopter mixer for helicopters with swash plate. - * - * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands - * for swash plate tilting and throttle- and pitch curves. - */ -class HelicopterMixer : public Mixer -{ -public: - /** - * Constructor. - * - * @param control_cb Callback invoked to read inputs. - * @param cb_handle Passed to control_cb. - * @param mixer_info Pointer to heli mixer configuration - */ - HelicopterMixer(ControlCallback control_cb, - uintptr_t cb_handle, - mixer_heli_s *mixer_info); - - ~HelicopterMixer() = default; - - /** - * 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; - - uint16_t get_saturation_status(void) override { return 0; } - - unsigned set_trim(float trim) override - { - return 4; - } - - unsigned get_trim(float *trim) override - { - return 4; - } - -private: - mixer_heli_s _mixer_info; - - /* do not allow to copy */ - HelicopterMixer(const HelicopterMixer &); - HelicopterMixer operator=(const HelicopterMixer &); -}; +#include "Mixer.hpp" +#include "MixerGroup.hpp" +#include "NullMixer.hpp" +#include "SimpleMixer.hpp" +#include "MultirotorMixer.hpp" +#include "HelicopterMixer.hpp"