diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp new file mode 100644 index 0000000000..1f7872df41 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessFixedWing.hpp" +#include + +using namespace matrix; + +ActuatorEffectivenessFixedWing::ActuatorEffectivenessFixedWing(ModuleParams *parent) + : ModuleParams(parent), _rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::FixedForward), + _control_surfaces(this) +{ +} + +bool +ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configuration, bool force) +{ + if (!force) { + return false; + } + + // Motors + _rotors.enableYawControl(false); + _rotors.getEffectivenessMatrix(configuration, true); + + // Control Surfaces + _first_control_surface_idx = configuration.num_actuators_matrix[0]; + _control_surfaces.getEffectivenessMatrix(configuration, true); + + return true; +} + +void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp) +{ + // apply flaps + actuator_controls_s actuator_controls_0; + + if (_actuator_controls_0_sub.copy(&actuator_controls_0)) { + float control_flaps = actuator_controls_0.control[actuator_controls_s::INDEX_FLAPS]; + float airbrakes_control = actuator_controls_0.control[actuator_controls_s::INDEX_AIRBRAKES]; + _control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp); + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp new file mode 100644 index 0000000000..e535952fd8 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" + +#include + +class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessFixedWing(ModuleParams *parent); + virtual ~ActuatorEffectivenessFixedWing() = default; + + bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + + const char *name() const override { return "Fixed Wing"; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp) override; + +private: + ActuatorEffectivenessRotors _rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; + + int _first_control_surface_idx{0}; ///< applies to matrix 1 +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 1303f00dcd..f5919f05fc 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -36,6 +36,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.hpp ActuatorEffectivenessControlSurfaces.cpp ActuatorEffectivenessControlSurfaces.hpp + ActuatorEffectivenessFixedWing.cpp + ActuatorEffectivenessFixedWing.hpp ActuatorEffectivenessTilts.cpp ActuatorEffectivenessTilts.hpp ActuatorEffectivenessRotors.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 49af5a2afa..607338217d 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -199,6 +199,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessRoverDifferential(); break; + case EffectivenessSource::FIXED_WING: + tmp = new ActuatorEffectivenessFixedWing(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 b85edb95ee..040b7b8cbd 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include @@ -125,6 +126,7 @@ private: TILTROTOR_VTOL = 2, ROVER_ACKERMANN = 3, ROVER_DIFFERENTIAL = 4, + FIXED_WING = 5, }; EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index f8cbc36b00..95057773e4 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -20,6 +20,7 @@ parameters: 2: Tiltrotor VTOL (WIP) 3: Rover (Ackermann) 4: Rover (Differential) + 5: Fixed Wing default: 0 CA_METHOD: @@ -492,5 +493,31 @@ mixer: - name: 'Right Motor' position: [ 0, 1., 0 ] + 5: # Fixed Wing + actuators: + - actuator_type: 'motor' + group_label: 'Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + - actuator_type: 'servo' + group_label: 'Control Surfaces' + count: 'CA_SV_CS_COUNT' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TYPE' + label: 'Type' + function: 'type' + identifier: 'servo-type' + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Torque' + identifier: 'servo-torque-roll' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Torque' + identifier: 'servo-torque-pitch' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Torque' + identifier: 'servo-torque-yaw'