diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp new file mode 100644 index 0000000000..ac51b80ae0 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 "ActuatorEffectivenessCustom.hpp" + +ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent): + ModuleParams(parent) +{ +} + +bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix &matrix, + bool force) +{ + if (_updated || force) { + _updated = false; + + int num_actuators = 0; + + for (int n = 0; n < NUM_ACTUATORS; n++) { + // CA_ACTn_TRQ_R + // CA_ACTn_TRQ_P + // CA_ACTn_TRQ_Y + char torque_str[3][17]; + sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n); + sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n); + sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n); + + // CA_ACTn_THR_X + // CA_ACTn_THR_Y + // CA_ACTn_THR_Z + char thrust_str[3][17]; + sprintf(thrust_str[0], "CA_ACT%u_THR_X", n); + sprintf(thrust_str[1], "CA_ACT%u_THR_Y", n); + sprintf(thrust_str[2], "CA_ACT%u_THR_Z", n); + + for (int i = 0; i < 3; i++) { + // CA_ACTn_TRQ_{R,P,Y} + param_get(param_find(torque_str[i]), &_matrix(n, i)); + + // CA_ACTn_THR_{X,Y,Z} + param_get(param_find(thrust_str[i]), &_matrix(n, i + 3)); + } + + if (_matrix.row(n).longerThan(0.f)) { + num_actuators++; + } + } + + _num_actuators = num_actuators; + matrix = _matrix; + return true; + } + + return false; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp new file mode 100644 index 0000000000..158ca4c00e --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 ActuatorEffectivenessCustom.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include + +using namespace time_literals; + +class ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessCustom(ModuleParams *parent); + virtual ~ActuatorEffectivenessCustom() = default; + + bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; + + int numActuators() const override { return _num_actuators; } +private: + + matrix::Matrix _matrix{}; + + int _num_actuators{0}; + + bool _updated{true}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt deleted file mode 100644 index 50eec6edcf..0000000000 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -############################################################################ -# -# Copyright (c) 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. -# -############################################################################ - -px4_add_library(ActuatorEffectiveness - ActuatorEffectiveness.hpp - ActuatorEffectivenessMultirotor.cpp - ActuatorEffectivenessMultirotor.hpp - ActuatorEffectivenessStandardVTOL.cpp - ActuatorEffectivenessStandardVTOL.hpp - ActuatorEffectivenessTiltrotorVTOL.cpp - ActuatorEffectivenessTiltrotorVTOL.hpp -) - -target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(ActuatorEffectiveness - PRIVATE - mathlib - ControlAllocation -) - -# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 06a0ff017a..28b9e046a3 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -31,22 +31,45 @@ # ############################################################################ -include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -add_subdirectory(ActuatorEffectiveness) -add_subdirectory(ControlAllocation) - px4_add_module( MODULE modules__control_allocator MAIN control_allocator COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} SRCS + ActuatorEffectiveness/ActuatorEffectiveness.hpp + ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp + ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp + ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp + ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp + ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp + ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp + ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp + + ControlAllocation/ControlAllocation.cpp + ControlAllocation/ControlAllocation.hpp + ControlAllocation/ControlAllocationPseudoInverse.cpp + ControlAllocation/ControlAllocationPseudoInverse.hpp + ControlAllocation/ControlAllocationSequentialDesaturation.cpp + ControlAllocation/ControlAllocationSequentialDesaturation.hpp + ControlAllocator.cpp ControlAllocator.hpp DEPENDS mathlib - ActuatorEffectiveness - ControlAllocation - mixer px4_work_queue + MODULE_CONFIG + module.yaml ) + +px4_add_library(ControlAllocation_testing + ControlAllocation/ControlAllocation.cpp + ControlAllocation/ControlAllocationPseudoInverse.cpp +) +target_link_libraries(ControlAllocation_testing PRIVATE mathlib) + +px4_add_unit_gtest(SRC ControlAllocation/ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation_testing) +# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt deleted file mode 100644 index ecf5f0d2e4..0000000000 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 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. -# -############################################################################ - -px4_add_library(ControlAllocation - ControlAllocation.cpp - ControlAllocation.hpp - ControlAllocationPseudoInverse.cpp - ControlAllocationPseudoInverse.hpp - ControlAllocationSequentialDesaturation.cpp - ControlAllocationSequentialDesaturation.hpp -) -target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(ControlAllocation PRIVATE mathlib) - -px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp index aba3cf8fd0..9d2b344ccb 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -40,7 +40,8 @@ */ #include -#include + +#include "ControlAllocationPseudoInverse.hpp" using namespace matrix; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index d2f6f91b14..d5abc4f6ec 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -212,6 +212,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessTiltrotorVTOL(); break; + case EffectivenessSource::CUSTOM: + tmp = new ActuatorEffectivenessCustom(this); + break; + default: PX4_ERR("Unknown airframe"); break; @@ -484,6 +488,10 @@ int ControlAllocator::print_status() case EffectivenessSource::TILTROTOR_VTOL: PX4_INFO("EffectivenessSource: Tiltrotor VTOL"); break; + + case EffectivenessSource::CUSTOM: + PX4_INFO("EffectivenessSource: Custom"); + break; } // Print current effectiveness matrix diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index f725d7930e..3faac9bfa1 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -41,14 +41,15 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include -#include -#include +#include +#include +#include #include #include @@ -123,10 +124,11 @@ private: ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations enum class EffectivenessSource { - NONE = -1, - MULTIROTOR = 0, - STANDARD_VTOL = 1, + NONE = -1, + MULTIROTOR = 0, + STANDARD_VTOL = 1, TILTROTOR_VTOL = 2, + CUSTOM = 3, }; EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index c1384d4414..4538142518 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -50,6 +50,7 @@ * @value 0 Multirotor * @value 1 Standard VTOL (WIP) * @value 2 Tiltrotor VTOL (WIP) + * @value 3 Custom (CA_ACTn* torque and thrust parameters) * @group Control Allocation */ PARAM_DEFINE_INT32(CA_AIRFRAME, 0); diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml new file mode 100644 index 0000000000..6e08a0aed7 --- /dev/null +++ b/src/modules/control_allocator/module.yaml @@ -0,0 +1,72 @@ +__max_num_config_instances: &max_num_config_instances 16 + +module_name: control_allocator + +parameters: + - group: Control Allocator + definitions: + CA_ACT${i}_TRQ_R: + description: + short: actuator ${i} roll + long: Actuator ${i} roll torque + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_TRQ_P: + description: + short: actuator ${i} pitch + long: Actuator ${i} pitch torque + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_TRQ_Y: + description: + short: actuator ${i} yaw + long: Actuator ${i} yaw torque + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_THR_X: + description: + short: actuator ${i} thrust x + long: Actuator ${i} thrust x + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_THR_Y: + description: + short: actuator ${i} thrust y + long: Actuator ${i} thrust y + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_THR_Z: + description: + short: actuator ${i} thrust y + long: Actuator ${i} thrust y + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0