From 69fe37e6be0a02608a18b2e8531c82551726b4fa Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 6 Jan 2020 17:55:11 -0500 Subject: [PATCH] Omni Mixer: The mixer for 6-DoF vehicles is refactored to the MultirotorMixer now --- src/lib/mixer/MixerGroup.cpp | 2 +- src/lib/mixer/MultirotorMixer/CMakeLists.txt | 18 +- .../mixer/MultirotorMixer/MultirotorMixer.cpp | 275 +++++++-- .../mixer/MultirotorMixer/MultirotorMixer.hpp | 285 ++------- .../MultirotorMixer/MultirotorMixer6dof.cpp | 567 ------------------ .../geometries/tools/px_generate_mixers.py | 45 +- 6 files changed, 306 insertions(+), 886 deletions(-) delete mode 100644 src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index c22f83fce0..343a96f8df 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -201,7 +201,7 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle break; case 'S': - m = MultirotorMixer6dof::from_text(control_cb, cb_handle, p, resid); + m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid); break; case 'H': diff --git a/src/lib/mixer/MultirotorMixer/CMakeLists.txt b/src/lib/mixer/MultirotorMixer/CMakeLists.txt index 0f2018c7b6..75d8bcb40e 100644 --- a/src/lib/mixer/MultirotorMixer/CMakeLists.txt +++ b/src/lib/mixer/MultirotorMixer/CMakeLists.txt @@ -76,32 +76,16 @@ add_custom_command( ) add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h) -# 6DOF mixers -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h - COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h - DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} - ) -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof_normalized.generated.h - COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --normalize --sixdof -f ${geometries_list} -o mixer_multirotor_6dof_normalized.generated.h - DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} - ) -add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof_normalized.generated.h) - 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}) -add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets) +add_dependencies(MultirotorMixer mixer_gen prebuild_targets) if(BUILD_TESTING) diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp index 17e2a839a3..1d903e2d5d 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * 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 @@ -87,6 +87,22 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle _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 */ + _is_6dof = false; +} + +MultirotorMixer::MultirotorMixer(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) : + MultirotorMixer(control_cb, cb_handle, _config_index_6dof[(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 */ + _is_6dof = true; } MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, @@ -102,6 +118,19 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle } } +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor6Dof *rotors, + unsigned rotor_count) : + Mixer(control_cb, cb_handle), + _rotor_count(rotor_count), + _rotors_6dof(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; + } +} + MultirotorMixer::~MultirotorMixer() { delete[] _outputs_prev; @@ -113,7 +142,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl { MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY; char geomname[8]; - int s[4]; + int s[7]; int used; /* enforce that the mixer ends with a new line */ @@ -121,9 +150,20 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl return nullptr; } - if (sscanf(buf, "R: %7s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { - debug("multirotor parse failed on '%s'", buf); - return nullptr; + bool is_6dof = (buf[0] == 'S'); + + if (is_6dof) { + 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; + } + + } else { + if (sscanf(buf, "R: %7s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } } if (used > (int)buflen) { @@ -155,14 +195,29 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl debug("adding multirotor mixer '%s'", geomname); - return new MultirotorMixer( - control_cb, - cb_handle, - geometry, - s[0] / 10000.0f, - s[1] / 10000.0f, - s[2] / 10000.0f, - s[3] / 10000.0f); + if (is_6dof) { + return new MultirotorMixer( + 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); + + } else { + return new MultirotorMixer( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); + } } float @@ -310,14 +365,52 @@ MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float mix_yaw(yaw, outputs); } +void +MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, + float z_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_6dof[i].roll_scale + + pitch * _rotors_6dof[i].pitch_scale + + x_thrust * _rotors_6dof[i].x_scale + + y_thrust * _rotors_6dof[i].y_scale + + z_thrust * _rotors_6dof[i].z_scale; + + // Z thrust will be used to unsaturate if needed + _tmp_array[i] = math::abs_t(_rotors_6dof[i].z_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_6dof[i].roll_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors_6dof[i].pitch_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + // Mix yaw independently + mix_yaw(yaw, outputs); +} + void MultirotorMixer::mix_yaw(float yaw, float *outputs) { // Add yaw to outputs for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += yaw * _rotors[i].yaw_scale; + outputs[i] += yaw * (_is_6dof ? _rotors_6dof[i].yaw_scale : _rotors[i].yaw_scale); // Yaw will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].yaw_scale; + _tmp_array[i] = (_is_6dof ? _rotors_6dof[i].yaw_scale : _rotors[i].yaw_scale); } // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), @@ -325,7 +418,7 @@ void MultirotorMixer::mix_yaw(float yaw, float *outputs) 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; + _tmp_array[i] = (_is_6dof ? _rotors_6dof[i].z_scale : _rotors[i].thrust_scale); } // reduce thrust only @@ -342,25 +435,33 @@ MultirotorMixer::mix(float *outputs, unsigned space) 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 thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f); // clean out class variable used to capture saturation _saturation_status.value = 0; - // Do the mixing using the strategy given by the current Airmode configuration - switch (_airmode) { - case Airmode::roll_pitch: - mix_airmode_rp(roll, pitch, yaw, thrust, outputs); - break; + if (!_is_6dof) - case Airmode::roll_pitch_yaw: - mix_airmode_rpy(roll, pitch, yaw, thrust, outputs); - break; + // Do the mixing using the strategy given by the current Airmode configuration + switch (_airmode) { + case Airmode::roll_pitch: + mix_airmode_rp(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs); + break; - case Airmode::disabled: - default: // just in case: default to disabled - mix_airmode_disabled(roll, pitch, yaw, thrust, outputs); - break; + case Airmode::roll_pitch_yaw: + mix_airmode_rpy(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs); + break; + + case Airmode::disabled: + default: // just in case: default to disabled + mix_airmode_disabled(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs); + break; + + } else { // if is 6-DoF vehicle + float x_thrust = math::constrain(get_control(0, 8), -1.0f, 1.0f); + float y_thrust = math::constrain(get_control(0, 9), -1.0f, 1.0f); + float z_thrust = math::constrain(get_control(0, 10), -1.0f, 1.0f); + + mix_airmode_disabled(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs); } // Apply thrust model and scale outputs to range [idle_speed, 1]. @@ -438,87 +539,161 @@ void MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw) { + float roll_scale = (_is_6dof) ? _rotors_6dof[index].roll_scale : _rotors[index].roll_scale; + float pitch_scale = (_is_6dof) ? _rotors_6dof[index].pitch_scale : _rotors[index].pitch_scale; + float yaw_scale = (_is_6dof) ? _rotors_6dof[index].yaw_scale : _rotors[index].yaw_scale; + float x_scale = (_is_6dof) ? _rotors_6dof[index].x_scale : 0.f; + float y_scale = (_is_6dof) ? _rotors_6dof[index].y_scale : 0.f; + float z_scale = (_is_6dof) ? _rotors_6dof[index].z_scale : 0.f; + // 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) { + if (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) { + } else if (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) { + if (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) { + } else if (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) { + if (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) { + } else if (yaw_scale < 0.0f) { // A negative change in yaw will increase saturation _saturation_status.flags.yaw_neg = true; } - // A negative change in Z thrust will increase saturation - _saturation_status.flags.z_thrust_neg = true; + if (_is_6dof) { + // check if the x input is saturating + if (x_scale > 0.0f) { + // A positive change in x will increase saturation + _saturation_status.flags.x_thrust_pos = true; + } else if (x_scale < 0.0f) { + // A negative change in x will increase saturation + _saturation_status.flags.x_thrust_neg = true; + } + + // check if the y input is saturating + if (y_scale > 0.0f) { + // A positive change in y will increase saturation + _saturation_status.flags.y_thrust_pos = true; + + } else if (y_scale < 0.0f) { + // A negative change in y will increase saturation + _saturation_status.flags.y_thrust_neg = true; + } + + // check if the z input is saturating + if (z_scale > 0.0f) { + // A positive change in z will increase saturation + _saturation_status.flags.z_thrust_pos = true; + + } else if (z_scale < 0.0f) { + // A negative change in z will increase saturation + _saturation_status.flags.z_thrust_neg = true; + } + + } else { + // A negative change in Z thrust (positive change in total thrust) will increase saturation + _saturation_status.flags.z_thrust_neg = 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) { + if (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) { + } else if (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) { + if (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) { + } else if (pitch_scale < 0.0f) { // A positive change in pitch will increase saturation _saturation_status.flags.pitch_pos = true; } - // A positive change in Z thrust will increase saturation - _saturation_status.flags.z_thrust_pos = true; + if (_is_6dof) { + // check if the x input is saturating + if (x_scale > 0.0f) { + // A negative change in x will increase saturation + _saturation_status.flags.x_thrust_neg = true; + + } else if (x_scale < 0.0f) { + // A positive change in x will increase saturation + _saturation_status.flags.x_thrust_pos = true; + } + + // check if the y input is saturating + if (y_scale > 0.0f) { + // A negative change in y will increase saturation + _saturation_status.flags.y_thrust_neg = true; + + } else if (y_scale < 0.0f) { + // A positive change in y will increase saturation + _saturation_status.flags.y_thrust_pos = true; + } + + // check if the z input is saturating + if (z_scale > 0.0f) { + // A negative change in z will increase saturation + _saturation_status.flags.z_thrust_neg = true; + + } else if (z_scale < 0.0f) { + // A positive change in z will increase saturation + _saturation_status.flags.z_thrust_pos = true; + } + + } else { + // A positive change in Z thrust (negative change in total thrust) will increase saturation + _saturation_status.flags.z_thrust_pos = true; + } } if (clipping_low_yaw) { // check if the yaw input is saturating - if (_rotors[index].yaw_scale > 0.0f) { + if (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) { + } else if (yaw_scale < 0.0f) { // A positive change in yaw will increase saturation _saturation_status.flags.yaw_pos = true; } } - // X and Y thrusts are not controlled - _saturation_status.flags.x_thrust_pos = true; - _saturation_status.flags.x_thrust_neg = true; - _saturation_status.flags.y_thrust_pos = true; - _saturation_status.flags.y_thrust_neg = true; + if (!_is_6dof) { + // X and Y thrusts are not controlled + _saturation_status.flags.x_thrust_pos = true; + _saturation_status.flags.x_thrust_neg = true; + _saturation_status.flags.y_thrust_pos = true; + _saturation_status.flags.y_thrust_neg = true; + } _saturation_status.flags.valid = true; } diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp index eb01db373f..7af3cd2de1 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp @@ -46,22 +46,30 @@ enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; /** * Multi-rotor mixer for pre-defined vehicle geometries. * - * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * Collects either four inputs (roll, pitch, yaw, thrust) or 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 MultirotorMixer : public Mixer { public: /** - * Precalculated rotor mix. */ struct Rotor { - float roll_scale; /**< scales roll for this 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 yaw_scale; /**< scales yaw for this rotor */ float thrust_scale; /**< scales thrust for this rotor */ }; + struct Rotor6Dof { + 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 */ + }; /** * Constructor. @@ -73,7 +81,7 @@ public: * 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 + * @param yaw_scale 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 @@ -82,6 +90,32 @@ public: MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry, float roll_scale, float pitch_scale, float yaw_scale, float idle_speed); + /** + * Constructor for 6-DoF. + * + * @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. + */ + MultirotorMixer(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). * @@ -91,6 +125,17 @@ public: * @param rotor_count length of rotors array (= number of motors) */ MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count); + + /** + * Constructor (for testing 6-DoF). + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param rotors control allocation matrix + * @param rotor_count length of rotors array (= number of motors) + */ + MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor6Dof *rotors, unsigned rotor_count); + virtual ~MultirotorMixer(); // no copy, assignment, move, move assignment @@ -186,14 +231,14 @@ private: * 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 + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale/z_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 desaturation_vector vector that is added to the outputs, e.g. thrust_scale/z_scale * @param outputs output vector that is modified * @param sat_status saturation status output * @param min_output minimum desired value in outputs @@ -232,232 +277,12 @@ private: 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}; -}; - -/** - * 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 */ - }; - - /** - * 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 (z_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. z_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 x_thrust, float y_thrust, float z_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 x_thrust, float y_thrust, float z_thrust, - float *outputs); - - /** - * Mix roll, pitch, yaw, thrust and set the outputs vector. + * Mix roll, pitch, yaw, x_thrust, y_thrust, z_thrust and set the outputs vector for 6-DoF vehicles. * * 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. + * This is the only airmode available for 6-DoF vehicles. * @see mix_yaw() for the exact yaw behavior. */ inline void mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, @@ -477,6 +302,7 @@ private: void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw); + bool _is_6dof{false}; float _roll_scale{1.0f}; float _pitch_scale{1.0f}; float _yaw_scale{1.0f}; @@ -493,6 +319,7 @@ private: unsigned _rotor_count; const Rotor *_rotors; + const Rotor6Dof *_rotors_6dof; float *_outputs_prev{nullptr}; float *_tmp_array{nullptr}; diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp deleted file mode 100644 index 4c4491f3d3..0000000000 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp +++ /dev/null @@ -1,567 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mixer_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 x_thrust, float y_thrust, float z_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 + - x_thrust * _rotors[i].x_scale + - y_thrust * _rotors[i].y_scale + - z_thrust * _rotors[i].z_scale; - - // Thrust will be used to unsaturate if needed - _tmp_array[i] = math::abs_t(_rotors[i].z_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 x_thrust, float y_thrust, float z_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 + - x_thrust * _rotors[i].x_scale + - y_thrust * _rotors[i].y_scale + - z_thrust * _rotors[i].z_scale; - - // Z thrust will be used to unsaturate if needed - _tmp_array[i] = math::abs_t(_rotors[i].z_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 x_thrust, float y_thrust, - float z_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 + - x_thrust * _rotors[i].x_scale + - y_thrust * _rotors[i].y_scale + - z_thrust * _rotors[i].z_scale; - - // Z thrust will be used to unsaturate if needed - _tmp_array[i] = math::abs_t(_rotors[i].z_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] = math::abs_t(_rotors[i].z_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, 8), -1.0f, 1.0f); - float y_thrust = math::constrain(get_control(0, 9), -1.0f, 1.0f); - float z_thrust = math::constrain(get_control(0, 10), -1.0f, 1.0f); - - // clean out class variable used to capture saturation - _saturation_status.value = 0; - - // Do the mixing using the strategy given by the current Airmode configuration - switch (_airmode) { - case Airmode::roll_pitch: - mix_airmode_rp(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs); - break; - - case Airmode::roll_pitch_yaw: - mix_airmode_rpy(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs); - break; - - case Airmode::disabled: - default: // just in case: default to disabled - mix_airmode_disabled(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs); - break; - } - - // Apply thrust model and scale outputs to range [idle_speed, 1]. - // At this point the outputs are expected to be in [0, 1], but they can be outside, for example - // if a roll command exceeds the motor band limit. - for (unsigned i = 0; i < _rotor_count; i++) { - // Implement simple model for static relationship between applied motor pwm and motor thrust - // model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2 - if (_thrust_factor > 0.0f) { - outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) * - (1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] / - _thrust_factor)); - } - - outputs[i] = math::constrain(_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) -{ - // 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; - } - - // check if the x input is saturating - if (_rotors[index].x_scale > 0.0f) { - // A positive change in x will increase saturation - _saturation_status.flags.x_thrust_pos = true; - - } else if (_rotors[index].x_scale < 0.0f) { - // A negative change in x will increase saturation - _saturation_status.flags.x_thrust_neg = true; - } - - // check if the y input is saturating - if (_rotors[index].y_scale > 0.0f) { - // A positive change in y will increase saturation - _saturation_status.flags.y_thrust_pos = true; - - } else if (_rotors[index].y_scale < 0.0f) { - // A negative change in y will increase saturation - _saturation_status.flags.y_thrust_neg = true; - } - - // check if the z input is saturating - if (_rotors[index].z_scale > 0.0f) { - // A positive change in z will increase saturation - _saturation_status.flags.z_thrust_pos = true; - - } else if (_rotors[index].z_scale < 0.0f) { - // A negative change in z will increase saturation - _saturation_status.flags.z_thrust_neg = 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; - } - - // check if the x input is saturating - if (_rotors[index].x_scale > 0.0f) { - // A negative change in x will increase saturation - _saturation_status.flags.x_thrust_neg = true; - - } else if (_rotors[index].x_scale < 0.0f) { - // A positive change in x will increase saturation - _saturation_status.flags.x_thrust_pos = true; - } - - // check if the y input is saturating - if (_rotors[index].y_scale > 0.0f) { - // A negative change in y will increase saturation - _saturation_status.flags.y_thrust_neg = true; - - } else if (_rotors[index].y_scale < 0.0f) { - // A positive change in y will increase saturation - _saturation_status.flags.y_thrust_pos = true; - } - - // check if the z input is saturating - if (_rotors[index].z_scale > 0.0f) { - // A negative change in z will increase saturation - _saturation_status.flags.z_thrust_neg = true; - - } else if (_rotors[index].z_scale < 0.0f) { - // A positive change in z will increase saturation - _saturation_status.flags.z_thrust_pos = true; - } - } - - if (clipping_low_yaw) { - // check if the yaw input is saturating - if (_rotors[index].yaw_scale > 0.0f) { - // A negative change in yaw will increase saturation - _saturation_status.flags.yaw_neg = true; - - } else if (_rotors[index].yaw_scale < 0.0f) { - // A positive change in yaw will increase saturation - _saturation_status.flags.yaw_pos = true; - } - } - - _saturation_status.flags.valid = true; -} diff --git a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py index f080442df3..e94f2b79e0 100755 --- a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py +++ b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py @@ -217,7 +217,7 @@ def normalize_mix_px4(B): return B_px -def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False): +def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False): ''' Generate C header file with same format as multi_tables.py TODO: rewrite using templates (see generation of uORB headers) @@ -225,12 +225,6 @@ def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, from io import StringIO buf = StringIO() - # Adapt fields based on options - if use_6dof: - mixer_class_name = "MultirotorMixer6dof" - else: - mixer_class_name = "MultirotorMixer" - # Print Header buf.write(u"/*\n") buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n") @@ -258,28 +252,38 @@ def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, else: mix = geometry['mix']['B'] - buf.write(u"static constexpr {}::Rotor _config_{}[] {{\n".format(mixer_class_name, geometry['info']['name'])) + buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name'])) for row in mix: - if use_6dof: - # 6dof mixer - buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( - row[0], row[1], row[2], - row[3], row[4], row[5])) - else: # 4dof mixer - buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( - row[0], row[1], row[2], - -row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly + buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + row[0], row[1], row[2], + -row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly + + buf.write(u"};\n\n") + + buf.write(u"static constexpr MultirotorMixer::Rotor6Dof _config_6dof_{}[] {{\n".format(geometry['info']['name'])) + + for row in mix: + # 6dof mixer + buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + row[0], row[1], row[2], + row[3], row[4], row[5])) buf.write(u"};\n\n") # Print geometry indices - buf.write(u"static constexpr const {}::Rotor *_config_index[] {{\n".format(mixer_class_name)) + buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n") for geometry in geometries_list: buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name'])) buf.write(u"};\n\n") + # Print geometry indices for 6Dof + buf.write(u"static constexpr const MultirotorMixer::Rotor6Dof *_config_index_6dof[] {\n") + for geometry in geometries_list: + buf.write(u"\t&_config_6dof_{}[0],\n".format(geometry['info']['name'])) + buf.write(u"};\n\n") + # Print geometry rotor counts buf.write(u"static constexpr unsigned _config_rotor_count[] {\n") for geometry in geometries_list: @@ -318,8 +322,6 @@ if __name__ == '__main__': action='store_true') parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)', action='store_true') - parser.add_argument('--sixdof', help='Use 6dof mixers', - action='store_true') args = parser.parse_args() # Find toml files @@ -386,8 +388,7 @@ if __name__ == '__main__': # Generate header file header = generate_mixer_multirotor_header(geometries_list, - use_normalized_mix=args.normalize, - use_6dof=args.sixdof) + use_normalized_mix=args.normalize) if args.outputfile is not None: # Write header file