From e61e0a08d8765df90af55b87079b7c403d554cfa Mon Sep 17 00:00:00 2001 From: bresch Date: Sun, 5 Feb 2023 14:45:25 +0100 Subject: [PATCH] CA: add thust vectoring actuator effectiveness --- .../ActuatorEffectivenessControlSurfaces.cpp | 8 ++ .../ActuatorEffectivenessControlSurfaces.hpp | 1 + .../ActuatorEffectivenessThrustVectoring.cpp | 100 ++++++++++++++++++ .../ActuatorEffectivenessThrustVectoring.hpp | 67 ++++++++++++ .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../control_allocator/ControlAllocator.cpp | 4 + .../control_allocator/ControlAllocator.hpp | 2 + src/modules/control_allocator/module.yaml | 1 + 8 files changed, 185 insertions(+) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.hpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 23dee70563..ecd7b51f4c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -186,3 +186,11 @@ void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control, actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler; } } + +void ActuatorEffectivenessControlSurfaces::applyScale(float scale, int first_actuator_idx, + ActuatorVector &actuator_sp) const +{ + for (int i = 0; i < _count; ++i) { + actuator_sp(i + first_actuator_idx) *= scale; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index a0b8b06c16..7fcd49f45c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -91,6 +91,7 @@ public: void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp); void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp); + void applyScale(float scale, int first_actuator_idx, ActuatorVector &actuator_sp) const; private: void updateParams() override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp new file mode 100644 index 0000000000..5a68352cbb --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessThrustVectoring.hpp" + +using namespace matrix; + +ActuatorEffectivenessThrustVectoring::ActuatorEffectivenessThrustVectoring(ModuleParams *parent) + : ModuleParams(parent), + _rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::FixedUpwards), + _control_surfaces(this) +{ +} + +bool +ActuatorEffectivenessThrustVectoring::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + _rotors.enablePropellerTorque(false); + const bool rotors_added_successfully = _rotors.addActuators(configuration); + + // Control Surfaces + _first_control_surface_idx = configuration.num_actuators_matrix[0]; + const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration); + + return (rotors_added_successfully && surfaces_added_successfully); +} + +void ActuatorEffectivenessThrustVectoring::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + esc_status_s esc_status; + hrt_abstime now = hrt_absolute_time(); + float rpm = 0.f; + + if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + 1_s)) { + + for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESCS); esc++) { + const esc_report_s &esc_report = esc_status.esc[esc]; + + const bool esc_connected = (esc_status.esc_online_flags & (1 << esc)) || (esc_report.esc_rpm != 0); + + if (esc_connected && (now < esc_report.timestamp + 1_s)) { + rpm = esc_report.esc_rpm; + break; + } + } + } + + constexpr float propeller_coefficient = 1.f / (20000.f * 20000.f); // scale down the thrust + float thrust = rpm * rpm * propeller_coefficient; + + // Use thrust setpoint if there is no ESC telemetry available + if (thrust < FLT_EPSILON) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + + if (_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint)) { + thrust = vehicle_thrust_setpoint.xyz[2]; + } + } + + const float scale = 1.f / fmaxf(thrust, 0.2f); + _control_surfaces.applyScale(scale, _first_control_surface_idx, actuator_sp); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.hpp new file mode 100644 index 0000000000..ca498462a7 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustVectoring.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" +#include "ActuatorEffectivenessControlSurfaces.hpp" + +#include +#include + +class ActuatorEffectivenessThrustVectoring : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessThrustVectoring(ModuleParams *parent); + virtual ~ActuatorEffectivenessThrustVectoring() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Thrust Vectoring"; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + +private: + static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]); + + ActuatorEffectivenessRotors _rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _esc_status_sub {ORB_ID(esc_status)}; + + int _first_control_surface_idx{0}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 2406b81bf8..35668e83e4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -50,6 +50,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessMCTilt.hpp ActuatorEffectivenessMultirotor.cpp ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessThrustVectoring.cpp + ActuatorEffectivenessThrustVectoring.hpp ActuatorEffectivenessTilts.cpp ActuatorEffectivenessTilts.hpp ActuatorEffectivenessRotors.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2ead22c570..36bd85e1b0 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -270,6 +270,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopterCoaxial(this); break; + case EffectivenessSource::THRUST_VECTORING: + tmp = new ActuatorEffectivenessThrustVectoring(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 303316f1ef..a5c4751b63 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 @@ -158,6 +159,7 @@ private: HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, HELICOPTER_COAXIAL = 12, + THRUST_VECTORING = 13, }; enum class FailureMode { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 8683d7477d..8d74377d0b 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -30,6 +30,7 @@ parameters: 10: Helicopter (tail ESC) 11: Helicopter (tail Servo) 12: Helicopter (Coaxial) + 13: Thrust Vectoring default: 0 CA_METHOD: