diff --git a/ROMFS/px4fmu_common/mixers/hexa_tilt_x.main.mix b/ROMFS/px4fmu_common/mixers/hexa_tilt_x.main.mix index 984665ef5c..d69c25d1d5 100644 --- a/ROMFS/px4fmu_common/mixers/hexa_tilt_x.main.mix +++ b/ROMFS/px4fmu_common/mixers/hexa_tilt_x.main.mix @@ -1,3 +1,4 @@ # Hexarotor with tilted arms -R: 6xt 10000 10000 10000 0 +# R: 6xt 10000 10000 10000 0 +S: 6xt 10000 10000 10000 10000 10000 10000 0 diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index c9575301d9..c22f83fce0 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -200,6 +200,10 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid); break; + case 'S': + m = MultirotorMixer6dof::from_text(control_cb, cb_handle, p, resid); + break; + case 'H': m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid); break; diff --git a/src/lib/mixer/MultirotorMixer/CMakeLists.txt b/src/lib/mixer/MultirotorMixer/CMakeLists.txt index 6bc3e0a7fe..0f2018c7b6 100644 --- a/src/lib/mixer/MultirotorMixer/CMakeLists.txt +++ b/src/lib/mixer/MultirotorMixer/CMakeLists.txt @@ -92,10 +92,12 @@ add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h ${CMA add_library(MultirotorMixer MultirotorMixer.cpp MultirotorMixer.hpp + MultirotorMixer6dof.cpp ${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 + ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof_normalized.generated.h ) target_link_libraries(MultirotorMixer PRIVATE Mixer) target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp index c7dd95bcef..a2103a0d38 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp @@ -258,3 +258,236 @@ private: float *_outputs_prev{nullptr}; float *_tmp_array{nullptr}; }; + +/** + * 6-DoF multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects six inputs (roll, pitch, yaw, x thrust, y thrust and z thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ +class MultirotorMixer6dof : 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 x_scale; /**< scales x thrust for this rotor */ + float y_scale; /**< scales y thrust for this rotor */ + float z_scale; /**< scales z thrust for this rotor */ + float thrust_scale; /**< TODO: Remove later after fixing the Airmod */ + }; + + /** + * 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_scale Scaling factor applied to yaw inputs compared + * to thrust. + * @param x_scale Scaling factor applied to x thrust inputs + * compared to thrust. + * @param y_scale Scaling factor applied to y thrust inputs + * compared to thrust. + * @param z_scale Scaling factor applied to z thrust 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. + */ + MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry, + float roll_scale, float pitch_scale, float yaw_scale, + float x_scale, float y_scale, float z_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) + */ + MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count); + + virtual ~MultirotorMixer6dof(); + + // no copy, assignment, move, move assignment + MultirotorMixer6dof(const MultirotorMixer6dof &) = delete; + MultirotorMixer6dof &operator=(const MultirotorMixer6dof &) = delete; + MultirotorMixer6dof(MultirotorMixer6dof &&) = delete; + MultirotorMixer6dof &operator=(MultirotorMixer6dof &&) = 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 MultirotorMixer6dof instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer6dof *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 x_thrust_pos : 1; // 9 - true when a positive x thrust demand change will increase saturation + uint16_t x_thrust_neg : 1; //10 - true when a negative x thrust demand change will increase saturation + uint16_t y_thrust_pos : 1; //11 - true when a positive y thrust demand change will increase saturation + uint16_t y_thrust_neg : 1; //12 - true when a negative y thrust demand change will increase saturation + uint16_t z_thrust_pos : 1; //13 - true when a positive z thrust demand change will increase saturation + uint16_t z_thrust_neg : 1; //14 - true when a negative z 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 _x_scale{1.0f}; + float _y_scale{1.0f}; + float _z_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/MultirotorMixer/MultirotorMixer6dof.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp new file mode 100644 index 0000000000..f391c1c543 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp @@ -0,0 +1,545 @@ +/**************************************************************************** + * + * 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 + +// 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_6dof_normalized.generated.h" + + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) syslog(fmt "\n", ##args) + +MultirotorMixer6dof::MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry, + float roll_scale, float pitch_scale, float yaw_scale, + float x_scale, float y_scale, float z_scale, float idle_speed) : + MultirotorMixer6dof(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry]) +{ + _roll_scale = roll_scale; + _pitch_scale = pitch_scale; + _yaw_scale = yaw_scale; + _x_scale = x_scale; + _y_scale = y_scale; + _z_scale = z_scale; + _idle_speed = -1.0f + idle_speed * 2.0f; /* shift to output range here to avoid runtime calculation */ +} + +MultirotorMixer6dof::MultirotorMixer6dof(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] = _idle_speed; + } +} + +MultirotorMixer6dof::~MultirotorMixer6dof() +{ + delete[] _outputs_prev; + delete[] _tmp_array; +} + +MultirotorMixer6dof * +MultirotorMixer6dof::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen) +{ + MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY; + char geomname[8]; + int s[7]; + int used; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + if (sscanf(buf, "S: %7s %d %d %d %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &s[6], + &used) != 8) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + if (used > (int)buflen) { + debug("OVERFLOW: multirotor 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; + } + + 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 MultirotorMixer6dof( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f, + s[4] / 10000.0f, + s[5] / 10000.0f, + s[6] / 10000.0f); +} + +float +MultirotorMixer6dof::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 +MultirotorMixer6dof::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 +MultirotorMixer6dof::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 +MultirotorMixer6dof::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 +MultirotorMixer6dof::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 MultirotorMixer6dof::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 +MultirotorMixer6dof::mix(float *outputs, unsigned space) +{ + if (space < _rotor_count) { + return 0; + } + + float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); + float pitch = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); + float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); + float x_thrust = math::constrain(get_control(0, 4), 0.0f, 1.0f); + float y_thrust = math::constrain(get_control(0, 5), 0.0f, 1.0f); + float z_thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f); + + // clean out class variable used to capture saturation + _saturation_status.value = 0; + + // TODO: Enable Airmod for 6dof + // // Do the mixing using the strategy given by the current Airmode configuration + // switch (_airmode) { + // case Airmode::roll_pitch: + // mix_airmode_rp(roll, pitch, yaw, z_thrust, outputs); + // break; + + // case Airmode::roll_pitch_yaw: + // mix_airmode_rpy(roll, pitch, yaw, z_thrust, outputs); + // break; + + // case Airmode::disabled: + // default: // just in case: default to disabled + // mix_airmode_disabled(roll, pitch, yaw, z_thrust, outputs); + // break; + // } + + // TODO: Remove later after fixing Airmod till --- point + float min_out = 1.0f; + float max_out = 0.0f; + + for (unsigned i = 0; i < _rotor_count; i++) { + float out = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale + + x_thrust * _rotors[i].x_scale + + y_thrust * _rotors[i].y_scale + + // TODO remove the - sign + - z_thrust * _rotors[i].z_scale; + + /* calculate min and max output values */ + if (out < min_out) { + min_out = out; + } + + if (out > max_out) { + max_out = out; + } + + outputs[i] = out; + } + + // capture saturation + if (min_out < 0.0f) { + _saturation_status.flags.motor_neg = true; + } + + if (max_out > 1.0f) { + _saturation_status.flags.motor_pos = true; + } + + // 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(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); + } + + // 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] < _idle_speed + 0.01f) { + 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 +MultirotorMixer6dof::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, + bool clipping_low_yaw) +{ + // TODO: handle saturation for 6dof + + // 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.x_thrust_pos = true; + _saturation_status.flags.y_thrust_pos = true; + _saturation_status.flags.z_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.x_thrust_neg = true; + _saturation_status.flags.y_thrust_neg = true; + _saturation_status.flags.z_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; +}