From 8726698afd128091b0e1f71efda76b7909b70221 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 13 Jul 2022 20:57:15 -0400 Subject: [PATCH] WIP: VTOL standard unfied control effectiveness --- ...tuatorEffectivenessStandardVTOLUnified.cpp | 121 ++++++++++++++++++ ...tuatorEffectivenessStandardVTOLUnified.hpp | 92 +++++++++++++ .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../control_allocator/ControlAllocator.cpp | 4 + .../control_allocator/ControlAllocator.hpp | 2 + src/modules/control_allocator/module.yaml | 1 + 6 files changed, 222 insertions(+) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp new file mode 100644 index 0000000000..4824bb444b --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 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 "ActuatorEffectivenessStandardVTOLUnified.hpp" +#include + +using namespace matrix; + +ActuatorEffectivenessStandardVTOLUnified::ActuatorEffectivenessStandardVTOLUnified(ModuleParams *parent) : + ModuleParams(parent), + _rotors(this), + _control_surfaces(this) +{ +} + +bool ActuatorEffectivenessStandardVTOLUnified::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // TODO: combine _rotors and _control_surfaces into one matrix + + // TODO: airspeed + + + + + // _rotors.addActuators(configuration); + // + // int num_actuators = _rotors.computeEffectivenessMatrix(_rotors.geometry(), configuration.effectiveness_matrices[0], + // configuration.num_actuators_matrix[0]); + // configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + + + + // Motors + configuration.selected_matrix = 0; + + _rotors.enablePropellerTorqueNonUpwards(false); + + const bool mc_rotors_added_successfully = _rotors.addActuators(configuration); + + _mc_motors_mask = _rotors.getUpwardsMotors(); + + // Control Surfaces + configuration.selected_matrix = 1; + _first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; + + const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration); + + return (mc_rotors_added_successfully && surfaces_added_successfully); +} + +void ActuatorEffectivenessStandardVTOLUnified::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp) +{ + // apply flaps + if (matrix_index == 1) { + actuator_controls_s actuator_controls_1; + + if (_actuator_controls_1_sub.copy(&actuator_controls_1)) { + float control_flaps = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS]; + float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES]; + _control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp); + } + } +} + +void ActuatorEffectivenessStandardVTOLUnified::setFlightPhase(const FlightPhase &flight_phase) +{ + if (_flight_phase == flight_phase) { + return; + } + + ActuatorEffectiveness::setFlightPhase(flight_phase); + + // update stopped motors + switch (flight_phase) { + case FlightPhase::FORWARD_FLIGHT: + _stopped_motors = _mc_motors_mask; + break; + + case FlightPhase::HOVER_FLIGHT: + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: + _stopped_motors = 0; + break; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp new file mode 100644 index 0000000000..5b888c7572 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessStandardVTOLUnified.hpp + * + * Actuator effectiveness for standard VTOL (unified control effectiveness) + * + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" + +#include + +class ActuatorEffectivenessStandardVTOLUnified : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessStandardVTOLUnified(ModuleParams *parent); + virtual ~ActuatorEffectivenessStandardVTOLUnified() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Standard VTOL Unified"; } + + int numMatrices() const override { return 1; } + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices"); + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + normalize[1] = false; + } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp) override; + + void setFlightPhase(const FlightPhase &flight_phase) override; + + uint32_t getStoppedMotors() const override { return _stopped_motors; } + +private: + ActuatorEffectivenessRotors _rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight) + uint32_t _stopped_motors{}; ///< currently stopped motors + + int _first_control_surface_idx{0}; ///< applies to matrix 1 + + uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)}; + +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index e99d718550..73d4a3217f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -52,6 +52,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessRotors.hpp ActuatorEffectivenessStandardVTOL.cpp ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectivenessStandardVTOLUnified.cpp + ActuatorEffectivenessStandardVTOLUnified.hpp ActuatorEffectivenessTiltrotorVTOL.cpp ActuatorEffectivenessTiltrotorVTOL.hpp ActuatorEffectivenessTailsitterVTOL.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index a90a0da4d7..b936844192 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -260,6 +260,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopter(this); break; + case EffectivenessSource::STANDARD_VTOL_UNIFIED: + tmp = new ActuatorEffectivenessStandardVTOLUnified(this); + break; + default: PX4_ERR("Unknown airframe"); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 1b016dd331..6692dcbcd1 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,7 @@ private: MULTIROTOR_WITH_TILT = 8, CUSTOM = 9, HELICOPTER = 10, + STANDARD_VTOL_UNIFIED = 11, }; enum class FailureMode { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index c92e29edeb..9aab16edaa 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -28,6 +28,7 @@ parameters: 8: Multirotor with Tilt 9: Custom 10: Helicopter + 11: Standard VTOL Unified (testing) default: 0 CA_METHOD: