diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp new file mode 100644 index 0000000000..e4c861a9d0 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessUUV.hpp" + +using namespace matrix; + +ActuatorEffectivenessUUV::ActuatorEffectivenessUUV(ModuleParams *parent) + : ModuleParams(parent), + _rotors(this) +{ +} + +bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + const bool rotors_added_successfully = _rotors.addActuators(configuration); + _motors_mask = _rotors.getMotors(); + + 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) +{ + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp new file mode 100644 index 0000000000..57e86f9436 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" + +class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessUUV(ModuleParams *parent); + virtual ~ActuatorEffectivenessUUV() = 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; + } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + const char *name() const override { return "UUV"; } + +protected: + ActuatorEffectivenessRotors _rotors; + + uint32_t _motors_mask{}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 31e338de82..18c0679d10 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -34,6 +34,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.cpp ActuatorEffectiveness.hpp + ActuatorEffectivenessUUV.cpp + ActuatorEffectivenessUUV.hpp ActuatorEffectivenessControlSurfaces.cpp ActuatorEffectivenessControlSurfaces.hpp ActuatorEffectivenessCustom.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2b419ab782..0a3f24df39 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -247,7 +247,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::MOTORS_6DOF: // just a different UI from MULTIROTOR - tmp = new ActuatorEffectivenessRotors(this); + tmp = new ActuatorEffectivenessUUV(this); break; case EffectivenessSource::MULTIROTOR_WITH_TILT: diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 0281aa925e..e445df72a5 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include