From 6a7e53e5de25a6c119ebe2fe407b3bf0e717d25a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 15 Apr 2025 19:07:40 +0200 Subject: [PATCH 01/13] ActuatorEffectivenessRotorsTest: add Hexarotor X --- .../ActuatorEffectivenessRotorsTest.cpp | 87 +++++++++++++------ 1 file changed, 60 insertions(+), 27 deletions(-) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp index 9a6e45e1ad6f..9f77fa9da5e7 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp @@ -44,50 +44,32 @@ using namespace matrix; -TEST(ActuatorEffectivenessRotors, AllZeroCase) +TEST(ActuatorEffectivenessRotors, QuadrotorX) { - // Quad wide geometry ActuatorEffectivenessRotors::Geometry geometry = {}; - geometry.rotors[0].position(0) = 1.0f; - geometry.rotors[0].position(1) = 1.0f; - geometry.rotors[0].position(2) = 0.0f; - geometry.rotors[0].axis(0) = 0.0f; - geometry.rotors[0].axis(1) = 0.0f; - geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].position = {1.f, 1.f, 0.f}; + geometry.rotors[0].axis = {0.f, 0.f, -1.f}; geometry.rotors[0].thrust_coef = 1.0f; geometry.rotors[0].moment_ratio = 0.05f; - geometry.rotors[1].position(0) = -1.0f; - geometry.rotors[1].position(1) = -1.0f; - geometry.rotors[1].position(2) = 0.0f; - geometry.rotors[1].axis(0) = 0.0f; - geometry.rotors[1].axis(1) = 0.0f; - geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].position = {-1.f, -1.f, 0.f}; + geometry.rotors[1].axis = {0.f, 0.f, -1.f}; geometry.rotors[1].thrust_coef = 1.0f; geometry.rotors[1].moment_ratio = 0.05f; - geometry.rotors[2].position(0) = 1.0f; - geometry.rotors[2].position(1) = -1.0f; - geometry.rotors[2].position(2) = 0.0f; - geometry.rotors[2].axis(0) = 0.0f; - geometry.rotors[2].axis(1) = 0.0f; - geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].position = {1.f, -1.f, 0.f}; + geometry.rotors[2].axis = {0.f, 0.f, -1.f}; geometry.rotors[2].thrust_coef = 1.0f; geometry.rotors[2].moment_ratio = -0.05f; - geometry.rotors[3].position(0) = -1.0f; - geometry.rotors[3].position(1) = 1.0f; - geometry.rotors[3].position(2) = 0.0f; - geometry.rotors[3].axis(0) = 0.0f; - geometry.rotors[3].axis(1) = 0.0f; - geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].position = {-1.f, 1.f, 0.f}; + geometry.rotors[3].axis = {0.f, 0.f, -1.f}; geometry.rotors[3].thrust_coef = 1.0f; geometry.rotors[3].moment_ratio = -0.05f; geometry.num_rotors = 4; ActuatorEffectiveness::EffectivenessMatrix effectiveness; - effectiveness.setZero(); ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { @@ -103,6 +85,57 @@ TEST(ActuatorEffectivenessRotors, AllZeroCase) EXPECT_EQ(effectiveness, effectiveness_expected); } +TEST(ActuatorEffectivenessRotors, HexarotorX) +{ + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position = {0.f, .5f, 0.f}; + geometry.rotors[0].axis = {0.f, 0.f, -1.f}; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = -0.05f; + + geometry.rotors[1].position = {0.f, -.5f, 0.f}; + geometry.rotors[1].axis = {0.f, 0.f, -1.f}; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position = {.43f, -.25f, 0.f}; + geometry.rotors[2].axis = {0.f, 0.f, -1.f}; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position = {-.43f, .25f, 0.f}; + geometry.rotors[3].axis = {0.f, 0.f, -1.f}; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = 0.05f; + + geometry.rotors[4].position = {.43f, .25f, 0.f}; + geometry.rotors[4].axis = {0.f, 0.f, -1.f}; + geometry.rotors[4].thrust_coef = 1.0f; + geometry.rotors[4].moment_ratio = 0.05f; + + geometry.rotors[5].position = {-.43f, -.25f, 0.f}; + geometry.rotors[5].axis = {0.f, 0.f, -1.f}; + geometry.rotors[5].thrust_coef = 1.0f; + geometry.rotors[5].moment_ratio = -0.05f; + + geometry.num_rotors = 6; + + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-0.5f, 0.5f, 0.25f, -0.25f, -0.25f, 0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.43f, -0.43f, 0.43f, -0.43f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.05f, 0.05f, -0.05f, 0.05f, 0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.f, -1.f, -1.f, -1.f, -1.f, -1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} + TEST(ActuatorEffectivenessRotors, Tilt) { Vector3f axis_expected{0.f, 0.f, -1.f}; From f42f925aafec9126099e64edd3796e2091c6e5ea Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 16 Apr 2025 11:11:06 +0200 Subject: [PATCH 02/13] Revert "Remove inclusion of rotors in library to enable test (#24286)" This reverts commit f7dadd9b89f3052735a12bd617755e78cf5305e2. --- .../control_allocation/CMakeLists.txt | 2 +- ...olAllocationSequentialDesaturationTest.cpp | 60 ++----------------- 2 files changed, 6 insertions(+), 56 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt index 4da638aac8cb..d65bd11fa0ec 100644 --- a/src/lib/control_allocation/control_allocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) -px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) +# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index cd478a1b003d..2e0af6bff46c 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,27 +40,17 @@ #include #include +#include using namespace matrix; namespace { -struct RotorGeometryTest { - matrix::Vector3f position; - matrix::Vector3f axis; - float thrust_coef; - float moment_ratio; -}; - -struct GeometryTest { - RotorGeometryTest rotors[ActuatorEffectiveness::NUM_ACTUATORS]; - int num_rotors{0}; -}; // Makes and returns a Geometry object for a "standard" quad-x quadcopter. -GeometryTest make_quad_x_geometry() +ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() { - GeometryTest geometry = {}; + ActuatorEffectivenessRotors::Geometry geometry = {}; geometry.rotors[0].position(0) = 1.0f; geometry.rotors[0].position(1) = 1.0f; geometry.rotors[0].position(2) = 0.0f; @@ -98,6 +88,7 @@ GeometryTest make_quad_x_geometry() geometry.rotors[3].moment_ratio = -0.05f; geometry.num_rotors = 4; + return geometry; } @@ -107,48 +98,7 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() ActuatorEffectiveness::EffectivenessMatrix effectiveness; effectiveness.setZero(); const auto geometry = make_quad_x_geometry(); - - // Minimalistically copied from ActuatorEffectivenessRotors::computeEffectivenessMatrix - for (int i = 0; i < geometry.num_rotors; i++) { - - // Get rotor axis - Vector3f axis = geometry.rotors[i].axis; - - // Normalize axis - float axis_norm = axis.norm(); - - if (axis_norm > FLT_EPSILON) { - axis /= axis_norm; - - } else { - // Bad axis definition, ignore this rotor - continue; - } - - // Get rotor position - const Vector3f &position = geometry.rotors[i].position; - - // Get coefficients - float ct = geometry.rotors[i].thrust_coef; - float km = geometry.rotors[i].moment_ratio; - - if (fabsf(ct) < FLT_EPSILON) { - continue; - } - - // Compute thrust generated by this rotor - matrix::Vector3f thrust = ct * axis; - - // Compute moment generated by this rotor - matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; - - // Fill corresponding items in effectiveness matrix - for (int j = 0; j < 3; j++) { - effectiveness(j, i) = moment(j); - effectiveness(j + 3, i) = thrust(j); - } - } - + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); return effectiveness; } From 2cbf10e1ed5ab37a9739f8526c367f7b80dfe9f2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 16 Apr 2025 11:21:09 +0200 Subject: [PATCH 03/13] ControlAllocationSequentialDesaturationTest: fix include instead of duplicating definitions --- src/lib/control_allocation/control_allocation/CMakeLists.txt | 2 +- .../ControlAllocationSequentialDesaturationTest.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt index d65bd11fa0ec..2f823e87766e 100644 --- a/src/lib/control_allocation/control_allocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) -# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) +px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation VehicleActuatorEffectiveness) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 2e0af6bff46c..232f3790bf61 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include using namespace matrix; From 4582bf8f9de9fea15c841451a1e9dae79c9562f1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 16 Apr 2025 11:21:36 +0200 Subject: [PATCH 04/13] control_allocator: fix PID dependency definition --- .../control_allocation/actuator_effectiveness/CMakeLists.txt | 1 - .../VehicleActuatorEffectiveness/CMakeLists.txt | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt index b8a3d0076daf..f4cdf0d0acf0 100644 --- a/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt +++ b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt @@ -41,5 +41,4 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D target_link_libraries(ActuatorEffectiveness PRIVATE mathlib - PID ) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt index 349893a3eaf2..76bb60c0f281 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt @@ -35,8 +35,9 @@ target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEV target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(VehicleActuatorEffectiveness PRIVATE - mathlib ActuatorEffectiveness + mathlib + PID ) px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness) From cb8bb6bae0daf07e9abce53140f0af8be059f4ac Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 21 Feb 2023 16:20:32 +0100 Subject: [PATCH 05/13] ActuatorEffectiveness: add comments to EffectivenessUpdateReason from information previously only available in the commit message. --- .../actuator_effectiveness/ActuatorEffectiveness.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 0ddd4988f335..444714b9604a 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -62,11 +62,10 @@ enum class ActuatorType { enum class EffectivenessUpdateReason { NO_EXTERNAL_UPDATE = 0, - CONFIGURATION_UPDATE = 1, - MOTOR_ACTIVATION_UPDATE = 2, + CONFIGURATION_UPDATE = 1, ///< config changes (parameter) + MOTOR_ACTIVATION_UPDATE = 2, ///< motor failure detected or certain redundant motors are switched off to save energy }; - class ActuatorEffectiveness { public: From 97f110a3ff98e51678fa761ca2534c4d9080982d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Apr 2025 17:41:12 +0200 Subject: [PATCH 06/13] control_allocation: Consistently replace with ActuatorVector alias for readability --- .../ActuatorEffectiveness.hpp | 5 ++- .../control_allocation/ControlAllocation.hpp | 32 +++++++++---------- .../control_allocator/ControlAllocator.cpp | 8 ++--- .../ActuatorEffectivenessCustom.cpp | 5 ++- .../ActuatorEffectivenessCustom.hpp | 5 ++- .../ActuatorEffectivenessFixedWing.cpp | 5 ++- .../ActuatorEffectivenessFixedWing.hpp | 5 ++- .../ActuatorEffectivenessHelicopter.cpp | 3 +- .../ActuatorEffectivenessHelicopter.hpp | 5 ++- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 3 +- ...ActuatorEffectivenessHelicopterCoaxial.hpp | 5 ++- .../ActuatorEffectivenessMCTilt.cpp | 5 ++- .../ActuatorEffectivenessMCTilt.hpp | 5 ++- .../ActuatorEffectivenessRoverAckermann.cpp | 3 +- .../ActuatorEffectivenessRoverAckermann.hpp | 5 ++- .../ActuatorEffectivenessStandardVTOL.cpp | 3 +- .../ActuatorEffectivenessStandardVTOL.hpp | 5 ++- .../ActuatorEffectivenessTailsitterVTOL.cpp | 3 +- .../ActuatorEffectivenessTailsitterVTOL.hpp | 5 ++- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 3 +- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 5 ++- .../ActuatorEffectivenessUUV.cpp | 5 ++- .../ActuatorEffectivenessUUV.hpp | 5 ++- 23 files changed, 56 insertions(+), 77 deletions(-) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp index 444714b9604a..ffc2315b7232 100644 --- a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -195,9 +195,8 @@ class ActuatorEffectiveness * It is called after the matrix multiplication and before final clipping. * @param actuator_sp input & output setpoint */ - virtual void updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) {} + virtual void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) {} /** * Get a bitmask of motors to be stopped diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp index a6e2e2b25420..a1061200262b 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -82,7 +82,7 @@ class ControlAllocation static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS; static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES; - typedef matrix::Vector ActuatorVector; + using ActuatorVector = matrix::Vector; enum ControlAxis { ROLL = 0, @@ -121,7 +121,7 @@ class ControlAllocation * * @return Actuator vector */ - const matrix::Vector &getActuatorSetpoint() const { return _actuator_sp; } + const ActuatorVector &getActuatorSetpoint() const { return _actuator_sp; } /** * Set the desired control vector @@ -157,28 +157,28 @@ class ControlAllocation * * @param actuator_min Minimum actuator values */ - void setActuatorMin(const matrix::Vector &actuator_min) { _actuator_min = actuator_min; } + void setActuatorMin(const ActuatorVector &actuator_min) { _actuator_min = actuator_min; } /** * Get the minimum actuator values * * @return Minimum actuator values */ - const matrix::Vector &getActuatorMin() const { return _actuator_min; } + const ActuatorVector &getActuatorMin() const { return _actuator_min; } /** * Set the maximum actuator values * * @param actuator_max Maximum actuator values */ - void setActuatorMax(const matrix::Vector &actuator_max) { _actuator_max = actuator_max; } + void setActuatorMax(const ActuatorVector &actuator_max) { _actuator_max = actuator_max; } /** * Get the maximum actuator values * * @return Maximum actuator values */ - const matrix::Vector &getActuatorMax() const { return _actuator_max; } + const ActuatorVector &getActuatorMax() const { return _actuator_max; } /** * Set the current actuator setpoint. @@ -189,9 +189,9 @@ class ControlAllocation * * @param actuator_sp Actuator setpoint */ - void setActuatorSetpoint(const matrix::Vector &actuator_sp); + void setActuatorSetpoint(const ActuatorVector &actuator_sp); - void setSlewRateLimit(const matrix::Vector &slew_rate_limit) + void setSlewRateLimit(const ActuatorVector &slew_rate_limit) { _actuator_slew_rate_limit = slew_rate_limit; } /** @@ -206,7 +206,7 @@ class ControlAllocation * * @param actuator Actuator vector to clip */ - void clipActuatorSetpoint(matrix::Vector &actuator) const; + void clipActuatorSetpoint(ActuatorVector &actuator) const; void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); } @@ -219,7 +219,7 @@ class ControlAllocation * * @return Clipped actuator setpoint */ - matrix::Vector normalizeActuatorSetpoint(const matrix::Vector &actuator) + ActuatorVector normalizeActuatorSetpoint(const ActuatorVector &actuator) const; virtual void updateParameters() {} @@ -233,12 +233,12 @@ class ControlAllocation matrix::Matrix _effectiveness; ///< Effectiveness matrix matrix::Vector _control_allocation_scale; ///< Scaling applied during allocation - matrix::Vector _actuator_trim; ///< Neutral actuator values - matrix::Vector _actuator_min; ///< Minimum actuator values - matrix::Vector _actuator_max; ///< Maximum actuator values - matrix::Vector _actuator_slew_rate_limit; ///< Slew rate limit - matrix::Vector _prev_actuator_sp; ///< Previous actuator setpoint - matrix::Vector _actuator_sp; ///< Actuator setpoint + ActuatorVector _actuator_trim; ///< Neutral actuator values + ActuatorVector _actuator_min; ///< Minimum actuator values + ActuatorVector _actuator_max; ///< Maximum actuator values + ActuatorVector _actuator_slew_rate_limit; ///< Slew rate limit + ActuatorVector _prev_actuator_sp; ///< Previous actuator setpoint + ActuatorVector _actuator_sp; ///< Actuator setpoint matrix::Vector _control_sp; ///< Control setpoint matrix::Vector _control_trim; ///< Control at trim actuator values int _num_actuators{0}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 99e280dd71b3..afba49d56604 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -150,7 +150,7 @@ ControlAllocator::update_allocation_method(bool force) if (_allocation_method_id != configured_method || force) { - matrix::Vector actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES]; + ActuatorVector actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES]; // Cleanup first for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) { @@ -639,9 +639,9 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f); // Actuator saturation - const matrix::Vector &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint(); - const matrix::Vector &actuator_min = _control_allocation[matrix_index]->getActuatorMin(); - const matrix::Vector &actuator_max = _control_allocation[matrix_index]->getActuatorMax(); + const ActuatorVector &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint(); + const ActuatorVector &actuator_min = _control_allocation[matrix_index]->getActuatorMin(); + const ActuatorVector &actuator_max = _control_allocation[matrix_index]->getActuatorMax(); for (int i = 0; i < NUM_ACTUATORS; i++) { if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index ead2043682c0..6e5ccbe44a54 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -59,9 +59,8 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration return (motors_added_successfully && torque_added_successfully); } -void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) +void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index b7906669f282..e2f5d4dfa60c 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -45,9 +45,8 @@ class ActuatorEffectivenessCustom : public ModuleParams, public ActuatorEffectiv bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; const char *name() const override { return "Custom"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 0a4516ba56fb..e91783cf8086 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -61,9 +61,8 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat return (rotors_added_successfully && surfaces_added_successfully); } -void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) +void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index f0b095709ebf..0633cbc875dd 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -51,9 +51,8 @@ class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffec void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; private: ActuatorEffectivenessRotors _rotors; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 967706a6d8aa..67182b059088 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -144,8 +144,7 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf } void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 78df3abec1ca..fda121c0b58a 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -81,9 +81,8 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe const Geometry &geometry() const { return _geometry; } - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 40e956df9933..ab38b3192242 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -102,8 +102,7 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio } void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { _saturation_flags = {}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index e97c31c0dad3..2156eb4a0276 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -69,9 +69,8 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua const Geometry &geometry() const { return _geometry; } - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index a8effa8cb315..984ab00f9de9 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -76,9 +76,8 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration return (rotors_added_successfully && tilts_added_successfully); } -void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) +void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { actuator_sp += _tilt_offsets; // TODO: dynamic matrix update diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 0b12482781fe..2fd5923824e7 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -55,9 +55,8 @@ class ActuatorEffectivenessMCTilt : public ModuleParams, public ActuatorEffectiv normalize[0] = true; } - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; const char *name() const override { return "MC Tilt"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index e9eda4c5387f..45256b87cc82 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -50,8 +50,7 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi } void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index f6108b4bafd9..82b07733bffc 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -43,9 +43,8 @@ class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; const char *name() const override { return "Rover (Ackermann)"; } private: diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index f15624dd6749..0b7d3934c1fa 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -84,8 +84,7 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, } void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { if (matrix_index == 0) { stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 9f945a6cd899..4879c6b022cf 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -75,9 +75,8 @@ class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEf void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; void setFlightPhase(const FlightPhase &flight_phase) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 559101c106d7..05fcaf675e46 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -89,8 +89,7 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d } void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { if (matrix_index == 0) { stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 708f104faa42..c7a588954f53 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -72,9 +72,8 @@ class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public Actuator void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; void setFlightPhase(const FlightPhase &flight_phase) override; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 93ac6783d65a..3042ab825b4a 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -125,8 +125,7 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt } void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) + int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { // apply tilt if (matrix_index == 0) { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 512bd41f419e..db7b37304bff 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -80,9 +80,8 @@ class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorE void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; const char *name() const override { return "VTOL Tiltrotor"; } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp index e4c861a9d0b0..7bc7730cfbfc 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp @@ -55,9 +55,8 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati return rotors_added_successfully; } -void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) +void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) { stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); } diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 1b89da8d9b62..78e52c34aa9b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -54,9 +54,8 @@ class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectivene normalize[0] = true; } - void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, - const matrix::Vector &actuator_max) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, + const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override; const char *name() const override { return "UUV"; } From 1dd92ee07e3b3cf53443ffcd1b590e5eb8271c2c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 16 Feb 2023 14:31:12 +0100 Subject: [PATCH 07/13] ControlAllocationSequentialDesaturation: add unit tests --- .../control_allocation/CMakeLists.txt | 1 + ...lAllocationSequentialDesaturationTest1.cpp | 210 ++++++++++++++++++ 2 files changed, 211 insertions(+) create mode 100644 src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp diff --git a/src/lib/control_allocation/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt index 2f823e87766e..75660e75e9eb 100644 --- a/src/lib/control_allocation/control_allocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -45,3 +45,4 @@ target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation VehicleActuatorEffectiveness) +px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest1.cpp LINKLIBS ControlAllocation VehicleActuatorEffectiveness) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp new file mode 100644 index 000000000000..0fd336a8fe57 --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 +#include +#include + +using namespace matrix; + +TEST(ControlAllocationSequentialDesaturationTest, AllZeroCase) +{ + ControlAllocationSequentialDesaturation control_allocation; + EXPECT_EQ(control_allocation.getActuatorSetpoint(), (Vector())); + control_allocation.allocate(); + EXPECT_EQ(control_allocation.getActuatorSetpoint(), (Vector())); +} + +TEST(ControlAllocationSequentialDesaturationTest, SetGetActuatorSetpoint) +{ + ControlAllocationSequentialDesaturation control_allocation; + float actuator_setpoint_array[ControlAllocation::NUM_ACTUATORS] = {1.f, 2.f, 3.f, 4.f, 5.f, 6.f}; + Vector actuator_setpoint(actuator_setpoint_array); + control_allocation.setActuatorSetpoint(actuator_setpoint); + EXPECT_EQ(control_allocation.getActuatorSetpoint(), actuator_setpoint); +} + +// Make protected updateParams() function available to the unit test to change airmode after initialization +class TestControlAllocationSequentialDesaturation : public ::ControlAllocationSequentialDesaturation +{ +public: + void updateParams() { ControlAllocationSequentialDesaturation::updateParams(); } +}; + +class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test +{ +public: + void SetUp() override + { + // Quadrotor x geometry + ActuatorEffectivenessRotors::Geometry quadx_geometry{}; + quadx_geometry.num_rotors = 4; + quadx_geometry.rotors[0].position = Vector3f(1.f, 1.f, 0.f); // clockwise motor numbering + quadx_geometry.rotors[1].position = Vector3f(-1.f, 1.f, 0.f); + quadx_geometry.rotors[2].position = Vector3f(-1.f, -1.f, 0.f); + quadx_geometry.rotors[3].position = Vector3f(1.f, -1.f, 0.f); + quadx_geometry.rotors[0].moment_ratio = 1.f; + quadx_geometry.rotors[1].moment_ratio = -1.f; + quadx_geometry.rotors[2].moment_ratio = 1.f; + quadx_geometry.rotors[3].moment_ratio = -1.f; + + for (int i = 0; i < 4; ++i) { + quadx_geometry.rotors[i].axis = Vector3f(0.f, 0.f, -1.f); // thrust downwards + quadx_geometry.rotors[i].thrust_coef = 1.f; + quadx_geometry.rotors[i].tilt_index = -1; + } + + // Compute actuator effectiveness + ActuatorEffectiveness::Configuration actuator_configuration{}; + int num_actuators = ActuatorEffectivenessRotors::computeEffectivenessMatrix(quadx_geometry, + actuator_configuration.effectiveness_matrices[0], + actuator_configuration.num_actuators_matrix[0]); + actuator_configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + + // Load effectiveness into allocation + _control_allocation.setEffectivenessMatrix(actuator_configuration.effectiveness_matrices[0], + actuator_configuration.trim[0], actuator_configuration.linearization_point[0], + actuator_configuration.num_actuators_matrix[0], true /*update_normalization_scale*/); + } + + void setAirmode(const int32_t mode) + { + param_control_autosave(false); // Disable autosaving parameters to avoid busy loop in param_set() + param_t param = param_find("MC_AIRMODE"); + param_set(param, &mode); + _control_allocation.updateParams(); + } + + Vector4f allocate(float roll, float pitch, float yaw, float thrust) + { + Vector control_setpoint; + control_setpoint(ControlAllocation::ControlAxis::ROLL) = roll; + control_setpoint(ControlAllocation::ControlAxis::PITCH) = pitch; + control_setpoint(ControlAllocation::ControlAxis::YAW) = yaw; + control_setpoint(ControlAllocation::ControlAxis::THRUST_Z) = thrust; + _control_allocation.setControlSetpoint(control_setpoint); + _control_allocation.allocate(); + return getQuadOutputs(); + } + + Vector4f getQuadOutputs() + { + // All unused actuators shall stay zero + static constexpr uint8_t NUM_UNUSED_ACTUATORS = ControlAllocation::NUM_ACTUATORS - 4; + EXPECT_EQ((Vector(_control_allocation.getActuatorSetpoint().slice + (4, 0))), (Vector())); + return Vector4f(_control_allocation.getActuatorSetpoint().slice<4, 1>(0, 0)); + } + + void collectiveThrustTest(const float thrust) + { + EXPECT_EQ(allocate(0.f, 0.f, 0.f, -thrust), Vector4f(thrust, thrust, thrust, thrust)); + } + + TestControlAllocationSequentialDesaturation _control_allocation; +}; + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, Zero) +{ + EXPECT_EQ(allocate(0.f, 0.f, 0.f, 0.f), Vector4f()); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, CollectiveThrust) +{ + for (float thrust = 0.f; thrust >= 1.f; thrust += .1f) { + collectiveThrustTest(thrust); + } +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYaw) +{ + EXPECT_EQ(allocate(1.f, 0.f, 0.f, -.5f), Vector4f(.25f, .25f, .75f, .75f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -.5f), Vector4f(.75f, .75f, .25f, .25f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, -.5f), Vector4f(.75f, .25f, .25f, .75f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, -.5f), Vector4f(.25f, .75f, .75f, .25f)); + EXPECT_EQ(allocate(0.f, 0.f, 1.f, -.5f), Vector4f(.75f, .25f, .75f, .25f)); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, -.5f), Vector4f(.25f, .75f, .25f, .75f)); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawFullThrust) +{ + EXPECT_EQ(allocate(1.f, 0.f, 0.f, -1.f), Vector4f(.5f, .5f, 1.f, 1.f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -1.f), Vector4f(1.f, 1.f, .5f, .5f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, -1.f), Vector4f(1.f, .5f, .5f, 1.f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, -1.f), Vector4f(.5f, 1.f, 1.f, .5f)); + // There is a special case to deprioritize yaw down to 30% authority with maximum thrust + EXPECT_EQ(allocate(0.f, 0.f, 1.f, -1.f), Vector4f(1.f, .7f, 1.f, .7f)); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, -1.f), Vector4f(.7f, 1.f, .7f, 1.f)); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrust) +{ + // No axis is allocated + EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f()); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustAirmodeRP) +{ + setAirmode(1); // Roll and pitch airmode + // Roll and pitch get fully allocated + EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f(0.5f, 0.5f, 0.f, 0.f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f(0.5f, 0.f, 0.f, 0.5f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f(0.f, 0.5f, 0.5f, 0.f)); + // Yaw is not allocated + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f()); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustAirmodeRPY) +{ + setAirmode(2); // Roll, pitch and yaw airmode + // All axis are fully allocated + EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f(0.5f, 0.5f, 0.f, 0.f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f(0.5f, 0.f, 0.f, 0.5f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f(0.f, 0.5f, 0.5f, 0.f)); + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f(.5f, 0.f, .5f, 0.f)); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f(0.f, .5f, 0.f, .5f)); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, TEMP) +{ + setAirmode(2); // Roll, pitch and yaw airmode + EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); +} From 677fe5c89f2c8a03fd27e2c0d6cf9d397a7600fb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 30 Apr 2025 13:45:22 +0200 Subject: [PATCH 08/13] ControlAllocationSequentialDesaturation: adopt improved unit test fixture This makes it much easier to go through allocation secnarios in one line with comprehensive numbers. --- .../control_allocation/CMakeLists.txt | 1 - ...olAllocationSequentialDesaturationTest.cpp | 180 ++++++++++++++- ...lAllocationSequentialDesaturationTest1.cpp | 210 ------------------ 3 files changed, 171 insertions(+), 220 deletions(-) delete mode 100644 src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp diff --git a/src/lib/control_allocation/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt index 75660e75e9eb..2f823e87766e 100644 --- a/src/lib/control_allocation/control_allocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -45,4 +45,3 @@ target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation VehicleActuatorEffectiveness) -px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest1.cpp LINKLIBS ControlAllocation VehicleActuatorEffectiveness) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 232f3790bf61..0a6feaef3ba3 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * Copyright (C) 2025 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 @@ -31,18 +31,180 @@ * ****************************************************************************/ -/** - * @file ControlAllocationSequentialDesaturationTest.cpp - * - * Tests for Control Allocation Sequential Desaturation Algorithms - * - */ - #include -#include #include +#include "ControlAllocationSequentialDesaturation.hpp" using namespace matrix; +using ActuatorVector = ControlAllocation::ActuatorVector; + +TEST(ControlAllocationSequentialDesaturationTest, AllZeroCase) +{ + ControlAllocationSequentialDesaturation control_allocation; + EXPECT_EQ(control_allocation.getActuatorSetpoint(), ActuatorVector()); + control_allocation.allocate(); + EXPECT_EQ(control_allocation.getActuatorSetpoint(), ActuatorVector()); +} + +TEST(ControlAllocationSequentialDesaturationTest, SetGetActuatorSetpoint) +{ + ControlAllocationSequentialDesaturation control_allocation; + float actuator_setpoint_array[ControlAllocation::NUM_ACTUATORS] = {1.f, 2.f, 3.f, 4.f, 5.f, 6.f}; + ActuatorVector actuator_setpoint(actuator_setpoint_array); + control_allocation.setActuatorSetpoint(actuator_setpoint); + EXPECT_EQ(control_allocation.getActuatorSetpoint(), actuator_setpoint); +} + +// Make protected updateParams() function available to the unit test to change airmode after initialization +class TestControlAllocationSequentialDesaturation : public ::ControlAllocationSequentialDesaturation +{ +public: + void updateParams() { ControlAllocationSequentialDesaturation::updateParams(); } +}; + +class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test +{ +public: + TestControlAllocationSequentialDesaturation _control_allocation; + + void SetUp() override + { + // Quadrotor x geometry + ActuatorEffectivenessRotors::Geometry quadx_geometry{}; + quadx_geometry.num_rotors = 4; + quadx_geometry.rotors[0].position = {1.f, 1.f, 0.f}; // clockwise motor numbering + quadx_geometry.rotors[1].position = {-1.f, 1.f, 0.f}; + quadx_geometry.rotors[2].position = {-1.f, -1.f, 0.f}; + quadx_geometry.rotors[3].position = {1.f, -1.f, 0.f}; + quadx_geometry.rotors[0].moment_ratio = 1.f; + quadx_geometry.rotors[1].moment_ratio = -1.f; + quadx_geometry.rotors[2].moment_ratio = 1.f; + quadx_geometry.rotors[3].moment_ratio = -1.f; + + for (int i = 0; i < 4; ++i) { + quadx_geometry.rotors[i].axis = Vector3f(0.f, 0.f, -1.f); // thrust downwards + quadx_geometry.rotors[i].thrust_coef = 1.f; + quadx_geometry.rotors[i].tilt_index = -1; + } + + // Compute actuator effectiveness + ActuatorEffectiveness::Configuration actuator_configuration{}; + int num_actuators = ActuatorEffectivenessRotors::computeEffectivenessMatrix(quadx_geometry, + actuator_configuration.effectiveness_matrices[0], + actuator_configuration.num_actuators_matrix[0]); + actuator_configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + + // Load effectiveness into allocation + _control_allocation.setEffectivenessMatrix(actuator_configuration.effectiveness_matrices[0], + actuator_configuration.trim[0], actuator_configuration.linearization_point[0], + actuator_configuration.num_actuators_matrix[0], true /*update_normalization_scale*/); + } + + void setAirmode(const int32_t mode) + { + param_control_autosave(false); // Disable autosaving parameters to avoid busy loop in param_set() + param_t param = param_find("MC_AIRMODE"); + param_set(param, &mode); + _control_allocation.updateParams(); + } + + Vector4f allocate(float roll, float pitch, float yaw, float thrust) + { + Vector control_setpoint{}; + control_setpoint(ControlAllocation::ControlAxis::ROLL) = roll; + control_setpoint(ControlAllocation::ControlAxis::PITCH) = pitch; + control_setpoint(ControlAllocation::ControlAxis::YAW) = yaw; + control_setpoint(ControlAllocation::ControlAxis::THRUST_Z) = thrust; + _control_allocation.setControlSetpoint(control_setpoint); + _control_allocation.allocate(); + return getQuadOutputs(); + } + + Vector4f getQuadOutputs() + { + const ActuatorVector &actuator_setpoint = _control_allocation.getActuatorSetpoint(); + // All unused actuators shall stay zero + static constexpr uint8_t NUM_UNUSED_ACTUATORS = ControlAllocation::NUM_ACTUATORS - 4; + EXPECT_EQ( + (Vector(actuator_setpoint.slice(4, 0))), + (Vector()) + ); + return Vector4f(actuator_setpoint.slice<4, 1>(0, 0)); + } +}; + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, Zero) +{ + EXPECT_EQ(allocate(0.f, 0.f, 0.f, 0.f), Vector4f()); +} + + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, CollectiveThrust) +{ + for (float thrust = 0.f; thrust <= (1.f + FLT_EPSILON); thrust += .1f) { + EXPECT_EQ(allocate(0.f, 0.f, 0.f, -thrust), Vector4f(thrust, thrust, thrust, thrust)); + } +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYaw) +{ + setAirmode(0); // Airmode disabled + EXPECT_EQ(allocate(1.f, 0.f, 0.f, -.5f), Vector4f(.25f, .25f, .75f, .75f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -.5f), Vector4f(.75f, .75f, .25f, .25f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, -.5f), Vector4f(.75f, .25f, .25f, .75f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, -.5f), Vector4f(.25f, .75f, .75f, .25f)); + EXPECT_EQ(allocate(0.f, 0.f, 1.f, -.5f), Vector4f(.75f, .25f, .75f, .25f)); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, -.5f), Vector4f(.25f, .75f, .25f, .75f)); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawFullThrust) +{ + setAirmode(0); // Airmode disabled + EXPECT_EQ(allocate(1.f, 0.f, 0.f, -1.f), Vector4f(.5f, .5f, 1.f, 1.f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -1.f), Vector4f(1.f, 1.f, .5f, .5f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, -1.f), Vector4f(1.f, .5f, .5f, 1.f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, -1.f), Vector4f(.5f, 1.f, 1.f, .5f)); + // There is a special case to deprioritize yaw down to 30% authority with maximum thrust + EXPECT_EQ(allocate(0.f, 0.f, 1.f, -1.f), Vector4f(1.f, .7f, 1.f, .7f)); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, -1.f), Vector4f(.7f, 1.f, .7f, 1.f)); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrust) +{ + setAirmode(0); // Airmode disabled + // No axis is allocated + EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f()); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustAirmodeRP) +{ + setAirmode(1); // Roll and pitch airmode + // Roll and pitch get fully allocated + EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f(0.5f, 0.5f, 0.f, 0.f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f(0.5f, 0.f, 0.f, 0.5f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f(0.f, 0.5f, 0.5f, 0.f)); + // Yaw is not allocated + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f()); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f()); +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustAirmodeRPY) +{ + setAirmode(2); // Roll, pitch and yaw airmode + // All axis are fully allocated + EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); + EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f(0.5f, 0.5f, 0.f, 0.f)); + EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f(0.5f, 0.f, 0.f, 0.5f)); + EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f(0.f, 0.5f, 0.5f, 0.f)); + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f(.5f, 0.f, .5f, 0.f)); + EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f(0.f, .5f, 0.f, .5f)); +} namespace { diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp deleted file mode 100644 index 0fd336a8fe57..000000000000 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest1.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2023 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 -#include -#include - -using namespace matrix; - -TEST(ControlAllocationSequentialDesaturationTest, AllZeroCase) -{ - ControlAllocationSequentialDesaturation control_allocation; - EXPECT_EQ(control_allocation.getActuatorSetpoint(), (Vector())); - control_allocation.allocate(); - EXPECT_EQ(control_allocation.getActuatorSetpoint(), (Vector())); -} - -TEST(ControlAllocationSequentialDesaturationTest, SetGetActuatorSetpoint) -{ - ControlAllocationSequentialDesaturation control_allocation; - float actuator_setpoint_array[ControlAllocation::NUM_ACTUATORS] = {1.f, 2.f, 3.f, 4.f, 5.f, 6.f}; - Vector actuator_setpoint(actuator_setpoint_array); - control_allocation.setActuatorSetpoint(actuator_setpoint); - EXPECT_EQ(control_allocation.getActuatorSetpoint(), actuator_setpoint); -} - -// Make protected updateParams() function available to the unit test to change airmode after initialization -class TestControlAllocationSequentialDesaturation : public ::ControlAllocationSequentialDesaturation -{ -public: - void updateParams() { ControlAllocationSequentialDesaturation::updateParams(); } -}; - -class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test -{ -public: - void SetUp() override - { - // Quadrotor x geometry - ActuatorEffectivenessRotors::Geometry quadx_geometry{}; - quadx_geometry.num_rotors = 4; - quadx_geometry.rotors[0].position = Vector3f(1.f, 1.f, 0.f); // clockwise motor numbering - quadx_geometry.rotors[1].position = Vector3f(-1.f, 1.f, 0.f); - quadx_geometry.rotors[2].position = Vector3f(-1.f, -1.f, 0.f); - quadx_geometry.rotors[3].position = Vector3f(1.f, -1.f, 0.f); - quadx_geometry.rotors[0].moment_ratio = 1.f; - quadx_geometry.rotors[1].moment_ratio = -1.f; - quadx_geometry.rotors[2].moment_ratio = 1.f; - quadx_geometry.rotors[3].moment_ratio = -1.f; - - for (int i = 0; i < 4; ++i) { - quadx_geometry.rotors[i].axis = Vector3f(0.f, 0.f, -1.f); // thrust downwards - quadx_geometry.rotors[i].thrust_coef = 1.f; - quadx_geometry.rotors[i].tilt_index = -1; - } - - // Compute actuator effectiveness - ActuatorEffectiveness::Configuration actuator_configuration{}; - int num_actuators = ActuatorEffectivenessRotors::computeEffectivenessMatrix(quadx_geometry, - actuator_configuration.effectiveness_matrices[0], - actuator_configuration.num_actuators_matrix[0]); - actuator_configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); - - // Load effectiveness into allocation - _control_allocation.setEffectivenessMatrix(actuator_configuration.effectiveness_matrices[0], - actuator_configuration.trim[0], actuator_configuration.linearization_point[0], - actuator_configuration.num_actuators_matrix[0], true /*update_normalization_scale*/); - } - - void setAirmode(const int32_t mode) - { - param_control_autosave(false); // Disable autosaving parameters to avoid busy loop in param_set() - param_t param = param_find("MC_AIRMODE"); - param_set(param, &mode); - _control_allocation.updateParams(); - } - - Vector4f allocate(float roll, float pitch, float yaw, float thrust) - { - Vector control_setpoint; - control_setpoint(ControlAllocation::ControlAxis::ROLL) = roll; - control_setpoint(ControlAllocation::ControlAxis::PITCH) = pitch; - control_setpoint(ControlAllocation::ControlAxis::YAW) = yaw; - control_setpoint(ControlAllocation::ControlAxis::THRUST_Z) = thrust; - _control_allocation.setControlSetpoint(control_setpoint); - _control_allocation.allocate(); - return getQuadOutputs(); - } - - Vector4f getQuadOutputs() - { - // All unused actuators shall stay zero - static constexpr uint8_t NUM_UNUSED_ACTUATORS = ControlAllocation::NUM_ACTUATORS - 4; - EXPECT_EQ((Vector(_control_allocation.getActuatorSetpoint().slice - (4, 0))), (Vector())); - return Vector4f(_control_allocation.getActuatorSetpoint().slice<4, 1>(0, 0)); - } - - void collectiveThrustTest(const float thrust) - { - EXPECT_EQ(allocate(0.f, 0.f, 0.f, -thrust), Vector4f(thrust, thrust, thrust, thrust)); - } - - TestControlAllocationSequentialDesaturation _control_allocation; -}; - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, Zero) -{ - EXPECT_EQ(allocate(0.f, 0.f, 0.f, 0.f), Vector4f()); -} - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, CollectiveThrust) -{ - for (float thrust = 0.f; thrust >= 1.f; thrust += .1f) { - collectiveThrustTest(thrust); - } -} - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYaw) -{ - EXPECT_EQ(allocate(1.f, 0.f, 0.f, -.5f), Vector4f(.25f, .25f, .75f, .75f)); - EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -.5f), Vector4f(.75f, .75f, .25f, .25f)); - EXPECT_EQ(allocate(0.f, 1.f, 0.f, -.5f), Vector4f(.75f, .25f, .25f, .75f)); - EXPECT_EQ(allocate(0.f, -1.f, 0.f, -.5f), Vector4f(.25f, .75f, .75f, .25f)); - EXPECT_EQ(allocate(0.f, 0.f, 1.f, -.5f), Vector4f(.75f, .25f, .75f, .25f)); - EXPECT_EQ(allocate(0.f, 0.f, -1.f, -.5f), Vector4f(.25f, .75f, .25f, .75f)); -} - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawFullThrust) -{ - EXPECT_EQ(allocate(1.f, 0.f, 0.f, -1.f), Vector4f(.5f, .5f, 1.f, 1.f)); - EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -1.f), Vector4f(1.f, 1.f, .5f, .5f)); - EXPECT_EQ(allocate(0.f, 1.f, 0.f, -1.f), Vector4f(1.f, .5f, .5f, 1.f)); - EXPECT_EQ(allocate(0.f, -1.f, 0.f, -1.f), Vector4f(.5f, 1.f, 1.f, .5f)); - // There is a special case to deprioritize yaw down to 30% authority with maximum thrust - EXPECT_EQ(allocate(0.f, 0.f, 1.f, -1.f), Vector4f(1.f, .7f, 1.f, .7f)); - EXPECT_EQ(allocate(0.f, 0.f, -1.f, -1.f), Vector4f(.7f, 1.f, .7f, 1.f)); -} - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrust) -{ - // No axis is allocated - EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f()); - EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f()); - EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f()); - EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f()); - EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f()); - EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f()); -} - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustAirmodeRP) -{ - setAirmode(1); // Roll and pitch airmode - // Roll and pitch get fully allocated - EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); - EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f(0.5f, 0.5f, 0.f, 0.f)); - EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f(0.5f, 0.f, 0.f, 0.5f)); - EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f(0.f, 0.5f, 0.5f, 0.f)); - // Yaw is not allocated - EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f()); - EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f()); -} - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustAirmodeRPY) -{ - setAirmode(2); // Roll, pitch and yaw airmode - // All axis are fully allocated - EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); - EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f(0.5f, 0.5f, 0.f, 0.f)); - EXPECT_EQ(allocate(0.f, 1.f, 0.f, 0.f), Vector4f(0.5f, 0.f, 0.f, 0.5f)); - EXPECT_EQ(allocate(0.f, -1.f, 0.f, 0.f), Vector4f(0.f, 0.5f, 0.5f, 0.f)); - EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f(.5f, 0.f, .5f, 0.f)); - EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f(0.f, .5f, 0.f, .5f)); -} - -TEST_F(ControlAllocationSequentialDesaturationTestQuadX, TEMP) -{ - setAirmode(2); // Roll, pitch and yaw airmode - EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f(0.f, 0.f, 0.5f, 0.5f)); -} From 80f4f6123e68501777d8b9429af27d91c9e10b29 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 30 Apr 2025 14:27:13 +0200 Subject: [PATCH 09/13] ControlAllocationSequentialDesaturationTest: convert existing unit test cases to improved fixture --- ...olAllocationSequentialDesaturationTest.cpp | 342 ++---------------- 1 file changed, 38 insertions(+), 304 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 0a6feaef3ba3..c5fa0b1bec0b 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -65,6 +65,7 @@ class TestControlAllocationSequentialDesaturation : public ::ControlAllocationSe class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test { public: + static constexpr uint8_t NUM_ACTUATORS = 4; TestControlAllocationSequentialDesaturation _control_allocation; void SetUp() override @@ -92,6 +93,7 @@ class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test int num_actuators = ActuatorEffectivenessRotors::computeEffectivenessMatrix(quadx_geometry, actuator_configuration.effectiveness_matrices[0], actuator_configuration.num_actuators_matrix[0]); + EXPECT_EQ(num_actuators, NUM_ACTUATORS); actuator_configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); // Load effectiveness into allocation @@ -133,6 +135,9 @@ class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test } }; +// Make constant available, see https://stackoverflow.com/questions/42756443/undefined-reference-with-gtest +constexpr uint8_t ControlAllocationSequentialDesaturationTestQuadX::NUM_ACTUATORS; + TEST_F(ControlAllocationSequentialDesaturationTestQuadX, Zero) { EXPECT_EQ(allocate(0.f, 0.f, 0.f, 0.f), Vector4f()); @@ -206,342 +211,71 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustA EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f(0.f, .5f, 0.f, .5f)); } -namespace -{ - -// Makes and returns a Geometry object for a "standard" quad-x quadcopter. -ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() -{ - ActuatorEffectivenessRotors::Geometry geometry = {}; - geometry.rotors[0].position(0) = 1.0f; - geometry.rotors[0].position(1) = 1.0f; - geometry.rotors[0].position(2) = 0.0f; - geometry.rotors[0].axis(0) = 0.0f; - geometry.rotors[0].axis(1) = 0.0f; - geometry.rotors[0].axis(2) = -1.0f; - geometry.rotors[0].thrust_coef = 1.0f; - geometry.rotors[0].moment_ratio = 0.05f; - - geometry.rotors[1].position(0) = -1.0f; - geometry.rotors[1].position(1) = -1.0f; - geometry.rotors[1].position(2) = 0.0f; - geometry.rotors[1].axis(0) = 0.0f; - geometry.rotors[1].axis(1) = 0.0f; - geometry.rotors[1].axis(2) = -1.0f; - geometry.rotors[1].thrust_coef = 1.0f; - geometry.rotors[1].moment_ratio = 0.05f; - - geometry.rotors[2].position(0) = 1.0f; - geometry.rotors[2].position(1) = -1.0f; - geometry.rotors[2].position(2) = 0.0f; - geometry.rotors[2].axis(0) = 0.0f; - geometry.rotors[2].axis(1) = 0.0f; - geometry.rotors[2].axis(2) = -1.0f; - geometry.rotors[2].thrust_coef = 1.0f; - geometry.rotors[2].moment_ratio = -0.05f; - - geometry.rotors[3].position(0) = -1.0f; - geometry.rotors[3].position(1) = 1.0f; - geometry.rotors[3].position(2) = 0.0f; - geometry.rotors[3].axis(0) = 0.0f; - geometry.rotors[3].axis(1) = 0.0f; - geometry.rotors[3].axis(2) = -1.0f; - geometry.rotors[3].thrust_coef = 1.0f; - geometry.rotors[3].moment_ratio = -0.05f; - - geometry.num_rotors = 4; - - return geometry; -} - -// Returns an effective matrix for a sample quad-copter configuration. -ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() -{ - ActuatorEffectiveness::EffectivenessMatrix effectiveness; - effectiveness.setZero(); - const auto geometry = make_quad_x_geometry(); - ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); - return effectiveness; -} - -// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter. -void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator) -{ - const auto effectiveness = make_quad_x_effectiveness(); - matrix::Vector actuator_trim; - matrix::Vector linearization_point; - constexpr bool UPDATE_NORMALIZATION_SCALE{false}; - allocator.setEffectivenessMatrix( - effectiveness, - actuator_trim, - linearization_point, - ActuatorEffectiveness::NUM_ACTUATORS, - UPDATE_NORMALIZATION_SCALE - ); -} - -static constexpr float EXPECT_NEAR_TOL{1e-4f}; - -} // namespace - // This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator // allocation. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledOnlyYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = 1.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - matrix::Vector zero; - EXPECT_EQ(actuator_sp, zero); + setAirmode(0); // Airmode disabled + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f(0.f, 0.f, 0.f, 0.f)); } // This tests that a control setpoint for z-thrust returns the desired actuator setpoint. // Each motor should have an actuator setpoint that when summed together should be equal to // control setpoint. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustZ) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - constexpr int MOTOR_COUNT{4}; - constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT}; - - for (int i{0}; i < MOTOR_COUNT; ++i) { - EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + EXPECT_EQ(allocate(0.f, 0.f, 0.f, -THRUST), Vector4f(THRUST, THRUST, THRUST, THRUST)); } // This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. // This test does not saturate the yaw response. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - // This is low enough to not saturate the motors. - constexpr float YAW_CONTROL_SP{0.02f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, - // this will need to be changed. - constexpr float YAW_EFFECTIVENESS_FACTOR{5.f}; - constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR}; - // At yaw condition, there will be 2 different actuator values. - constexpr int MOTOR_COUNT{4}; - constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR}; - constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR}; - - for (int i{0}; i < MOTOR_COUNT / 2; ++i) { - EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) { - EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + constexpr float YAW_TORQUE = 0.02f; + constexpr float YAW = YAW_TORQUE / NUM_ACTUATORS; + EXPECT_EQ(allocate(0.f, 0.f, YAW_TORQUE, -THRUST), Vector4f(THRUST + YAW, THRUST - YAW, THRUST + YAW, THRUST - YAW)); } // This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. // This test saturates the yaw response, but does not reduce total thrust. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndSaturatedYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - // This is arbitrarily high to trigger strongest possible (saturated) yaw response. - constexpr float YAW_CONTROL_SP{0.25f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // At max yaw, only 2 motors will carry all of the thrust. - constexpr int YAW_MOTORS{2}; - constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS}; - - for (int i{0}; i < YAW_MOTORS; ++i) { - EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + constexpr float YAW_TORQUE = 1.f; + constexpr float YAW = YAW_TORQUE / NUM_ACTUATORS; + EXPECT_EQ(allocate(0.f, 0.f, YAW_TORQUE, -THRUST), Vector4f(THRUST + YAW, THRUST - YAW, THRUST + YAW, THRUST - YAW)); } // This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. // This test does not saturate the pitch response. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndPitch) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - // This is low enough to not saturate the motors. - constexpr float PITCH_CONTROL_SP{0.1f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, - // this will need to be changed. - constexpr int MOTOR_COUNT{4}; - constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT}; - // At control set point, there will be 2 different actuator values. - constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR}; - constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR}; - EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + constexpr float PITCH_TORQUE = 0.1f; + constexpr float PITCH = PITCH_TORQUE / NUM_ACTUATORS; + EXPECT_EQ(allocate(0.f, PITCH_TORQUE, 0.f, -THRUST), + Vector4f(THRUST + PITCH, THRUST - PITCH, THRUST - PITCH, THRUST + PITCH)); } // This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. // This test saturates yaw and demonstrates reduction of thrust for yaw. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledReducedThrustAndYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f}; - constexpr int MOTOR_COUNT{4}; - constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT}; - // This is arbitrarily high to trigger strongest possible (saturated) yaw response. - constexpr float YAW_CONTROL_SP{1.f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // In the case of yaw saturation, thrust per motor will be reduced by the hard-coded - // magic-number yaw margin of 0.15f. - constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available. - constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR}; - // At control set point, there will be 2 different actuator values. - constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN}; - constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN}; - EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float YAW_MARGIN = ControlAllocationSequentialDesaturation::MINIMUM_YAW_MARGIN; + EXPECT_EQ(allocate(0.f, 0.f, 1.f, -3.2f), Vector4f(1.f, 1.f - (2.f * YAW_MARGIN), 1.f, 1.f - (2.f * YAW_MARGIN))); } // This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. // This test saturates the pitch response such that thrust is reduced to (partially) compensate. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledReducedThrustAndPitch) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f * 4.f}; - // This is high enough to saturate the pitch control. - constexpr float PITCH_CONTROL_SP{2.f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - constexpr int MOTOR_COUNT{4}; - // The maximum actuator value is - // THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT. - // The amount over 1 is the amount that each motor is reduced by. - // At control set point, there will be 2 different actuator values. - constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1}; - EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f); - constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; - constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; - EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + EXPECT_EQ(allocate(0.f, 2.f, 0.f, -3.f), Vector4f(1.f, 0.f, 0.f, 1.f)); } From 20c863c9d61be9f148d0b8ec287adeb569d3e3f8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Apr 2025 17:33:27 +0200 Subject: [PATCH 10/13] Temporarily add old multirotor mixer test to take over the exact cases into new unit tests --- CMakeLists.txt | 1 + src/lib/CMakeLists.txt | 1 + src/lib/mixer/.gitignore | 2 + .../AllocatedActuatorMixer.cpp | 149 ++++++ .../AllocatedActuatorMixer.hpp | 103 ++++ .../AllocatedActuatorMixer/CMakeLists.txt | 39 ++ src/lib/mixer/CMakeLists.txt | 58 ++ src/lib/mixer/HelicopterMixer/CMakeLists.txt | 39 ++ .../mixer/HelicopterMixer/HelicopterMixer.cpp | 240 +++++++++ .../mixer/HelicopterMixer/HelicopterMixer.hpp | 111 ++++ src/lib/mixer/MixerBase/CMakeLists.txt | 38 ++ src/lib/mixer/MixerBase/Mixer.cpp | 124 +++++ src/lib/mixer/MixerBase/Mixer.hpp | 279 ++++++++++ src/lib/mixer/MixerGroup.cpp | 258 +++++++++ src/lib/mixer/MixerGroup.hpp | 172 ++++++ src/lib/mixer/MultirotorMixer/CMakeLists.txt | 114 ++++ .../mixer/MultirotorMixer/MultirotorMixer.cpp | 498 ++++++++++++++++++ .../mixer/MultirotorMixer/MultirotorMixer.hpp | 255 +++++++++ .../geometries/dodeca_bottom_cox.toml | 40 ++ .../geometries/dodeca_top_cox.toml | 40 ++ .../MultirotorMixer/geometries/hex_cox.toml | 40 ++ .../MultirotorMixer/geometries/hex_plus.toml | 40 ++ .../MultirotorMixer/geometries/hex_t.toml | 40 ++ .../MultirotorMixer/geometries/hex_x.toml | 40 ++ .../MultirotorMixer/geometries/octa_cox.toml | 50 ++ .../geometries/octa_cox_wide.toml | 50 ++ .../MultirotorMixer/geometries/octa_plus.toml | 50 ++ .../MultirotorMixer/geometries/octa_x.toml | 50 ++ .../geometries/quad_deadcat.toml | 30 ++ .../MultirotorMixer/geometries/quad_h.toml | 30 ++ .../MultirotorMixer/geometries/quad_plus.toml | 29 + .../geometries/quad_s250aq.toml | 30 ++ .../geometries/quad_vtail.toml | 33 ++ .../MultirotorMixer/geometries/quad_wide.toml | 30 ++ .../MultirotorMixer/geometries/quad_x.toml | 29 + .../MultirotorMixer/geometries/quad_x_cw.toml | 30 ++ .../geometries/quad_x_pusher.toml | 37 ++ .../MultirotorMixer/geometries/quad_y.toml | 31 ++ .../geometries/tools/px_generate_mixers.py | 398 ++++++++++++++ .../MultirotorMixer/geometries/tri_y.toml | 23 + .../geometries/twin_engine.toml | 19 + .../mixer/MultirotorMixer/mixer_multirotor.py | 386 ++++++++++++++ .../MultirotorMixer/test_mixer_multirotor.cpp | 178 +++++++ src/lib/mixer/NullMixer/CMakeLists.txt | 39 ++ src/lib/mixer/NullMixer/NullMixer.cpp | 67 +++ src/lib/mixer/NullMixer/NullMixer.hpp | 75 +++ src/lib/mixer/SimpleMixer/CMakeLists.txt | 39 ++ src/lib/mixer/SimpleMixer/SimpleMixer.cpp | 390 ++++++++++++++ src/lib/mixer/SimpleMixer/SimpleMixer.hpp | 153 ++++++ src/lib/mixer/load_mixer_file.cpp | 112 ++++ src/lib/mixer/mixer_load.h | 51 ++ 51 files changed, 5160 insertions(+) create mode 100644 src/lib/mixer/.gitignore create mode 100644 src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp create mode 100644 src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp create mode 100644 src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt create mode 100644 src/lib/mixer/CMakeLists.txt create mode 100644 src/lib/mixer/HelicopterMixer/CMakeLists.txt create mode 100644 src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp create mode 100644 src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp create mode 100644 src/lib/mixer/MixerBase/CMakeLists.txt create mode 100644 src/lib/mixer/MixerBase/Mixer.cpp create mode 100644 src/lib/mixer/MixerBase/Mixer.hpp create mode 100644 src/lib/mixer/MixerGroup.cpp create mode 100644 src/lib/mixer/MixerGroup.hpp create mode 100644 src/lib/mixer/MultirotorMixer/CMakeLists.txt create mode 100644 src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp create mode 100644 src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp create mode 100644 src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_t.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/hex_x.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/octa_x.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_h.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_x.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/quad_y.toml create mode 100755 src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py create mode 100644 src/lib/mixer/MultirotorMixer/geometries/tri_y.toml create mode 100644 src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml create mode 100755 src/lib/mixer/MultirotorMixer/mixer_multirotor.py create mode 100644 src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp create mode 100644 src/lib/mixer/NullMixer/CMakeLists.txt create mode 100644 src/lib/mixer/NullMixer/NullMixer.cpp create mode 100644 src/lib/mixer/NullMixer/NullMixer.hpp create mode 100644 src/lib/mixer/SimpleMixer/CMakeLists.txt create mode 100644 src/lib/mixer/SimpleMixer/SimpleMixer.cpp create mode 100644 src/lib/mixer/SimpleMixer/SimpleMixer.hpp create mode 100644 src/lib/mixer/load_mixer_file.cpp create mode 100644 src/lib/mixer/mixer_load.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 75fc8afb6726..6c03aef7073f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -420,6 +420,7 @@ if(BUILD_TESTING) DEPENDS px4 examples__dyn_hello + test_mixer_multirotor USES_TERMINAL COMMENT "Running tests" WORKING_DIRECTORY ${PX4_BINARY_DIR}) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 6454c9e40ce7..d72d5ee47d29 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -57,6 +57,7 @@ add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL) add_subdirectory(led EXCLUDE_FROM_ALL) add_subdirectory(matrix EXCLUDE_FROM_ALL) add_subdirectory(mathlib EXCLUDE_FROM_ALL) +add_subdirectory(mixer) add_subdirectory(mixer_module EXCLUDE_FROM_ALL) add_subdirectory(motion_planning EXCLUDE_FROM_ALL) add_subdirectory(npfg EXCLUDE_FROM_ALL) diff --git a/src/lib/mixer/.gitignore b/src/lib/mixer/.gitignore new file mode 100644 index 000000000000..df7e57e698f9 --- /dev/null +++ b/src/lib/mixer/.gitignore @@ -0,0 +1,2 @@ + +/test_mixer_multirotor diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp new file mode 100644 index 000000000000..9e03335c12d3 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_AllocatedActuatorMixer.cpp + * + * Mixer for allocated actuators. + * + * @author Julien Lecoeur + */ + +#include "AllocatedActuatorMixer.hpp" + +#include +#include +#include + +// #define debug(fmt, args...) do { } while(0) +#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +AllocatedActuatorMixer::AllocatedActuatorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + uint8_t index) : + Mixer(control_cb, cb_handle) +{ + if (index < 8) { + _control_group = 4; + _control_index = index; + + } else if (index < 16) { + _control_group = 5; + _control_index = index - 8; + + } else { + debug("'A:' invalid index"); + } +} + +unsigned AllocatedActuatorMixer::set_trim(float trim) +{ + return 1; +} + +unsigned AllocatedActuatorMixer::get_trim(float *trim) +{ + *trim = 0.0f; + return 1; +} + +int +AllocatedActuatorMixer::parse(const char *buf, unsigned &buflen, uint8_t &index) +{ + int ret; + int i; + + // enforce that the mixer ends with a new line + if (!string_well_formed(buf, buflen)) { + return -1; + } + + // parse line + if ((ret = sscanf(buf, "A: %d", &i)) != 1) { + debug("'A:' parser: failed on '%s'", buf); + return -1; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("'A:' parser: no line ending, line is incomplete"); + return -1; + } + + // check parsed index + if (i < 16) { + index = i; + + } else { + debug("'A:' parser: invalid index"); + return -1; + } + + return 0; +} + +AllocatedActuatorMixer * +AllocatedActuatorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen) +{ + uint8_t index; + + if (parse(buf, buflen, index) == 0) { + return new AllocatedActuatorMixer(control_cb, cb_handle, index); + + } else { + return nullptr; + } +} + +unsigned +AllocatedActuatorMixer::mix(float *outputs, unsigned space) +{ + if (space < 1) { + return 0; + } + + _control_cb(_cb_handle, + _control_group, + _control_index, + *outputs); + + return 1; +} + +void +AllocatedActuatorMixer::groups_required(uint32_t &groups) +{ + groups |= 1 << _control_group; +} diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp new file mode 100644 index 000000000000..b36cecfe0df8 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_AllocatedActuatorMixer.hpp + * + * Mixer for allocated actuators. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +/** + * Mixer for allocated actuators. + * + * Copies a single actuator to a single output. + */ +class AllocatedActuatorMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param index Actuator index (0..15) + */ + AllocatedActuatorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + uint8_t index); + virtual ~AllocatedActuatorMixer() = default; + + // no copy, assignment, move, move assignment + AllocatedActuatorMixer(const AllocatedActuatorMixer &) = delete; + AllocatedActuatorMixer &operator=(const AllocatedActuatorMixer &) = delete; + AllocatedActuatorMixer(AllocatedActuatorMixer &&) = delete; + AllocatedActuatorMixer &operator=(AllocatedActuatorMixer &&) = delete; + + /** + * Factory method with full external configuration. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new AllocatedActuatorMixer instance, or nullptr + * if the text format is bad. + */ + static AllocatedActuatorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + void groups_required(uint32_t &groups) override; + unsigned set_trim(float trim) override; + unsigned get_trim(float *trim) override; + +private: + uint8_t _control_group; /**< group from which the input reads */ + uint8_t _control_index; /**< index within the control group */ + + static int parse(const char *buf, + unsigned &buflen, + uint8_t &index); +}; diff --git a/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt new file mode 100644 index 000000000000..0a8526545c3c --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(AllocatedActuatorMixer + AllocatedActuatorMixer.cpp + AllocatedActuatorMixer.hpp +) +target_link_libraries(AllocatedActuatorMixer PRIVATE MixerBase) +add_dependencies(AllocatedActuatorMixer prebuild_targets) diff --git a/src/lib/mixer/CMakeLists.txt b/src/lib/mixer/CMakeLists.txt new file mode 100644 index 000000000000..088e6fa36615 --- /dev/null +++ b/src/lib/mixer/CMakeLists.txt @@ -0,0 +1,58 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# required by other mixers +add_subdirectory(MixerBase) + +add_subdirectory(AllocatedActuatorMixer) +add_subdirectory(HelicopterMixer) +add_subdirectory(MultirotorMixer) +add_subdirectory(NullMixer) +add_subdirectory(SimpleMixer) + +add_library(mixer + MixerGroup.cpp + MixerGroup.hpp + load_mixer_file.cpp + mixer_load.h +) + +target_link_libraries(mixer + PRIVATE + AllocatedActuatorMixer + HelicopterMixer + MultirotorMixer + NullMixer + SimpleMixer + ) +add_dependencies(mixer prebuild_targets) diff --git a/src/lib/mixer/HelicopterMixer/CMakeLists.txt b/src/lib/mixer/HelicopterMixer/CMakeLists.txt new file mode 100644 index 000000000000..264f3335efe1 --- /dev/null +++ b/src/lib/mixer/HelicopterMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(HelicopterMixer + HelicopterMixer.cpp + HelicopterMixer.hpp +) +target_link_libraries(HelicopterMixer PRIVATE MixerBase) +add_dependencies(HelicopterMixer prebuild_targets) diff --git a/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp b/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp new file mode 100644 index 000000000000..e202367e978b --- /dev/null +++ b/src/lib/mixer/HelicopterMixer/HelicopterMixer.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_helicopter.cpp + * + * Helicopter mixers. + */ + +#include "HelicopterMixer.hpp" + +#include +#include +#include + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) + +using math::constrain; + +HelicopterMixer::HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info) : + Mixer(control_cb, cb_handle), + _mixer_info(mixer_info) +{ +} + +HelicopterMixer * +HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + mixer_heli_s mixer_info; + unsigned swash_plate_servo_count = 0; + unsigned u[5]; + int s[5]; + int used; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + if (sscanf(buf, "H: %u%n", &swash_plate_servo_count, &used) != 1) { + debug("helicopter parse failed on '%s'", buf); + return nullptr; + } + + if (swash_plate_servo_count < 3 || swash_plate_servo_count > 4) { + debug("only supporting swash plate with 3 or 4 servos"); + return nullptr; + } + + if (used > (int)buflen) { + debug("OVERFLOW: helicopter spec used %d of %u", used, buflen); + return nullptr; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + buf = findtag(buf, buflen, 'T'); + + if ((buf == nullptr) || (buflen < 12)) { + debug("control parser failed finding tag, ret: '%s'", buf); + return nullptr; + } + + if (sscanf(buf, "T: %u %u %u %u %u", + &u[0], &u[1], &u[2], &u[3], &u[4]) != 5) { + debug("control parse failed on '%s'", buf); + return nullptr; + } + + for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) { + mixer_info.throttle_curve[i] = ((float) u[i]) / 10000.0f; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + buf = findtag(buf, buflen, 'P'); + + if ((buf == nullptr) || (buflen < 12)) { + debug("control parser failed finding tag, ret: '%s'", buf); + return nullptr; + } + + if (sscanf(buf, "P: %d %d %d %d %d", + &s[0], &s[1], &s[2], &s[3], &s[4]) != 5) { + debug("control parse failed on '%s'", buf); + return nullptr; + } + + for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) { + mixer_info.pitch_curve[i] = ((float) s[i]) / 10000.0f; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + mixer_info.control_count = swash_plate_servo_count; + + /* Now loop through the servos */ + for (unsigned i = 0; i < mixer_info.control_count; i++) { + + buf = findtag(buf, buflen, 'S'); + + if ((buf == nullptr) || (buflen < 12)) { + debug("control parser failed finding tag, ret: '%s'", buf); + return nullptr; + } + + if (sscanf(buf, "S: %u %u %d %d %d %d", + &u[0], + &u[1], + &s[0], + &s[1], + &s[2], + &s[3]) != 6) { + debug("control parse failed on '%s'", buf); + return nullptr; + } + + mixer_info.servos[i].angle = ((float) u[0]) * M_PI_F / 180.0f; + mixer_info.servos[i].arm_length = ((float) u[1]) / 10000.0f; + mixer_info.servos[i].scale = ((float) s[0]) / 10000.0f; + mixer_info.servos[i].offset = ((float) s[1]) / 10000.0f; + mixer_info.servos[i].min_output = ((float) s[2]) / 10000.0f; + mixer_info.servos[i].max_output = ((float) s[3]) / 10000.0f; + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + } + + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); + + HelicopterMixer *hm = new HelicopterMixer(control_cb, cb_handle, mixer_info); + + if (hm != nullptr) { + debug("loaded heli mixer with %d swash plate input(s)", mixer_info.control_count); + + } else { + debug("could not allocate memory for mixer"); + } + + return hm; +} + +unsigned +HelicopterMixer::mix(float *outputs, unsigned space) +{ + if (space < _mixer_info.control_count + 1u) { + return 0; + } + + /* Find index to use for curves */ + float thrust_cmd = get_control(0, 3); + int idx = (thrust_cmd / 0.25f); + + /* Make sure idx is in range */ + if (idx < 0) { + idx = 0; + + } else if (idx > HELI_CURVES_NR_POINTS - 2) { + /* We access idx + 1 below, so max legal index is (size - 2) */ + idx = HELI_CURVES_NR_POINTS - 2; + } + + /* Local throttle curve gradient and offset */ + float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f; + float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f); + float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f); + + /* Local pitch curve gradient and offset */ + float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f; + float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f); + float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f); + + float roll_cmd = get_control(0, 0); + float pitch_cmd = get_control(0, 1); + + outputs[0] = throttle; + + for (unsigned i = 0; i < _mixer_info.control_count; i++) { + outputs[i + 1] = collective_pitch + + cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length + - sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length; + outputs[i + 1] *= _mixer_info.servos[i].scale; + outputs[i + 1] += _mixer_info.servos[i].offset; + outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output); + } + + return _mixer_info.control_count + 1; +} diff --git a/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp b/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp new file mode 100644 index 000000000000..ce50a8f58cda --- /dev/null +++ b/src/lib/mixer/HelicopterMixer/HelicopterMixer.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** helicopter swash servo mixer */ +struct mixer_heli_servo_s { + float angle; + float arm_length; + float scale; + float offset; + float min_output; + float max_output; +}; + +#define HELI_CURVES_NR_POINTS 5 + +/** helicopter swash plate mixer */ +struct mixer_heli_s { + uint8_t control_count; /**< number of inputs */ + float throttle_curve[HELI_CURVES_NR_POINTS]; + float pitch_curve[HELI_CURVES_NR_POINTS]; + mixer_heli_servo_s servos[4]; /**< up to four inputs */ +}; + +/** + * Generic helicopter mixer for helicopters with swash plate. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands + * for swash plate tilting and throttle- and pitch curves. + */ +class HelicopterMixer : public Mixer +{ +public: + /** + * Constructor. + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param mixer_info Pointer to heli mixer configuration + */ + HelicopterMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_heli_s mixer_info); + virtual ~HelicopterMixer() = default; + + // no copy, assignment, move, move assignment + HelicopterMixer(const HelicopterMixer &) = delete; + HelicopterMixer &operator=(const HelicopterMixer &) = delete; + HelicopterMixer(HelicopterMixer &&) = delete; + HelicopterMixer &operator=(HelicopterMixer &&) = delete; + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new HelicopterMixer instance, or nullptr + * if the text format is bad. + */ + static HelicopterMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + + void groups_required(uint32_t &groups) override { groups |= (1 << 0); } + + unsigned set_trim(float trim) override { return 4; } + unsigned get_trim(float *trim) override { return 4; } + +private: + mixer_heli_s _mixer_info; +}; diff --git a/src/lib/mixer/MixerBase/CMakeLists.txt b/src/lib/mixer/MixerBase/CMakeLists.txt new file mode 100644 index 000000000000..d4b1ec2915b0 --- /dev/null +++ b/src/lib/mixer/MixerBase/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(MixerBase + Mixer.cpp + Mixer.hpp +) +add_dependencies(MixerBase prebuild_targets) diff --git a/src/lib/mixer/MixerBase/Mixer.cpp b/src/lib/mixer/MixerBase/Mixer.cpp new file mode 100644 index 000000000000..b38b759a3ffd --- /dev/null +++ b/src/lib/mixer/MixerBase/Mixer.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.cpp + * + * Programmable multi-channel mixer library. + */ + +#include "Mixer.hpp" + +#include +#include +#include + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +float +Mixer::get_control(uint8_t group, uint8_t index) +{ + float value; + + _control_cb(_cb_handle, group, index, value); + + return value; +} + +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) { + return buf; + } + + buf++; + buflen--; + } + + return nullptr; +} + +char +Mixer::findnexttag(const char *buf, unsigned buflen) +{ + while (buflen >= 2) { + if (isupper(buf[0]) && buf[1] == ':') { + return buf[0]; + } + + buf++; + buflen--; + } + + return 0; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + +bool +Mixer::string_well_formed(const char *buf, unsigned &buflen) +{ + /* enforce that the mixer ends with a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') { + continue; + } + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending, so no split symbols / numbers. good. */ + return true; + } + + } + + debug("pre-parser rejected: No newline in buf"); + + return false; +} diff --git a/src/lib/mixer/MixerBase/Mixer.hpp b/src/lib/mixer/MixerBase/Mixer.hpp new file mode 100644 index 000000000000..e90290e14356 --- /dev/null +++ b/src/lib/mixer/MixerBase/Mixer.hpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Mixer.hpp + * + * Generic, programmable, procedural control signal mixers. + * + * This library implements a generic mixer interface that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * control value + * A mixer input value, typically provided by some controlling + * component of the system. + * + * control group + * A collection of controls provided by a single controlling component. + * + * actuator + * The mixer output value. + * + * + * Mixing basics + * ============= + * + * An actuator derives its value from the combination of one or more + * control values. Each of the control values is scaled according to + * the actuator's configuration and then combined to produce the + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing behaviour varies based on the specific mixer class; each + * mixer class describes its behaviour in more detail. + * + * + * Controls + * -------- + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. Some mixer classes have specific assumptions + * about the assignment of controls. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + +#pragma once + +#include +#include + +/** + * Abstract class defining a mixer mixing zero or more inputs to + * one or more outputs. + */ +class Mixer : public ListNode +{ +public: + enum class Airmode : int32_t { + disabled = 0, + roll_pitch = 1, + roll_pitch_yaw = 2 + }; + + /** + * Fetch a control value. + * + * @param handle Token passed when the callback is registered. + * @param control_group The group to fetch the control from. + * @param control_index The group-relative index to fetch the control from. + * @param control The returned control + * @return Zero if the value was fetched, nonzero otherwise. + */ + typedef int (* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control); + + /** + * Constructor. + * + * @param control_cb Callback invoked when reading controls. + */ + Mixer(ControlCallback control_cb, uintptr_t cb_handle) : _control_cb(control_cb), _cb_handle(cb_handle) {} + virtual ~Mixer() = default; + + // no copy, assignment, move, move assignment + Mixer(const Mixer &) = delete; + Mixer &operator=(const Mixer &) = delete; + Mixer(Mixer &&) = delete; + Mixer &operator=(Mixer &&) = delete; + + /** + * Perform the mixing function. + * + * @param outputs Array into which mixed output(s) should be placed. + * @param space The number of available entries in the output array; + * @return The number of entries in the output array that were populated. + */ + virtual unsigned mix(float *outputs, unsigned space) = 0; + + /** + * Get the saturation status. + * + * @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg. + */ + virtual uint16_t get_saturation_status() { return 0; } + + /** + * Analyses the mix configuration and updates a bitmask of groups + * that are required. + * + * @param groups A bitmask of groups (0-31) that the mixer requires. + */ + virtual void groups_required(uint32_t &groups) {}; + + /** + * @brief Empty method, only implemented for MultirotorMixer and MixerGroup class. + * + * @param[in] delta_out_max Maximum delta output. + * + */ + virtual void set_max_delta_out_once(float delta_out_max) {} + + /** + * @brief Set trim offset for this mixer + * + * @return the number of outputs this mixer feeds to + */ + virtual unsigned set_trim(float trim) { return 0; } + + /** + * @brief Get trim offset for this mixer + * + * @return the number of outputs this mixer feeds to + */ + virtual unsigned get_trim(float *trim) { return 0; } + + /* + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. + * + * @param[in] val The value + */ + virtual void set_thrust_factor(float val) {} + + /** + * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors. + * + * @param[in] airmode Select airmode type (0 = disabled, 1 = roll/pitch, 2 = roll/pitch/yaw) + */ + virtual void set_airmode(Airmode airmode) {}; + + virtual unsigned get_multirotor_count() { return 0; } + + virtual void set_dt_once(float dt) {} + +protected: + + /** client-supplied callback used when fetching control values */ + ControlCallback _control_cb; + uintptr_t _cb_handle; + + /** + * Invoke the client callback to fetch a control value. + * + * @param group Control group to fetch from. + * @param index Control index to fetch. + * @return The control value. + */ + float get_control(uint8_t group, uint8_t index); + + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char *findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Find next tag and return it (0 is returned if no tag is found) + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + */ + static char findnexttag(const char *buf, unsigned buflen); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char *skipline(const char *buf, unsigned &buflen); + + /** + * Check wether the string is well formed and suitable for parsing + */ + static bool string_well_formed(const char *buf, unsigned &buflen); +}; diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp new file mode 100644 index 000000000000..3321b42253b7 --- /dev/null +++ b/src/lib/mixer/MixerGroup.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_group.cpp + * + * Mixer collection. + */ + +#include "MixerGroup.hpp" + +#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp" +#include "HelicopterMixer/HelicopterMixer.hpp" +#include "MultirotorMixer/MultirotorMixer.hpp" +#include "NullMixer/NullMixer.hpp" +#include "SimpleMixer/SimpleMixer.hpp" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) syslog(fmt "\n", ##args) + +unsigned +MixerGroup::mix(float *outputs, unsigned space) +{ + unsigned index = 0; + + for (auto mixer : _mixers) { + index += mixer->mix(outputs + index, space - index); + + if (index >= space) { + break; + } + } + + return index; +} + +/* + * set_trims() has no effect except for the SimpleMixer implementation for which set_trim() + * always returns the value one. + * The only other existing implementation is MultirotorMixer, which ignores the trim value + * and returns _rotor_count. + */ +unsigned +MixerGroup::set_trims(int16_t *values, unsigned n) +{ + unsigned index = 0; + + for (auto mixer : _mixers) { + // convert from integer to float + // to be safe, clamp offset to range of [-500, 500] usec + float offset = math::constrain((float)values[index] / 10000, -1.0f, 1.0f); + + debug("set trim: %d, offset: %5.3f", values[index], (double)offset); + index += mixer->set_trim(offset); + + if (index >= n) { + break; + } + } + + return index; +} + +/* + * get_trims() has no effect except for the SimpleMixer implementation for which get_trim() + * always returns the value one and sets the trim value. + * The only other existing implementation is MultirotorMixer, which ignores the trim value + * and returns _rotor_count. + */ +unsigned +MixerGroup::get_trims(int16_t *values) +{ + unsigned index_mixer = 0; + unsigned index = 0; + + for (auto mixer : _mixers) { + float trim = 0; + index_mixer += mixer->get_trim(&trim); + + // MultirotorMixer returns the number of motors so we + // loop through index_mixer and set the same trim value for all motors + while (index < index_mixer) { + values[index] = trim * 10000; + index++; + } + } + + return index; +} + +void +MixerGroup::set_thrust_factor(float val) +{ + for (auto mixer : _mixers) { + mixer->set_thrust_factor(val); + } +} + +void +MixerGroup::set_airmode(Mixer::Airmode airmode) +{ + for (auto mixer : _mixers) { + mixer->set_airmode(airmode); + } +} + +unsigned +MixerGroup::get_multirotor_count() +{ + for (auto mixer : _mixers) { + unsigned rotor_count = mixer->get_multirotor_count(); + + if (rotor_count > 0) { + return rotor_count; + } + } + + return 0; +} + +uint16_t +MixerGroup::get_saturation_status() +{ + uint16_t sat = 0; + + for (auto mixer : _mixers) { + sat |= mixer->get_saturation_status(); + } + + return sat; +} + +void +MixerGroup::groups_required(uint32_t &groups) +{ + for (auto mixer : _mixers) { + mixer->groups_required(groups); + } +} + +int +MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + int ret = -1; + const char *end = buf + buflen; + + /* + * Loop until either we have emptied the buffer, or we have failed to + * allocate something when we expected to. + */ + while (buflen > 0) { + Mixer *m = nullptr; + const char *p = end - buflen; + unsigned resid = buflen; + + /* + * Use the next character as a hint to decide which mixer class to construct. + */ + switch (*p) { + case 'Z': + m = NullMixer::from_text(p, resid); + break; + + case 'A': + m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid); + break; + + case 'M': + m = SimpleMixer::from_text(control_cb, cb_handle, p, resid); + break; + + case 'R': + m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid); + break; + + case 'H': + m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid); + break; + + default: + /* it's probably junk or whitespace, skip a byte and retry */ + buflen--; + continue; + } + + /* + * If we constructed something, add it to the group. + */ + if (m != nullptr) { + add_mixer(m); + + /* we constructed something */ + ret = 0; + + /* only adjust buflen if parsing was successful */ + buflen = resid; + debug("SUCCESS - buflen: %d", buflen); + + } else { + + /* + * There is data in the buffer that we expected to parse, but it didn't, + * so give up for now. + */ + break; + } + } + + /* nothing more in the buffer for us now */ + return ret; +} + +void MixerGroup::set_max_delta_out_once(float delta_out_max) +{ + for (auto mixer : _mixers) { + mixer->set_max_delta_out_once(delta_out_max); + } +} + +void +MixerGroup::set_dt_once(float dt) +{ + for (auto mixer : _mixers) { + mixer->set_dt_once(dt); + } +} diff --git a/src/lib/mixer/MixerGroup.hpp b/src/lib/mixer/MixerGroup.hpp new file mode 100644 index 000000000000..f5b7fedb50cb --- /dev/null +++ b/src/lib/mixer/MixerGroup.hpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "MixerBase/Mixer.hpp" + +/** + * Group of mixers, built up from single mixers and processed + * in order when mixing. + */ +class MixerGroup +{ +public: + MixerGroup() = default; + + ~MixerGroup() + { + reset(); + } + + // no copy, assignment, move, move assignment + MixerGroup(const MixerGroup &) = delete; + MixerGroup &operator=(const MixerGroup &) = delete; + MixerGroup(MixerGroup &&) = delete; + MixerGroup &operator=(MixerGroup &&) = delete; + + unsigned mix(float *outputs, unsigned space); + + uint16_t get_saturation_status(); + + void groups_required(uint32_t &groups); + + /** + * Add a mixer to the group. + * + * @param mixer The mixer to be added. + */ + void add_mixer(Mixer *mixer) { _mixers.add(mixer); } + + /** + * Remove all the mixers from the group. + */ + void reset() { _mixers.clear(); } + + /** + * Count the mixers in the group. + */ + unsigned count() const { return _mixers.size(); } + + /** + * Adds mixers to the group based on a text description in a buffer. + * + * Mixer definitions begin with a single capital letter and a colon. + * The actual format of the mixer definition varies with the individual + * mixers; they are summarised here, but see ROMFS/mixers/README for + * more details. + * + * Null Mixer + * .......... + * + * The null mixer definition has the form: + * + * Z: + * + * Simple Mixer + * ............ + * + * A simple mixer definition begins with: + * + * M: + * O: <-ve scale> <+ve scale> + * + * The second line O: can be omitted. In that case 'O: 10000 10000 0 -10000 10000' is used. + * The definition continues with entries describing the control + * inputs and their scaling, in the form: + * + * S: <-ve scale> <+ve scale> + * + * Multirotor Mixer + * ................ + * + * The multirotor mixer definition is a single line of the form: + * + * R: + * + * Helicopter Mixer + * ................ + * + * The helicopter mixer includes throttle and pitch curves + * + * H: + * T: <0> <2500> <5000> <7500> <10000> + * P: <-10000> <-5000> <0> <5000> <10000> + * + * The definition continues with entries describing + * the position of the servo, in the following form: + * + * S: + * + * @param buf The mixer configuration buffer. + * @param buflen The length of the buffer, updated to reflect + * bytes as they are consumed. + * @return Zero on successful load, nonzero otherwise. + */ + int load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen); + + /** + * @brief Update slew rate parameter. This tells instances of the class MultirotorMixer + * the maximum allowed change of the output values per cycle. + * The value is only valid for one cycle, in order to have continuous + * slew rate limiting this function needs to be called before every call + * to mix(). + * + * @param[in] delta_out_max Maximum delta output. + * + */ + void set_max_delta_out_once(float delta_out_max); + + /* + * Invoke the set_offset method of each mixer in the group + * for each value in page r_page_servo_control_trim + */ + unsigned set_trims(int16_t *v, unsigned n); + unsigned get_trims(int16_t *values); + + /** + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. + * + * @param[in] val The value + */ + void set_thrust_factor(float val); + + void set_airmode(Mixer::Airmode airmode); + + unsigned get_multirotor_count(); + + void set_dt_once(float dt); + +private: + List _mixers; /**< linked list of mixers */ +}; diff --git a/src/lib/mixer/MultirotorMixer/CMakeLists.txt b/src/lib/mixer/MultirotorMixer/CMakeLists.txt new file mode 100644 index 000000000000..5ccc17be1513 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/CMakeLists.txt @@ -0,0 +1,114 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +set(geometry_files + dodeca_bottom_cox.toml + dodeca_top_cox.toml + hex_cox.toml + hex_plus.toml + hex_t.toml + hex_x.toml + octa_cox.toml + octa_cox_wide.toml + octa_plus.toml + octa_x.toml + quad_deadcat.toml + quad_h.toml + quad_plus.toml + quad_s250aq.toml + quad_vtail.toml + quad_wide.toml + quad_x_cw.toml + quad_x.toml + quad_x_pusher.toml + quad_y.toml + tri_y.toml + twin_engine.toml +) + +set(geometries_list) +foreach(geom_file ${geometry_files}) + list(APPEND geometries_list ${CMAKE_CURRENT_SOURCE_DIR}/geometries/${geom_file}) +endforeach() + +set(MIXER_TOOLS ${CMAKE_CURRENT_SOURCE_DIR}/geometries/tools) + +# generate mixers and normalize +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h + COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py -f ${geometries_list} -o mixer_multirotor.generated.h + DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} + ) +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h + COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --normalize -f ${geometries_list} -o mixer_multirotor_normalized.generated.h + DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} + ) +add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h) + +# 6DOF mixers +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h + COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h + DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list} + ) +add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h) + +add_library(MultirotorMixer + MultirotorMixer.cpp + MultirotorMixer.hpp + + ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h + ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h + ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h +) +target_link_libraries(MultirotorMixer PRIVATE MixerBase) +target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) +add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets) + +if(BUILD_TESTING) + + add_executable(test_mixer_multirotor + test_mixer_multirotor.cpp + MultirotorMixer.cpp + ) + target_compile_definitions(test_mixer_multirotor PRIVATE MIXER_MULTIROTOR_USE_MOCK_GEOMETRY) + target_compile_options(test_mixer_multirotor PRIVATE -Wno-unused-result) + target_link_libraries(test_mixer_multirotor PRIVATE MixerBase) + + add_test(NAME mixer_multirotor + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/mixer_multirotor.py --test --mixer-multirotor-binary $ + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + ) + +endif() diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp new file mode 100644 index 000000000000..92780e977c4f --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp @@ -0,0 +1,498 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_multirotor.cpp + * + * Multi-rotor mixers. + */ + +#include "MultirotorMixer.hpp" + +#include +#include +#include + +#include + +#ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY +enum class MultirotorGeometry : MultirotorGeometryUnderlyingType { + QUAD_X, + MAX_GEOMETRY +}; +namespace +{ +const MultirotorMixer::Rotor _config_quad_x[] = { + { -0.707107, 0.707107, 1.000000, 1.000000 }, + { 0.707107, -0.707107, 1.000000, 1.000000 }, + { 0.707107, 0.707107, -1.000000, 1.000000 }, + { -0.707107, -0.707107, -1.000000, 1.000000 }, +}; +const MultirotorMixer::Rotor *_config_index[] = { + &_config_quad_x[0] +}; +const unsigned _config_rotor_count[] = {4}; +const char *_config_key[] = {"4x"}; +} + +#else + +// This file is generated by the px_generate_mixers.py script which is invoked during the build process +// #include "mixer_multirotor.generated.h" +#include "mixer_multirotor_normalized.generated.h" + +#endif /* MIXER_MULTIROTOR_USE_MOCK_GEOMETRY */ + + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) syslog(fmt "\n", ##args) + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry) : + MultirotorMixer(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry]) +{ +} + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, + unsigned rotor_count) : + Mixer(control_cb, cb_handle), + _rotor_count(rotor_count), + _rotors(rotors), + _outputs_prev(new float[_rotor_count]), + _tmp_array(new float[_rotor_count]) +{ + for (unsigned i = 0; i < _rotor_count; ++i) { + _outputs_prev[i] = -1.f; + } +} + +MultirotorMixer::~MultirotorMixer() +{ + delete[] _outputs_prev; + delete[] _tmp_array; +} + +MultirotorMixer * +MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY; + char geomname[16]; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + if (sscanf(buf, "R: %15s", geomname) != 1) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return nullptr; + } + + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); + + for (MultirotorGeometryUnderlyingType i = 0; i < (MultirotorGeometryUnderlyingType)MultirotorGeometry::MAX_GEOMETRY; + i++) { + if (!strcmp(geomname, _config_key[i])) { + geometry = (MultirotorGeometry)i; + break; + } + } + + if (geometry == MultirotorGeometry::MAX_GEOMETRY) { + debug("unrecognised geometry '%s'", geomname); + return nullptr; + } + + debug("adding multirotor mixer '%s'", geomname); + + return new MultirotorMixer(control_cb, cb_handle, geometry); +} + +float +MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, const float *outputs, + saturation_status &sat_status, float min_output, float max_output) const +{ + float k_min = 0.f; + float k_max = 0.f; + + for (unsigned i = 0; i < _rotor_count; i++) { + // Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway + if (fabsf(desaturation_vector[i]) < FLT_EPSILON) { + continue; + } + + if (outputs[i] < min_output) { + float k = (min_output - outputs[i]) / desaturation_vector[i]; + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + + sat_status.flags.motor_neg = true; + } + + if (outputs[i] > max_output) { + float k = (max_output - outputs[i]) / desaturation_vector[i]; + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + + sat_status.flags.motor_pos = true; + } + } + + // Reduce the saturation as much as possible + return k_min + k_max; +} + +void +MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *outputs, + saturation_status &sat_status, float min_output, float max_output, bool reduce_only) const +{ + float k1 = compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); + + if (reduce_only && k1 > 0.f) { + return; + } + + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += k1 * desaturation_vector[i]; + } + + // Compute the desaturation gain again based on the updated outputs. + // In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output. + // In that case adding 0.5 of the gain will equilibrate saturations. + float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); + + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += k2 * desaturation_vector[i]; + } +} + +void +MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs) +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + thrust * _rotors[i].thrust_scale; + + // Thrust will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].thrust_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + // Mix yaw independently + mix_yaw(yaw, outputs); +} + +void +MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs) +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust * _rotors[i].thrust_scale; + + // Thrust will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].thrust_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].yaw_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); +} + +void +MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs) +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + thrust * _rotors[i].thrust_scale; + + // Thrust will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].thrust_scale; + } + + // only reduce thrust + minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].roll_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].pitch_scale; + } + + minimize_saturation(_tmp_array, outputs, _saturation_status); + + // Mix yaw independently + mix_yaw(yaw, outputs); +} + +void MultirotorMixer::mix_yaw(float yaw, float *outputs) +{ + // Add yaw to outputs + for (unsigned i = 0; i < _rotor_count; i++) { + outputs[i] += yaw * _rotors[i].yaw_scale; + + // Yaw will be used to unsaturate if needed + _tmp_array[i] = _rotors[i].yaw_scale; + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f); + + for (unsigned i = 0; i < _rotor_count; i++) { + _tmp_array[i] = _rotors[i].thrust_scale; + } + + // reduce thrust only + minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true); +} + +unsigned +MultirotorMixer::mix(float *outputs, unsigned space) +{ + if (space < _rotor_count) { + return 0; + } + + float roll = math::constrain(get_control(0, 0), -1.0f, 1.0f); + float pitch = math::constrain(get_control(0, 1), -1.0f, 1.0f); + float yaw = math::constrain(get_control(0, 2), -1.0f, 1.0f); + float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f); + + // clean out class variable used to capture saturation + _saturation_status.value = 0; + + // Do the mixing using the strategy given by the current Airmode configuration + switch (_airmode) { + case Airmode::roll_pitch: + mix_airmode_rp(roll, pitch, yaw, thrust, outputs); + break; + + case Airmode::roll_pitch_yaw: + mix_airmode_rpy(roll, pitch, yaw, thrust, outputs); + break; + + case Airmode::disabled: + default: // just in case: default to disabled + mix_airmode_disabled(roll, pitch, yaw, thrust, outputs); + break; + } + + // Apply thrust model and scale outputs to range [idle_speed, 1]. + // At this point the outputs are expected to be in [0, 1], but they can be outside, for example + // if a roll command exceeds the motor band limit. + for (unsigned i = 0; i < _rotor_count; i++) { + // Implement simple model for static relationship between applied motor pwm and motor thrust + // model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2 + if (_thrust_factor > 0.0f) { + outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) * + (1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] / + _thrust_factor)); + } + + outputs[i] = math::constrain((2.f * outputs[i] - 1.f), -1.f, 1.f); + } + + // Slew rate limiting and saturation checking + for (unsigned i = 0; i < _rotor_count; i++) { + bool clipping_high = false; + bool clipping_low_roll_pitch = false; + bool clipping_low_yaw = false; + + // Check for saturation against static limits. + // We only check for low clipping if airmode is disabled (or yaw + // clipping if airmode==roll/pitch), since in all other cases thrust will + // be reduced or boosted and we can keep the integrators enabled, which + // leads to better tracking performance. + if (outputs[i] < -0.99f) { + if (_airmode == Airmode::disabled) { + clipping_low_roll_pitch = true; + clipping_low_yaw = true; + + } else if (_airmode == Airmode::roll_pitch) { + clipping_low_yaw = true; + } + } + + // check for saturation against slew rate limits + if (_delta_out_max > 0.0f) { + float delta_out = outputs[i] - _outputs_prev[i]; + + if (delta_out > _delta_out_max) { + outputs[i] = _outputs_prev[i] + _delta_out_max; + clipping_high = true; + + } else if (delta_out < -_delta_out_max) { + outputs[i] = _outputs_prev[i] - _delta_out_max; + clipping_low_roll_pitch = true; + clipping_low_yaw = true; + } + } + + _outputs_prev[i] = outputs[i]; + + // update the saturation status report + update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw); + } + + // this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen + _delta_out_max = 0.0f; + + return _rotor_count; +} + +/* + * This function update the control saturation status report using the following inputs: + * + * index: 0 based index identifying the motor that is saturating + * clipping_high: true if the motor demand is being limited in the positive direction + * clipping_low_roll_pitch: true if the motor demand is being limited in the negative direction (roll/pitch) + * clipping_low_yaw: true if the motor demand is being limited in the negative direction (yaw) +*/ +void +MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, + bool clipping_low_yaw) +{ + // The motor is saturated at the upper limit + // check which control axes and which directions are contributing + if (clipping_high) { + if (_rotors[index].roll_scale > 0.0f) { + // A positive change in roll will increase saturation + _saturation_status.flags.roll_pos = true; + + } else if (_rotors[index].roll_scale < 0.0f) { + // A negative change in roll will increase saturation + _saturation_status.flags.roll_neg = true; + } + + // check if the pitch input is saturating + if (_rotors[index].pitch_scale > 0.0f) { + // A positive change in pitch will increase saturation + _saturation_status.flags.pitch_pos = true; + + } else if (_rotors[index].pitch_scale < 0.0f) { + // A negative change in pitch will increase saturation + _saturation_status.flags.pitch_neg = true; + } + + // check if the yaw input is saturating + if (_rotors[index].yaw_scale > 0.0f) { + // A positive change in yaw will increase saturation + _saturation_status.flags.yaw_pos = true; + + } else if (_rotors[index].yaw_scale < 0.0f) { + // A negative change in yaw will increase saturation + _saturation_status.flags.yaw_neg = true; + } + + // A positive change in thrust will increase saturation + _saturation_status.flags.thrust_pos = true; + } + + // The motor is saturated at the lower limit + // check which control axes and which directions are contributing + if (clipping_low_roll_pitch) { + // check if the roll input is saturating + if (_rotors[index].roll_scale > 0.0f) { + // A negative change in roll will increase saturation + _saturation_status.flags.roll_neg = true; + + } else if (_rotors[index].roll_scale < 0.0f) { + // A positive change in roll will increase saturation + _saturation_status.flags.roll_pos = true; + } + + // check if the pitch input is saturating + if (_rotors[index].pitch_scale > 0.0f) { + // A negative change in pitch will increase saturation + _saturation_status.flags.pitch_neg = true; + + } else if (_rotors[index].pitch_scale < 0.0f) { + // A positive change in pitch will increase saturation + _saturation_status.flags.pitch_pos = true; + } + + // A negative change in thrust will increase saturation + _saturation_status.flags.thrust_neg = true; + } + + if (clipping_low_yaw) { + // check if the yaw input is saturating + if (_rotors[index].yaw_scale > 0.0f) { + // A negative change in yaw will increase saturation + _saturation_status.flags.yaw_neg = true; + + } else if (_rotors[index].yaw_scale < 0.0f) { + // A positive change in yaw will increase saturation + _saturation_status.flags.yaw_pos = true; + } + } + + _saturation_status.flags.valid = true; +} diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp new file mode 100644 index 000000000000..d8e73634177a --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * Supported multirotor geometries. + * + * Values are generated by the px_generate_mixers.py script and placed to mixer_multirotor_normalized.generated.h + */ +typedef uint8_t MultirotorGeometryUnderlyingType; +enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; + +/** + * Multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ +class MultirotorMixer : public Mixer +{ +public: + /** + + * Precalculated rotor mix. + */ + struct Rotor { + float roll_scale; /**< scales roll for this rotor */ + float pitch_scale; /**< scales pitch for this rotor */ + float yaw_scale; /**< scales yaw for this rotor */ + float thrust_scale; /**< scales thrust for this rotor */ + }; + + /** + * Constructor. + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param geometry The selected geometry. + * @param roll_scale Scaling factor applied to roll inputs + * compared to thrust. + * @param pitch_scale Scaling factor applied to pitch inputs + * compared to thrust. + * @param yaw_wcale Scaling factor applied to yaw inputs compared + * to thrust. + * @param idle_speed Minimum rotor control output value; usually + * tuned to ensure that rotors never stall at the + * low end of their control range. + */ + MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry); + + /** + * Constructor (for testing). + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param rotors control allocation matrix + * @param rotor_count length of rotors array (= number of motors) + */ + MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count); + virtual ~MultirotorMixer(); + + // no copy, assignment, move, move assignment + MultirotorMixer(const MultirotorMixer &) = delete; + MultirotorMixer &operator=(const MultirotorMixer &) = delete; + MultirotorMixer(MultirotorMixer &&) = delete; + MultirotorMixer &operator=(MultirotorMixer &&) = delete; + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new MultirotorMixer instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + + uint16_t get_saturation_status() override { return _saturation_status.value; } + + void groups_required(uint32_t &groups) override { groups |= (1 << 0); } + + /** + * @brief Update slew rate parameter. This tells the multicopter mixer + * the maximum allowed change of the output values per cycle. + * The value is only valid for one cycle, in order to have continuous + * slew rate limiting this function needs to be called before every call + * to mix(). + * + * @param[in] delta_out_max Maximum delta output. + * + */ + void set_max_delta_out_once(float delta_out_max) override { _delta_out_max = delta_out_max; } + + unsigned set_trim(float trim) override { return _rotor_count; } + unsigned get_trim(float *trim) override { return _rotor_count; } + + /** + * @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. + * + * @param[in] val The value + */ + void set_thrust_factor(float val) override { _thrust_factor = math::constrain(val, 0.0f, 1.0f); } + + void set_airmode(Airmode airmode) override { _airmode = airmode; } + + unsigned get_multirotor_count() override { return _rotor_count; } + + union saturation_status { + struct { + uint16_t valid : 1; // 0 - true when the saturation status is used + uint16_t motor_pos : 1; // 1 - true when any motor has saturated in the positive direction + uint16_t motor_neg : 1; // 2 - true when any motor has saturated in the negative direction + uint16_t roll_pos : 1; // 3 - true when a positive roll demand change will increase saturation + uint16_t roll_neg : 1; // 4 - true when a negative roll demand change will increase saturation + uint16_t pitch_pos : 1; // 5 - true when a positive pitch demand change will increase saturation + uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation + uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation + uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation + uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation + uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation + } flags; + uint16_t value; + }; + +private: + /** + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. + * @see also minimize_saturation(). + * + * @return desaturation gain + */ + float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status, + float min_output, float max_output) const; + + /** + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param outputs output vector that is modified + * @param sat_status saturation status output + * @param min_output minimum desired value in outputs + * @param max_output maximum desired value in outputs + * @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector + */ + void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status, + float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const; + + /** + * Mix roll, pitch, yaw, thrust and set the outputs vector. + * + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + */ + inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs); + + /** + * Mix roll, pitch, yaw, thrust and set the outputs vector. + * + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. + */ + inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs); + + /** + * Mix roll, pitch, yaw, thrust and set the outputs vector. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mix_yaw() for the exact yaw behavior. + */ + inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs); + + /** + * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + * + * @param yaw demanded yaw + * @param outputs output vector that is updated + */ + inline void mix_yaw(float yaw, float *outputs); + + void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw); + + float _delta_out_max{0.0f}; + float _thrust_factor{0.0f}; + + Airmode _airmode{Airmode::disabled}; + + saturation_status _saturation_status{}; + + unsigned _rotor_count; + const Rotor *_rotors; + + float *_outputs_prev{nullptr}; + float *_tmp_array{nullptr}; +}; diff --git a/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml new file mode 100644 index 000000000000..7470bff67bba --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/dodeca_bottom_cox.toml @@ -0,0 +1,40 @@ +# Generic Dodecacopter in X coax configuration, bottom half + +[info] +key = "6a" +description = "Generic Dodecacopter in X coax configuration, bottom half" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "mid_right_bottom" +position = [0.0, 1.0, 0.1] +direction = "CCW" + +[[rotors]] +name = "mid_left_bottom" +position = [0.0, -1.0, 0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.866025, -0.5, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_right_bottom" +position = [-0.866025, 0.5, 0.1] +direction = "CW" + +[[rotors]] +name = "front_right_bottom" +position = [0.866025, 0.5, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_left_bottom" +position = [-0.866025, -0.5, 0.1] +direction = "CCW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml new file mode 100644 index 000000000000..e875240bac9c --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/dodeca_top_cox.toml @@ -0,0 +1,40 @@ +# Generic Dodecacopter in X coax configuration, top half + +[info] +key = "6m" +description = "Generic Dodecacopter in X coax configuration, top half" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "mid_right_top" +position = [0.0, 1.0, -0.1] +direction = "CW" + +[[rotors]] +name = "mid_left_top" +position = [0.0, -1.0, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.866025, -0.5, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_right_top" +position = [-0.866025, 0.5, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_right_top" +position = [0.866025, 0.5, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_left_top" +position = [-0.866025, -0.5, -0.1] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml new file mode 100644 index 000000000000..a344c16e1f2e --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_cox.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in coaxial configuration + +[info] +key = "6c" +description = "Generic Hexacopter in coaxial configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.5, 0.866, -0.1] +direction = "CW" + +[[rotors]] +name = "front_right_bottom" +position = [0.5, 0.866, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_top" +position = [-1.0, 0.0, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_bottom" +position = [-1.0, 0.0, 0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.5, -0.866, -0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.5, -0.866, 0.1] +direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml new file mode 100644 index 000000000000..6b86cb40b635 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_plus.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in + configuration + +[info] +key = "6+" +description = "Generic Hexacopter in + configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front" +position = [1.0, 0.0, 0.0] +direction = "CW" + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.5, -0.866025, 0.0] +direction = "CW" + +[[rotors]] +name = "front_right" +position = [0.5, 0.866025, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.5, -0.866025, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.5, 0.866025, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml new file mode 100644 index 000000000000..fc8c32b9644f --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_t.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in T configuration + +[info] +key = "6t" +description = "Generic Hexacopter in T configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.729, 0.684, 0.1] +direction = "CW" + +[[rotors]] +name = "front_right_bottom" +position = [0.729, 0.684, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_top" +position = [-1.0, 0.0, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_bottom" +position = [-1.0, 0.0, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.729, -0.684, 0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.729, -0.684, -0.1] +direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml b/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml new file mode 100644 index 000000000000..a546a83f72eb --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/hex_x.toml @@ -0,0 +1,40 @@ +# Generic Hexacopter in X configuration + +[info] +key = "6x" +description = "Generic Hexacopter in X configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "mid_right" +position = [0.0, 1.0, 0.0] +direction = "CW" + +[[rotors]] +name = "mid_left" +position = [0.0, -1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.866025, -0.5, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_right" +position = [-0.866025, 0.5, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_right" +position = [0.866025, 0.5, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.866025, -0.5, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml new file mode 100644 index 000000000000..6c42bfa1f750 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_cox.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in coax configuration + +[info] +key = "8c" +description = "GenericOctacopter in coax configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.707107, 0.707107, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.707107, -0.707107, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_left_top" +position = [-0.707107, -0.707107, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_right_top" +position = [-0.707107, 0.707107, -0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.707107, -0.707107, 0.1] +direction = "CCW" + +[[rotors]] +name = "front_right_bottom" +position = [0.707107, 0.707107, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_right_bottom" +position = [-0.707107, 0.707107, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_left_bottom" +position = [-0.707107, -0.707107, 0.1] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml new file mode 100644 index 000000000000..785c6a4c0e88 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_cox_wide.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in wide coax configuration + +[info] +key = "8cw" +description = "Generic Octacopter in wide coax configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right_top" +position = [0.3746066, 0.927184, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left_top" +position = [0.3746066, -0.927184, -0.1] +direction = "CW" + +[[rotors]] +name = "rear_left_top" +position = [-0.62932, -0.777146, -0.1] +direction = "CCW" + +[[rotors]] +name = "rear_right_top" +position = [-0.62932, 0.777146, -0.1] +direction = "CW" + +[[rotors]] +name = "front_left_bottom" +position = [0.3746066, -0.927184, 0.1] +direction = "CCW" + +[[rotors]] +name = "front_right_bottom" +position = [0.3746066, 0.927184, 0.1] +direction = "CW" + +[[rotors]] +name = "rear_right_bottom" +position = [-0.62932, 0.777146, 0.1] +direction = "CCW" + +[[rotors]] +name = "rear_left_bottom" +position = [-0.62932, -0.777146, 0.1] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml new file mode 100644 index 000000000000..7c13bc728d11 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_plus.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in + configuration + +[info] +key = "8+" +description = "Generic Octacopter in + configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front" +position = [1.0, 0.0, 0.0] +direction = "CW" + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] +direction = "CW" + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "left" +position = [0.0, -1.0, 0.0] +direction = "CW" + +[[rotors]] +name = "right" +position = [0.0, 1.0, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml b/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml new file mode 100644 index 000000000000..e85403b03234 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/octa_x.toml @@ -0,0 +1,50 @@ +# Generic Octacopter in X configuration + +[info] +key = "8x" +description = "Generic Octacopter in X configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.9238795, 0.3826834, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_left" +position = [-0.9238795, -0.3826834, 0.0] +direction = "CW" + +[[rotors]] +name = "mid_front_right" +position = [0.3826834, 0.9238795, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.9238795, 0.3826834, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.9238795, -0.3826834, 0.0] +direction = "CCW" + +[[rotors]] +name = "mid_rear_left" +position = [-0.3826834, -0.9238795, 0.0] +direction = "CCW" + +[[rotors]] +name = "mid_front_left" +position = [0.3826834, -0.9238795, 0.0] +direction = "CW" + +[[rotors]] +name = "mid_rear_right" +position = [-0.3826834, 0.9238795, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml new file mode 100644 index 000000000000..1e7febaef45b --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_deadcat.toml @@ -0,0 +1,30 @@ +# SK450 DeadCat Quadcopter. +# Same geometry as quad_wide, except CG is located at intersection of rear arms, so front motors are more loaded. + +[info] +key = "4dc" +description = "SK450 DeadCat Quadcopter, CG at intersection of rear arms" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.01 + +[[rotors]] +name = "front_right" +position = [0.1155, 0.245, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.1875, -0.1875, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.1155, -0.245, 0.0] + +[[rotors]] +name = "rear_right" +position = [-0.1875, 0.1875, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml new file mode 100644 index 000000000000..e58b5a5ed65f --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_h.toml @@ -0,0 +1,30 @@ +# Generic Quadcopter in H configuration + +[info] +key = "4h" +description = "Generic Quadcopter in H configuration" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] +direction = "CCW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml new file mode 100644 index 000000000000..e28f8db7f0f4 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_plus.toml @@ -0,0 +1,29 @@ +# Generic Quadcopter in + configuration + +[info] +key = "4+" +description = "Generic Quadcopter in + configuration" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "right" +position = [0.0, 1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "left" +position = [0.0, -1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "front" +position = [1.0, 0.0, 0.0] + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml new file mode 100644 index 000000000000..f6c360bc21b3 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_s250aq.toml @@ -0,0 +1,30 @@ +# Spedix S250AQ Quadcopter +# Note: Rotor positions computed from specifications: length:210mm, width:260mm, wheel base:280mm + +[info] +key = "4s" +description = "Spedix S250AQ Quadcopter" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.01 + +[[rotors]] +name = "front_right" +position = [0.105, 0.130, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.105, -0.0552, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.105, -0.130, 0.0] + +[[rotors]] +name = "rear_right" +position = [-0.105, 0.0552, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml new file mode 100644 index 000000000000..3b934018db10 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_vtail.toml @@ -0,0 +1,33 @@ +# Quadcopter in Y configuration with rear props tilted at 45 degrees + +[info] +key = "4vt" +description = "Quadcopter in Y configuration with rear props tilted at 45 degrees" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.2, 0.2, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_left" +position = [-0.3, -0.1, -0.1] +axis = [0.0, 0.707106, -0.707106] +direction = "CW" + +[[rotors]] +name = "front_left" +position = [0.2, -0.2, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.3, 0.1, -0.1] +axis = [0.0, -0.707106, -0.707106] +direction = "CCW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml new file mode 100644 index 000000000000..d2fde7861b89 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_wide.toml @@ -0,0 +1,30 @@ +# Generic Quadcopter in wide configuration + +[info] +key = "4w" +description = "Quadcopter in wide configuration. Same geometry as SK450 Deadcat except the CG is moved backward to load all motors equally" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.1515, 0.245, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.1515, -0.1875, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.1515, -0.245, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_right" +position = [-0.1515, 0.1875, 0.0] +direction = "CW" \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml new file mode 100644 index 000000000000..b4cf484c2127 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_x.toml @@ -0,0 +1,29 @@ +# Generic Quadcopter in X configuration + +[info] +key = "4x" +description = "Generic Quadcopter in X configuration" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml new file mode 100644 index 000000000000..1af7882b23b6 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_x_cw.toml @@ -0,0 +1,30 @@ +# Generic Quadcopter in X configuration +# with clock-wise motor numbering + +[info] +key = "4xcw" +description = "Quadcopter in X configuration with clock-wise motor numbering" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.707107, 0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_right" +position = [-0.707107, 0.707107, 0.0] + +[[rotors]] +name = "rear_left" +position = [-0.707107, -0.707107, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.707107, -0.707107, 0.0] diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml new file mode 100644 index 000000000000..a63ff0652bf0 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_x_pusher.toml @@ -0,0 +1,37 @@ +# Quadcopter in X configuration, +# with added pusher motor in the back + +[info] +key = "4x1p" +description = "Quadcopter in X configuration, with added pusher motor in the back" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [1.0, 1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_left" +position = [-1.0, -1.0, 0.0] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [1.0, -1.0, 0.0] + +[[rotors]] +name = "rear_right" +position = [-1.0, 1.0, 0.0] + +[[rotors]] +name = "pusher" +position = [-1.0, 0.0, 0.0] +axis = [1.0, 0.0, 0.0] +Ct = 2.0 +Cm = 0.0 diff --git a/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml b/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml new file mode 100644 index 000000000000..b75756c8d962 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/quad_y.toml @@ -0,0 +1,31 @@ +# Quadcopter in Y configuration with coax rear props + +[info] +key = "4y" +description = "Quadcopter in Y configuration with coax rear props" + +[rotor_default] +direction = "CW" +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.05 + +[[rotors]] +name = "front_right" +position = [0.2, 0.2, 0.0] +direction = "CCW" + +[[rotors]] +name = "rear_top" +position = [-0.2, 0.0, -0.1] +direction = "CCW" + +[[rotors]] +name = "front_left" +position = [0.2, -0.2, 0.0] +direction = "CW" + +[[rotors]] +name = "rear_bottom" +position = [-0.2, 0.0, 0.1] +direction = "CW" diff --git a/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py new file mode 100755 index 000000000000..932a265fb487 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py @@ -0,0 +1,398 @@ +#!/usr/bin/env python +############################################################################# +# +# Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +""" +px_generate_mixers.py +Generates c/cpp header/source files for multirotor mixers +from geometry descriptions files (.toml format) +""" +import sys + +try: + import toml +except ImportError as e: + print("Failed to import toml: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user toml") + print("") + sys.exit(1) + +try: + import numpy as np +except ImportError as e: + print("Failed to import numpy: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user numpy") + print("") + sys.exit(1) + +__author__ = "Julien Lecoeur" +__copyright__ = "Copyright (C) 2013-2017 PX4 Development Team." +__license__ = "BSD" +__email__ = "julien.lecoeur@gmail.com" + + +def parse_geometry_toml(filename): + ''' + Parses toml geometry file and returns a dictionary with curated list of rotors + ''' + import os + + # Load toml file + d = toml.load(filename) + + # Check info section + if 'info' not in d: + raise AttributeError('{}: Error, missing info section'.format(filename)) + + # Check info section + for field in ['key', 'description']: + if field not in d['info']: + raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field)) + + # Use filename as mixer name + d['info']['name'] = os.path.basename(filename).split('.')[0].lower() + + # Store filename + d['info']['filename'] = filename + + # Check default rotor config + if 'rotor_default' in d: + default = d['rotor_default'] + else: + default = {} + + # Convert rotors + rotor_list = [] + if 'rotors' in d: + for r in d['rotors']: + # Make sure all fields are defined, fill missing with default + for field in ['name', 'position', 'axis', 'direction', 'Ct', 'Cm']: + if field not in r: + if field in default: + r[field] = default[field] + else: + raise AttributeError('{}: Error, unspecified field "{}" for rotor "{}"' + .format(filename, field, r['name'])) + + # Check direction field + r['direction'] = r['direction'].upper() + if r['direction'] not in ['CW', 'CCW']: + raise AttributeError('{}: Error, invalid direction value "{}" for rotor "{}"' + .format(filename, r['direction'], r['name'])) + + # Check vector3 fields + for field in ['position', 'axis']: + if len(r[field]) != 3: + raise AttributeError('{}: Error, field "{}" for rotor "{}"' + .format(filename, field, r['name']) + + ' must be an array of length 3') + + # Add rotor to list + rotor_list.append(r) + + # Clean dictionary + geometry = {'info': d['info'], + 'rotors': rotor_list} + + return geometry + +def torque_matrix(center, axis, dirs, Ct, Cm): + ''' + Compute torque generated by rotors + ''' + # normalize rotor axis + ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] + torque = Ct * np.cross(center, ax) - Cm * ax * dirs + return torque + +def geometry_to_torque_matrix(geometry): + ''' + Compute torque matrix Am and Bm from geometry dictionnary + Am is a 3xN matrix where N is the number of rotors + Each column is the torque generated by one rotor + ''' + Am = torque_matrix(center=np.array([rotor['position'] for rotor in geometry['rotors']]), + axis=np.array([rotor['axis'] for rotor in geometry['rotors']]), + dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0] + for rotor in geometry['rotors']]), + Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']]), + Cm=np.array([[rotor['Cm']] for rotor in geometry['rotors']])).T + return Am + +def thrust_matrix(axis, Ct): + ''' + Compute thrust generated by rotors + ''' + # Normalize rotor axis + ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] + thrust = Ct * ax + return thrust + +def geometry_to_thrust_matrix(geometry): + ''' + Compute thrust matrix At from geometry dictionnary + At is a 3xN matrix where N is the number of rotors + Each column is the thrust generated by one rotor + ''' + At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geometry['rotors']]), + Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T + + return At + +def geometry_to_mix(geometry): + ''' + Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary + + A is a 6xN matrix where N is the number of rotors + Each column is the torque and thrust generated by one rotor + + B is a Nx6 matrix where N is the number of rotors + Each column is the command to apply to the servos to get + roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust + ''' + # Combined torque & thrust matrix + At = geometry_to_thrust_matrix(geometry) + Am = geometry_to_torque_matrix(geometry) + A = np.vstack([Am, At]) + + # Mix matrix computed as pseudoinverse of A + B = np.linalg.pinv(A) + + return A, B + +def normalize_mix_px4(B): + ''' + Normalize mix for PX4 + This is for compatibility only and should ideally not be used + ''' + B_norm = np.linalg.norm(B, axis=0) + B_max = np.abs(B).max(axis=0) + B_sum = np.sum(B, axis=0) + + # Same scale on roll and pitch + B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0) + B_norm[1] = B_norm[0] + + # Scale yaw separately + B_norm[2] = B_max[2] + + # Same scale on x, y + B_norm[3] = max(B_max[3], B_max[4]) + B_norm[4] = B_norm[3] + + # Scale z thrust separately + B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5]) + + # Normalize + B_norm[np.abs(B_norm) < 1e-3] = 1 + B_px = (B / B_norm) + + return B_px + +def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False): + ''' + Generate C header file with same format as multi_tables.py + TODO: rewrite using templates (see generation of uORB headers) + ''' + from io import StringIO + buf = StringIO() + + # Print Header + buf.write(u"/*\n") + buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n") + buf.write(u"*/\n") + buf.write(u"\n") + buf.write(u"#ifndef _MIXER_MULTI_TABLES\n") + buf.write(u"#define _MIXER_MULTI_TABLES\n") + buf.write(u"\n") + + # Print enum + buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n") + for i, geometry in enumerate(geometries_list): + buf.write(u"\t{},{}// {} (text key {})\n".format( + geometry['info']['name'].upper(), ' ' * (max(0, 30 - len(geometry['info']['name']))), + geometry['info']['description'], geometry['info']['key'])) + buf.write(u"\n\tMAX_GEOMETRY\n") + buf.write(u"}; // enum class MultirotorGeometry\n\n") + + # Print mixer gains + buf.write(u"namespace {\n") + for geometry in geometries_list: + # Get desired mix matrix + if use_normalized_mix: + mix = geometry['mix']['B_px'] + else: + mix = geometry['mix']['B'] + + buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name'])) + + for row in mix: + if use_6dof: + # 6dof mixer + buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + row[0], row[1], row[2], + row[3], row[4], row[5])) + else: + # 4dof mixer + buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format( + row[0], row[1], row[2], + -row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly + + buf.write(u"};\n\n") + + # Print geometry indeces + buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n") + for geometry in geometries_list: + buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name'])) + buf.write(u"};\n\n") + + # Print geometry rotor counts + buf.write(u"static constexpr unsigned _config_rotor_count[] {\n") + for geometry in geometries_list: + buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name'])) + buf.write(u"};\n\n") + + # Print geometry key + buf.write(u"const char* _config_key[] {\n") + for geometry in geometries_list: + buf.write(u"\t\"{}\",\t/* {} */\n".format(geometry['info']['key'], geometry['info']['name'])) + buf.write(u"};\n\n") + + # Print footer + buf.write(u"} // anonymous namespace\n\n") + buf.write(u"#endif /* _MIXER_MULTI_TABLES */\n\n") + + return buf.getvalue() + + +if __name__ == '__main__': + import argparse + import glob + import os + + # Parse arguments + parser = argparse.ArgumentParser( + description='Convert geometry .toml files to mixer headers') + parser.add_argument('-d', dest='dir', + help='directory with geometry files') + parser.add_argument('-f', dest='files', + help="files to convert (use only without -d)", + nargs="+") + parser.add_argument('-o', dest='outputfile', + help='output header file') + parser.add_argument('--verbose', help='Print details on standard output', + action='store_true') + parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)', + action='store_true') + parser.add_argument('--sixdof', help='Use 6dof mixers', + action='store_true') + args = parser.parse_args() + + # Find toml files + if args.files is not None: + filenames = args.files + elif args.dir is not None: + filenames = glob.glob(os.path.join(args.dir, '*.toml')) + else: + parser.print_usage() + raise Exception("Missing input directory (-d) or list of geometry files (-f)") + + # List of geometries + geometries_list = [] + + for filename in filenames: + # Parse geometry file + geometry = parse_geometry_toml(filename) + + # Compute torque and thrust matrices + A, B = geometry_to_mix(geometry) + + # Normalize mixer + B_px = normalize_mix_px4(B) + + # Store matrices in geometry + geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px} + + # Add to list + geometries_list.append(geometry) + + if args.verbose: + print('\nFilename') + print(filename) + print('\nGeometry') + print(geometry) + print('\nA:') + print(A.round(2)) + print('\nB:') + print(B.round(2)) + print('\nNormalized Mix (as in PX4):') + print(B_px.round(2)) + print('\n-----------------------------') + + # Check that there are no duplicated mixer names or keys + for i in range(len(geometries_list)): + name_i = geometries_list[i]['info']['name'] + key_i = geometries_list[i]['info']['key'] + + for j in range(i + 1, len(geometries_list)): + name_j = geometries_list[j]['info']['name'] + key_j = geometries_list[j]['info']['key'] + + # Mixers cannot share the same name + if name_i == name_j: + raise ValueError('Duplicated mixer name "{}" in files {} and {}'.format( + name_i, + geometries_list[i]['info']['filename'], + geometries_list[j]['info']['filename'])) + + # Mixers cannot share the same key + if key_i == key_j: + raise ValueError('Duplicated mixer key "{}" for mixers "{}" and "{}"'.format( + key_i, name_i, name_j)) + + # Generate header file + header = generate_mixer_multirotor_header(geometries_list, + use_normalized_mix=args.normalize, + use_6dof=args.sixdof) + + if args.outputfile is not None: + # Write header file + with open(args.outputfile, 'w') as fd: + fd.write(header) + else: + # Print to standard output + print(header) diff --git a/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml b/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml new file mode 100644 index 000000000000..b41dbd8cbd5e --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/tri_y.toml @@ -0,0 +1,23 @@ +# Tri Y + +[info] +key = "3y" +description = "Tri Y" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.0 +direction = "CW" + +[[rotors]] +name = "front_right" +position = [0.5, 0.866025, 0.0] + +[[rotors]] +name = "front_left" +position = [0.5, -0.866025, 0.0] + +[[rotors]] +name = "rear" +position = [-1.0, 0.0, 0.0] \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml b/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml new file mode 100644 index 000000000000..6a547a165468 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/geometries/twin_engine.toml @@ -0,0 +1,19 @@ +# Twin engine + +[info] +key = "2-" +description = "Twin engine" + +[rotor_default] +axis = [0.0, 0.0, -1.0] +Ct = 1.0 +Cm = 0.0 +direction = "CW" + +[[rotors]] +name = "right" +position = [0.0, 1.0, 0.0] + +[[rotors]] +name = "left" +position = [0.0, -1.0, 0.0] \ No newline at end of file diff --git a/src/lib/mixer/MultirotorMixer/mixer_multirotor.py b/src/lib/mixer/MultirotorMixer/mixer_multirotor.py new file mode 100755 index 000000000000..b3badaade3e7 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/mixer_multirotor.py @@ -0,0 +1,386 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +Mixer multirotor test and prototyping script. + +Author: Mathieu Bresciani , Beat Kueng +Description: This script can be used to prototype new mixer algorithms and test +it against the C++ implementation. +""" + + +from __future__ import print_function + +from argparse import ArgumentParser +import numpy as np +import numpy.matlib +import subprocess + + +# -------------------------------------------------- +# mixing algorithms +# -------------------------------------------------- + +def compute_desaturation_gain(u, u_min, u_max, desaturation_vector): + """ + Computes the gain k by which desaturation_vector has to be multiplied + in order to unsaturate the output that has the greatest saturation + """ + d_u_sat_plus = u_max - u + d_u_sat_minus = u_min - u + k = np.zeros(u.size*2) + for i in range(u.size): + if abs(desaturation_vector[i]) < 0.000001: + # avoid division by zero + continue + + if d_u_sat_minus[i] > 0.0: + k[2*i] = d_u_sat_minus[i] / desaturation_vector[i] + if d_u_sat_plus[i] < 0.0: + k[2*i+1] = d_u_sat_plus[i] / desaturation_vector[i] + + k_min = min(k) + k_max = max(k) + + # Reduce the saturation as much as possible + k = k_min + k_max + return k + + +def minimize_sat(u, u_min, u_max, desaturation_vector): + """ + Minimize the saturation of the actuators by + adding or substracting a fraction of desaturation_vector. + desaturation_vector is the vector that added to the output u, + modifies the thrust or angular acceleration on a + specific axis. + For example, if desaturation_vector is given + to slide along the vertical thrust axis, the saturation will + be minimized by shifting the vertical thrust setpoint, + without changing the roll/pitch/yaw accelerations. + """ + k_1 = compute_desaturation_gain(u, u_min, u_max, desaturation_vector) + u_1 = u + k_1 * desaturation_vector # Try to unsaturate + + + # Compute the desaturation gain again based on the updated outputs. + # In most cases it will be zero. It won't be if max(outputs) - min(outputs) + # > max_output - min_output. + # In that case adding 0.5 of the gain will equilibrate saturations. + k_2 = compute_desaturation_gain(u_1, u_min, u_max, desaturation_vector) + + k_opt = k_1 + 0.5 * k_2 + + u_prime = u + k_opt * desaturation_vector + return u_prime + +def mix_yaw(m_sp, u, P, u_min, u_max): + """ + Mix yaw by adding it to an existing output vector u + + Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + some yaw control on the upper end. On the lower end thrust will never be increased, + but yaw is decreased as much as required. + """ + m_sp_yaw_only = np.matlib.zeros(m_sp.size).T + m_sp_yaw_only[2, 0] = m_sp[2, 0] + u_p = u + P * m_sp_yaw_only + + # Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + # and allow some yaw response at maximum thrust + u_r_dot = P[:,2] + u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot) + u_T = P[:, 3] + u_ppp = minimize_sat(u_pp, 0, u_max, u_T) + # reduce thrust only + if (u_ppp > (u_pp)).any(): + u_ppp = u_pp + return u_ppp + +def airmode_rp(m_sp, P, u_min, u_max): + """ + Mix roll, pitch, yaw and thrust. + + Desaturation behavior: airmode for roll/pitch: + thrust is increased/decreased as much as required to meet the demanded roll/pitch. + Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + """ + # Mix without yaw + m_sp_no_yaw = m_sp.copy() + m_sp_no_yaw[2, 0] = 0.0 + u = P * m_sp_no_yaw + + # Use thrust to unsaturate the outputs if needed + u_T = P[:, 3] + u_prime = minimize_sat(u, u_min, u_max, u_T) + + # Mix yaw axis independently + u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max) + + return (u, u_final) + + +def airmode_rpy(m_sp, P, u_min, u_max): + """ + Mix roll, pitch, yaw and thrust. + + Desaturation behavior: full airmode for roll/pitch/yaw: + thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw. + """ + # Mix with yaw + u = P * m_sp + + # Use thrust to unsaturate the outputs if needed + u_T = P[:, 3] + u_prime = minimize_sat(u, u_min, u_max, u_T) + + # Unsaturate yaw (in case upper and lower bounds are exceeded) + # to prioritize roll/pitch over yaw. + u_T = P[:, 2] + u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T) + + return (u, u_prime_yaw) + + +def normal_mode(m_sp, P, u_min, u_max): + """ + Mix roll, pitch, yaw and thrust. + + Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + Thrust can be reduced to unsaturate the upper side. + @see mix_yaw() for the exact yaw behavior. + """ + # Mix without yaw + m_sp_no_yaw = m_sp.copy() + m_sp_no_yaw[2, 0] = 0.0 + u = P * m_sp_no_yaw + + # Use thrust to unsaturate the outputs if needed + # by reducing the thrust only + u_T = P[:, 3] + u_prime = minimize_sat(u, u_min, u_max, u_T) + if (u_prime > (u)).any(): + u_prime = u + + # Reduce roll/pitch acceleration if needed to unsaturate + u_p_dot = P[:, 0] + u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot) + u_q_dot = P[:, 1] + u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot) + + # Mix yaw axis independently + u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max) + return (u, u_final) + + +# -------------------------------------------------- +# test cases +# -------------------------------------------------- + +# normalized control allocation test matrices (B_px from px_generate_mixers.py) +# quad_x +P1 = np.matrix([ + [-0.25, 0.25, 0.25, 1], + [-0.25, -0.25, -0.25, 1], + [0.25, -0.25, 0.25, 1], + [0.25, 0.25, -0.25, 1]]) +# quad_wide +P2 = np.matrix([ + [-0.5, 0.71, 0.77, 1. ], + [ 0.5, -0.71, 1., 1. ], + [ 0.5, 0.71, -0.77, 1. ], + [-0.5, -0.71, -1., 1. ]]) +# hex_x +P3 = np.matrix([ + [-1., 0., -1., 1. ], + [ 1., -0., 1., 1. ], + [ 0.5, 0.87, -1., 1. ], + [-0.5, -0.87, 1., 1. ], + [-0.5, 0.87, 1., 1. ], + [ 0.5, -0.87, -1., 1. ]]) +# hex_cox +P4 = np.matrix([ + [-0.87, 0.5, -1., 1. ], + [-0.87, 0.5, 1., 1. ], + [ 0., -1., -1., 1. ], + [ 0., -1., 1., 1. ], + [ 0.87, 0.5, -1., 1. ], + [ 0.87, 0.5, 1., 1. ]]) +# octa_plus +P5 = np.matrix([ + [-0., 1., -1., 1. ], + [ 0., -1., -1., 1. ], + [-0.71, 0.71, 1., 1. ], + [-0.71, -0.71, 1., 1. ], + [ 0.71, 0.71, 1., 1. ], + [ 0.71, -0.71, 1., 1. ], + [ 1., 0., -1., 1. ], + [-1., -0., -1., 1. ]]) + +P_tests = [ P1 ] + +test_cases_input = np.matrix([ + # desired accelerations (must be within [-1, 1]): + #roll pitch yaw thrust + [ 0.0, 0.0, 0.0, 0.0], + [-0.05, 0.0, 0.0, 0.0], + [ 0.05, -0.05, 0.0, 0.0], + [ 0.05, 0.05, -0.025, 0.0], + [ 0.0, 0.2, -0.025, 0.0], + [ 0.2, 0.05, 0.09, 0.0], + [-0.125, 0.02, 0.04, 0.0], + + # extreme cases + [ 1.0, 0.0, 0.0, 0.0], + [ 0.0, -1.0, 0.0, 0.0], + [ 0.0, 0.0, 1.0, 0.0], + [ 1.0, 1.0, -1.0, 0.0], + [-1.0, 0.9, -0.9, 0.0], + [-1.0, 0.9, 0.0, 0.0], + ]) + +# use the following thrust values for all test cases (must be within [0, 1]) +thrust_values = [0, 0.1, 0.45, 0.9, 1.0] +test_cases = np.zeros((test_cases_input.shape[0] * len(thrust_values), 4)) +for i in range(test_cases_input.shape[0]): + for k in range(len(thrust_values)): + test_case = test_cases_input[i] + test_case[0, 3] = thrust_values[k] + test_cases[i * len(thrust_values) + k, :] = test_case + + +def run_tests(mixer_cb, P, test_mixer_binary, test_index=None): + """ + Run all (or a specific) tests for a certain mixer method an control + allocation matrix P + """ + B = np.linalg.pinv(P) + proc = subprocess.Popen( + test_mixer_binary, + #'cat > /tmp/test_'+str(mode_idx), shell=True, # just to test the output + #stdout=subprocess.PIPE, + stdin=subprocess.PIPE) + proc.stdin.write("{:}\n".format(mode_idx).encode('utf-8')) # airmode + motor_count = P.shape[0] + proc.stdin.write("{:}\n".format(motor_count).encode('utf-8')) # motor count + # control allocation matrix + for row in P.getA(): + for col in row: + proc.stdin.write("{:.8f} ".format(col).encode('utf-8')) + proc.stdin.write("\n".encode('utf-8')) + proc.stdin.write("\n".encode('utf-8')) + + failed = False + try: + if test_index is None: + # go through all test cases + test_indices = range(test_cases.shape[0]) + else: + test_indices = [test_index] + for i in test_indices: + actuator_controls = test_cases[[i], :].T + proc.stdin.write("{:.8f} {:.8f} {:.8f} {:.8f}\n" + .format(actuator_controls[0, 0], actuator_controls[1, 0], + actuator_controls[2, 0], actuator_controls[3, 0]).encode('utf-8')) + + (u, u_new) = mixer_cb(actuator_controls, P, 0.0, 1.0) + # Saturate the outputs between 0 and 1 + u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T) + u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T) + # write expected outputs + for j in range(motor_count): + proc.stdin.write("{:.8f} ".format(u_new_sat[j, 0]).encode('utf-8')) + proc.stdin.write("\n".encode('utf-8')) + + proc.stdin.close() + except IOError as e: + failed = True + #result = proc.stdout.read() + proc.wait() + if proc.returncode != 0: failed = True + print("B:\n{}".format(B)) + print("P:\n{}".format(P)) + #print(result) + +parser = ArgumentParser(description=__doc__) +parser.add_argument('--test', action='store_true', default=False, help='Run tests') +parser.add_argument("--mixer-multirotor-binary", + help="select test_mixer_multirotor binary file name", + default='./test_mixer_multirotor') +parser.add_argument("--mode", "-m", dest="mode", + help="mixer mode: none, rp, rpy", default=None) +parser.add_argument("-i", dest="index", type=int, + help="Select a single test to run (starting at 1)", default=None) + +args = parser.parse_args() +mixer_mode = args.mode + +if args.test: + mixer_binary = args.mixer_multirotor_binary + test_index = args.index + if test_index is not None: test_index -= 1 + for mode_idx, (airmode, mixer_cb) in enumerate([ + ('none', normal_mode), + ('rp', airmode_rp), + ('rpy', airmode_rpy)]): + if mixer_mode is not None and mixer_mode != airmode: + continue + print('Testing mode: '+airmode) + for P in P_tests: + run_tests(mixer_cb, P, mixer_binary, test_index) + exit(1) + +# -------------------------------------------------- +# Prototyping and corner case testing playground +# -------------------------------------------------- + +# Compute the control allocation matrix +# u = P * m +P = P1 # normal quad +#P = P2 # wide quad + +# Normalized actuator effectiveness matrix using the pseudo inverse of P +# m = B * u +B = np.linalg.pinv(P) + +# Desired accelerations (actuator controls, in [-1, 1]) +p_dot_sp = 0.0 # roll acceleration (p is the roll rate) +q_dot_sp = 0.1 # pitch acceleration +r_dot_sp = 0.1 # yaw acceleration +T_sp = 0.0 # vertical thrust +m_sp = np.matrix([p_dot_sp, q_dot_sp, r_dot_sp, T_sp]).T # Vector of desired "accelerations" + +# Airmode type (none/rp/rpy) +airmode = mixer_mode +if airmode is None: airmode = "none" + +# Actuators output saturations +u_max = 1.0 +u_min = 0.0 + +if airmode == "none": + (u, u_new) = normal_mode(m_sp, P, u_min, u_max) +elif airmode == "rp": + (u, u_new) = airmode_rp(m_sp, P, u_min, u_max) +elif airmode == "rpy": + (u, u_new) = airmode_rpy(m_sp, P, u_min, u_max) +else: + u = 0.0 + u_new = 0.0 + +# Saturate the outputs between 0 and 1 +u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T) +u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T) + +np.set_printoptions(suppress=True) + +# Display some results +print("u = \n{}\n".format(u)) +print("u_new = \n{}\n".format(u_new)) +print("u_new_sat = \n{}\n".format(u_new_sat)) +print("Desired accelerations = \n{}\n".format(m_sp)) +# Compute back the allocated accelerations +m_new = B * u_new_sat +print("Allocated accelerations = \n{}\n".format(m_new)) diff --git a/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp b/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp new file mode 100644 index 000000000000..aebfe4830477 --- /dev/null +++ b/src/lib/mixer/MultirotorMixer/test_mixer_multirotor.cpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * testing binary that runs the multirotor mixer through test cases given + * via file or stdin and compares the mixer output against expected values. + */ + +#include "MultirotorMixer.hpp" + +#include +#include + +static const unsigned output_max = 16; +static float actuator_controls[output_max] {}; + +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + +static int +mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control) +{ + control = actuator_controls[control_index]; + return 0; +} + +int main(int argc, char *argv[]) +{ + FILE *file_in = stdin; + + if (argc > 1) { + file_in = fopen(argv[1], "r"); + } + + unsigned rotor_count = 0; + MultirotorMixer::Rotor rotors[output_max]; + float actuator_outputs[output_max]; + + // read airmode + int airmode; + fscanf(file_in, "%i", &airmode); + + // read the motor count & control allocation matrix + fscanf(file_in, "%i", &rotor_count); + + if (rotor_count > output_max) { + return -1; + } + + for (unsigned i = 0; i < rotor_count; ++i) { + fscanf(file_in, "%f %f %f %f", &rotors[i].roll_scale, &rotors[i].pitch_scale, + &rotors[i].yaw_scale, &rotors[i].thrust_scale); + } + + MultirotorMixer mixer(mixer_callback, 0, rotors, rotor_count); + mixer.set_airmode((Mixer::Airmode)airmode); + + int test_counter = 0; + int num_failed = 0; + + while (!feof(file_in)) { + + // read actuator controls + unsigned count = 0; + + while (count < 4 && fscanf(file_in, "%f", &actuator_controls[count]) == 1) { + ++count; + } + + if (count < 4) { + break; + } + + // do the mixing + if (mixer.mix(actuator_outputs, output_max) != rotor_count) { + return -1; + } + + // Account for MultirotorMixer outputing [-1,1] and python script [0,1] + for (unsigned i = 0; i < rotor_count; i++) { + actuator_outputs[i] = (actuator_outputs[i] + 1.f) * .5f; + } + + // read expected outputs + count = 0; + float expected_output[output_max]; + bool failed = false; + + while (count < rotor_count && fscanf(file_in, "%f", &expected_output[count]) == 1) { + if (fabsf(expected_output[count] - actuator_outputs[count]) > 0.00001f) { + failed = true; + } + + ++count; + } + + if (count < rotor_count) { + break; + } + + printf("EXPECT_EQ(allocate(%.3ff, %.3ff, %.3ff, -%.3ff), Vector4f(%.6ff, %.6ff, %.6ff, %.6ff)); // %i\n", + (double)actuator_controls[0], (double)actuator_controls[1], (double)actuator_controls[2], (double)actuator_controls[3], + (double)expected_output[0], (double)expected_output[1], (double)expected_output[2], (double)expected_output[3], + test_counter + 1); + + if (failed) { + printf("test %i failed:\n", test_counter + 1); + printf("control input : %.3f %.3f %.3f %.3f\n", (double)actuator_controls[0], (double)actuator_controls[1], + (double)actuator_controls[2], (double)actuator_controls[3]); + printf("mixer output : "); + + for (unsigned i = 0; i < rotor_count; ++i) { + printf("%.3f ", (double)actuator_outputs[i]); + } + + printf("\n"); + printf("expected output: "); + + for (unsigned i = 0; i < rotor_count; ++i) { + printf("%.3f ", (double)expected_output[i]); + } + + printf("\n"); + printf("diff : "); + + for (unsigned i = 0; i < rotor_count; ++i) { + printf("%.3f ", (double)(expected_output[i] - actuator_outputs[i])); + } + + printf("\n"); + ++num_failed; + } + + ++test_counter; + } + + printf("tested %i cases: %i success, %i failed\n", test_counter, + test_counter - num_failed, num_failed); + + + if (file_in != stdin) { + fclose(file_in); + } + + return num_failed > 0 ? -1 : 0; +} diff --git a/src/lib/mixer/NullMixer/CMakeLists.txt b/src/lib/mixer/NullMixer/CMakeLists.txt new file mode 100644 index 000000000000..6b2eeb3bbd04 --- /dev/null +++ b/src/lib/mixer/NullMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(NullMixer + NullMixer.cpp + NullMixer.hpp +) +target_link_libraries(NullMixer PRIVATE MixerBase) +add_dependencies(NullMixer prebuild_targets) diff --git a/src/lib/mixer/NullMixer/NullMixer.cpp b/src/lib/mixer/NullMixer/NullMixer.cpp new file mode 100644 index 000000000000..636cddd62db7 --- /dev/null +++ b/src/lib/mixer/NullMixer/NullMixer.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "NullMixer.hpp" + +#include +#include +#include + +unsigned +NullMixer::mix(float *outputs, unsigned space) +{ + if (space > 0) { + *outputs = NAN; + return 1; + } + + return 0; +} + +NullMixer * +NullMixer::from_text(const char *buf, unsigned &buflen) +{ + NullMixer *nm = nullptr; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { + nm = new NullMixer; + buflen -= 2; + } + + return nm; +} diff --git a/src/lib/mixer/NullMixer/NullMixer.hpp b/src/lib/mixer/NullMixer/NullMixer.hpp new file mode 100644 index 000000000000..4f098298b363 --- /dev/null +++ b/src/lib/mixer/NullMixer/NullMixer.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** + * Null mixer; returns zero. + * + * Used as a placeholder for output channels that are unassigned in groups. + */ +class NullMixer : public Mixer +{ +public: + NullMixer() : Mixer(nullptr, 0) {} + virtual ~NullMixer() = default; + + // no copy, assignment, move, move assignment + NullMixer(const NullMixer &) = delete; + NullMixer &operator=(const NullMixer &) = delete; + NullMixer(NullMixer &&) = delete; + NullMixer &operator=(NullMixer &&) = delete; + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new NullMixer instance, or nullptr + * if the text format is bad. + */ + static NullMixer *from_text(const char *buf, unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + + unsigned set_trim(float trim) override { return 1; } + unsigned get_trim(float *trim) override { return 1; } + +}; diff --git a/src/lib/mixer/SimpleMixer/CMakeLists.txt b/src/lib/mixer/SimpleMixer/CMakeLists.txt new file mode 100644 index 000000000000..ef01747d26d6 --- /dev/null +++ b/src/lib/mixer/SimpleMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(SimpleMixer + SimpleMixer.cpp + SimpleMixer.hpp +) +target_link_libraries(SimpleMixer PRIVATE MixerBase) +add_dependencies(SimpleMixer prebuild_targets) diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.cpp b/src/lib/mixer/SimpleMixer/SimpleMixer.cpp new file mode 100644 index 000000000000..270b77ffcf22 --- /dev/null +++ b/src/lib/mixer/SimpleMixer/SimpleMixer.cpp @@ -0,0 +1,390 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_simple.cpp + * + * Simple summing mixer. + */ + +#include "SimpleMixer.hpp" + +#include +#include + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _pinfo(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_pinfo != nullptr) { + free(_pinfo); + } +} + +unsigned SimpleMixer::set_trim(float trim) +{ + _pinfo->output_scaler.offset = trim; + return 1; +} + +unsigned SimpleMixer::get_trim(float *trim) +{ + *trim = _pinfo->output_scaler.offset; + return 1; +} + +void +SimpleMixer::set_dt_once(float dt) +{ + _dt = dt; +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time) +{ + int ret; + int s[6]; + int n = -1; + + buf = findtag(buf, buflen, 'O'); + + if ((buf == nullptr) || (buflen < 12)) { + debug("output parser failed finding tag, ret: '%s'", buf); + return -1; + } + + if ((ret = sscanf(buf, "O: %d %d %d %d %d %d %n", + &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &n)) < 5) { + debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); + return -1; + } + + // set slew rate limit to 0 if no 6th number is specified in mixer file + if (ret == 5) { + s[5] = 0; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } + + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + slew_rate_rise_time = s[5] / 10000.0f; + + return 0; +} + +int +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index) +{ + unsigned u[2]; + int s[5]; + + buf = findtag(buf, buflen, 'S'); + + if ((buf == nullptr) || (buflen < 16)) { + debug("control parser failed finding tag, ret: '%s'", buf); + return -1; + } + + if (sscanf(buf, "S: %u %u %d %d %d %d %d", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } + + control_group = u[0]; + control_index = u[1]; + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + unsigned inputs; + int used; + const char *end = buf + buflen; + char next_tag; + + /* enforce that the mixer ends with a new line */ + if (!string_well_formed(buf, buflen)) { + return nullptr; + } + + /* get the base info for the mixer */ + if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { + debug("simple parse failed on '%s'", buf); + goto out; + } + + /* at least 1 input is required */ + if (inputs == 0) { + debug("simple parse got 0 inputs"); + goto out; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = inputs; + + /* find the next tag */ + next_tag = findnexttag(end - buflen, buflen); + + if (next_tag == 'S') { + /* No output scalers specified. Use default values. + * Corresponds to: + * O: 10000 10000 0 -10000 10000 0 + */ + mixinfo->output_scaler.negative_scale = 1.0f; + mixinfo->output_scaler.positive_scale = 1.0f; + mixinfo->output_scaler.offset = 0.f; + mixinfo->output_scaler.min_output = -1.0f; + mixinfo->output_scaler.max_output = 1.0f; + mixinfo->slew_rate_rise_time = 0.0f; + + } else { + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler, mixinfo->slew_rate_rise_time)) { + debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); + goto out; + } + } + + for (unsigned i = 0; i < inputs; i++) { + if (parse_control_scaler(end - buflen, buflen, + mixinfo->controls[i].scaler, + mixinfo->controls[i].control_group, + mixinfo->controls[i].control_index)) { + debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); + goto out; + } + } + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("loaded mixer with %d input(s)", inputs); + + } else { + debug("could not allocate memory for mixer"); + } + +out: + + if (mixinfo != nullptr) { + free(mixinfo); + } + + return sm; +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_pinfo == nullptr) { + return 0; + } + + if (space < 1) { + return 0; + } + + for (unsigned i = 0; i < _pinfo->control_count; i++) { + float input = 0.0f; + + _control_cb(_cb_handle, + _pinfo->controls[i].control_group, + _pinfo->controls[i].control_index, + input); + + sum += scale(_pinfo->controls[i].scaler, input); + } + + *outputs = scale(_pinfo->output_scaler, sum); + + if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) { + + // factor 2 is needed because actuator outputs are in the range [-1,1] + const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time; + + float delta_out = *outputs - _output_prev; + + if (delta_out > output_delta_max) { + *outputs = _output_prev + output_delta_max; + + } else if (delta_out < -output_delta_max) { + *outputs = _output_prev - output_delta_max; + } + + } + + // this will force the caller of the mixer to always supply dt values, otherwise no slew rate limiting will happen + _dt = 0.f; + + _output_prev = *outputs; + + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _pinfo->control_count; i++) { + groups |= 1 << _pinfo->controls[i].control_group; + } +} + +int +SimpleMixer::check() +{ + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_pinfo->control_count > 32) { + return -2; + } + + /* validate the output scaler */ + int ret = scale_check(_pinfo->output_scaler); + + if (ret != 0) { + return ret; + } + + /* validate input scalers */ + for (unsigned i = 0; i < _pinfo->control_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _pinfo->controls[i].control_group, + _pinfo->controls[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_pinfo->controls[i].scaler); + + if (ret != 0) { + return (10 * i + ret); + } + } + + return 0; +} + +float +SimpleMixer::scale(const mixer_scaler_s &scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler.negative_scale) + scaler.offset; + + } else { + output = (input * scaler.positive_scale) + scaler.offset; + } + + return math::constrain(output, scaler.min_output, scaler.max_output); +} + +int +SimpleMixer::scale_check(mixer_scaler_s &scaler) +{ + if (scaler.offset > 1.001f) { + return 1; + } + + if (scaler.offset < -1.001f) { + return 2; + } + + if (scaler.min_output > scaler.max_output) { + return 3; + } + + if (scaler.min_output < -1.001f) { + return 4; + } + + if (scaler.max_output > 1.001f) { + return 5; + } + + return 0; +} diff --git a/src/lib/mixer/SimpleMixer/SimpleMixer.hpp b/src/lib/mixer/SimpleMixer/SimpleMixer.hpp new file mode 100644 index 000000000000..d2811e6d59d9 --- /dev/null +++ b/src/lib/mixer/SimpleMixer/SimpleMixer.hpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +/** simple channel scaler */ +struct mixer_scaler_s { + float negative_scale{1.0f}; + float positive_scale{1.0f}; + float offset{0.0f}; + float min_output{-1.0f}; + float max_output{1.0f}; +}; + +/** mixer input */ +struct mixer_control_s { + uint8_t control_group; /**< group from which the input reads */ + uint8_t control_index; /**< index within the control group */ + mixer_scaler_s scaler; /**< scaling applied to the input before use */ +}; + +#define MIXER_SIMPLE_SIZE(_icount) (sizeof(struct mixer_simple_s) + (_icount) * sizeof(struct mixer_control_s)) + +/** simple mixer */ +struct mixer_simple_s { + uint8_t control_count; /**< number of inputs */ + mixer_scaler_s output_scaler; /**< scaling for the output */ + float slew_rate_rise_time{0.0f}; /**< output max rise time (slew rate limit)*/ + mixer_control_s controls[]; /**< actual size of the array is set by control_count */ +}; + +/** + * Simple summing mixer. + * + * Collects zero or more inputs and mixes them to a single output. + */ +class SimpleMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param mixinfo Mixer configuration. The pointer passed + * becomes the property of the mixer and + * will be freed when the mixer is deleted. + */ + SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo); + virtual ~SimpleMixer(); + + // no copy, assignment, move, move assignment + SimpleMixer(const SimpleMixer &) = delete; + SimpleMixer &operator=(const SimpleMixer &) = delete; + SimpleMixer(SimpleMixer &&) = delete; + SimpleMixer &operator=(SimpleMixer &&) = delete; + + /** + * Factory method with full external configuration. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new SimpleMixer instance, or nullptr + * if the text format is bad. + */ + static SimpleMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + + void groups_required(uint32_t &groups) override; + + /** + * Check that the mixer configuration as loaded is sensible. + * + * Note that this function will call control_cb, but only cares about + * error returns, not the input value. + * + * @return Zero if the mixer makes sense, nonzero otherwise. + */ + int check(); + + unsigned set_trim(float trim) override; + unsigned get_trim(float *trim) override; + void set_dt_once(float dt) override; + +private: + + /** + * Perform simpler linear scaling. + * + * @param scaler The scaler configuration. + * @param input The value to be scaled. + * @return The scaled value. + */ + static float scale(const mixer_scaler_s &scaler, float input); + + /** + * Validate a scaler + * + * @param scaler The scaler to be validated. + * @return Zero if good, nonzero otherwise. + */ + static int scale_check(struct mixer_scaler_s &scaler); + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, float &slew_rate_rise_time); + static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, + uint8_t &control_index); + + float _output_prev{0.f}; + float _dt{0.f}; + + mixer_simple_s *_pinfo; + +}; diff --git a/src/lib/mixer/load_mixer_file.cpp b/src/lib/mixer/load_mixer_file.cpp new file mode 100644 index 000000000000..52fd84b6defc --- /dev/null +++ b/src/lib/mixer/load_mixer_file.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_load.c + * + * Programmable multi-channel mixer library. + */ + +#include +#include +#include + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf, unsigned maxlen) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + + if (fp == nullptr) { + printf("file not found\n"); + return -1; + } + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + + if (fgets(line, sizeof(line), fp) == nullptr) { + break; + } + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) { + continue; + } + + /* compact whitespace in the buffer */ + char *t, *f; + + for (f = line; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + + while (*t == ' ') { + t++; + } + + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + printf("line too long\n"); + fclose(fp); + return -1; + } + + /* add the line to the buffer */ + strcat(buf, line); + } + + fclose(fp); + return 0; +} diff --git a/src/lib/mixer/mixer_load.h b/src/lib/mixer/mixer_load.h new file mode 100644 index 000000000000..d61e93b4f81a --- /dev/null +++ b/src/lib/mixer/mixer_load.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_load.h + * + */ + + +#ifndef _SYSTEMLIB_MIXER_LOAD_H +#define _SYSTEMLIB_MIXER_LOAD_H value + +#include + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); + +__END_DECLS + +#endif From 4a9fadc5114c979766ee73946ca505fba0019fc1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 21 Feb 2023 16:28:01 +0100 Subject: [PATCH 11/13] Draft for adding back original unit test cases from old multirotor mixer --- ...ontrolAllocationSequentialDesaturation.cpp | 79 ++++-- ...ontrolAllocationSequentialDesaturation.hpp | 16 +- ...olAllocationSequentialDesaturationTest.cpp | 239 ++++++++++++++++-- 3 files changed, 295 insertions(+), 39 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp index 4b28f44dacb9..f15b66819e05 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp @@ -39,7 +39,7 @@ */ #include "ControlAllocationSequentialDesaturation.hpp" - +#include "mathlib/mathlib.h" void ControlAllocationSequentialDesaturation::allocate() @@ -51,34 +51,33 @@ ControlAllocationSequentialDesaturation::allocate() switch (_param_mc_airmode.get()) { case 1: - mixAirmodeRP(); + mix(1.f, 0.f); break; case 2: - mixAirmodeRPY(); + mix(1.f, 1.f); break; default: - mixAirmodeDisabled(); + mix(0.f, 0.f); break; } } void ControlAllocationSequentialDesaturation::desaturateActuators( ActuatorVector &actuator_sp, - const ActuatorVector &desaturation_vector, bool increase_only) + const ActuatorVector &desaturation_vector, + float increase_limit, float decrease_limit) { float gain = computeDesaturationGain(desaturation_vector, actuator_sp); - - if (increase_only && gain < 0.f) { - return; - } + ActuatorVector u1 = actuator_sp; for (int i = 0; i < _num_actuators; i++) { - actuator_sp(i) += gain * desaturation_vector(i); + u1(i) += gain * desaturation_vector(i); } - gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp); + gain += 0.5f * computeDesaturationGain(desaturation_vector, u1); + gain = math::constrain(gain, -increase_limit, decrease_limit); for (int i = 0; i < _num_actuators; i++) { actuator_sp(i) += gain * desaturation_vector(i); @@ -92,7 +91,7 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act float k_max = 0.f; for (int i = 0; i < _num_actuators; i++) { - // Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains + // Do not try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains if (fabsf(desaturation_vector(i)) < 0.2f) { continue; } @@ -118,6 +117,56 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act return k_min + k_max; } +void ControlAllocationSequentialDesaturation::mix(float roll_pitch_limit, float yaw_limit) +{ + ActuatorVector roll; + ActuatorVector pitch; + ActuatorVector yaw; + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + //_mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + roll(i) = _mix(i, ControlAxis::ROLL); + pitch(i) = _mix(i, ControlAxis::PITCH); + yaw(i) = _mix(i, ControlAxis::YAW); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + const bool debug = false; + + if (debug) { printf("Roll Pitch Thrust Mixed\n"); _actuator_sp.print(); }; + + desaturateActuators(_actuator_sp, thrust_z, roll_pitch_limit); + + if (debug) { printf("Thrust Roll Pitch\n"); _actuator_sp.print(); }; + + desaturateActuators(_actuator_sp, roll); + + desaturateActuators(_actuator_sp, pitch); + + if (debug) { printf("Roll Pitch\n"); _actuator_sp.print(); }; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + } + + if (debug) { printf("Yaw mixed\n"); _actuator_sp.print(); }; + + desaturateActuators(_actuator_sp, thrust_z, yaw_limit, 0.15f); + + if (debug) { printf("Thrust Yaw\n"); _actuator_sp.print(); }; + + desaturateActuators(_actuator_sp, yaw); + + if (debug) { printf("Yaw\n"); _actuator_sp.print(); }; +} + void ControlAllocationSequentialDesaturation::mixAirmodeRP() { @@ -193,7 +242,7 @@ ControlAllocationSequentialDesaturation::mixAirmodeDisabled() } // only reduce thrust - desaturateActuators(_actuator_sp, thrust_z, true); + desaturateActuators(_actuator_sp, thrust_z, 0.f); // Reduce roll/pitch acceleration if needed to unsaturate desaturateActuators(_actuator_sp, roll); @@ -204,7 +253,7 @@ ControlAllocationSequentialDesaturation::mixAirmodeDisabled() } void -ControlAllocationSequentialDesaturation::mixYaw() +ControlAllocationSequentialDesaturation::mixYaw(float yaw_limit) { // Add yaw to outputs ActuatorVector yaw; @@ -224,7 +273,7 @@ ControlAllocationSequentialDesaturation::mixYaw() _actuator_max = max_prev; // reduce thrust only - desaturateActuators(_actuator_sp, thrust_z, true); + desaturateActuators(_actuator_sp, thrust_z, 0.f); } void diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp index dfa69d977762..60a987aba48a 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp @@ -76,19 +76,25 @@ class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInv * * @param actuator_sp Actuator setpoint, vector that is modified * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale - * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector + * @param increase_limit if value below 1, only allow to increase (add) a fraction of desaturation_vector to the specified amount */ void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, - bool increase_only = false); + float increase_limit = 1.f, float decrease_limit = 1.f); /** - * Computes the gain k by which desaturation_vector has to be multiplied - * in order to unsaturate the output that has the greatest saturation. + * Computes the gain by which desaturation_vector has to be multiplied + * in order to unsaturate the output in actuator_sp that has the greatest saturation. * * @return desaturation gain */ float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + */ + void mix(float roll_pitch_limit, float yaw); + /** * Mix roll, pitch, yaw, thrust and set the actuator setpoint. * @@ -124,7 +130,7 @@ class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInv * some yaw control on the upper end. On the lower end thrust will never be increased, * but yaw is decreased as much as required. */ - void mixYaw(); + void mixYaw(float yaw_limit = 0.f); DEFINE_PARAMETERS( (ParamInt) _param_mc_airmode ///< air-mode diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index c5fa0b1bec0b..b2093e8e2c15 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -55,21 +55,16 @@ TEST(ControlAllocationSequentialDesaturationTest, SetGetActuatorSetpoint) EXPECT_EQ(control_allocation.getActuatorSetpoint(), actuator_setpoint); } -// Make protected updateParams() function available to the unit test to change airmode after initialization -class TestControlAllocationSequentialDesaturation : public ::ControlAllocationSequentialDesaturation -{ -public: - void updateParams() { ControlAllocationSequentialDesaturation::updateParams(); } -}; - class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test { public: static constexpr uint8_t NUM_ACTUATORS = 4; - TestControlAllocationSequentialDesaturation _control_allocation; + ControlAllocationSequentialDesaturation _control_allocation; void SetUp() override { + setAirmode(0); // No airmode by default + // Quadrotor x geometry ActuatorEffectivenessRotors::Geometry quadx_geometry{}; quadx_geometry.num_rotors = 4; @@ -107,7 +102,7 @@ class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test param_control_autosave(false); // Disable autosaving parameters to avoid busy loop in param_set() param_t param = param_find("MC_AIRMODE"); param_set(param, &mode); - _control_allocation.updateParams(); + _control_allocation.updateParameters(); } Vector4f allocate(float roll, float pitch, float yaw, float thrust) @@ -153,7 +148,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, CollectiveThrust) TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYaw) { - setAirmode(0); // Airmode disabled EXPECT_EQ(allocate(1.f, 0.f, 0.f, -.5f), Vector4f(.25f, .25f, .75f, .75f)); EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -.5f), Vector4f(.75f, .75f, .25f, .25f)); EXPECT_EQ(allocate(0.f, 1.f, 0.f, -.5f), Vector4f(.75f, .25f, .25f, .75f)); @@ -164,7 +158,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYaw) TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawFullThrust) { - setAirmode(0); // Airmode disabled EXPECT_EQ(allocate(1.f, 0.f, 0.f, -1.f), Vector4f(.5f, .5f, 1.f, 1.f)); EXPECT_EQ(allocate(-1.f, 0.f, 0.f, -1.f), Vector4f(1.f, 1.f, .5f, .5f)); EXPECT_EQ(allocate(0.f, 1.f, 0.f, -1.f), Vector4f(1.f, .5f, .5f, 1.f)); @@ -176,7 +169,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawFullThrust) TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrust) { - setAirmode(0); // Airmode disabled // No axis is allocated EXPECT_EQ(allocate(1.f, 0.f, 0.f, 0.f), Vector4f()); EXPECT_EQ(allocate(-1.f, 0.f, 0.f, 0.f), Vector4f()); @@ -215,7 +207,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustA // allocation. TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledOnlyYaw) { - setAirmode(0); // Airmode disabled EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f(0.f, 0.f, 0.f, 0.f)); } @@ -224,7 +215,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledOnlyYaw) // control setpoint. TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustZ) { - setAirmode(0); // Airmode disabled constexpr float THRUST = 0.75f; EXPECT_EQ(allocate(0.f, 0.f, 0.f, -THRUST), Vector4f(THRUST, THRUST, THRUST, THRUST)); } @@ -233,7 +223,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustZ) // This test does not saturate the yaw response. TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndYaw) { - setAirmode(0); // Airmode disabled constexpr float THRUST = 0.75f; constexpr float YAW_TORQUE = 0.02f; constexpr float YAW = YAW_TORQUE / NUM_ACTUATORS; @@ -244,7 +233,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAn // This test saturates the yaw response, but does not reduce total thrust. TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndSaturatedYaw) { - setAirmode(0); // Airmode disabled constexpr float THRUST = 0.75f; constexpr float YAW_TORQUE = 1.f; constexpr float YAW = YAW_TORQUE / NUM_ACTUATORS; @@ -255,7 +243,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAn // This test does not saturate the pitch response. TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndPitch) { - setAirmode(0); // Airmode disabled constexpr float THRUST = 0.75f; constexpr float PITCH_TORQUE = 0.1f; constexpr float PITCH = PITCH_TORQUE / NUM_ACTUATORS; @@ -267,7 +254,6 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAn // This test saturates yaw and demonstrates reduction of thrust for yaw. TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledReducedThrustAndYaw) { - setAirmode(0); // Airmode disabled constexpr float YAW_MARGIN = ControlAllocationSequentialDesaturation::MINIMUM_YAW_MARGIN; EXPECT_EQ(allocate(0.f, 0.f, 1.f, -3.2f), Vector4f(1.f, 1.f - (2.f * YAW_MARGIN), 1.f, 1.f - (2.f * YAW_MARGIN))); } @@ -276,6 +262,221 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledReducedT // This test saturates the pitch response such that thrust is reduced to (partially) compensate. TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledReducedThrustAndPitch) { - setAirmode(0); // Airmode disabled EXPECT_EQ(allocate(0.f, 2.f, 0.f, -3.f), Vector4f(1.f, 0.f, 0.f, 1.f)); } + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, PreviousMixingTestsNoAirmode) +{ + setAirmode(0); // No airmode + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 1 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.100f), Vector4f(0.100000f, 0.100000f, 0.100000f, 0.100000f)); // 2 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.450f), Vector4f(0.450000f, 0.450000f, 0.450000f, 0.450000f)); // 3 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.900f), Vector4f(0.900000f, 0.900000f, 0.900000f, 0.900000f)); // 4 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -1.000f), Vector4f(1.000000f, 1.000000f, 1.000000f, 1.000000f)); // 5 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 6 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.100f), Vector4f(0.112500f, 0.112500f, 0.087500f, 0.087500f)); // 7 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.450f), Vector4f(0.462500f, 0.462500f, 0.437500f, 0.437500f)); // 8 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.900f), Vector4f(0.912500f, 0.912500f, 0.887500f, 0.887500f)); // 9 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -1.000f), Vector4f(1.000000f, 1.000000f, 0.975000f, 0.975000f)); // 10 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 11 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.100f), Vector4f(0.075000f, 0.100000f, 0.125000f, 0.100000f)); // 12 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.450f), Vector4f(0.425000f, 0.450000f, 0.475000f, 0.450000f)); // 13 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.900f), Vector4f(0.875000f, 0.900000f, 0.925000f, 0.900000f)); // 14 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -1.000f), Vector4f(0.950000f, 0.975000f, 1.000000f, 0.975000f)); // 15 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 16 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.100f), Vector4f(0.093750f, 0.081250f, 0.093750f, 0.131250f)); // 17 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.450f), Vector4f(0.443750f, 0.431250f, 0.443750f, 0.481250f)); // 18 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.900f), Vector4f(0.893750f, 0.881250f, 0.893750f, 0.931250f)); // 19 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -1.000f), Vector4f(0.962500f, 0.950000f, 0.962500f, 1.000000f)); // 20 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 21 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.100f), Vector4f(0.143750f, 0.056250f, 0.043750f, 0.156250f)); // 22 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.450f), Vector4f(0.493750f, 0.406250f, 0.393750f, 0.506250f)); // 23 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.900f), Vector4f(0.943750f, 0.856250f, 0.843750f, 0.956250f)); // 24 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -1.000f), Vector4f(0.987500f, 0.900000f, 0.887500f, 1.000000f)); // 25 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 26 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.100f), Vector4f(0.085000f, 0.015000f, 0.160000f, 0.140000f)); // 27 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.450f), Vector4f(0.435000f, 0.365000f, 0.510000f, 0.490000f)); // 28 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.900f), Vector4f(0.885000f, 0.815000f, 0.960000f, 0.940000f)); // 29 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -1.000f), Vector4f(0.922500f, 0.852500f, 0.997500f, 0.977500f)); // 30 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 31 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.100f), Vector4f(0.146250f, 0.116250f, 0.073750f, 0.063750f)); // 32 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.450f), Vector4f(0.496250f, 0.466250f, 0.423750f, 0.413750f)); // 33 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.900f), Vector4f(0.946250f, 0.916250f, 0.873750f, 0.863750f)); // 34 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -1.000f), Vector4f(1.000000f, 0.970000f, 0.927500f, 0.917500f)); // 35 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 36 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.100f), Vector4f(0.000000f, 0.000000f, 0.200000f, 0.200000f)); // 37 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.450f), Vector4f(0.200000f, 0.200000f, 0.700000f, 0.700000f)); // 38 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.900f), Vector4f(0.500000f, 0.500000f, 1.000000f, 1.000000f)); // 39 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -1.000f), Vector4f(0.500000f, 0.500000f, 1.000000f, 1.000000f)); // 40 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 41 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.100f), Vector4f(0.000000f, 0.200000f, 0.200000f, 0.000000f)); // 42 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.450f), Vector4f(0.200000f, 0.700000f, 0.700000f, 0.200000f)); // 43 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.900f), Vector4f(0.500000f, 1.000000f, 1.000000f, 0.500000f)); // 44 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -1.000f), Vector4f(0.500000f, 1.000000f, 1.000000f, 0.500000f)); // 45 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 46 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.100f), Vector4f(0.200000f, 0.000000f, 0.200000f, 0.000000f)); // 47 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.450f), Vector4f(0.700000f, 0.200000f, 0.700000f, 0.200000f)); // 48 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.900f), Vector4f(1.000000f, 0.500000f, 1.000000f, 0.500000f)); // 49 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -1.000f), Vector4f(1.000000f, 0.700000f, 1.000000f, 0.700000f)); // 50 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 51 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.100f), Vector4f(0.200000f, 0.000000f, 0.000000f, 0.200000f)); // 52 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.450f), Vector4f(0.100000f, 0.100000f, 0.000000f, 1.000000f)); // 53 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.900f), Vector4f(0.200000f, 0.000000f, 0.200000f, 1.000000f)); // 54 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -1.000f), Vector4f(0.200000f, 0.000000f, 0.200000f, 1.000000f)); // 55 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 56 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.100f), Vector4f(0.200000f, 0.000000f, 0.000000f, 0.200000f)); // 57 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.450f), Vector4f(0.900000f, 0.450000f, 0.000000f, 0.450000f)); // 58 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.900f), Vector4f(0.950000f, 0.600000f, 0.000000f, 0.550000f)); // 59 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -1.000f), Vector4f(0.950000f, 0.600000f, 0.000000f, 0.550000f)); // 60 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 61 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.100f), Vector4f(0.200000f, 0.000000f, 0.000000f, 0.200000f)); // 62 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.450f), Vector4f(0.900000f, 0.450000f, 0.000000f, 0.450000f)); // 63 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.900f), Vector4f(1.000000f, 0.550000f, 0.050000f, 0.500000f)); // 64 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -1.000f), Vector4f(1.000000f, 0.550000f, 0.050000f, 0.500000f)); // 65 +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, PreviousMixingTestsAirmodeRP) +{ + setAirmode(1); // Roll and pitch airmode + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 1 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.100f), Vector4f(0.100000f, 0.100000f, 0.100000f, 0.100000f)); // 2 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.450f), Vector4f(0.450000f, 0.450000f, 0.450000f, 0.450000f)); // 3 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.900f), Vector4f(0.900000f, 0.900000f, 0.900000f, 0.900000f)); // 4 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -1.000f), Vector4f(1.000000f, 1.000000f, 1.000000f, 1.000000f)); // 5 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.000f), Vector4f(0.025000f, 0.025000f, 0.000000f, 0.000000f)); // 6 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.100f), Vector4f(0.112500f, 0.112500f, 0.087500f, 0.087500f)); // 7 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.450f), Vector4f(0.462500f, 0.462500f, 0.437500f, 0.437500f)); // 8 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.900f), Vector4f(0.912500f, 0.912500f, 0.887500f, 0.887500f)); // 9 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -1.000f), Vector4f(1.000000f, 1.000000f, 0.975000f, 0.975000f)); // 10 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.000f), Vector4f(0.000000f, 0.025000f, 0.050000f, 0.025000f)); // 11 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.100f), Vector4f(0.075000f, 0.100000f, 0.125000f, 0.100000f)); // 12 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.450f), Vector4f(0.425000f, 0.450000f, 0.475000f, 0.450000f)); // 13 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.900f), Vector4f(0.875000f, 0.900000f, 0.925000f, 0.900000f)); // 14 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -1.000f), Vector4f(0.950000f, 0.975000f, 1.000000f, 0.975000f)); // 15 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.000f), Vector4f(0.018750f, 0.006250f, 0.018750f, 0.056250f)); // 16 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.100f), Vector4f(0.093750f, 0.081250f, 0.093750f, 0.131250f)); // 17 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.450f), Vector4f(0.443750f, 0.431250f, 0.443750f, 0.481250f)); // 18 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.900f), Vector4f(0.893750f, 0.881250f, 0.893750f, 0.931250f)); // 19 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -1.000f), Vector4f(0.962500f, 0.950000f, 0.962500f, 1.000000f)); // 20 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.000f), Vector4f(0.100000f, 0.000000f, 0.000000f, 0.100000f)); // 21 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.100f), Vector4f(0.143750f, 0.056250f, 0.043750f, 0.156250f)); // 22 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.450f), Vector4f(0.493750f, 0.406250f, 0.393750f, 0.506250f)); // 23 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.900f), Vector4f(0.943750f, 0.856250f, 0.843750f, 0.956250f)); // 24 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -1.000f), Vector4f(0.987500f, 0.900000f, 0.887500f, 1.000000f)); // 25 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.000f), Vector4f(0.025000f, 0.000000f, 0.100000f, 0.125000f)); // 26 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.100f), Vector4f(0.085000f, 0.015000f, 0.160000f, 0.140000f)); // 27 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.450f), Vector4f(0.435000f, 0.365000f, 0.510000f, 0.490000f)); // 28 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.900f), Vector4f(0.885000f, 0.815000f, 0.960000f, 0.940000f)); // 29 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -1.000f), Vector4f(0.922500f, 0.852500f, 0.997500f, 0.977500f)); // 30 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.000f), Vector4f(0.082500f, 0.052500f, 0.010000f, 0.000000f)); // 31 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.100f), Vector4f(0.146250f, 0.116250f, 0.073750f, 0.063750f)); // 32 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.450f), Vector4f(0.496250f, 0.466250f, 0.423750f, 0.413750f)); // 33 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.900f), Vector4f(0.946250f, 0.916250f, 0.873750f, 0.863750f)); // 34 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -1.000f), Vector4f(1.000000f, 0.970000f, 0.927500f, 0.917500f)); // 35 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.500000f, 0.500000f)); // 36 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.100f), Vector4f(0.000000f, 0.000000f, 0.500000f, 0.500000f)); // 37 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.450f), Vector4f(0.200000f, 0.200000f, 0.700000f, 0.700000f)); // 38 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.900f), Vector4f(0.500000f, 0.500000f, 1.000000f, 1.000000f)); // 39 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -1.000f), Vector4f(0.500000f, 0.500000f, 1.000000f, 1.000000f)); // 40 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.500000f, 0.500000f, 0.000000f)); // 41 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.100f), Vector4f(0.000000f, 0.500000f, 0.500000f, 0.000000f)); // 42 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.450f), Vector4f(0.200000f, 0.700000f, 0.700000f, 0.200000f)); // 43 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.900f), Vector4f(0.500000f, 1.000000f, 1.000000f, 0.500000f)); // 44 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -1.000f), Vector4f(0.500000f, 1.000000f, 1.000000f, 0.500000f)); // 45 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 46 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.100f), Vector4f(0.200000f, 0.000000f, 0.200000f, 0.000000f)); // 47 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.450f), Vector4f(0.700000f, 0.200000f, 0.700000f, 0.200000f)); // 48 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.900f), Vector4f(1.000000f, 0.500000f, 1.000000f, 0.500000f)); // 49 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -1.000f), Vector4f(1.000000f, 0.700000f, 1.000000f, 0.700000f)); // 50 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.000f), Vector4f(0.200000f, 0.000000f, 0.200000f, 1.000000f)); // 51 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.100f), Vector4f(0.200000f, 0.000000f, 0.200000f, 1.000000f)); // 52 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.450f), Vector4f(0.200000f, 0.000000f, 0.200000f, 1.000000f)); // 53 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.900f), Vector4f(0.200000f, 0.000000f, 0.200000f, 1.000000f)); // 54 + EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -1.000f), Vector4f(0.200000f, 0.000000f, 0.200000f, 1.000000f)); // 55 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.000f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 56 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.100f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 57 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.450f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 58 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.900f), Vector4f(0.950000f, 0.600000f, 0.000000f, 0.550000f)); // 59 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -1.000f), Vector4f(0.950000f, 0.600000f, 0.000000f, 0.550000f)); // 60 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.000f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 61 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.100f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 62 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.450f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 63 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.900f), Vector4f(1.000000f, 0.550000f, 0.050000f, 0.500000f)); // 64 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -1.000f), Vector4f(1.000000f, 0.550000f, 0.050000f, 0.500000f)); // 65 +} + +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, PreviousMixingTestsAirmodeRPY) +{ + setAirmode(2); // Roll, pitch and yaw airmode + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 1 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.100f), Vector4f(0.100000f, 0.100000f, 0.100000f, 0.100000f)); // 2 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.450f), Vector4f(0.450000f, 0.450000f, 0.450000f, 0.450000f)); // 3 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -0.900f), Vector4f(0.900000f, 0.900000f, 0.900000f, 0.900000f)); // 4 + EXPECT_EQ(allocate(0.000f, 0.000f, 0.000f, -1.000f), Vector4f(1.000000f, 1.000000f, 1.000000f, 1.000000f)); // 5 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.000f), Vector4f(0.025000f, 0.025000f, 0.000000f, 0.000000f)); // 6 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.100f), Vector4f(0.112500f, 0.112500f, 0.087500f, 0.087500f)); // 7 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.450f), Vector4f(0.462500f, 0.462500f, 0.437500f, 0.437500f)); // 8 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -0.900f), Vector4f(0.912500f, 0.912500f, 0.887500f, 0.887500f)); // 9 + EXPECT_EQ(allocate(-0.050f, 0.000f, 0.000f, -1.000f), Vector4f(1.000000f, 1.000000f, 0.975000f, 0.975000f)); // 10 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.000f), Vector4f(0.000000f, 0.025000f, 0.050000f, 0.025000f)); // 11 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.100f), Vector4f(0.075000f, 0.100000f, 0.125000f, 0.100000f)); // 12 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.450f), Vector4f(0.425000f, 0.450000f, 0.475000f, 0.450000f)); // 13 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.900f), Vector4f(0.875000f, 0.900000f, 0.925000f, 0.900000f)); // 14 + EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -1.000f), Vector4f(0.950000f, 0.975000f, 1.000000f, 0.975000f)); // 15 + // Note: This case is also not correctly allocated with only roll and pitch airmode. Now RPY airmode is consistent. + // The case is where roll and pitch saturate and get unsaturated using thrust but yaw in the next step would also + // unsaturate a part and hence the solution is not optimal. + // EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.000f), Vector4f(0.012500f, 0.000000f, 0.012500f, 0.050000f)); // 16 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.100f), Vector4f(0.093750f, 0.081250f, 0.093750f, 0.131250f)); // 17 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.450f), Vector4f(0.443750f, 0.431250f, 0.443750f, 0.481250f)); // 18 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.900f), Vector4f(0.893750f, 0.881250f, 0.893750f, 0.931250f)); // 19 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -1.000f), Vector4f(0.962500f, 0.950000f, 0.962500f, 1.000000f)); // 20 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.000f), Vector4f(0.100000f, 0.012500f, 0.000000f, 0.112500f)); // 21 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.100f), Vector4f(0.143750f, 0.056250f, 0.043750f, 0.156250f)); // 22 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.450f), Vector4f(0.493750f, 0.406250f, 0.393750f, 0.506250f)); // 23 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -0.900f), Vector4f(0.943750f, 0.856250f, 0.843750f, 0.956250f)); // 24 + EXPECT_EQ(allocate(0.000f, 0.200f, -0.025f, -1.000f), Vector4f(0.987500f, 0.900000f, 0.887500f, 1.000000f)); // 25 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.000f), Vector4f(0.070000f, 0.000000f, 0.145000f, 0.125000f)); // 26 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.100f), Vector4f(0.085000f, 0.015000f, 0.160000f, 0.140000f)); // 27 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.450f), Vector4f(0.435000f, 0.365000f, 0.510000f, 0.490000f)); // 28 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.900f), Vector4f(0.885000f, 0.815000f, 0.960000f, 0.940000f)); // 29 + // Same as 16 but with full thrust instead + // EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -1.000f), Vector4f(0.925000f, 0.855000f, 1.000000f, 0.980000f)); // 30 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.000f), Vector4f(0.082500f, 0.052500f, 0.010000f, 0.000000f)); // 31 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.100f), Vector4f(0.146250f, 0.116250f, 0.073750f, 0.063750f)); // 32 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.450f), Vector4f(0.496250f, 0.466250f, 0.423750f, 0.413750f)); // 33 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.900f), Vector4f(0.946250f, 0.916250f, 0.873750f, 0.863750f)); // 34 + EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -1.000f), Vector4f(1.000000f, 0.970000f, 0.927500f, 0.917500f)); // 35 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.500000f, 0.500000f)); // 36 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.100f), Vector4f(0.000000f, 0.000000f, 0.500000f, 0.500000f)); // 37 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.450f), Vector4f(0.200000f, 0.200000f, 0.700000f, 0.700000f)); // 38 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -0.900f), Vector4f(0.500000f, 0.500000f, 1.000000f, 1.000000f)); // 39 + EXPECT_EQ(allocate(1.000f, 0.000f, 0.000f, -1.000f), Vector4f(0.500000f, 0.500000f, 1.000000f, 1.000000f)); // 40 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.000f), Vector4f(0.000000f, 0.500000f, 0.500000f, 0.000000f)); // 41 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.100f), Vector4f(0.000000f, 0.500000f, 0.500000f, 0.000000f)); // 42 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.450f), Vector4f(0.200000f, 0.700000f, 0.700000f, 0.200000f)); // 43 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -0.900f), Vector4f(0.500000f, 1.000000f, 1.000000f, 0.500000f)); // 44 + EXPECT_EQ(allocate(0.000f, -1.000f, 0.000f, -1.000f), Vector4f(0.500000f, 1.000000f, 1.000000f, 0.500000f)); // 45 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.000f), Vector4f(0.500000f, 0.000000f, 0.500000f, 0.000000f)); // 46 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.100f), Vector4f(0.500000f, 0.000000f, 0.500000f, 0.000000f)); // 47 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.450f), Vector4f(0.700000f, 0.200000f, 0.700000f, 0.200000f)); // 48 + EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -0.900f), Vector4f(1.000000f, 0.500000f, 1.000000f, 0.500000f)); // 49 + // Yaw unsaturation reducing thrust is limited to 15% consistent with RP airmode + // EXPECT_EQ(allocate(0.000f, 0.000f, 1.000f, -1.000f), Vector4f(1.000000f, 0.500000f, 1.000000f, 0.500000f)); // 50 + // EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 1.000000f)); // 51 + // EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.100f), Vector4f(0.000000f, 0.000000f, 0.000000f, 1.000000f)); // 52 + // EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.450f), Vector4f(0.000000f, 0.000000f, 0.000000f, 1.000000f)); // 53 + // EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -0.900f), Vector4f(0.000000f, 0.000000f, 0.000000f, 1.000000f)); // 54 + // EXPECT_EQ(allocate(1.000f, 1.000f, -1.000f, -1.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 1.000000f)); // 55 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.000f), Vector4f(0.950000f, 0.950000f, 0.000000f, 0.900000f)); // 56 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.100f), Vector4f(0.950000f, 0.950000f, 0.000000f, 0.900000f)); // 57 + EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.450f), Vector4f(0.950000f, 0.950000f, 0.000000f, 0.900000f)); // 58 + // Same as 16 but with full thrust and opposite roll, yaw direction instead + // EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -0.900f), Vector4f(1.000000f, 1.000000f, 0.050000f, 0.950000f)); // 59 + // EXPECT_EQ(allocate(-1.000f, 0.900f, -0.900f, -1.000f), Vector4f(1.000000f, 1.000000f, 0.050000f, 0.950000f)); // 60 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.000f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 61 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.100f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 62 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.450f), Vector4f(0.950000f, 0.500000f, 0.000000f, 0.450000f)); // 63 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -0.900f), Vector4f(1.000000f, 0.550000f, 0.050000f, 0.500000f)); // 64 + EXPECT_EQ(allocate(-1.000f, 0.900f, 0.000f, -1.000f), Vector4f(1.000000f, 0.550000f, 0.050000f, 0.500000f)); // 65 +} From 75f7f19b0a6654a4c2728f55776b91d6ce245232 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Apr 2025 17:35:17 +0200 Subject: [PATCH 12/13] Draft for limited airmode --- ...ontrolAllocationSequentialDesaturation.cpp | 141 +++--------------- ...ontrolAllocationSequentialDesaturation.hpp | 45 +----- 2 files changed, 24 insertions(+), 162 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp index f15b66819e05..5ad88e46ffd1 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp @@ -36,6 +36,7 @@ * * @author Roman Bapst * @author Beat Küng + * @author Matthias Grob */ #include "ControlAllocationSequentialDesaturation.hpp" @@ -64,7 +65,7 @@ ControlAllocationSequentialDesaturation::allocate() } } -void ControlAllocationSequentialDesaturation::desaturateActuators( +float ControlAllocationSequentialDesaturation::desaturateActuators( ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, float increase_limit, float decrease_limit) @@ -82,6 +83,8 @@ void ControlAllocationSequentialDesaturation::desaturateActuators( for (int i = 0; i < _num_actuators; i++) { actuator_sp(i) += gain * desaturation_vector(i); } + + return gain; } float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, @@ -124,6 +127,8 @@ void ControlAllocationSequentialDesaturation::mix(float roll_pitch_limit, float ActuatorVector yaw; ActuatorVector thrust_z; + ActuatorVector mixed_with_yaw; + for (int i = 0; i < _num_actuators; i++) { _actuator_sp(i) = _actuator_trim(i) + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + @@ -132,148 +137,44 @@ void ControlAllocationSequentialDesaturation::mix(float roll_pitch_limit, float _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + mixed_with_yaw(i) = _actuator_sp(i) + _mix(i, + ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); roll(i) = _mix(i, ControlAxis::ROLL); pitch(i) = _mix(i, ControlAxis::PITCH); yaw(i) = _mix(i, ControlAxis::YAW); thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); } - const bool debug = false; + const bool debug = true; if (debug) { printf("Roll Pitch Thrust Mixed\n"); _actuator_sp.print(); }; - desaturateActuators(_actuator_sp, thrust_z, roll_pitch_limit); - - if (debug) { printf("Thrust Roll Pitch\n"); _actuator_sp.print(); }; - - desaturateActuators(_actuator_sp, roll); - - desaturateActuators(_actuator_sp, pitch); - - if (debug) { printf("Roll Pitch\n"); _actuator_sp.print(); }; - - for (int i = 0; i < _num_actuators; i++) { - _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); - } - - if (debug) { printf("Yaw mixed\n"); _actuator_sp.print(); }; - - desaturateActuators(_actuator_sp, thrust_z, yaw_limit, 0.15f); - - if (debug) { printf("Thrust Yaw\n"); _actuator_sp.print(); }; - - desaturateActuators(_actuator_sp, yaw); - - if (debug) { printf("Yaw\n"); _actuator_sp.print(); }; -} - -void -ControlAllocationSequentialDesaturation::mixAirmodeRP() -{ - // Airmode for roll and pitch, but not yaw - - // Mix without yaw - ActuatorVector thrust_z; - - for (int i = 0; i < _num_actuators; i++) { - _actuator_sp(i) = _actuator_trim(i) + - _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + - _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + - _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + - _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + - _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); - } - - desaturateActuators(_actuator_sp, thrust_z); - - // Mix yaw independently - mixYaw(); -} - -void -ControlAllocationSequentialDesaturation::mixAirmodeRPY() -{ - // Airmode for roll, pitch and yaw - - // Do full mixing - ActuatorVector thrust_z; - ActuatorVector yaw; - - for (int i = 0; i < _num_actuators; i++) { - _actuator_sp(i) = _actuator_trim(i) + - _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + - _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + - _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + - _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + - _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + - _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); - yaw(i) = _mix(i, ControlAxis::YAW); - } + if (debug) { printf("Roll Pitch Yaw Thrust Mixed\n"); mixed_with_yaw.print(); }; - desaturateActuators(_actuator_sp, thrust_z); + float rp = desaturateActuators(_actuator_sp, thrust_z, roll_pitch_limit); - // Unsaturate yaw (in case upper and lower bounds are exceeded) - // to prioritize roll/pitch over yaw. - desaturateActuators(_actuator_sp, yaw); -} + float rpy = desaturateActuators(mixed_with_yaw, thrust_z, roll_pitch_limit); -void -ControlAllocationSequentialDesaturation::mixAirmodeDisabled() -{ - // Airmode disabled: never allow to increase the thrust to unsaturate a motor + printf("%.3f %.3f\n", (double)rp, (double)rpy); - // Mix without yaw - ActuatorVector thrust_z; - ActuatorVector roll; - ActuatorVector pitch; + const bool use_rpy = fabsf(rpy) < fabsf(rp); - for (int i = 0; i < _num_actuators; i++) { - _actuator_sp(i) = _actuator_trim(i) + - _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + - _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + - _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + - _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + - _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); - roll(i) = _mix(i, ControlAxis::ROLL); - pitch(i) = _mix(i, ControlAxis::PITCH); + if (use_rpy) { + _actuator_sp = mixed_with_yaw; } - // only reduce thrust - desaturateActuators(_actuator_sp, thrust_z, 0.f); - - // Reduce roll/pitch acceleration if needed to unsaturate desaturateActuators(_actuator_sp, roll); desaturateActuators(_actuator_sp, pitch); - // Mix yaw independently - mixYaw(); -} - -void -ControlAllocationSequentialDesaturation::mixYaw(float yaw_limit) -{ - // Add yaw to outputs - ActuatorVector yaw; - ActuatorVector thrust_z; + if (!use_rpy) { + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + } - for (int i = 0; i < _num_actuators; i++) { - _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); - yaw(i) = _mix(i, ControlAxis::YAW); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + desaturateActuators(_actuator_sp, thrust_z, yaw_limit, 0.15f); } - // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), - // and allow some yaw response at maximum thrust - ActuatorVector max_prev = _actuator_max; - _actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN; desaturateActuators(_actuator_sp, yaw); - _actuator_max = max_prev; - - // reduce thrust only - desaturateActuators(_actuator_sp, thrust_z, 0.f); } void diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp index 60a987aba48a..59214ffd4e3f 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp @@ -53,9 +53,7 @@ class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInv ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {} virtual ~ControlAllocationSequentialDesaturation() = default; - void allocate() override; - void updateParameters() override; // This is the minimum actuator yaw granted when the controller is saturated. @@ -78,8 +76,8 @@ class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInv * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale * @param increase_limit if value below 1, only allow to increase (add) a fraction of desaturation_vector to the specified amount */ - void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, - float increase_limit = 1.f, float decrease_limit = 1.f); + float desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, + float increase_limit = 1.f, float decrease_limit = 1.f); /** * Computes the gain by which desaturation_vector has to be multiplied @@ -93,44 +91,7 @@ class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInv * Mix roll, pitch, yaw, thrust and set the actuator setpoint. * */ - void mix(float roll_pitch_limit, float yaw); - - /** - * Mix roll, pitch, yaw, thrust and set the actuator setpoint. - * - * 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. - */ - void mixAirmodeRP(); - - /** - * Mix roll, pitch, yaw, thrust and set the actuator setpoint. - * - * 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. - */ - void mixAirmodeRPY(); - - /** - * Mix roll, pitch, yaw, thrust and set the actuator setpoint. - * - * 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 mixYaw() for the exact yaw behavior. - */ - void mixAirmodeDisabled(); - - /** - * Mix yaw by updating the actuator setpoint (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. - */ - void mixYaw(float yaw_limit = 0.f); + void mix(float roll_pitch_limit, float yaw_limit); DEFINE_PARAMETERS( (ParamInt) _param_mc_airmode ///< air-mode From 9fa204095bb71571602f6bf2deb2a3e4ecab216e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 25 Apr 2025 17:41:31 +0200 Subject: [PATCH 13/13] Random tests I was last checking --- .../ControlAllocationSequentialDesaturationTest.cpp | 9 ++++++--- src/lib/matrix/test/MatrixDualTest.cpp | 1 + 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index b2093e8e2c15..d973a10f8693 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -297,7 +297,8 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, PreviousMixingTestsNoAi EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.100f), Vector4f(0.085000f, 0.015000f, 0.160000f, 0.140000f)); // 27 EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.450f), Vector4f(0.435000f, 0.365000f, 0.510000f, 0.490000f)); // 28 EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.900f), Vector4f(0.885000f, 0.815000f, 0.960000f, 0.940000f)); // 29 - EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -1.000f), Vector4f(0.922500f, 0.852500f, 0.997500f, 0.977500f)); // 30 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -1.000f), Vector4f(0.922500f, 0.852500f, 0.997500f, + 0.977500f) + 0.0025f); // 30 EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.000f), Vector4f(0.000000f, 0.000000f, 0.000000f, 0.000000f)); // 31 EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.100f), Vector4f(0.146250f, 0.116250f, 0.073750f, 0.063750f)); // 32 EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.450f), Vector4f(0.496250f, 0.466250f, 0.423750f, 0.413750f)); // 33 @@ -353,7 +354,8 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, PreviousMixingTestsAirm EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.450f), Vector4f(0.425000f, 0.450000f, 0.475000f, 0.450000f)); // 13 EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -0.900f), Vector4f(0.875000f, 0.900000f, 0.925000f, 0.900000f)); // 14 EXPECT_EQ(allocate(0.050f, -0.050f, 0.000f, -1.000f), Vector4f(0.950000f, 0.975000f, 1.000000f, 0.975000f)); // 15 - EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.000f), Vector4f(0.018750f, 0.006250f, 0.018750f, 0.056250f)); // 16 + EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.000f), Vector4f(0.018750f, 0.006250f, 0.018750f, + 0.056250f) - 0.00625f); // 16 EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.100f), Vector4f(0.093750f, 0.081250f, 0.093750f, 0.131250f)); // 17 EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.450f), Vector4f(0.443750f, 0.431250f, 0.443750f, 0.481250f)); // 18 EXPECT_EQ(allocate(0.050f, 0.050f, -0.025f, -0.900f), Vector4f(0.893750f, 0.881250f, 0.893750f, 0.931250f)); // 19 @@ -367,7 +369,8 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, PreviousMixingTestsAirm EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.100f), Vector4f(0.085000f, 0.015000f, 0.160000f, 0.140000f)); // 27 EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.450f), Vector4f(0.435000f, 0.365000f, 0.510000f, 0.490000f)); // 28 EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -0.900f), Vector4f(0.885000f, 0.815000f, 0.960000f, 0.940000f)); // 29 - EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -1.000f), Vector4f(0.922500f, 0.852500f, 0.997500f, 0.977500f)); // 30 + EXPECT_EQ(allocate(0.200f, 0.050f, 0.090f, -1.000f), Vector4f(0.922500f, 0.852500f, 0.997500f, + 0.977500f) + 0.0025f); // 30 EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.000f), Vector4f(0.082500f, 0.052500f, 0.010000f, 0.000000f)); // 31 EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.100f), Vector4f(0.146250f, 0.116250f, 0.073750f, 0.063750f)); // 32 EXPECT_EQ(allocate(-0.125f, 0.020f, 0.040f, -0.450f), Vector4f(0.496250f, 0.466250f, 0.423750f, 0.413750f)); // 33 diff --git a/src/lib/matrix/test/MatrixDualTest.cpp b/src/lib/matrix/test/MatrixDualTest.cpp index d3b404899e56..90d028db9545 100644 --- a/src/lib/matrix/test/MatrixDualTest.cpp +++ b/src/lib/matrix/test/MatrixDualTest.cpp @@ -253,6 +253,7 @@ TEST(MatrixDualTest, Dual) // set our starting point, requesting partial derivatives of x and y in column 0 and 1 Vector3 dualPoint(D(0.5f, 0), D(-0.8f, 1), D(2.f)); + EXPECT_EQ(D(0.5f, 0), D(-0.8f, 1)); Dual dualResult = testFunction(dualPoint);