diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index a80ce0113d..4acf350caa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -57,6 +57,12 @@ enum class ActuatorType { COUNT }; +enum class EffectivenessUpdateReason { + NO_EXTERNAL_UPDATE = 0, + CONFIGURATION_UPDATE = 1, + MOTOR_ACTIVATION_UPDATE = 2, +}; + class ActuatorEffectiveness { @@ -147,7 +153,7 @@ public: * * @return true if updated and matrix is set */ - virtual bool getEffectivenessMatrix(Configuration &configuration, bool force) = 0; + virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;} /** * Get the current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 04acb7dfee..95bf818320 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -124,12 +124,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams() } } -bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration &configuration, bool force) +bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configuration) { - if (!force) { - return false; - } - for (int i = 0; i < _count; i++) { int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{}); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 7fa0c98d85..4bc34bab9b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -69,7 +69,7 @@ public: ActuatorEffectivenessControlSurfaces(ModuleParams *parent); virtual ~ActuatorEffectivenessControlSurfaces() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool addActuators(Configuration &configuration); const char *name() const override { return "Control Surfaces"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index 4412989f4a..083ad05fe0 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -41,19 +41,20 @@ ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent) } bool -ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!force) { + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } // motors _motors.enableYawControl(false); - _motors.getEffectivenessMatrix(configuration, true); + const bool motors_added_successfully = _motors.addActuators(configuration); // Torque - _torque.getEffectivenessMatrix(configuration, true); + const bool torque_added_successfully = _torque.addActuators(configuration); - return true; + return (motors_added_successfully && torque_added_successfully); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index 23dc9ae929..e06278f08f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -43,7 +43,7 @@ public: ActuatorEffectivenessCustom(ModuleParams *parent); virtual ~ActuatorEffectivenessCustom() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; const char *name() const override { return "Custom"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 1f7872df41..9bc52bd5d0 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -43,21 +43,22 @@ ActuatorEffectivenessFixedWing::ActuatorEffectivenessFixedWing(ModuleParams *par } bool -ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!force) { + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } // Motors _rotors.enableYawControl(false); - _rotors.getEffectivenessMatrix(configuration, true); + const bool rotors_added_successfully = _rotors.addActuators(configuration); // Control Surfaces _first_control_surface_idx = configuration.num_actuators_matrix[0]; - _control_surfaces.getEffectivenessMatrix(configuration, true); + const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration); - return true; + return (rotors_added_successfully && surfaces_added_successfully); } void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index e535952fd8..35583eaf8b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -45,7 +45,7 @@ public: ActuatorEffectivenessFixedWing(ModuleParams *parent); virtual ~ActuatorEffectivenessFixedWing() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; const char *name() const override { return "Fixed Wing"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index a6cd62d49d..33a02269f2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -43,20 +43,21 @@ ActuatorEffectivenessMCTilt::ActuatorEffectivenessMCTilt(ModuleParams *parent) } bool -ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!force) { + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } // MC motors _mc_rotors.enableYawControl(!_tilts.hasYawControl()); - _mc_rotors.getEffectivenessMatrix(configuration, true); + const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); // Tilts int first_tilt_idx = configuration.num_actuators_matrix[0]; _tilts.updateTorqueSign(_mc_rotors.geometry()); - _tilts.getEffectivenessMatrix(configuration, true); + const bool tilts_added_successfully = _tilts.addActuators(configuration); // Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle). // Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated @@ -72,7 +73,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration } } - return true; + return (rotors_added_successfully && tilts_added_successfully); } void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector &control_sp, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 4bdae41efe..db9f790c51 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -43,7 +43,7 @@ public: ActuatorEffectivenessMCTilt(ModuleParams *parent); virtual ~ActuatorEffectivenessMCTilt() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp new file mode 100644 index 0000000000..41ed5cb2c5 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "ActuatorEffectivenessMultirotor.hpp" + +using namespace matrix; + +ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent) + : ModuleParams(parent), + _mc_rotors(this) +{ +} + +bool +ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); + + return rotors_added_successfully; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp new file mode 100644 index 0000000000..e0a355edd2 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" + +class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessMultirotor(ModuleParams *parent); + virtual ~ActuatorEffectivenessMultirotor() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + } + + const char *name() const override { return "Multirotor"; } + +protected: + ActuatorEffectivenessRotors _mc_rotors; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 8c8caa1701..e6ef1a1773 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -136,24 +136,18 @@ void ActuatorEffectivenessRotors::updateParams() } bool -ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessRotors::addActuators(Configuration &configuration) { - if (_updated || force) { - _updated = false; - - if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { - PX4_ERR("Wrong actuator ordering: servos need to be after motors"); - return false; - } - - int num_actuators = computeEffectivenessMatrix(_geometry, - configuration.effectiveness_matrices[configuration.selected_matrix], - configuration.num_actuators_matrix[configuration.selected_matrix]); - configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); - return true; + if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { + PX4_ERR("Wrong actuator ordering: servos need to be after motors"); + return false; } - return false; + int num_actuators = computeEffectivenessMatrix(_geometry, + configuration.effectiveness_matrices[configuration.selected_matrix], + configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + return true; } int diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 0625d74628..0824c4fe6b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -93,7 +93,7 @@ public: static int computeEffectivenessMatrix(const Geometry &geometry, EffectivenessMatrix &effectiveness, int actuator_start_index = 0); - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool addActuators(Configuration &configuration); const char *name() const override { return "Multirotor"; } @@ -117,8 +117,6 @@ public: private: void updateParams() override; - - bool _updated{true}; const AxisConfiguration _axis_config; const bool _tilt_support; ///< if true, tilt servo assignment params are loaded diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 298e09c54b..37b3acb5e0 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -37,9 +37,10 @@ using namespace matrix; bool -ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!force) { + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 6c2003e1cb..c59ffc8741 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -41,7 +41,7 @@ public: ActuatorEffectivenessRoverAckermann() = default; virtual ~ActuatorEffectivenessRoverAckermann() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; const char *name() const override { return "Rover (Ackermann)"; } private: diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp index 4048e779aa..e9bc32477e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp @@ -37,9 +37,10 @@ using namespace matrix; bool -ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!force) { + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp index 4d77e455f6..6dde6b2bbd 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp @@ -41,7 +41,7 @@ public: ActuatorEffectivenessRoverDifferential() = default; virtual ~ActuatorEffectivenessRoverDifferential() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; const char *name() const override { return "Rover (Differential)"; } private: diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 46ed2c12fe..0bce8d5cb0 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -42,15 +42,16 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParam } bool -ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!force) { + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } // MC motors configuration.selected_matrix = 0; - _mc_rotors.getEffectivenessMatrix(configuration, true); + const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); _mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1; // Pusher/Puller @@ -63,9 +64,9 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu // Control Surfaces configuration.selected_matrix = 1; _first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; - _control_surfaces.getEffectivenessMatrix(configuration, true); + const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration); - return true; + return (mc_rotors_added_successfully && surfaces_added_successfully); } void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 50b84a9083..4051791a9a 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -53,7 +53,7 @@ public: ActuatorEffectivenessStandardVTOL(ModuleParams *parent); virtual ~ActuatorEffectivenessStandardVTOL() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; const char *name() const override { return "Standard VTOL"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 94150ec4a1..2d37eb82ab 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -47,24 +47,24 @@ ActuatorEffectivenessTailsitterVTOL::ActuatorEffectivenessTailsitterVTOL(ModuleP setFlightPhase(FlightPhase::HOVER_FLIGHT); } bool -ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!force) { + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } // MC motors configuration.selected_matrix = 0; _mc_rotors.enableYawControl(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors - - _mc_rotors.getEffectivenessMatrix(configuration, true); + const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); // Control Surfaces configuration.selected_matrix = 1; _first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; - _control_surfaces.getEffectivenessMatrix(configuration, true); + const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration); - return true; + return (mc_rotors_added_successfully && surfaces_added_successfully); } void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 58c01fcfd4..17b881dca9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -52,7 +52,7 @@ public: ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent); virtual ~ActuatorEffectivenessTailsitterVTOL() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; int numMatrices() const override { return 2; } @@ -75,7 +75,6 @@ public: uint32_t getStoppedMotors() const override { return _stopped_motors; } protected: - bool _updated{true}; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 0ae567a54e..3bba746965 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -51,9 +51,10 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModulePar setFlightPhase(FlightPhase::HOVER_FLIGHT); } bool -ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, bool force) +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) { - if (!_updated && !force) { + if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } @@ -62,30 +63,31 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config _mc_rotors.enableYawControl(!_tilts.hasYawControl()); // Update matrix with tilts in vertical position when update is triggered by a manual - // configuration (parameter) change (=force update). This is to make sure the normalization - // scales are tilt-invariant. Note: force update only possible when disarm. - const float tilt_control_applied = force ? -1.f : _last_tilt_control; + // configuration (parameter) change. This is to make sure the normalization + // scales are tilt-invariant. Note: configuration updates are only possible when disarmed. + const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f : + _last_tilt_control; _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied) << configuration.num_actuators[(int)ActuatorType::MOTORS]; - _mc_rotors.getEffectivenessMatrix(configuration, true); + const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); // Control Surfaces configuration.selected_matrix = 1; _first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; - _control_surfaces.getEffectivenessMatrix(configuration, true); + const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration); // Tilts configuration.selected_matrix = 0; _first_tilt_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; _tilts.updateTorqueSign(_mc_rotors.geometry(), true /* disable pitch to avoid configuration errors */); - _tilts.getEffectivenessMatrix(configuration, true); + const bool tilts_added_successfully = _tilts.addActuators(configuration); - // If it was a forced update (thus coming from a config change), then make sure to update matrix in + // If it was an update coming from a config change, then make sure to update matrix in // the next iteration again with the correct tilt (but without updating the normalization scale). - _updated = force; + _combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE); - return true; + return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully); } void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, @@ -114,7 +116,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector 0.01f) { - _updated = true; + _combined_tilt_updated = true; _last_tilt_control = control_tilt; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 705c596acb..00d5c2ffda 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -55,7 +55,7 @@ public: ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent); virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; int numMatrices() const override { return 2; } @@ -81,7 +81,7 @@ public: uint32_t getStoppedMotors() const override { return _stopped_motors; } protected: - bool _updated{true}; + bool _combined_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; ActuatorEffectivenessTilts _tilts; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp index 4cc481a69e..3cce6b24fb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp @@ -84,12 +84,8 @@ void ActuatorEffectivenessTilts::updateParams() } } -bool ActuatorEffectivenessTilts::getEffectivenessMatrix(Configuration &configuration, bool force) +bool ActuatorEffectivenessTilts::addActuators(Configuration &configuration) { - if (!force) { - return false; - } - for (int i = 0; i < _count; i++) { configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{}); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index 9cddfc2326..a6f1733c10 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -67,7 +67,7 @@ public: ActuatorEffectivenessTilts(ModuleParams *parent); virtual ~ActuatorEffectivenessTilts() = default; - bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + bool addActuators(Configuration &configuration); const char *name() const override { return "Tilts"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 089bc9d919..2dbceae013 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -42,6 +42,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessFixedWing.hpp ActuatorEffectivenessMCTilt.cpp ActuatorEffectivenessMCTilt.hpp + ActuatorEffectivenessMultirotor.cpp + ActuatorEffectivenessMultirotor.hpp ActuatorEffectivenessTilts.cpp ActuatorEffectivenessTilts.hpp ActuatorEffectivenessRotors.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 9a570d6e52..ab3d339f87 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -124,7 +124,7 @@ ControlAllocator::parameters_updated() _control_allocation[i]->updateParameters(); } - update_effectiveness_matrix_if_needed(true); + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE); } void @@ -208,7 +208,7 @@ ControlAllocator::update_effectiveness_source() switch (source) { case EffectivenessSource::NONE: case EffectivenessSource::MULTIROTOR: - tmp = new ActuatorEffectivenessRotors(this); + tmp = new ActuatorEffectivenessMultirotor(this); break; case EffectivenessSource::STANDARD_VTOL: @@ -362,7 +362,7 @@ ControlAllocator::Run() if (do_update) { _last_run = now; - update_effectiveness_matrix_if_needed(); + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE); // Set control setpoint vector(s) matrix::Vector c[ActuatorEffectiveness::MAX_NUM_MATRICES]; @@ -414,15 +414,16 @@ ControlAllocator::Run() } void -ControlAllocator::update_effectiveness_matrix_if_needed(bool force) +ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason) { ActuatorEffectiveness::Configuration config{}; - if (!force && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates + if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE + && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates return; } - if (_actuator_effectiveness->getEffectivenessMatrix(config, force)) { + if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) { _last_effectiveness_update = hrt_absolute_time(); memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes, @@ -496,7 +497,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force) // Assign control effectiveness matrix int total_num_actuators = config.num_actuators_matrix[i]; _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], - config.linearization_point[i], total_num_actuators, force); + config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE); } trims.timestamp = hrt_absolute_time(); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 5da515f6af..f1ea6888dc 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -42,7 +42,7 @@ #pragma once #include -#include +#include #include #include #include @@ -125,7 +125,7 @@ private: void update_allocation_method(bool force); bool update_effectiveness_source(); - void update_effectiveness_matrix_if_needed(bool force = false); + void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason); void publish_control_allocator_status();