From 51833f66924df92c51543fc2204f252e1bef567f Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Tue, 7 Jan 2025 13:27:25 +0100 Subject: [PATCH] Copy control allocator to add metric allocator module --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 3 +- boards/px4/sitl/default.px4board | 3 +- .../ActuatorEffectiveness.cpp | 104 ++ .../ActuatorEffectiveness.hpp | 225 ++++ .../ActuatorEffectivenessControlSurfaces.cpp | 188 +++ .../ActuatorEffectivenessControlSurfaces.hpp | 114 ++ .../ActuatorEffectivenessCustom.cpp | 67 + .../ActuatorEffectivenessCustom.hpp | 59 + .../ActuatorEffectivenessFixedWing.cpp | 88 ++ .../ActuatorEffectivenessFixedWing.hpp | 68 + .../ActuatorEffectivenessHelicopter.cpp | 264 ++++ .../ActuatorEffectivenessHelicopter.hpp | 134 ++ ...ActuatorEffectivenessHelicopterCoaxial.cpp | 222 ++++ ...ActuatorEffectivenessHelicopterCoaxial.hpp | 117 ++ .../ActuatorEffectivenessHelicopterTest.cpp | 111 ++ .../ActuatorEffectivenessMCTilt.cpp | 130 ++ .../ActuatorEffectivenessMCTilt.hpp | 78 ++ .../ActuatorEffectivenessMultirotor.cpp | 56 + .../ActuatorEffectivenessMultirotor.hpp | 61 + .../ActuatorEffectivenessRotors.cpp | 309 +++++ .../ActuatorEffectivenessRotors.hpp | 156 +++ .../ActuatorEffectivenessRotorsTest.cpp | 135 ++ .../ActuatorEffectivenessRoverAckermann.cpp | 58 + .../ActuatorEffectivenessRoverAckermann.hpp | 53 + .../ActuatorEffectivenessStandardVTOL.cpp | 116 ++ .../ActuatorEffectivenessStandardVTOL.hpp | 95 ++ .../ActuatorEffectivenessTailsitterVTOL.cpp | 121 ++ .../ActuatorEffectivenessTailsitterVTOL.hpp | 94 ++ .../ActuatorEffectivenessTiltrotorVTOL.cpp | 268 ++++ .../ActuatorEffectivenessTiltrotorVTOL.hpp | 134 ++ .../ActuatorEffectivenessTilts.cpp | 139 ++ .../ActuatorEffectivenessTilts.hpp | 101 ++ .../ActuatorEffectivenessUUV.cpp | 63 + .../ActuatorEffectivenessUUV.hpp | 67 + .../ActuatorEffectiveness/CMakeLists.txt | 75 ++ src/modules/metric_allocator/CMakeLists.txt | 56 + .../ControlAllocation/CMakeLists.txt | 47 + .../ControlAllocation/ControlAllocation.cpp | 126 ++ .../ControlAllocation/ControlAllocation.hpp | 247 ++++ .../ControlAllocationPseudoInverse.cpp | 181 +++ .../ControlAllocationPseudoInverse.hpp | 76 ++ .../ControlAllocationPseudoInverseTest.cpp | 69 + ...ontrolAllocationSequentialDesaturation.cpp | 234 ++++ ...ontrolAllocationSequentialDesaturation.hpp | 132 ++ ...olAllocationSequentialDesaturationTest.cpp | 385 ++++++ .../metric_allocator/ControlAllocator.cpp | 879 +++++++++++++ .../metric_allocator/ControlAllocator.hpp | 220 ++++ src/modules/metric_allocator/Kconfig | 12 + src/modules/metric_allocator/module.yaml | 1157 +++++++++++++++++ 49 files changed, 7895 insertions(+), 2 deletions(-) create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp create mode 100644 src/modules/metric_allocator/ActuatorEffectiveness/CMakeLists.txt create mode 100644 src/modules/metric_allocator/CMakeLists.txt create mode 100644 src/modules/metric_allocator/ControlAllocation/CMakeLists.txt create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocation.cpp create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocation.hpp create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp create mode 100644 src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp create mode 100644 src/modules/metric_allocator/ControlAllocator.cpp create mode 100644 src/modules/metric_allocator/ControlAllocator.hpp create mode 100644 src/modules/metric_allocator/Kconfig create mode 100644 src/modules/metric_allocator/module.yaml diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index d5d9989c8666..b187b1b4ea78 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -8,7 +8,8 @@ # # Start Control Allocator # -control_allocator start +# control_allocator start +metric_allocator start # # Start Multicopter Rate Controller. diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef647..666041d39f9a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -11,7 +11,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_CONTROL_ALLOCATOR=n +CONFIG_MODULES_METRIC_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp new file mode 100644 index 000000000000..901c469746e6 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectiveness.hpp" +#include "../ControlAllocation/ControlAllocation.hpp" + +#include + +int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const matrix::Vector3f &torque, + const matrix::Vector3f &thrust) +{ + int actuator_idx = num_actuators_matrix[selected_matrix]; + + if (actuator_idx >= NUM_ACTUATORS) { + PX4_ERR("Too many actuators"); + return -1; + } + + if ((int)type < (int)ActuatorType::COUNT - 1 && num_actuators[(int)type + 1] > 0) { + PX4_ERR("Trying to add actuators in the wrong order (add motors first, then servos)"); + return -1; + } + + effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0); + effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1); + effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2); + effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0); + effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1); + effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2); + matrix_selection_indexes[totalNumActuators()] = selected_matrix; + ++num_actuators[(int)type]; + return num_actuators_matrix[selected_matrix]++; +} + +void ActuatorEffectiveness::Configuration::actuatorsAdded(ActuatorType type, int count) +{ + int total_count = totalNumActuators(); + + for (int i = 0; i < count; ++i) { + matrix_selection_indexes[i + total_count] = selected_matrix; + } + + num_actuators[(int)type] += count; + num_actuators_matrix[selected_matrix] += count; +} + +int ActuatorEffectiveness::Configuration::totalNumActuators() const +{ + int total_count = 0; + + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + total_count += num_actuators_matrix[i]; + } + + return total_count; +} + +void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp) +{ + for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) { + const uint32_t motor_mask = (1u << actuator_idx); + + if (stoppable_motors_mask & motor_mask) { + + // Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy) + if (fabsf(actuator_sp(actuator_idx)) < .02f) { + _stopped_motors_mask |= motor_mask; + + } else { + _stopped_motors_mask &= ~motor_mask; + } + } + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp new file mode 100644 index 000000000000..0ddd4988f335 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -0,0 +1,225 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectiveness.hpp + * + * Interface for Actuator Effectiveness + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +#include +#include + +enum class AllocationMethod { + NONE = -1, + PSEUDO_INVERSE = 0, + SEQUENTIAL_DESATURATION = 1, + AUTO = 2, +}; + +enum class ActuatorType { + MOTORS = 0, + SERVOS, + + COUNT +}; + +enum class EffectivenessUpdateReason { + NO_EXTERNAL_UPDATE = 0, + CONFIGURATION_UPDATE = 1, + MOTOR_ACTIVATION_UPDATE = 2, +}; + + +class ActuatorEffectiveness +{ +public: + ActuatorEffectiveness() = default; + virtual ~ActuatorEffectiveness() = default; + + static constexpr int NUM_ACTUATORS = 16; + static constexpr int NUM_AXES = 6; + + enum ControlAxis { + ROLL = 0, + PITCH, + YAW, + THRUST_X, + THRUST_Y, + THRUST_Z + }; + + static constexpr int MAX_NUM_MATRICES = 2; + + using EffectivenessMatrix = matrix::Matrix; + using ActuatorVector = matrix::Vector; + + enum class FlightPhase { + HOVER_FLIGHT = 0, + FORWARD_FLIGHT = 1, + TRANSITION_HF_TO_FF = 2, + TRANSITION_FF_TO_HF = 3 + }; + + struct Configuration { + /** + * Add an actuator to the selected matrix, returning the index, or -1 on error + */ + int addActuator(ActuatorType type, const matrix::Vector3f &torque, const matrix::Vector3f &thrust); + + /** + * Call this after manually adding N actuators to the selected matrix + */ + void actuatorsAdded(ActuatorType type, int count); + + int totalNumActuators() const; + + /// Configured effectiveness matrix. Actuators are expected to be filled in order, motors first, then servos + EffectivenessMatrix effectiveness_matrices[MAX_NUM_MATRICES]; + + int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices + ActuatorVector trim[MAX_NUM_MATRICES]; + + ActuatorVector linearization_point[MAX_NUM_MATRICES]; + + int selected_matrix; + + uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES]; + int num_actuators[(int)ActuatorType::COUNT]; + }; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + virtual void setFlightPhase(const FlightPhase &flight_phase) + { + _flight_phase = flight_phase; + } + + /** + * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES. + * This is expected to stay constant. + */ + virtual int numMatrices() const { return 1; } + + /** + * Get the desired allocation method(s) for each matrix, if configured as AUTO + */ + virtual void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const + { + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + allocation_method_out[i] = AllocationMethod::PSEUDO_INVERSE; + } + } + + /** + * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized + */ + virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const + { + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + normalize[i] = false; + } + } + + /** + * Get the control effectiveness matrix if updated + * + * @return true if updated and matrix is set + */ + virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;} + + /** + * Get the current flight phase + * + * @return Flight phase + */ + const FlightPhase &getFlightPhase() const + { + return _flight_phase; + } + + /** + * Display name + */ + virtual const char *name() const = 0; + + /** + * Callback from the control allocation, allowing to manipulate the setpoint. + * Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers). + * + * @param actuator_sp input & output setpoint + */ + virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {} + + /** + * Callback from the control allocation, allowing to manipulate the setpoint. + * This can be used to e.g. add non-linear or external terms. + * It is called after the matrix multiplication and before final clipping. + * @param actuator_sp input & output setpoint + */ + virtual void updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) {} + + /** + * Get a bitmask of motors to be stopped + */ + virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; } + + /** + * Fill in the unallocated torque and thrust, customized by effectiveness type. + * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead. + */ + virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + + /** + * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero + * + * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand + * @param actuator_sp outcome of the allocation to determine if the motor should be stopped + */ + virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp); + +protected: + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; + uint32_t _stopped_motors_mask{0}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp new file mode 100644 index 000000000000..23dee70563ad --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * 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 + +#include "ActuatorEffectivenessControlSurfaces.hpp" + +using namespace matrix; + +ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(ModuleParams *parent) + : ModuleParams(parent) +{ + for (int i = 0; i < MAX_COUNT; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TYPE", i); + _param_handles[i].type = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_R", i); + _param_handles[i].torque[0] = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_P", i); + _param_handles[i].torque[1] = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRQ_Y", i); + _param_handles[i].torque[2] = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i); + _param_handles[i].trim = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_FLAP", i); + _param_handles[i].scale_flap = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_SPOIL", i); + _param_handles[i].scale_spoiler = param_find(buffer); + } + + _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate); + _spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate); + + _count_handle = param_find("CA_SV_CS_COUNT"); + updateParams(); +} + +void ActuatorEffectivenessControlSurfaces::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_count_handle, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _count = count; + + for (int i = 0; i < _count; i++) { + param_get(_param_handles[i].type, (int32_t *)&_params[i].type); + + Vector3f &torque = _params[i].torque; + + for (int n = 0; n < 3; ++n) { + param_get(_param_handles[i].torque[n], &torque(n)); + } + + param_get(_param_handles[i].trim, &_params[i].trim); + param_get(_param_handles[i].scale_flap, &_params[i].scale_flap); + param_get(_param_handles[i].scale_spoiler, &_params[i].scale_spoiler); + + // TODO: enforce limits (note that tailsitter uses different limits)? + switch (_params[i].type) { + + case Type::LeftAileron: + break; + + case Type::RightAileron: + break; + + case Type::Elevator: + break; + + case Type::Rudder: + break; + + case Type::LeftElevon: + break; + + case Type::RightElevon: + break; + + case Type::LeftVTail: + break; + + case Type::RightVTail: + break; + + case Type::LeftFlap: + case Type::RightFlap: + torque.setZero(); + break; + + case Type::Airbrake: + torque.setZero(); + break; + + case Type::Custom: + break; + + case Type::LeftATail: + break; + + case Type::RightATail: + break; + + case Type::SingleChannelAileron: + break; + + case Type::SteeringWheel: + torque.setZero(); + break; + + case Type::LeftSpoiler: + case Type::RightSpoiler: + torque.setZero(); + break; + } + } +} + +bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configuration) +{ + for (int i = 0; i < _count; i++) { + int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{}); + + if (actuator_idx >= 0) { + configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim; + } + } + + return true; +} + +void ActuatorEffectivenessControlSurfaces::applyFlaps(float flaps_control, int first_actuator_idx, float dt, + ActuatorVector &actuator_sp) +{ + _flaps_setpoint_with_slewrate.update(flaps_control, dt); + + for (int i = 0; i < _count; ++i) { + // map [0, 1] to [-1, 1] + // TODO: this currently only works for dedicated flaps, not flaperons + actuator_sp(i + first_actuator_idx) += (_flaps_setpoint_with_slewrate.getState() * 2.f - 1.f) * _params[i].scale_flap; + } +} + +void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control, int first_actuator_idx, float dt, + ActuatorVector &actuator_sp) +{ + _spoilers_setpoint_with_slewrate.update(spoilers_control, dt); + + for (int i = 0; i < _count; ++i) { + // TODO: this currently only works for spoilerons, not dedicated spoilers + actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler; + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp new file mode 100644 index 000000000000..a0b8b06c16c6 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 +#include + +static constexpr float kFlapSlewRate = 0.5f; // slew rate for normalized flaps setpoint [1/s] +static constexpr float kSpoilersSlewRate = 0.5f; // slew rate for normalized spoilers setpoint [1/s] + +class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int MAX_COUNT = 8; + + enum class Type : int32_t { + // This matches with the parameter + LeftAileron = 1, + RightAileron = 2, + Elevator = 3, + Rudder = 4, + LeftElevon = 5, + RightElevon = 6, + LeftVTail = 7, + RightVTail = 8, + LeftFlap = 9, + RightFlap = 10, + Airbrake = 11, + Custom = 12, + LeftATail = 13, + RightATail = 14, + SingleChannelAileron = 15, + SteeringWheel = 16, + LeftSpoiler = 17, + RightSpoiler = 18, + }; + + struct Params { + Type type; + + matrix::Vector3f torque; + float trim; + float scale_flap; + float scale_spoiler; + }; + + ActuatorEffectivenessControlSurfaces(ModuleParams *parent); + virtual ~ActuatorEffectivenessControlSurfaces() = default; + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "Control Surfaces"; } + + int count() const { return _count; } + + const Params &config(int idx) const { return _params[idx]; } + + 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); + +private: + void updateParams() override; + + struct ParamHandles { + param_t type; + + param_t torque[3]; + param_t trim; + param_t scale_flap; + param_t scale_spoiler; + }; + ParamHandles _param_handles[MAX_COUNT]; + param_t _count_handle; + + Params _params[MAX_COUNT] {}; + int _count{0}; + + SlewRate _flaps_setpoint_with_slewrate; + SlewRate _spoilers_setpoint_with_slewrate; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp new file mode 100644 index 000000000000..ead2043682c0 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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" + +using namespace matrix; + +ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent) + : ModuleParams(parent), _motors(this), _torque(this) +{ +} + +bool +ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + _motors.enablePropellerTorque(false); + const bool motors_added_successfully = _motors.addActuators(configuration); + _motors_mask = _motors.getMotors(); + + // Torque + const bool torque_added_successfully = _torque.addActuators(configuration); + + return (motors_added_successfully && torque_added_successfully); +} + +void ActuatorEffectivenessCustom::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/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp new file mode 100644 index 000000000000..0cc1390bd272 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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" + +class ActuatorEffectivenessCustom : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessCustom(ModuleParams *parent); + virtual ~ActuatorEffectivenessCustom() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + 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 "Custom"; } + +protected: + ActuatorEffectivenessRotors _motors; + ActuatorEffectivenessControlSurfaces _torque; + + uint32_t _motors_mask{}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp new file mode 100644 index 000000000000..c7c2bba7d709 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + _rotors.enablePropellerTorque(false); + const bool rotors_added_successfully = _rotors.addActuators(configuration); + _forwards_motors_mask = _rotors.getForwardsMotors(); + + // 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 ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); +} + +void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index, + ActuatorVector &actuator_sp) +{ + // apply flaps + normalized_unsigned_setpoint_s flaps_setpoint; + + if (_flaps_setpoint_sub.copy(&flaps_setpoint)) { + _control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } + + // apply spoilers + normalized_unsigned_setpoint_s spoilers_setpoint; + + if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) { + _control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp new file mode 100644 index 000000000000..5b4b7785b251 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.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. + * + ****************************************************************************/ + +#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, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Fixed Wing"; } + + void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; + + 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: + ActuatorEffectivenessRotors _rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; + uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; + + int _first_control_surface_idx{0}; ///< applies to matrix 1 + + uint32_t _forwards_motors_mask{}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp new file mode 100644 index 000000000000..f403424b678c --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessHelicopter.hpp" +#include + +using namespace matrix; +using namespace time_literals; + +ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent, ActuatorType tail_actuator_type) + : ModuleParams(parent), _tail_actuator_type(tail_actuator_type) +{ + for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i); + _param_handles.swash_plate_servos[i].angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i); + _param_handles.swash_plate_servos[i].arm_length = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i); + _param_handles.swash_plate_servos[i].trim = param_find(buffer); + } + + _param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT"); + + for (int i = 0; i < NUM_CURVE_POINTS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i); + _param_handles.throttle_curve[i] = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i); + _param_handles.pitch_curve[i] = param_find(buffer); + } + + _param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S"); + _param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O"); + _param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S"); + _param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW"); + _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); +} + +void ActuatorEffectivenessHelicopter::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + float angle_deg{}; + param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg); + _geometry.swash_plate_servos[i].angle = math::radians(angle_deg); + param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length); + param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim); + } + + for (int i = 0; i < NUM_CURVE_POINTS; ++i) { + param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]); + param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]); + } + + param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale); + param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset); + param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale); + param_get(_param_handles.spoolup_time, &_geometry.spoolup_time); + int32_t yaw_ccw = 0; + param_get(_param_handles.yaw_ccw, &yaw_ccw); + _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; +} + +bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // As the allocation is non-linear, we use updateSetpoint() instead of the matrix + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); + + // Tail (yaw) (either ESC or Servo) + configuration.addActuator(_tail_actuator_type, Vector3f{}, Vector3f{}); + + // N swash plate servos + _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{}); + configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim; + } + + return true; +} + +void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + _saturation_flags = {}; + + // throttle/collective pitch curve + const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), + _geometry.throttle_curve) * throttleSpoolupProgress(); + const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve); + + // actuator mapping + actuator_sp(0) = mainMotorEnaged() ? throttle : NAN; + + actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign + + fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale + + throttle * _geometry.yaw_throttle_scale; + + // Saturation check for yaw + if (actuator_sp(1) < actuator_min(1)) { + setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); + + } else if (actuator_sp(1) > actuator_max(1)) { + setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + } + + for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { + float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch + + control_sp(ControlAxis::PITCH) * pitch_coeff + - control_sp(ControlAxis::ROLL) * roll_coeff + + _geometry.swash_plate_servos[i].trim; + + // Saturation check for roll & pitch + if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos); + + } else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg); + } + } +} + +bool ActuatorEffectivenessHelicopter::mainMotorEnaged() +{ + manual_control_switches_s manual_control_switches; + + if (_manual_control_switches_sub.update(&manual_control_switches)) { + _main_motor_engaged = manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_NONE + || manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_ON; + } + + return _main_motor_engaged; +} + +float ActuatorEffectivenessHelicopter::throttleSpoolupProgress() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f; + const float spoolup_progress = time_since_arming / _geometry.spoolup_time; + + if (_armed && spoolup_progress < 1.f) { + return spoolup_progress; + } + + return 1.f; +} + + +void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag) +{ + if (coeff > 0.f) { + // A positive change in given axis will increase saturation + positive_flag = true; + + } else if (coeff < 0.f) { + // A negative change in given axis will increase saturation + negative_flag = true; + } +} + +void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_saturation_flags.roll_pos) { + status.unallocated_torque[0] = 1.f; + + } else if (_saturation_flags.roll_neg) { + status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; + } + + if (_saturation_flags.pitch_pos) { + status.unallocated_torque[1] = 1.f; + + } else if (_saturation_flags.pitch_neg) { + status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; + } + + if (_saturation_flags.yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_saturation_flags.yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } + + if (_saturation_flags.thrust_pos) { + status.unallocated_thrust[2] = 1.f; + + } else if (_saturation_flags.thrust_neg) { + status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp new file mode 100644 index 000000000000..439e0f4ab9ce --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include + +#include +#include +#include + +class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4; + static constexpr int NUM_CURVE_POINTS = 5; + + struct SwashPlateGeometry { + float angle; + float arm_length; + float trim; + }; + + struct Geometry { + SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + int num_swash_plate_servos{0}; + float throttle_curve[NUM_CURVE_POINTS]; + float pitch_curve[NUM_CURVE_POINTS]; + float yaw_collective_pitch_scale; + float yaw_collective_pitch_offset; + float yaw_throttle_scale; + float yaw_sign; + float spoolup_time; + }; + + ActuatorEffectivenessHelicopter(ModuleParams *parent, ActuatorType tail_actuator_type); + virtual ~ActuatorEffectivenessHelicopter() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Helicopter"; } + + + const Geometry &geometry() const { return _geometry; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; +private: + float throttleSpoolupProgress(); + bool mainMotorEnaged(); + + void updateParams() override; + + struct SaturationFlags { + bool roll_pos; + bool roll_neg; + bool pitch_pos; + bool pitch_neg; + bool yaw_pos; + bool yaw_neg; + bool thrust_pos; + bool thrust_neg; + }; + static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag); + + struct ParamHandlesSwashPlate { + param_t angle; + param_t arm_length; + param_t trim; + }; + struct ParamHandles { + ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + param_t num_swash_plate_servos; + param_t throttle_curve[NUM_CURVE_POINTS]; + param_t pitch_curve[NUM_CURVE_POINTS]; + param_t yaw_collective_pitch_scale; + param_t yaw_collective_pitch_offset; + param_t yaw_throttle_scale; + param_t yaw_ccw; + param_t spoolup_time; + }; + ParamHandles _param_handles{}; + + Geometry _geometry{}; + + int _first_swash_plate_servo_index{}; + SaturationFlags _saturation_flags; + + // Throttle spoolup state + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; + + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; + bool _main_motor_engaged{true}; + + const ActuatorType _tail_actuator_type; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp new file mode 100644 index 000000000000..0c06f5963f08 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessHelicopterCoaxial.hpp" +#include + +using namespace matrix; +using namespace time_literals; + +ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent) + : ModuleParams(parent) +{ + for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i); + _param_handles.swash_plate_servos[i].angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i); + _param_handles.swash_plate_servos[i].arm_length = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i); + _param_handles.swash_plate_servos[i].trim = param_find(buffer); + } + + _param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT"); + _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); +} + +void ActuatorEffectivenessHelicopterCoaxial::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_swash_plate_servos = math::constrain((int)count, 2, NUM_SWASH_PLATE_SERVOS_MAX); + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + float angle_deg{}; + param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg); + _geometry.swash_plate_servos[i].angle = math::radians(angle_deg); + param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length); + param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim); + } + + param_get(_param_handles.spoolup_time, &_geometry.spoolup_time); +} + +bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // As the allocation is non-linear, we use updateSetpoint() instead of the matrix + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Clockwise rotor + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Counter-clockwise rotor + + // N swash plate servos + _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{}); + configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim; + } + + return true; +} + +void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + _saturation_flags = {}; + + // throttle/collective pitch curve + const float throttle = -control_sp(ControlAxis::THRUST_Z) * throttleSpoolupProgress(); + const float yaw = control_sp(ControlAxis::YAW); + + // actuator mapping + actuator_sp(0) = throttle - yaw; // Clockwise + actuator_sp(1) = throttle + yaw; // Counter-clockwise + + // Saturation check for yaw + if ((actuator_sp(0) < actuator_min(0)) || (actuator_sp(1) > actuator_max(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); + + } else if ((actuator_sp(0) > actuator_max(0)) || (actuator_sp(1) < actuator_min(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + } + + for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { + float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + actuator_sp(_first_swash_plate_servo_index + i) = + + control_sp(ControlAxis::PITCH) * pitch_coeff + - control_sp(ControlAxis::ROLL) * roll_coeff + + _geometry.swash_plate_servos[i].trim; + + // Saturation check for roll & pitch + if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos); + + } else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg); + } + } +} + +float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f; + const float spoolup_progress = time_since_arming / _geometry.spoolup_time; + + if (_armed && spoolup_progress < 1.f) { + return spoolup_progress; + } + + return 1.f; +} + + +void ActuatorEffectivenessHelicopterCoaxial::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag) +{ + if (coeff > 0.f) { + // A positive change in given axis will increase saturation + positive_flag = true; + + } else if (coeff < 0.f) { + // A negative change in given axis will increase saturation + negative_flag = true; + } +} + +void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_saturation_flags.roll_pos) { + status.unallocated_torque[0] = 1.f; + + } else if (_saturation_flags.roll_neg) { + status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; + } + + if (_saturation_flags.pitch_pos) { + status.unallocated_torque[1] = 1.f; + + } else if (_saturation_flags.pitch_neg) { + status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; + } + + if (_saturation_flags.yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_saturation_flags.yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } + + if (_saturation_flags.thrust_pos) { + status.unallocated_thrust[2] = 1.f; + + } else if (_saturation_flags.thrust_neg) { + status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp new file mode 100644 index 000000000000..d5316bf498c8 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include + +class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4; + + struct SwashPlateGeometry { + float angle; + float arm_length; + float trim; + }; + + struct Geometry { + SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + int num_swash_plate_servos{0}; + float spoolup_time; + }; + + ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent); + virtual ~ActuatorEffectivenessHelicopterCoaxial() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Helicopter"; } + + + const Geometry &geometry() const { return _geometry; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; +private: + float throttleSpoolupProgress(); + + void updateParams() override; + + struct SaturationFlags { + bool roll_pos; + bool roll_neg; + bool pitch_pos; + bool pitch_neg; + bool yaw_pos; + bool yaw_neg; + bool thrust_pos; + bool thrust_neg; + }; + static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag); + + struct ParamHandlesSwashPlate { + param_t angle; + param_t arm_length; + param_t trim; + }; + struct ParamHandles { + ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + param_t num_swash_plate_servos; + param_t spoolup_time; + }; + ParamHandles _param_handles{}; + + Geometry _geometry{}; + + int _first_swash_plate_servo_index{}; + SaturationFlags _saturation_flags; + + // Throttle spoolup state + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; + + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp new file mode 100644 index 000000000000..29ca1f1117fd --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 +#include "ActuatorEffectivenessHelicopter.hpp" + +using namespace matrix; + +TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve) +{ + // Disable autosaving parameters to avoid busy loop in param_set() + param_control_autosave(false); + + // Set parameters for a predefined throttle curve + float values[5] = {0.f, .3f, .6f, .8f, 1.f}; + param_t param; + + for (int i = 0; i < 5; i++) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i); + param = param_find(buffer); + param_set(param, &values[i]); + } + + ActuatorEffectivenessHelicopter helicopter(nullptr, ActuatorType::MOTORS); + // run getEffectivenessMatrix with empty configuration to correctly initialize _first_swash_plate_servo_index + ActuatorEffectiveness::Configuration configuration{}; + EffectivenessUpdateReason external_update = EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE; + helicopter.getEffectivenessMatrix(configuration, external_update); + + Vector control_sp{}; + ActuatorEffectiveness::ActuatorVector actuator_sp{}; + ActuatorEffectiveness::ActuatorVector actuator_min{}; + actuator_min.setAll(0.f); + ActuatorEffectiveness::ActuatorVector actuator_max{}; + actuator_max.setAll(1.f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.1f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), 0.f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), 0.f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.125f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), .15f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.25f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), .3f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.375f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), .45f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.5f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), .6f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.625f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), .7f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.75f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), .8f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.875f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), .9f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), 1.f); + + control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.1f; + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); + EXPECT_FLOAT_EQ(actuator_sp(0), 1.f); +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp new file mode 100644 index 000000000000..a8effa8cb315 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2021-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 "ActuatorEffectivenessMCTilt.hpp" + +using namespace matrix; + +ActuatorEffectivenessMCTilt::ActuatorEffectivenessMCTilt(ModuleParams *parent) + : ModuleParams(parent), + _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::FixedUpwards, true), + _tilts(this) +{ +} + +bool +ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // MC motors + _mc_rotors.enableYawByDifferentialThrust(!_tilts.hasYawControl()); + const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); + + // Tilts + _first_tilt_idx = configuration.num_actuators_matrix[0]; + _tilts.updateTorqueSign(_mc_rotors.geometry()); + const bool tilts_added_successfully = _tilts.addActuators(configuration); + + // Set offset such that tilts point upwards when control input == 0 (trim is 0 if min_angle == -max_angle). + // Note that we don't set configuration.trim here, because in the case of trim == +-1, yaw is always saturated + // and reduced to 0 with the sequential desaturation method. Instead we add it after. + _tilt_offsets.setZero(); + + for (int i = 0; i < _tilts.count(); ++i) { + float delta_angle = _tilts.config(i).max_angle - _tilts.config(i).min_angle; + + if (delta_angle > FLT_EPSILON) { + float trim = -1.f - 2.f * _tilts.config(i).min_angle / delta_angle; + _tilt_offsets(_first_tilt_idx + i) = trim; + } + } + + return (rotors_added_successfully && tilts_added_successfully); +} + +void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + actuator_sp += _tilt_offsets; + // TODO: dynamic matrix update + + bool yaw_saturated_positive = true; + bool yaw_saturated_negative = true; + + for (int i = 0; i < _tilts.count(); ++i) { + + // custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit + if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } + } + } + + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; +} + +void ActuatorEffectivenessMCTilt::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_yaw_tilt_saturation_flags.tilt_yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp new file mode 100644 index 000000000000..848c8b8853d3 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2021-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 "ActuatorEffectivenessTilts.hpp" + +class ActuatorEffectivenessMCTilt : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessMCTilt(ModuleParams *parent); + virtual ~ActuatorEffectivenessMCTilt() = 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 "MC Tilt"; } + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + +protected: + ActuatorVector _tilt_offsets; + ActuatorEffectivenessRotors _mc_rotors; + ActuatorEffectivenessTilts _tilts; + int _first_tilt_idx{0}; + + struct YawTiltSaturationFlags { + bool tilt_yaw_pos; + bool tilt_yaw_neg; + }; + + YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp new file mode 100644 index 000000000000..41ed5cb2c55f --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "ActuatorEffectivenessMultirotor.hpp" + +using namespace matrix; + +ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent) + : ModuleParams(parent), + _mc_rotors(this) +{ +} + +bool +ActuatorEffectivenessMultirotor::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); + + return rotors_added_successfully; +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp new file mode 100644 index 000000000000..e0a355edd24e --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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. + * + ****************************************************************************/ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" + +class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessMultirotor(ModuleParams *parent); + virtual ~ActuatorEffectivenessMultirotor() = 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; + } + + const char *name() const override { return "Multirotor"; } + +protected: + ActuatorEffectivenessRotors _mc_rotors; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp new file mode 100644 index 000000000000..b4edd0f2b01c --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -0,0 +1,309 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectivenessRotors.cpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessRotors.hpp" + +#include "ActuatorEffectivenessTilts.hpp" + +using namespace matrix; + +ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config, + bool tilt_support) + : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) +{ + for (int i = 0; i < NUM_ROTORS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i); + _param_handles[i].position_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i); + _param_handles[i].position_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i); + _param_handles[i].position_z = param_find(buffer); + + if (_axis_config == AxisConfiguration::Configurable) { + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i); + _param_handles[i].axis_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i); + _param_handles[i].axis_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i); + _param_handles[i].axis_z = param_find(buffer); + } + + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i); + _param_handles[i].thrust_coef = param_find(buffer); + + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i); + _param_handles[i].moment_ratio = param_find(buffer); + + if (_tilt_support) { + snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i); + _param_handles[i].tilt_index = param_find(buffer); + } + } + + updateParams(); +} + +void ActuatorEffectivenessRotors::updateParams() +{ + ModuleParams::updateParams(); + + _geometry.num_rotors = math::min(NUM_ROTORS_MAX, static_cast(_param_ca_rotor_count.get())); + + for (int i = 0; i < _geometry.num_rotors; ++i) { + Vector3f &position = _geometry.rotors[i].position; + param_get(_param_handles[i].position_x, &position(0)); + param_get(_param_handles[i].position_y, &position(1)); + param_get(_param_handles[i].position_z, &position(2)); + + Vector3f &axis = _geometry.rotors[i].axis; + + switch (_axis_config) { + case AxisConfiguration::Configurable: + param_get(_param_handles[i].axis_x, &axis(0)); + param_get(_param_handles[i].axis_y, &axis(1)); + param_get(_param_handles[i].axis_z, &axis(2)); + break; + + case AxisConfiguration::FixedForward: + axis = Vector3f(1.f, 0.f, 0.f); + break; + + case AxisConfiguration::FixedUpwards: + axis = Vector3f(0.f, 0.f, -1.f); + break; + } + + param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef); + param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio); + + if (_tilt_support) { + int32_t tilt_param{0}; + param_get(_param_handles[i].tilt_index, &tilt_param); + _geometry.rotors[i].tilt_index = tilt_param - 1; + + } else { + _geometry.rotors[i].tilt_index = -1; + } + } +} + +bool +ActuatorEffectivenessRotors::addActuators(Configuration &configuration) +{ + if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { + PX4_ERR("Wrong actuator ordering: servos need to be after motors"); + return false; + } + + int num_actuators = computeEffectivenessMatrix(_geometry, + configuration.effectiveness_matrices[configuration.selected_matrix], + configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); + return true; +} + +int +ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index) +{ + int num_actuators = 0; + + for (int i = 0; i < geometry.num_rotors; i++) { + + if (i + actuator_start_index >= NUM_ACTUATORS) { + break; + } + + ++num_actuators; + + // Get rotor axis + Vector3f axis = geometry.rotors[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.rotors[i].position; + + // Get coefficients + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; + + if (geometry.propeller_torque_disabled) { + km = 0.f; + } + + if (geometry.propeller_torque_disabled_non_upwards) { + bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f; + + if (!upwards) { + km = 0.f; + } + } + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + + // Fill corresponding items in effectiveness matrix + for (size_t j = 0; j < 3; j++) { + effectiveness(j, i + actuator_start_index) = moment(j); + effectiveness(j + 3, i + actuator_start_index) = thrust(j); + } + + if (geometry.yaw_by_differential_thrust_disabled) { + // set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts) + effectiveness(2, i + actuator_start_index) = 0.f; + } + + if (geometry.three_dimensional_thrust_disabled) { + // Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC), + // pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they + // can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated + // by the allocator directly. + + effectiveness(0 + 3, i + actuator_start_index) = 0.f; + effectiveness(1 + 3, i + actuator_start_index) = 0.f; + effectiveness(2 + 3, i + actuator_start_index) = -ct; + } + } + + return num_actuators; +} + +uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, + float collective_tilt_control) +{ + if (!PX4_ISFINITE(collective_tilt_control)) { + collective_tilt_control = -1.f; + } + + uint32_t nontilted_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + int tilt_index = _geometry.rotors[i].tilt_index; + + if (tilt_index == -1 || tilt_index >= tilts.count()) { + nontilted_motors |= 1u << i; + continue; + } + + const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index); + const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f); + const float tilt_direction = math::radians((float)tilt.tilt_direction); + _geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction); + } + + return nontilted_motors; +} + +Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction) +{ + Vector3f axis{0.f, 0.f, -1.f}; + return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis; +} + +uint32_t ActuatorEffectivenessRotors::getMotors() const +{ + uint32_t motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + motors |= 1u << i; + } + + return motors; +} + +uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const +{ + uint32_t upwards_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) { + upwards_motors |= 1u << i; + } + } + + return upwards_motors; +} + +uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const +{ + uint32_t forward_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { + forward_motors |= 1u << i; + } + } + + return forward_motors; +} + +bool +ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + return addActuators(configuration); +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp new file mode 100644 index 000000000000..3844df4c8481 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectivenessRotors.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include + +class ActuatorEffectivenessTilts; + +using namespace time_literals; + +class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness +{ +public: + enum class AxisConfiguration { + Configurable, ///< axis can be configured + FixedForward, ///< axis is fixed, pointing forwards (positive X) + FixedUpwards, ///< axis is fixed, pointing upwards (negative Z) + }; + + static constexpr int NUM_ROTORS_MAX = 12; + + struct RotorGeometry { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + float moment_ratio; + int tilt_index; + }; + + struct Geometry { + RotorGeometry rotors[NUM_ROTORS_MAX]; + int num_rotors{0}; + bool propeller_torque_disabled{false}; + bool yaw_by_differential_thrust_disabled{false}; + bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors + bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt + }; + + ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, + bool tilt_support = false); + virtual ~ActuatorEffectivenessRotors() = 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; + } + + static int computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index = 0); + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "Rotors"; } + + /** + * Sets the motor axis from tilt configurations and current tilt control. + * @param tilts configured tilt servos + * @param tilt_control current tilt control in [-1, 1] (can be NAN) + * @return the motors as bitset which are not tiltable + */ + uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control); + + const Geometry &geometry() const { return _geometry; } + + /** + * Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then + * rotated by tilt_direction around z. + */ + static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction); + + void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; } + + void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; } + + void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; } + + void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } + + uint32_t getMotors() const; + uint32_t getUpwardsMotors() const; + uint32_t getForwardsMotors() const; + +private: + void updateParams() override; + const AxisConfiguration _axis_config; + const bool _tilt_support; ///< if true, tilt servo assignment params are loaded + + struct ParamHandles { + param_t position_x; + param_t position_y; + param_t position_z; + param_t axis_x; + param_t axis_y; + param_t axis_z; + param_t thrust_coef; + param_t moment_ratio; + param_t tilt_index; + }; + ParamHandles _param_handles[NUM_ROTORS_MAX]; + + Geometry _geometry{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_rotor_count + ) +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp new file mode 100644 index 000000000000..9a6e45e1ad6f --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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 ActuatorEffectivenessRotorsTest.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include "ActuatorEffectivenessRotors.hpp" + +using namespace matrix; + +TEST(ActuatorEffectivenessRotors, AllZeroCase) +{ + // Quad wide geometry + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} + +TEST(ActuatorEffectivenessRotors, Tilt) +{ + Vector3f axis_expected{0.f, 0.f, -1.f}; + Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{-1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 0.f, -1.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, -1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp new file mode 100644 index 000000000000..94f5db16f36b --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessRoverAckermann.hpp" +#include + +using namespace matrix; + +bool +ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f}); + _motors_mask = 1u << 0; + configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{}); + return true; +} + +void ActuatorEffectivenessRoverAckermann::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/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp new file mode 100644 index 000000000000..294e453ec664 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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" + +class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessRoverAckermann() = default; + virtual ~ActuatorEffectivenessRoverAckermann() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + 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 "Rover (Ackermann)"; } +private: + uint32_t _motors_mask{}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp new file mode 100644 index 000000000000..79853c7518c5 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 "ActuatorEffectivenessStandardVTOL.hpp" +#include + +using namespace matrix; + +ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParams *parent) + : ModuleParams(parent), _rotors(this), _control_surfaces(this) +{ +} + +bool +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + configuration.selected_matrix = 0; + _rotors.enablePropellerTorqueNonUpwards(false); + const bool mc_rotors_added_successfully = _rotors.addActuators(configuration); + _upwards_motors_mask = _rotors.getUpwardsMotors(); + _forwards_motors_mask = _rotors.getForwardsMotors(); + + // 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 ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, int matrix_index, + ActuatorVector &actuator_sp) +{ + if (matrix_index == 1) { + // apply flaps + normalized_unsigned_setpoint_s flaps_setpoint; + + if (_flaps_setpoint_sub.copy(&flaps_setpoint)) { + _control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } + + // apply spoilers + normalized_unsigned_setpoint_s spoilers_setpoint; + + if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) { + _control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } + } +} + +void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + if (matrix_index == 0) { + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + } +} + +void ActuatorEffectivenessStandardVTOL::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_mask |= _upwards_motors_mask; + break; + + case FlightPhase::HOVER_FLIGHT: + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: + _stopped_motors_mask &= ~_upwards_motors_mask; + break; + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp new file mode 100644 index 000000000000..0e5d1bfa44a7 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectivenessStandardVTOL.hpp + * + * Actuator effectiveness for standard VTOL + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" + +#include + + +class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessStandardVTOL(ModuleParams *parent); + virtual ~ActuatorEffectivenessStandardVTOL() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Standard VTOL"; } + + int numMatrices() const override { return 2; } + + 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 allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + void setFlightPhase(const FlightPhase &flight_phase) override; + +private: + ActuatorEffectivenessRotors _rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + uint32_t _upwards_motors_mask{}; + uint32_t _forwards_motors_mask{}; + + int _first_control_surface_idx{0}; ///< applies to matrix 1 + + uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; + uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp new file mode 100644 index 000000000000..559101c106d7 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessTailsitterVTOL.cpp + * + * Actuator effectiveness for tailsitter VTOL + */ + +#include "ActuatorEffectivenessTailsitterVTOL.hpp" + +using namespace matrix; + +ActuatorEffectivenessTailsitterVTOL::ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent) + : ModuleParams(parent), _mc_rotors(this), _control_surfaces(this) +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} +bool +ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // MC motors + configuration.selected_matrix = 0; + // enable MC yaw control if more than 3 rotors + _mc_rotors.enableYawByDifferentialThrust(_mc_rotors.geometry().num_rotors > 3); + const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); + + // 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 ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float dt, int matrix_index, + ActuatorVector &actuator_sp) +{ + if (matrix_index == 1) { + // apply flaps + normalized_unsigned_setpoint_s flaps_setpoint; + + if (_flaps_setpoint_sub.copy(&flaps_setpoint)) { + _control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } + + // apply spoilers + normalized_unsigned_setpoint_s spoilers_setpoint; + + if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) { + _control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } + } +} + +void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + if (matrix_index == 0) { + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + } +} + +void ActuatorEffectivenessTailsitterVTOL::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: + _forwards_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards + break; + + case FlightPhase::HOVER_FLIGHT: + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: + _forwards_motors_mask = 0; + _stopped_motors_mask = 0; + break; + } +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp new file mode 100644 index 000000000000..604f05f9e0b8 --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessTailsitterVTOL.hpp + * + * Actuator effectiveness for tailsitter VTOL + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" + +#include + +#include + +class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent); + virtual ~ActuatorEffectivenessTailsitterVTOL() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + int numMatrices() const override { return 2; } + + 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 allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + + void setFlightPhase(const FlightPhase &flight_phase) override; + + const char *name() const override { return "VTOL Tailsitter"; } + +protected: + ActuatorEffectivenessRotors _mc_rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + uint32_t _forwards_motors_mask{}; + + int _first_control_surface_idx{0}; ///< applies to matrix 1 + + uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; + uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp new file mode 100644 index 000000000000..93ac6783d65a --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectivenessTiltrotorVTOL.cpp + * + * Actuator effectiveness for tiltrotor VTOL + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessTiltrotorVTOL.hpp" + +using namespace matrix; + +ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent) + : ModuleParams(parent), + _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true), + _control_surfaces(this), _tilts(this) +{ + _param_handles.com_spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} + +void ActuatorEffectivenessTiltrotorVTOL::updateParams() +{ + ModuleParams::updateParams(); + + param_get(_param_handles.com_spoolup_time, &_param_spoolup_time); +} + +bool +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // MC motors + configuration.selected_matrix = 0; + _mc_rotors.enableYawByDifferentialThrust(!_tilts.hasYawControl()); + _mc_rotors.enableThreeDimensionalThrust(false); + + // Update matrix with tilts in vertical position when update is triggered by a manual + // configuration (parameter) change. This is to make sure the normalization + // scales are tilt-invariant. Note: configuration updates are only possible when disarmed. + const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? + -1.f : _last_collective_tilt_control; + _untiltable_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied) + << configuration.num_actuators[(int)ActuatorType::MOTORS]; + + const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); + _motors = _mc_rotors.getMotors(); + + // 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); + + // Tilts + configuration.selected_matrix = 0; + _first_tilt_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; + _tilts.updateTorqueSign(_mc_rotors.geometry(), true /* disable pitch to avoid configuration errors */); + const bool tilts_added_successfully = _tilts.addActuators(configuration); + + // If it was an update coming from a config change, then make sure to update matrix in + // the next iteration again with the correct tilt (but without updating the normalization scale). + _collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE); + + return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully); +} + +void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt, int matrix_index, + ActuatorVector &actuator_sp) +{ + if (matrix_index == 1) { + // apply flaps + normalized_unsigned_setpoint_s flaps_setpoint; + + if (_flaps_setpoint_sub.copy(&flaps_setpoint)) { + _control_surfaces.applyFlaps(flaps_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } + + // apply spoilers + normalized_unsigned_setpoint_s spoilers_setpoint; + + if (_spoilers_setpoint_sub.copy(&spoilers_setpoint)) { + _control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); + } + } + +} + +void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + // apply tilt + if (matrix_index == 0) { + tiltrotor_extra_controls_s tiltrotor_extra_controls; + + if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) { + float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f; + + // set control_collective_tilt to exactly -1 or 1 if close to these end points + control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt; + control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt; + + // initialize _last_collective_tilt_control + if (!PX4_ISFINITE(_last_collective_tilt_control)) { + _last_collective_tilt_control = control_collective_tilt; + + } else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) { + _collective_tilt_updated = true; + _last_collective_tilt_control = control_collective_tilt; + } + + // During transition to FF, only allow update thrust axis up to 45° as with a high tilt angle the effectiveness + // of the thrust axis in z is apporaching 0, and by that is increasing the motor output to max. + // Transition to HF: disable thrust axis tilting, and assume motors are vertical. This is to avoid + // a thrust spike when the transition is initiated (as then the tilt is fully forward). + if (_flight_phase == FlightPhase::TRANSITION_HF_TO_FF) { + _last_collective_tilt_control = math::constrain(_last_collective_tilt_control, -1.f, 0.f); + + } else if (_flight_phase == FlightPhase::TRANSITION_FF_TO_HF) { + _last_collective_tilt_control = -1.f; + } + + bool yaw_saturated_positive = true; + bool yaw_saturated_negative = true; + + for (int i = 0; i < _tilts.count(); ++i) { + if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) { + + // as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover) + if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) { + actuator_sp(i + _first_tilt_idx) += control_collective_tilt; + + } else { + actuator_sp(i + _first_tilt_idx) = NAN; // NaN sets tilts to disarmed position + } + } + + // custom yaw saturation logic: only declare yaw saturated if all tilts are at the negative or positive yawing limit + if (_tilts.getYawTorqueOfTilt(i) > FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } + } + } + + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; + + // in FW directly use throttle sp + if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { + for (int i = 0; i < _first_tilt_idx; ++i) { + actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint; + } + } + } + + if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { + stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp); + } + } +} + +void ActuatorEffectivenessTiltrotorVTOL::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_mask |= _untiltable_motors; + break; + + case FlightPhase::HOVER_FLIGHT: + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: + _stopped_motors_mask = 0; + break; + } +} + +void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // only handle matrix 0 (motors and tilts) + if (matrix_index == 1) { + return; + } + + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_yaw_tilt_saturation_flags.tilt_yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } +} + +bool ActuatorEffectivenessTiltrotorVTOL::throttleSpoolupFinished() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + return _armed && hrt_elapsed_time(&_armed_time) > _param_spoolup_time * 1_s; +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp new file mode 100644 index 000000000000..310d937064dc --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectivenessTiltrotorVTOL.hpp + * + * Actuator effectiveness for tiltrotor VTOL + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" +#include "ActuatorEffectivenessTilts.hpp" + +#include + +#include +#include +#include +#include + +class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent); + virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + int numMatrices() const override { return 2; } + + 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 setFlightPhase(const FlightPhase &flight_phase) override; + + void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; + + 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 "VTOL Tiltrotor"; } + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + +protected: + bool _collective_tilt_updated{true}; + ActuatorEffectivenessRotors _mc_rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + ActuatorEffectivenessTilts _tilts; + + uint32_t _motors{}; + uint32_t _untiltable_motors{}; + + int _first_control_surface_idx{0}; ///< applies to matrix 1 + int _first_tilt_idx{0}; ///< applies to matrix 0 + + float _last_collective_tilt_control{NAN}; + + uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; + uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; + + struct YawTiltSaturationFlags { + bool tilt_yaw_pos; + bool tilt_yaw_neg; + }; + + YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; + + uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; + +private: + + void updateParams() override; + + struct ParamHandles { + param_t com_spoolup_time; + }; + + ParamHandles _param_handles{}; + + float _param_spoolup_time{1.f}; + + // Tilt handling during motor spoolup: leave the tilts in their disarmed position unitil 1s after arming + bool throttleSpoolupFinished(); + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp new file mode 100644 index 000000000000..3cce6b24fb8e --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessTilts.hpp" + +#include +#include + +using namespace matrix; + +ActuatorEffectivenessTilts::ActuatorEffectivenessTilts(ModuleParams *parent) + : ModuleParams(parent) +{ + for (int i = 0; i < MAX_COUNT; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_CT", i); + _param_handles[i].control = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_MINA", i); + _param_handles[i].min_angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_MAXA", i); + _param_handles[i].max_angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_TL%u_TD", i); + _param_handles[i].tilt_direction = param_find(buffer); + } + + _count_handle = param_find("CA_SV_TL_COUNT"); + updateParams(); +} + +void ActuatorEffectivenessTilts::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_count_handle, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _count = count; + + for (int i = 0; i < count; i++) { + param_get(_param_handles[i].control, (int32_t *)&_params[i].control); + param_get(_param_handles[i].tilt_direction, (int32_t *)&_params[i].tilt_direction); + param_get(_param_handles[i].min_angle, &_params[i].min_angle); + param_get(_param_handles[i].max_angle, &_params[i].max_angle); + + _params[i].min_angle = math::radians(_params[i].min_angle); + _params[i].max_angle = math::radians(_params[i].max_angle); + + _torque[i].setZero(); + } +} + +bool ActuatorEffectivenessTilts::addActuators(Configuration &configuration) +{ + for (int i = 0; i < _count; i++) { + configuration.addActuator(ActuatorType::SERVOS, _torque[i], Vector3f{}); + } + + return true; +} + +void ActuatorEffectivenessTilts::updateTorqueSign(const ActuatorEffectivenessRotors::Geometry &geometry, + bool disable_pitch) +{ + for (int i = 0; i < geometry.num_rotors; ++i) { + int tilt_index = geometry.rotors[i].tilt_index; + + if (tilt_index == -1 || tilt_index >= _count) { + continue; + } + + if (_params[tilt_index].control == Control::Yaw || _params[tilt_index].control == Control::YawAndPitch) { + + // Find the yaw torque sign by checking the motor position and tilt direction. + // Rotate position by -tilt_direction around z, then check the sign of y pos + float tilt_direction = math::radians((float)_params[tilt_index].tilt_direction); + Vector3f rotated_pos = Dcmf{Eulerf{0.f, 0.f, -tilt_direction}} * geometry.rotors[i].position; + + if (rotated_pos(1) < -0.01f) { // add minimal margin + _torque[tilt_index](2) = 1.f; + + } else if (rotated_pos(1) > 0.01f) { + _torque[tilt_index](2) = -1.f; + } + } + + if (!disable_pitch && (_params[tilt_index].control == Control::Pitch + || _params[tilt_index].control == Control::YawAndPitch)) { + bool tilting_forwards = (int)_params[tilt_index].tilt_direction < 90 || (int)_params[tilt_index].tilt_direction > 270; + _torque[tilt_index](1) = tilting_forwards ? -1.f : 1.f; + } + + } +} + +bool ActuatorEffectivenessTilts::hasYawControl() const +{ + for (int i = 0; i < _count; i++) { + if (_params[i].control == Control::Yaw || _params[i].control == Control::YawAndPitch) { + return true; + } + } + + return false; +} diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp new file mode 100644 index 000000000000..d885a091552b --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 + +class ActuatorEffectivenessTilts : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int MAX_COUNT = 4; + + enum class Control : int32_t { + // This matches with the parameter + None = 0, + Yaw = 1, + Pitch = 2, + YawAndPitch = 3, + }; + enum class TiltDirection : int32_t { + // This matches with the parameter + TowardsFront = 0, + TowardsRight = 90, + }; + + struct Params { + Control control; + float min_angle; + float max_angle; + TiltDirection tilt_direction; + }; + + ActuatorEffectivenessTilts(ModuleParams *parent); + virtual ~ActuatorEffectivenessTilts() = default; + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "Tilts"; } + + int count() const { return _count; } + + const Params &config(int idx) const { return _params[idx]; } + + void updateTorqueSign(const ActuatorEffectivenessRotors::Geometry &geometry, bool disable_pitch = false); + + bool hasYawControl() const; + + float getYawTorqueOfTilt(int tilt_index) const { return _torque[tilt_index](2); } + +private: + void updateParams() override; + + struct ParamHandles { + param_t control; + param_t min_angle; + param_t max_angle; + param_t tilt_direction; + }; + + ParamHandles _param_handles[MAX_COUNT]; + param_t _count_handle; + + Params _params[MAX_COUNT] {}; + int _count{0}; + + matrix::Vector3f _torque[MAX_COUNT] {}; +}; diff --git a/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp new file mode 100644 index 000000000000..e4c861a9d0b0 --- /dev/null +++ b/src/modules/metric_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/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/metric_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp new file mode 100644 index 000000000000..57e86f943653 --- /dev/null +++ b/src/modules/metric_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/metric_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/metric_allocator/ActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 000000000000..2406b81bf80b --- /dev/null +++ b/src/modules/metric_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -0,0 +1,75 @@ +############################################################################ +# +# 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.cpp + ActuatorEffectiveness.hpp + ActuatorEffectivenessUUV.cpp + ActuatorEffectivenessUUV.hpp + ActuatorEffectivenessControlSurfaces.cpp + ActuatorEffectivenessControlSurfaces.hpp + ActuatorEffectivenessCustom.cpp + ActuatorEffectivenessCustom.hpp + ActuatorEffectivenessFixedWing.cpp + ActuatorEffectivenessFixedWing.hpp + ActuatorEffectivenessHelicopter.cpp + ActuatorEffectivenessHelicopter.hpp + ActuatorEffectivenessHelicopterCoaxial.cpp + ActuatorEffectivenessHelicopterCoaxial.hpp + ActuatorEffectivenessMCTilt.cpp + ActuatorEffectivenessMCTilt.hpp + ActuatorEffectivenessMultirotor.cpp + ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessTilts.cpp + ActuatorEffectivenessTilts.hpp + ActuatorEffectivenessRotors.cpp + ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessStandardVTOL.cpp + ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectivenessTiltrotorVTOL.cpp + ActuatorEffectivenessTiltrotorVTOL.hpp + ActuatorEffectivenessTailsitterVTOL.cpp + ActuatorEffectivenessTailsitterVTOL.hpp + ActuatorEffectivenessRoverAckermann.hpp + ActuatorEffectivenessRoverAckermann.cpp +) + +target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(ActuatorEffectiveness + PRIVATE + mathlib +) + +px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/metric_allocator/CMakeLists.txt b/src/modules/metric_allocator/CMakeLists.txt new file mode 100644 index 000000000000..e9dc29e5f012 --- /dev/null +++ b/src/modules/metric_allocator/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# 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. +# +############################################################################ + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(ActuatorEffectiveness) +add_subdirectory(ControlAllocation) + +px4_add_module( + MODULE modules__metric_allocator + MAIN metric_allocator + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + STACK_MAIN + 3000 + SRCS + ControlAllocator.cpp + ControlAllocator.hpp + MODULE_CONFIG + module.yaml + DEPENDS + mathlib + ActuatorEffectiveness + ControlAllocation + px4_work_queue + SlewRate +) diff --git a/src/modules/metric_allocator/ControlAllocation/CMakeLists.txt b/src/modules/metric_allocator/ControlAllocation/CMakeLists.txt new file mode 100644 index 000000000000..4da638aac8cb --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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) +px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocation.cpp new file mode 100644 index 000000000000..dfdf9ca7f765 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocation.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocation.cpp + * + * Interface for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include "ControlAllocation.hpp" + +ControlAllocation::ControlAllocation() +{ + _control_allocation_scale.setAll(1.f); + _actuator_min.setAll(0.f); + _actuator_max.setAll(1.f); +} + +void +ControlAllocation::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) +{ + _effectiveness = effectiveness; + ActuatorVector linearization_point_clipped = linearization_point; + clipActuatorSetpoint(linearization_point_clipped); + _actuator_trim = actuator_trim + linearization_point_clipped; + clipActuatorSetpoint(_actuator_trim); + _num_actuators = num_actuators; + _control_trim = _effectiveness * linearization_point_clipped; +} + +void +ControlAllocation::setActuatorSetpoint( + const matrix::Vector &actuator_sp) +{ + // Set actuator setpoint + _actuator_sp = actuator_sp; + + // Clip + clipActuatorSetpoint(_actuator_sp); +} + +void +ControlAllocation::clipActuatorSetpoint(matrix::Vector &actuator) const +{ + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_max(i) < _actuator_min(i)) { + actuator(i) = _actuator_trim(i); + + } else if (actuator(i) < _actuator_min(i)) { + actuator(i) = _actuator_min(i); + + } else if (actuator(i) > _actuator_max(i)) { + actuator(i) = _actuator_max(i); + } + } +} + +matrix::Vector +ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector &actuator) +const +{ + matrix::Vector actuator_normalized; + + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_min(i) < _actuator_max(i)) { + actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + + } else { + actuator_normalized(i) = (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + } + } + + return actuator_normalized; +} + +void ControlAllocation::applySlewRateLimit(float dt) +{ + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_slew_rate_limit(i) > FLT_EPSILON) { + float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i); + float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i); + + if (delta_sp > delta_sp_max) { + _actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max; + + } else if (delta_sp < -delta_sp_max) { + _actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max; + } + } + } +} diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocation.hpp new file mode 100644 index 000000000000..c60784a03c2c --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocation.hpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocation.hpp + * + * Interface for Control Allocation Algorithms + * + * Implementers of this interface are expected to update the members + * of this base class in the `allocate` method. + * + * Example usage: + * ``` + * [...] + * // Initialization + * ControlAllocationMethodImpl alloc(); + * alloc.setEffectivenessMatrix(effectiveness, actuator_trim); + * alloc.setActuatorMin(actuator_min); + * alloc.setActuatorMin(actuator_max); + * + * while (1) { + * [...] + * + * // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint + * alloc.setControlSetpoint(control_sp); + * alloc.allocate(); + * actuator_sp = alloc.getActuatorSetpoint(); + * + * // Check if the control setpoint was fully allocated + * unallocated_control = control_sp - alloc.getAllocatedControl() + * + * [...] + * } + * ``` + * + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp" + +class ControlAllocation +{ +public: + ControlAllocation(); + virtual ~ControlAllocation() = default; + + static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS; + static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES; + + typedef matrix::Vector ActuatorVector; + + enum ControlAxis { + ROLL = 0, + PITCH, + YAW, + THRUST_X, + THRUST_Y, + THRUST_Z + }; + + /** + * Allocate control setpoint to actuators + */ + virtual void allocate() = 0; + + /** + * Set actuator failure flag + * This prevents a change of the scaling in the matrix normalization step + * in case of a motor failure. + * + * @param failure Motor failure flag + */ + void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; } + + /** + * Set the control effectiveness matrix + * + * @param B Effectiveness matrix + */ + virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale); + + /** + * Get the allocated actuator vector + * + * @return Actuator vector + */ + const matrix::Vector &getActuatorSetpoint() const { return _actuator_sp; } + + /** + * Set the desired control vector + * + * @param Control vector + */ + void setControlSetpoint(const matrix::Vector &control) { _control_sp = control; } + + /** + * Get the desired control vector + * + * @return Control vector + */ + const matrix::Vector &getControlSetpoint() const { return _control_sp; } + + /** + * Get the allocated control vector + * + * @return Control vector + */ + matrix::Vector getAllocatedControl() const + { return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); } + + /** + * Get the control effectiveness matrix + * + * @return Effectiveness matrix + */ + const matrix::Matrix &getEffectivenessMatrix() const { return _effectiveness; } + + /** + * Set the minimum actuator values + * + * @param actuator_min Minimum actuator values + */ + void setActuatorMin(const matrix::Vector &actuator_min) { _actuator_min = actuator_min; } + + /** + * Get the minimum actuator values + * + * @return Minimum actuator values + */ + const matrix::Vector &getActuatorMin() const { return _actuator_min; } + + /** + * Set the maximum actuator values + * + * @param actuator_max Maximum actuator values + */ + void setActuatorMax(const matrix::Vector &actuator_max) { _actuator_max = actuator_max; } + + /** + * Get the maximum actuator values + * + * @return Maximum actuator values + */ + const matrix::Vector &getActuatorMax() const { return _actuator_max; } + + /** + * Set the current actuator setpoint. + * + * Use this when a new allocation method is started to initialize it properly. + * In most cases, it is not needed to call this method before `allocate()`. + * Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`. + * + * @param actuator_sp Actuator setpoint + */ + void setActuatorSetpoint(const matrix::Vector &actuator_sp); + + void setSlewRateLimit(const matrix::Vector &slew_rate_limit) + { _actuator_slew_rate_limit = slew_rate_limit; } + + /** + * Apply slew rate to current actuator setpoint + */ + void applySlewRateLimit(float dt); + + /** + * Clip the actuator setpoint between minimum and maximum values. + * + * The output is in the range [min; max] + * + * @param actuator Actuator vector to clip + */ + void clipActuatorSetpoint(matrix::Vector &actuator) const; + + void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); } + + /** + * Normalize the actuator setpoint between minimum and maximum values. + * + * The output is in the range [-1; +1] + * + * @param actuator Actuator vector to normalize + * + * @return Clipped actuator setpoint + */ + matrix::Vector normalizeActuatorSetpoint(const matrix::Vector &actuator) + const; + + virtual void updateParameters() {} + + int numConfiguredActuators() const { return _num_actuators; } + + void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; } + +protected: + friend class ControlAllocator; // for _actuator_sp + + matrix::Matrix _effectiveness; ///< Effectiveness matrix + matrix::Vector _control_allocation_scale; ///< Scaling applied during allocation + matrix::Vector _actuator_trim; ///< Neutral actuator values + matrix::Vector _actuator_min; ///< Minimum actuator values + matrix::Vector _actuator_max; ///< Maximum actuator values + matrix::Vector _actuator_slew_rate_limit; ///< Slew rate limit + matrix::Vector _prev_actuator_sp; ///< Previous actuator setpoint + matrix::Vector _actuator_sp; ///< Actuator setpoint + matrix::Vector _control_sp; ///< Control setpoint + matrix::Vector _control_trim; ///< Control at trim actuator values + int _num_actuators{0}; + bool _normalize_rpy{false}; ///< if true, normalize roll, pitch and yaw columns + bool _had_actuator_failure{false}; +}; diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp new file mode 100644 index 000000000000..5d86814d7e67 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocationPseudoInverse.cpp + * + * Simple Control Allocation Algorithm + * + * @author Julien Lecoeur + */ + +#include "ControlAllocationPseudoInverse.hpp" + +void +ControlAllocationPseudoInverse::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) +{ + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators, + update_normalization_scale); + _mix_update_needed = true; + _normalization_needs_update = update_normalization_scale; +} + +void +ControlAllocationPseudoInverse::updatePseudoInverse() +{ + if (_mix_update_needed) { + matrix::geninv(_effectiveness, _mix); + + if (_normalization_needs_update && !_had_actuator_failure) { + updateControlAllocationMatrixScale(); + _normalization_needs_update = false; + } + + normalizeControlAllocationMatrix(); + _mix_update_needed = false; + } +} + +void +ControlAllocationPseudoInverse::updateControlAllocationMatrixScale() +{ + // Same scale on roll and pitch + if (_normalize_rpy) { + + int num_non_zero_roll_torque = 0; + int num_non_zero_pitch_torque = 0; + + for (int i = 0; i < _num_actuators; i++) { + + if (fabsf(_mix(i, 0)) > 1e-3f) { + ++num_non_zero_roll_torque; + } + + if (fabsf(_mix(i, 1)) > 1e-3f) { + ++num_non_zero_pitch_torque; + } + } + + float roll_norm_scale = 1.f; + + if (num_non_zero_roll_torque > 0) { + roll_norm_scale = sqrtf(_mix.col(0).norm_squared() / (num_non_zero_roll_torque / 2.f)); + } + + float pitch_norm_scale = 1.f; + + if (num_non_zero_pitch_torque > 0) { + pitch_norm_scale = sqrtf(_mix.col(1).norm_squared() / (num_non_zero_pitch_torque / 2.f)); + } + + _control_allocation_scale(0) = fmaxf(roll_norm_scale, pitch_norm_scale); + _control_allocation_scale(1) = _control_allocation_scale(0); + + // Scale yaw separately + _control_allocation_scale(2) = _mix.col(2).max(); + + } else { + _control_allocation_scale(0) = 1.f; + _control_allocation_scale(1) = 1.f; + _control_allocation_scale(2) = 1.f; + } + + // Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators + // (for tilted actuators) + _control_allocation_scale(THRUST_Z) = 1.f; + + for (int axis_idx = 2; axis_idx >= 0; --axis_idx) { + int num_non_zero_thrust = 0; + float norm_sum = 0.f; + + for (int i = 0; i < _num_actuators; i++) { + float norm = fabsf(_mix(i, 3 + axis_idx)); + norm_sum += norm; + + if (norm > FLT_EPSILON) { + ++num_non_zero_thrust; + } + } + + if (num_non_zero_thrust > 0) { + _control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust; + + } else { + _control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z); + } + } +} + +void +ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() +{ + if (_control_allocation_scale(0) > FLT_EPSILON) { + _mix.col(0) /= _control_allocation_scale(0); + _mix.col(1) /= _control_allocation_scale(1); + } + + if (_control_allocation_scale(2) > FLT_EPSILON) { + _mix.col(2) /= _control_allocation_scale(2); + } + + if (_control_allocation_scale(3) > FLT_EPSILON) { + _mix.col(3) /= _control_allocation_scale(3); + _mix.col(4) /= _control_allocation_scale(4); + _mix.col(5) /= _control_allocation_scale(5); + } + + // Set all the small elements to 0 to avoid issues + // in the control allocation algorithms + for (int i = 0; i < _num_actuators; i++) { + for (int j = 0; j < NUM_AXES; j++) { + if (fabsf(_mix(i, j)) < 1e-3f) { + _mix(i, j) = 0.f; + } + } + } +} + +void +ControlAllocationPseudoInverse::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + _prev_actuator_sp = _actuator_sp; + + // Allocate + _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); +} diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp new file mode 100644 index 000000000000..27c367376bd5 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocationPseudoInverse.hpp + * + * Simple Control Allocation Algorithm + * + * It computes the pseudo-inverse of the effectiveness matrix + * Actuator saturation is handled by simple clipping, do not + * expect good performance in case of actuator saturation. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ControlAllocation.hpp" + +class ControlAllocationPseudoInverse: public ControlAllocation +{ +public: + ControlAllocationPseudoInverse() = default; + virtual ~ControlAllocationPseudoInverse() = default; + + void allocate() override; + void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) override; + +protected: + matrix::Matrix _mix; + + bool _mix_update_needed{false}; + + /** + * Recalculate pseudo inverse if required. + * + */ + void updatePseudoInverse(); + +private: + void normalizeControlAllocationMatrix(); + void updateControlAllocationMatrixScale(); + bool _normalization_needs_update{false}; +}; diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp new file mode 100644 index 000000000000..89faab8c92ea --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocationPseudoInverseTest.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include + +using namespace matrix; + +TEST(ControlAllocationTest, AllZeroCase) +{ + ControlAllocationPseudoInverse method; + + matrix::Vector control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix effectiveness; + matrix::Vector actuator_sp; + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + matrix::Vector actuator_sp_expected; + + method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false); + method.setControlSetpoint(control_sp); + method.allocate(); + method.clipActuatorSetpoint(); + actuator_sp = method.getActuatorSetpoint(); + control_allocated_expected = method.getAllocatedControl(); + + EXPECT_EQ(actuator_sp, actuator_sp_expected); + EXPECT_EQ(control_allocated, control_allocated_expected); +} diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp new file mode 100644 index 000000000000..4b28f44dacb9 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocationSequentialDesaturation.cpp + * + * @author Roman Bapst + * @author Beat Küng + */ + +#include "ControlAllocationSequentialDesaturation.hpp" + + +void +ControlAllocationSequentialDesaturation::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + _prev_actuator_sp = _actuator_sp; + + switch (_param_mc_airmode.get()) { + case 1: + mixAirmodeRP(); + break; + + case 2: + mixAirmodeRPY(); + break; + + default: + mixAirmodeDisabled(); + break; + } +} + +void ControlAllocationSequentialDesaturation::desaturateActuators( + ActuatorVector &actuator_sp, + const ActuatorVector &desaturation_vector, bool increase_only) +{ + float gain = computeDesaturationGain(desaturation_vector, actuator_sp); + + if (increase_only && gain < 0.f) { + return; + } + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } + + gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp); + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } +} + +float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, + const ActuatorVector &actuator_sp) +{ + float k_min = 0.f; + float k_max = 0.f; + + for (int i = 0; i < _num_actuators; i++) { + // Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains + if (fabsf(desaturation_vector(i)) < 0.2f) { + continue; + } + + if (actuator_sp(i) < _actuator_min(i)) { + float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + + if (actuator_sp(i) > _actuator_max(i)) { + float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + } + + // Reduce the saturation as much as possible + return k_min + k_max; +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRP() +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRPY() +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + ActuatorVector thrust_z; + ActuatorVector yaw; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + yaw(i) = _mix(i, ControlAxis::YAW); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + desaturateActuators(_actuator_sp, yaw); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeDisabled() +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + ActuatorVector thrust_z; + ActuatorVector roll; + ActuatorVector pitch; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + roll(i) = _mix(i, ControlAxis::ROLL); + pitch(i) = _mix(i, ControlAxis::PITCH); + } + + // only reduce thrust + desaturateActuators(_actuator_sp, thrust_z, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + desaturateActuators(_actuator_sp, roll); + desaturateActuators(_actuator_sp, pitch); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixYaw() +{ + // Add yaw to outputs + ActuatorVector yaw; + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + yaw(i) = _mix(i, ControlAxis::YAW); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + ActuatorVector max_prev = _actuator_max; + _actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN; + desaturateActuators(_actuator_sp, yaw); + _actuator_max = max_prev; + + // reduce thrust only + desaturateActuators(_actuator_sp, thrust_z, true); +} + +void +ControlAllocationSequentialDesaturation::updateParameters() +{ + updateParams(); +} diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp new file mode 100644 index 000000000000..dfa69d977762 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocationSequentialDesaturation.hpp + * + * Control Allocation Algorithm which sequentially modifies control demands in order to + * eliminate the saturation of the actuator setpoint vector. + * + * + * @author Roman Bapst + */ + +#pragma once + +#include "ControlAllocationPseudoInverse.hpp" + +#include + +class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams +{ +public: + + ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {} + virtual ~ControlAllocationSequentialDesaturation() = default; + + void allocate() override; + + void updateParameters() override; + + // This is the minimum actuator yaw granted when the controller is saturated. + // In the yaw-only case where outputs are saturated, thrust is reduced by up to this amount. + static constexpr float MINIMUM_YAW_MARGIN{0.15f}; +private: + + /** + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param actuator_sp Actuator setpoint, vector that is modified + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector + */ + void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, + bool increase_only = false); + + /** + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. + * + * @return desaturation gain + */ + float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + */ + void mixAirmodeRP(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. + */ + void mixAirmodeRPY(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mixYaw() for the exact yaw behavior. + */ + void mixAirmodeDisabled(); + + /** + * Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + */ + void mixYaw(); + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode ///< air-mode + ); +}; diff --git a/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp b/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp new file mode 100644 index 000000000000..60392330c939 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp @@ -0,0 +1,385 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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 ControlAllocationSequentialDesaturationTest.cpp + * + * Tests for Control Allocation Sequential Desaturation Algorithms + * + */ + +#include +#include +#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp> + +using namespace matrix; + +namespace +{ + +// Makes and returns a Geometry object for a "standard" quad-x quadcopter. +ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() +{ + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + return geometry; +} + +// Returns an effective matrix for a sample quad-copter configuration. +ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() +{ + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + const auto geometry = make_quad_x_geometry(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + return effectiveness; +} + +// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter. +void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator) +{ + const auto effectiveness = make_quad_x_effectiveness(); + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + constexpr bool UPDATE_NORMALIZATION_SCALE{false}; + allocator.setEffectivenessMatrix( + effectiveness, + actuator_trim, + linearization_point, + ActuatorEffectiveness::NUM_ACTUATORS, + UPDATE_NORMALIZATION_SCALE + ); +} + +static constexpr float EXPECT_NEAR_TOL{1e-4f}; + +} // namespace + +// This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator +// allocation. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = 1.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + matrix::Vector zero; + EXPECT_EQ(actuator_sp, zero); +} + +// This tests that a control setpoint for z-thrust returns the desired actuator setpoint. +// Each motor should have an actuator setpoint that when summed together should be equal to +// control setpoint. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + constexpr int MOTOR_COUNT{4}; + constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT}; + + for (int i{0}; i < MOTOR_COUNT; ++i) { + EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test does not saturate the yaw response. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is low enough to not saturate the motors. + constexpr float YAW_CONTROL_SP{0.02f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, + // this will need to be changed. + constexpr float YAW_EFFECTIVENESS_FACTOR{5.f}; + constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR}; + // At yaw condition, there will be 2 different actuator values. + constexpr int MOTOR_COUNT{4}; + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR}; + + for (int i{0}; i < MOTOR_COUNT / 2; ++i) { + EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) { + EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test saturates the yaw response, but does not reduce total thrust. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is arbitrarily high to trigger strongest possible (saturated) yaw response. + constexpr float YAW_CONTROL_SP{0.25f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // At max yaw, only 2 motors will carry all of the thrust. + constexpr int YAW_MOTORS{2}; + constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS}; + + for (int i{0}; i < YAW_MOTORS; ++i) { + EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. +// This test does not saturate the pitch response. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is low enough to not saturate the motors. + constexpr float PITCH_CONTROL_SP{0.1f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, + // this will need to be changed. + constexpr int MOTOR_COUNT{4}; + constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT}; + // At control set point, there will be 2 different actuator values. + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test saturates yaw and demonstrates reduction of thrust for yaw. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f}; + constexpr int MOTOR_COUNT{4}; + constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT}; + // This is arbitrarily high to trigger strongest possible (saturated) yaw response. + constexpr float YAW_CONTROL_SP{1.f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // In the case of yaw saturation, thrust per motor will be reduced by the hard-coded + // magic-number yaw margin of 0.15f. + constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available. + constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR}; + // At control set point, there will be 2 different actuator values. + constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN}; + constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. +// This test saturates the pitch response such that thrust is reduced to (partially) compensate. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f * 4.f}; + // This is high enough to saturate the pitch control. + constexpr float PITCH_CONTROL_SP{2.f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + constexpr int MOTOR_COUNT{4}; + // The maximum actuator value is + // THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT. + // The amount over 1 is the amount that each motor is reduced by. + // At control set point, there will be 2 different actuator values. + constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1}; + EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f); + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} diff --git a/src/modules/metric_allocator/ControlAllocator.cpp b/src/modules/metric_allocator/ControlAllocator.cpp new file mode 100644 index 000000000000..8155118e13e0 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocator.cpp @@ -0,0 +1,879 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * @file ControlAllocator.cpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#include "ControlAllocator.hpp" + +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; + +ControlAllocator::ControlAllocator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _control_allocator_status_pub[0].advertise(); + _control_allocator_status_pub[1].advertise(); + + _actuator_motors_pub.advertise(); + _actuator_servos_pub.advertise(); + _actuator_servos_trim_pub.advertise(); + + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i); + _param_handles.slew_rate_motors[i] = param_find(buffer); + } + + for (int i = 0; i < MAX_NUM_SERVOS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SV%u_SLEW", i); + _param_handles.slew_rate_servos[i] = param_find(buffer); + } + + parameters_updated(); +} + +ControlAllocator::~ControlAllocator() +{ + for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) { + delete _control_allocation[i]; + } + + delete _actuator_effectiveness; + + perf_free(_loop_perf); +} + +bool +ControlAllocator::init() +{ + if (!_vehicle_torque_setpoint_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + if (!_vehicle_thrust_setpoint_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + +#ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep + ScheduleDelayed(50_ms); +#endif + + return true; +} + +void +ControlAllocator::parameters_updated() +{ + _has_slew_rate = false; + + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { + param_get(_param_handles.slew_rate_motors[i], &_params.slew_rate_motors[i]); + _has_slew_rate |= _params.slew_rate_motors[i] > FLT_EPSILON; + } + + for (int i = 0; i < MAX_NUM_SERVOS; ++i) { + param_get(_param_handles.slew_rate_servos[i], &_params.slew_rate_servos[i]); + _has_slew_rate |= _params.slew_rate_servos[i] > FLT_EPSILON; + } + + // Allocation method & effectiveness source + // Do this first: in case a new method is loaded, it will be configured below + bool updated = update_effectiveness_source(); + update_allocation_method(updated); // must be called after update_effectiveness_source() + + if (_num_control_allocation == 0) { + return; + } + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->updateParameters(); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::CONFIGURATION_UPDATE); +} + +void +ControlAllocator::update_allocation_method(bool force) +{ + AllocationMethod configured_method = (AllocationMethod)_param_ca_method.get(); + + if (!_actuator_effectiveness) { + PX4_ERR("_actuator_effectiveness null"); + return; + } + + if (_allocation_method_id != configured_method || force) { + + matrix::Vector actuator_sp[ActuatorEffectiveness::MAX_NUM_MATRICES]; + + // Cleanup first + for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) { + // Save current state + if (_control_allocation[i] != nullptr) { + actuator_sp[i] = _control_allocation[i]->getActuatorSetpoint(); + } + + delete _control_allocation[i]; + _control_allocation[i] = nullptr; + } + + _num_control_allocation = _actuator_effectiveness->numMatrices(); + + AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES]; + _actuator_effectiveness->getDesiredAllocationMethod(desired_methods); + + bool normalize_rpy[ActuatorEffectiveness::MAX_NUM_MATRICES]; + _actuator_effectiveness->getNormalizeRPY(normalize_rpy); + + for (int i = 0; i < _num_control_allocation; ++i) { + AllocationMethod method = configured_method; + + if (configured_method == AllocationMethod::AUTO) { + method = desired_methods[i]; + } + + switch (method) { + case AllocationMethod::PSEUDO_INVERSE: + _control_allocation[i] = new ControlAllocationPseudoInverse(); + break; + + case AllocationMethod::SEQUENTIAL_DESATURATION: + _control_allocation[i] = new ControlAllocationSequentialDesaturation(); + break; + + default: + PX4_ERR("Unknown allocation method"); + break; + } + + if (_control_allocation[i] == nullptr) { + PX4_ERR("alloc failed"); + _num_control_allocation = 0; + + } else { + _control_allocation[i]->setNormalizeRPY(normalize_rpy[i]); + _control_allocation[i]->setActuatorSetpoint(actuator_sp[i]); + } + } + + _allocation_method_id = configured_method; + } +} + +bool +ControlAllocator::update_effectiveness_source() +{ + const EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get(); + + if (_effectiveness_source_id != source) { + + // try to instanciate new effectiveness source + ActuatorEffectiveness *tmp = nullptr; + + switch (source) { + case EffectivenessSource::NONE: + case EffectivenessSource::MULTIROTOR: + tmp = new ActuatorEffectivenessMultirotor(this); + break; + + case EffectivenessSource::STANDARD_VTOL: + tmp = new ActuatorEffectivenessStandardVTOL(this); + break; + + case EffectivenessSource::TILTROTOR_VTOL: + tmp = new ActuatorEffectivenessTiltrotorVTOL(this); + break; + + case EffectivenessSource::TAILSITTER_VTOL: + tmp = new ActuatorEffectivenessTailsitterVTOL(this); + break; + + case EffectivenessSource::ROVER_ACKERMANN: + tmp = new ActuatorEffectivenessRoverAckermann(); + break; + + case EffectivenessSource::ROVER_DIFFERENTIAL: + // rover_differential_control does allocation and publishes directly to actuator_motors topic + break; + + case EffectivenessSource::FIXED_WING: + tmp = new ActuatorEffectivenessFixedWing(this); + break; + + case EffectivenessSource::MOTORS_6DOF: // just a different UI from MULTIROTOR + tmp = new ActuatorEffectivenessUUV(this); + break; + + case EffectivenessSource::MULTIROTOR_WITH_TILT: + tmp = new ActuatorEffectivenessMCTilt(this); + break; + + case EffectivenessSource::CUSTOM: + tmp = new ActuatorEffectivenessCustom(this); + break; + + case EffectivenessSource::HELICOPTER_TAIL_ESC: + tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::MOTORS); + break; + + case EffectivenessSource::HELICOPTER_TAIL_SERVO: + tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS); + break; + + case EffectivenessSource::HELICOPTER_COAXIAL: + tmp = new ActuatorEffectivenessHelicopterCoaxial(this); + break; + + default: + PX4_ERR("Unknown airframe"); + break; + } + + // Replace previous source with new one + if (tmp == nullptr) { + // It did not work, forget about it + PX4_ERR("Actuator effectiveness init failed"); + _param_ca_airframe.set((int)_effectiveness_source_id); + + } else { + // Swap effectiveness sources + delete _actuator_effectiveness; + _actuator_effectiveness = tmp; + + // Save source id + _effectiveness_source_id = source; + } + + return true; + } + + return false; +} + +void +ControlAllocator::Run() +{ + if (should_exit()) { + _vehicle_torque_setpoint_sub.unregisterCallback(); + _vehicle_thrust_setpoint_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + +#ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep + // Push backup schedule + ScheduleDelayed(50_ms); +#endif + + // Check if parameters have changed + if (_parameter_update_sub.updated() && !_armed) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + if (_handled_motor_failure_bitmask == 0) { + // We don't update the geometry after an actuator failure, as it could lead to unexpected results + // (e.g. a user could add/remove motors, such that the bitmask isn't correct anymore) + updateParams(); + parameters_updated(); + } + } + + if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) { + return; + } + + { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + + ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; + + // Check if the current flight phase is HOVER or FIXED_WING + if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT; + + } else { + flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT; + } + + // Special cases for VTOL in transition + if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { + if (vehicle_status.in_transition_to_fw) { + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF; + + } else { + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF; + } + } + + // Forward to effectiveness source + _actuator_effectiveness->setFlightPhase(flight_phase); + } + } + + { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _publish_controls = vehicle_control_mode.flag_control_allocation_enabled; + } + } + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + + bool do_update = false; + vehicle_torque_setpoint_s vehicle_torque_setpoint; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + + // Run allocator on torque changes + if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) { + _torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz); + + do_update = true; + _timestamp_sample = vehicle_torque_setpoint.timestamp_sample; + + } + + // Also run allocator on thrust setpoint changes if the torque setpoint + // has not been updated for more than 5ms + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { + _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz); + + if (dt > 0.005f) { + do_update = true; + _timestamp_sample = vehicle_thrust_setpoint.timestamp_sample; + } + } + + if (do_update) { + _last_run = now; + + check_for_motor_failures(); + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE); + + // Set control setpoint vector(s) + matrix::Vector c[ActuatorEffectiveness::MAX_NUM_MATRICES]; + c[0](0) = _torque_sp(0); + c[0](1) = _torque_sp(1); + c[0](2) = _torque_sp(2); + c[0](3) = _thrust_sp(0); + c[0](4) = _thrust_sp(1); + c[0](5) = _thrust_sp(2); + + if (_num_control_allocation > 1) { + if (_vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)) { + c[1](0) = vehicle_torque_setpoint.xyz[0]; + c[1](1) = vehicle_torque_setpoint.xyz[1]; + c[1](2) = vehicle_torque_setpoint.xyz[2]; + } + + if (_vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)) { + c[1](3) = vehicle_thrust_setpoint.xyz[0]; + c[1](4) = vehicle_thrust_setpoint.xyz[1]; + c[1](5) = vehicle_thrust_setpoint.xyz[2]; + } + } + + for (int i = 0; i < _num_control_allocation; ++i) { + + _control_allocation[i]->setControlSetpoint(c[i]); + + // Do allocation + _control_allocation[i]->allocate(); + _actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers + _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, + _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); + + if (_has_slew_rate) { + _control_allocation[i]->applySlewRateLimit(dt); + } + + _control_allocation[i]->clipActuatorSetpoint(); + } + } + + // Publish actuator setpoint and allocator status + publish_actuator_controls(); + + // Publish status at limited rate, as it's somewhat expensive and we use it for slower dynamics + // (i.e. anti-integrator windup) + if (now - _last_status_pub >= 5_ms) { + publish_control_allocator_status(0); + + if (_num_control_allocation > 1) { + publish_control_allocator_status(1); + } + + _last_status_pub = now; + } + + perf_end(_loop_perf); +} + +void +ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason) +{ + ActuatorEffectiveness::Configuration config{}; + + if (reason == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE + && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates + return; + } + + if (_actuator_effectiveness->getEffectivenessMatrix(config, reason)) { + _last_effectiveness_update = hrt_absolute_time(); + + memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes, + sizeof(_control_allocation_selection_indexes)); + + // Get the minimum and maximum depending on type and configuration + ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES]; + ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES]; + ActuatorEffectiveness::ActuatorVector slew_rate[ActuatorEffectiveness::MAX_NUM_MATRICES]; + int actuator_idx = 0; + int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + + actuator_servos_trim_s trims{}; + static_assert(actuator_servos_trim_s::NUM_CONTROLS == actuator_servos_s::NUM_CONTROLS, "size mismatch"); + + for (int actuator_type = 0; actuator_type < (int)ActuatorType::COUNT; ++actuator_type) { + _num_actuators[actuator_type] = config.num_actuators[actuator_type]; + + for (int actuator_type_idx = 0; actuator_type_idx < config.num_actuators[actuator_type]; ++actuator_type_idx) { + if (actuator_idx >= NUM_ACTUATORS) { + _num_actuators[actuator_type] = 0; + PX4_ERR("Too many actuators"); + break; + } + + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + + if ((ActuatorType)actuator_type == ActuatorType::MOTORS) { + if (actuator_type_idx >= MAX_NUM_MOTORS) { + PX4_ERR("Too many motors"); + _num_actuators[actuator_type] = 0; + break; + } + + if (_param_r_rev.get() & (1u << actuator_type_idx)) { + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; + + } else { + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f; + } + + slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_motors[actuator_type_idx]; + + } else if ((ActuatorType)actuator_type == ActuatorType::SERVOS) { + if (actuator_type_idx >= MAX_NUM_SERVOS) { + PX4_ERR("Too many servos"); + _num_actuators[actuator_type] = 0; + break; + } + + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; + slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_servos[actuator_type_idx]; + trims.trim[actuator_type_idx] = config.trim[selected_matrix](actuator_idx_matrix[selected_matrix]); + + } else { + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = -1.f; + } + + maximum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 1.f; + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + } + + // Handle failed actuators + if (_handled_motor_failure_bitmask) { + actuator_idx = 0; + memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix)); + + for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + + if (_handled_motor_failure_bitmask & (1 << motors_idx)) { + ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix]; + + for (int i = 0; i < NUM_AXES; i++) { + matrix(i, actuator_idx_matrix[selected_matrix]) = 0.0f; + } + } + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + } + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setActuatorMin(minimum[i]); + _control_allocation[i]->setActuatorMax(maximum[i]); + _control_allocation[i]->setSlewRateLimit(slew_rate[i]); + + // Set all the elements of a row to 0 if that row has weak authority. + // That ensures that the algorithm doesn't try to control axes with only marginal control authority, + // which in turn would degrade the control of the main axes that actually should and can be controlled. + + ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[i]; + + for (int n = 0; n < NUM_AXES; n++) { + bool all_entries_small = true; + + for (int m = 0; m < config.num_actuators_matrix[i]; m++) { + if (fabsf(matrix(n, m)) > 0.05f) { + all_entries_small = false; + } + } + + if (all_entries_small) { + matrix.row(n) = 0.f; + } + } + + // Assign control effectiveness matrix + int total_num_actuators = config.num_actuators_matrix[i]; + _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], + config.linearization_point[i], total_num_actuators, reason == EffectivenessUpdateReason::CONFIGURATION_UPDATE); + } + + trims.timestamp = hrt_absolute_time(); + _actuator_servos_trim_pub.publish(trims); + } +} + +void +ControlAllocator::publish_control_allocator_status(int matrix_index) +{ + control_allocator_status_s control_allocator_status{}; + control_allocator_status.timestamp = hrt_absolute_time(); + + // TODO: disabled motors (?) + + // Allocated control + const matrix::Vector &allocated_control = _control_allocation[matrix_index]->getAllocatedControl(); + + // Unallocated control + const matrix::Vector unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - + allocated_control; + control_allocator_status.unallocated_torque[0] = unallocated_control(0); + control_allocator_status.unallocated_torque[1] = unallocated_control(1); + control_allocator_status.unallocated_torque[2] = unallocated_control(2); + control_allocator_status.unallocated_thrust[0] = unallocated_control(3); + control_allocator_status.unallocated_thrust[1] = unallocated_control(4); + control_allocator_status.unallocated_thrust[2] = unallocated_control(5); + + // override control_allocator_status in customized saturation logic for certain effectiveness types + _actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status); + + // Allocation success flags + control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0], + control_allocator_status.unallocated_torque[1], + control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0], + control_allocator_status.unallocated_thrust[1], + control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f); + + // Actuator saturation + const matrix::Vector &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint(); + const matrix::Vector &actuator_min = _control_allocation[matrix_index]->getActuatorMin(); + const matrix::Vector &actuator_max = _control_allocation[matrix_index]->getActuatorMax(); + + for (int i = 0; i < NUM_ACTUATORS; i++) { + if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER; + + } else if (actuator_sp(i) < (actuator_min(i) + FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_LOWER; + } + } + + // Handled motor failures + control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask; + + _control_allocator_status_pub[matrix_index].publish(control_allocator_status); +} + +void +ControlAllocator::publish_actuator_controls() +{ + if (!_publish_controls) { + return; + } + + actuator_motors_s actuator_motors; + actuator_motors.timestamp = hrt_absolute_time(); + actuator_motors.timestamp_sample = _timestamp_sample; + + actuator_servos_s actuator_servos; + actuator_servos.timestamp = actuator_motors.timestamp; + actuator_servos.timestamp_sample = _timestamp_sample; + + actuator_motors.reversible_flags = _param_r_rev.get(); + + int actuator_idx = 0; + int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + + uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask; + + // motors + int motors_idx; + + for (motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); + actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + + if (stopped_motors & (1u << motors_idx)) { + actuator_motors.control[motors_idx] = NAN; + } + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + + for (int i = motors_idx; i < actuator_motors_s::NUM_CONTROLS; i++) { + actuator_motors.control[i] = NAN; + } + + _actuator_motors_pub.publish(actuator_motors); + + // servos + if (_num_actuators[1] > 0) { + int servos_idx; + + for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); + actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + + for (int i = servos_idx; i < actuator_servos_s::NUM_CONTROLS; i++) { + actuator_servos.control[i] = NAN; + } + + _actuator_servos_pub.publish(actuator_servos); + } +} + +void +ControlAllocator::check_for_motor_failures() +{ + failure_detector_status_s failure_detector_status; + + if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE + && _failure_detector_status_sub.update(&failure_detector_status)) { + if (failure_detector_status.fd_motor) { + + if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) { + // motor failure bitmask changed + switch ((FailureMode)_param_ca_failure_mode.get()) { + case FailureMode::REMOVE_FIRST_FAILING_MOTOR: { + // Count number of failed motors + const int num_motors_failed = math::countSetBits(failure_detector_status.motor_failure_mask); + + // Only handle if it is the first failure + if (_handled_motor_failure_bitmask == 0 && num_motors_failed == 1) { + _handled_motor_failure_bitmask = failure_detector_status.motor_failure_mask; + PX4_WARN("Removing motor from allocation (0x%x)", _handled_motor_failure_bitmask); + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setHadActuatorFailure(true); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); + } + } + break; + + default: + break; + } + + } + + } else if (_handled_motor_failure_bitmask != 0) { + // Clear bitmask completely + PX4_INFO("Restoring all motors"); + _handled_motor_failure_bitmask = 0; + + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setHadActuatorFailure(false); + } + + update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE); + } + } +} + +int ControlAllocator::task_spawn(int argc, char *argv[]) +{ + ControlAllocator *instance = new ControlAllocator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ControlAllocator::print_status() +{ + PX4_INFO("Running"); + + // Print current allocation method + switch (_allocation_method_id) { + case AllocationMethod::NONE: + PX4_INFO("Method: None"); + break; + + case AllocationMethod::PSEUDO_INVERSE: + PX4_INFO("Method: Pseudo-inverse"); + break; + + case AllocationMethod::SEQUENTIAL_DESATURATION: + PX4_INFO("Method: Sequential desaturation"); + break; + + case AllocationMethod::AUTO: + PX4_INFO("Method: Auto"); + break; + } + + // Print current airframe + if (_actuator_effectiveness != nullptr) { + PX4_INFO("Effectiveness Source: %s", _actuator_effectiveness->name()); + } + + // Print current effectiveness matrix + for (int i = 0; i < _num_control_allocation; ++i) { + const ActuatorEffectiveness::EffectivenessMatrix &effectiveness = _control_allocation[i]->getEffectivenessMatrix(); + + if (_num_control_allocation > 1) { + PX4_INFO("Instance: %i", i); + } + + PX4_INFO(" Effectiveness.T ="); + effectiveness.T().print(); + PX4_INFO(" minimum ="); + _control_allocation[i]->getActuatorMin().T().print(); + PX4_INFO(" maximum ="); + _control_allocation[i]->getActuatorMax().T().print(); + PX4_INFO(" Configured actuators: %i", _control_allocation[i]->numConfiguredActuators()); + } + + if (_handled_motor_failure_bitmask) { + PX4_INFO("Failed motors: %i (0x%x)", math::countSetBits(_handled_motor_failure_bitmask), + _handled_motor_failure_bitmask); + } + + // Print perf + perf_print_counter(_loop_perf); + + return 0; +} + +int ControlAllocator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int ControlAllocator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements control allocation. It takes torque and thrust setpoints +as inputs and outputs actuator setpoint messages. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("control_allocator", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Control Allocator app start / stop handling function + */ +extern "C" __EXPORT int metric_allocator_main(int argc, char *argv[]); + +int metric_allocator_main(int argc, char *argv[]) +{ + return ControlAllocator::main(argc, argv); +} diff --git a/src/modules/metric_allocator/ControlAllocator.hpp b/src/modules/metric_allocator/ControlAllocator.hpp new file mode 100644 index 000000000000..303316f1ef52 --- /dev/null +++ b/src/modules/metric_allocator/ControlAllocator.hpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * @file ControlAllocator.hpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + static constexpr int NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; + static constexpr int NUM_AXES = ControlAllocation::NUM_AXES; + + static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS; + static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS; + + using ActuatorVector = ActuatorEffectiveness::ActuatorVector; + + ControlAllocator(); + + virtual ~ControlAllocator(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); + +private: + + struct ParamHandles { + param_t slew_rate_motors[MAX_NUM_MOTORS]; + param_t slew_rate_servos[MAX_NUM_SERVOS]; + }; + + struct Params { + float slew_rate_motors[MAX_NUM_MOTORS]; + float slew_rate_servos[MAX_NUM_SERVOS]; + }; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void update_allocation_method(bool force); + bool update_effectiveness_source(); + + void update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason); + + void check_for_motor_failures(); + + void publish_control_allocator_status(int matrix_index); + + void publish_actuator_controls(); + + AllocationMethod _allocation_method_id{AllocationMethod::NONE}; + ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; ///< class for control allocation calculations + int _num_control_allocation{0}; + hrt_abstime _last_effectiveness_update{0}; + + enum class EffectivenessSource { + NONE = -1, + MULTIROTOR = 0, + FIXED_WING = 1, + STANDARD_VTOL = 2, + TILTROTOR_VTOL = 3, + TAILSITTER_VTOL = 4, + ROVER_ACKERMANN = 5, + ROVER_DIFFERENTIAL = 6, + MOTORS_6DOF = 7, + MULTIROTOR_WITH_TILT = 8, + CUSTOM = 9, + HELICOPTER_TAIL_ESC = 10, + HELICOPTER_TAIL_SERVO = 11, + HELICOPTER_COAXIAL = 12, + }; + + enum class FailureMode { + IGNORE = 0, + REMOVE_FIRST_FAILING_MOTOR = 1, + }; + + EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; + ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness + + uint8_t _control_allocation_selection_indexes[NUM_ACTUATORS * ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + int _num_actuators[(int)ActuatorType::COUNT] {}; + + // Inputs + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */ + uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */ + + uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1}; /**< vehicle torque setpoint subscription (2. instance) */ + uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; /**< vehicle thrust setpoint subscription (2. instance) */ + + // Outputs + uORB::PublicationMulti _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)}; + + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; + + matrix::Vector3f _torque_sp; + matrix::Vector3f _thrust_sp; + bool _publish_controls{true}; + + // Reflects motor failures that are currently handled, not motor failures that are reported. + // For example, the system might report two motor failures, but only the first one is handled by CA + uint16_t _handled_motor_failure_bitmask{0}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + bool _armed{false}; + hrt_abstime _last_run{0}; + hrt_abstime _timestamp_sample{0}; + hrt_abstime _last_status_pub{0}; + + ParamHandles _param_handles{}; + Params _params{}; + bool _has_slew_rate{false}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_airframe, + (ParamInt) _param_ca_method, + (ParamInt) _param_ca_failure_mode, + (ParamInt) _param_r_rev + ) + +}; diff --git a/src/modules/metric_allocator/Kconfig b/src/modules/metric_allocator/Kconfig new file mode 100644 index 000000000000..ea4449854d44 --- /dev/null +++ b/src/modules/metric_allocator/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_METRIC_ALLOCATOR + bool "metric_allocator" + default n + ---help--- + Enable support for metric_allocator + +menuconfig USER_METRIC_ALLOCATOR + bool "metric_allocator running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_METRIC_ALLOCATOR + ---help--- + Put metric_allocator in userspace memory diff --git a/src/modules/metric_allocator/module.yaml b/src/modules/metric_allocator/module.yaml new file mode 100644 index 000000000000..8cc5e2fab946 --- /dev/null +++ b/src/modules/metric_allocator/module.yaml @@ -0,0 +1,1157 @@ +__max_num_mc_motors: &max_num_mc_motors 12 +__max_num_servos: &max_num_servos 8 +__max_num_tilts: &max_num_tilts 4 + +module_name: Metric Allocation + +parameters: + - group: Geometry + definitions: + CA_AIRFRAME: + description: + short: Airframe selection + long: | + Defines which mixer implementation to use. + Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators. + + 'Custom' should only be used if noting else can be used. + type: enum + values: + 0: Multirotor + 1: Fixed-wing + 2: Standard VTOL + 3: Tiltrotor VTOL + 4: Tailsitter VTOL + 5: Rover (Ackermann) + 6: Rover (Differential) + 7: Motors (6DOF) + 8: Multirotor with Tilt + 9: Custom + 10: Helicopter (tail ESC) + 11: Helicopter (tail Servo) + 12: Helicopter (Coaxial) + 13: Rover (Mecanum) + default: 0 + + CA_METHOD: + description: + short: Control allocation method + long: | + Selects the algorithm and desaturation method. + If set to Automtic, the selection is based on the airframe (CA_AIRFRAME). + type: enum + values: + 0: Pseudo-inverse with output clipping + 1: Pseudo-inverse with sequential desaturation technique + 2: Automatic + default: 2 + + # Motor parameters + CA_R_REV: + description: + short: Bidirectional/Reversible motors + long: | + Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well. + type: bitmask + bit: + 0: Motor 1 + 1: Motor 2 + 2: Motor 3 + 3: Motor 4 + 4: Motor 5 + 5: Motor 6 + 6: Motor 7 + 7: Motor 8 + 8: Motor 9 + 9: Motor 10 + 10: Motor 11 + 11: Motor 12 + default: 0 + CA_R${i}_SLEW: + description: + short: Motor ${i} slew rate limit + long: | + Minimum time allowed for the motor input signal to pass through + the full output range. A value x means that the motor signal + can only go from 0 to 1 in minimum x seconds (in case of + reversible motors, the range is -1 to 1). + + Zero means that slew rate limiting is disabled. + type: float + decimal: 2 + increment: 0.01 + num_instances: *max_num_mc_motors + min: 0 + max: 10 + default: 0.0 + + # Servo params + CA_SV${i}_SLEW: + description: + short: Servo ${i} slew rate limit + long: | + Minimum time allowed for the servo input signal to pass through + the full output range. A value x means that the servo signal + can only go from -1 to 1 in minimum x seconds. + + Zero means that slew rate limiting is disabled. + type: float + decimal: 2 + increment: 0.05 + num_instances: *max_num_servos + min: 0 + max: 10 + default: 0.0 + + + # (MC) Rotors + CA_ROTOR_COUNT: + description: + short: Total number of rotors + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + 9: '9' + 10: '10' + 11: '11' + 12: '12' + default: 0 + CA_ROTOR${i}_PX: + description: + short: Position of rotor ${i} along X body axis relative to center of gravity + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_PY: + description: + short: Position of rotor ${i} along Y body axis relative to center of gravity + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_PZ: + description: + short: Position of rotor ${i} along Z body axis relative to center of gravity + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + + CA_ROTOR${i}_AX: + description: + short: Axis of rotor ${i} thrust vector, X body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_AY: + description: + short: Axis of rotor ${i} thrust vector, Y body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: 0.0 + CA_ROTOR${i}_AZ: + description: + short: Axis of rotor ${i} thrust vector, Z body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_mc_motors + min: -100 + max: 100 + default: -1.0 + + CA_ROTOR${i}_CT: + description: + short: Thrust coefficient of rotor ${i} + long: | + The thrust coefficient if defined as Thrust = CT * u^2, + where u (with value between actuator minimum and maximum) + is the output signal sent to the motor controller. + type: float + decimal: 1 + increment: 1 + num_instances: *max_num_mc_motors + min: 0 + max: 100 + default: 6.5 + + CA_ROTOR${i}_KM: + description: + short: Moment coefficient of rotor ${i} + long: | + The moment coefficient if defined as Torque = KM * Thrust. + + Use a positive value for a rotor with CCW rotation. + Use a negative value for a rotor with CW rotation. + type: float + decimal: 3 + increment: 0.01 + num_instances: *max_num_mc_motors + min: -1 + max: 1 + default: 0.05 + CA_ROTOR${i}_TILT: + description: + short: Rotor ${i} tilt assignment + long: If not set to None, this motor is tilted by the configured tilt servo. + type: enum + values: + 0: 'None' + 1: 'Tilt 1' + 2: 'Tilt 2' + 3: 'Tilt 3' + 4: 'Tilt 4' + num_instances: *max_num_mc_motors + instance_start: 0 + default: 0 + + # control surfaces + CA_SV_CS_COUNT: + description: + short: Total number of Control Surfaces + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + default: 0 + CA_SV_CS${i}_TYPE: + description: + short: Control Surface ${i} type + type: enum + values: + 0: (Not set) + 1: Left Aileron + 2: Right Aileron + 3: Elevator + 4: Rudder + 5: Left Elevon + 6: Right Elevon + 7: Left V-Tail + 8: Right V-Tail + 9: Left Flap + 10: Right Flap + 11: Airbrake + 12: Custom + 13: Left A-tail + 14: Right A-tail + 15: Single Channel Aileron + 16: Steering Wheel + 17: Left Spoiler + 18: Right Spoiler + num_instances: *max_num_servos + instance_start: 0 + default: 0 + + CA_SV_CS${i}_TRQ_R: + description: + short: Control Surface ${i} roll torque scaling + type: float + decimal: 2 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_TRQ_P: + description: + short: Control Surface ${i} pitch torque scaling + type: float + decimal: 2 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_TRQ_Y: + description: + short: Control Surface ${i} yaw torque scaling + type: float + decimal: 2 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_TRIM: + description: + short: Control Surface ${i} trim + long: Can be used to add an offset to the servo control. + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_servos + instance_start: 0 + default: 0.0 + + CA_SV_CS${i}_FLAP: + description: + short: Control Surface ${i} configuration as flap + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_servos + instance_start: 0 + default: 0 + + CA_SV_CS${i}_SPOIL: + description: + short: Control Surface ${i} configuration as spoiler + type: float + decimal: 2 + min: -1.0 + max: 1.0 + num_instances: *max_num_servos + instance_start: 0 + default: 0 + + # Tilts + CA_SV_TL_COUNT: + description: + short: Total number of Tilt Servos + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + default: 0 + CA_SV_TL${i}_CT: + description: + short: Tilt ${i} is used for control + long: Define if this servo is used for additional control. + type: enum # This could be a bitset, but the enum shows up nicer in the UI + values: + 0: 'None' + 1: 'Yaw' + 2: 'Pitch' + 3: 'Yaw and Pitch' + num_instances: *max_num_tilts + instance_start: 0 + default: 1 + CA_SV_TL${i}_MINA: + description: + short: Tilt Servo ${i} Tilt Angle at Minimum + long: | + Defines the tilt angle when the servo is at the minimum. + An angle of zero means upwards. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_tilts + instance_start: 0 + min: -90.0 + max: 90.0 + default: 0.0 + CA_SV_TL${i}_MAXA: + description: + short: Tilt Servo ${i} Tilt Angle at Maximum + long: | + Defines the tilt angle when the servo is at the maximum. + An angle of zero means upwards. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_tilts + instance_start: 0 + min: -90.0 + max: 90.0 + default: 90.0 + CA_SV_TL${i}_TD: + description: + short: Tilt Servo ${i} Tilt Direction + long: | + Defines the direction the servo tilts towards when moving towards the maximum tilt angle. + For example if the minimum tilt angle is -90, the maximum 90, and the direction 'Towards Front', + the motor axis aligns with the XZ-plane, points towards -X at the minimum and +X at the maximum tilt. + type: enum + values: + 0: 'Towards Front' + 90: 'Towards Right' + min: 0 + max: 359 + num_instances: *max_num_tilts + instance_start: 0 + default: 0 + + # helicopter + CA_SP0_COUNT: + description: + short: Number of swash plates servos + type: enum + values: + 2: '2' + 3: '3' + 4: '4' + default: 3 + CA_SP0_ANG${i}: + description: + short: Angle for swash plate servo ${i} + long: | + The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis). + type: float + decimal: 0 + increment: 10 + unit: deg + num_instances: 4 + min: 0 + max: 360 + default: [0, 140, 220, 0] + CA_SP0_ARM_L${i}: + description: + short: Arm length for swash plate servo ${i} + long: | + This is relative to the other arm lengths. + type: float + decimal: 3 + increment: 0.1 + num_instances: 4 + min: 0 + max: 10 + default: 1.0 + CA_HELI_THR_C${i}: + description: + short: Throttle curve at position ${i} + long: | + Defines the output throttle at the interval position ${i}. + type: float + decimal: 3 + increment: 0.1 + num_instances: 5 + min: 0 + max: 1 + default: [1, 1, 1, 1, 1] + CA_HELI_PITCH_C${i}: + description: + short: Collective pitch curve at position ${i} + long: | + Defines the collective pitch at the interval position ${i} for a given thrust setpoint. + + Use negative values if the swash plate needs to move down to provide upwards thrust. + type: float + decimal: 3 + increment: 0.1 + num_instances: 5 + min: -1 + max: 1 + default: [-0.05, 0.0725, 0.2, 0.325, 0.45] + CA_HELI_YAW_CP_S: + description: + short: Scale for yaw compensation based on collective pitch + long: | + This allows to add a proportional factor of the collective pitch command to the yaw command. + A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction. + + tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O) + type: float + decimal: 3 + increment: 0.1 + min: -2 + max: 2 + default: 0.0 + CA_HELI_YAW_CP_O: + description: + short: Offset for yaw compensation based on collective pitch + long: | + This allows to specify which collective pitch command results in the least amount of rotor drag. + This is used to increase the accuracy of the yaw drag torque compensation based on collective pitch + by aligning the lowest rotor drag with zero compensation. + For symmetric profile blades this is the command that results in exactly 0° collective blade angle. + For lift profile blades this is typically a command resulting in slightly negative collective blade angle. + + tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O) + type: float + decimal: 3 + increment: 0.1 + min: -2 + max: 2 + default: 0.0 + CA_HELI_YAW_TH_S: + description: + short: Scale for yaw compensation based on throttle + long: | + This allows to add a proportional factor of the throttle command to the yaw command. + A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction. + + tail_output += CA_HELI_YAW_TH_S * throttle + type: float + decimal: 3 + increment: 0.1 + min: -2 + max: 2 + default: 0.0 + CA_HELI_YAW_CCW: + description: + short: Main rotor turns counter-clockwise + long: | + Default configuration is for a clockwise turning main rotor and + positive thrust of the tail rotor is expected to rotate the vehicle clockwise. + Set this parameter to true if the tail rotor provides thrust in counter-clockwise direction + which is mostly the case when the main rotor turns counter-clockwise. + type: boolean + default: 0 + + # Others + CA_FAILURE_MODE: + description: + short: Motor failure handling mode + long: | + This is used to specify how to handle motor failures + reported by failure detector. + type: enum + values: + 0: Ignore + 1: Remove first failed motor from effectiveness + default: 0 + +# Mixer +mixer: + actuator_types: + motor: + functions: 'Motor' + actuator_testing_values: + min: 0 + max: 1 + default_is_nan: true + per_item_parameters: + - name: 'CA_R_REV' + label: 'Bidirectional' + show_as: 'bitset' + index_offset: 0 + advanced: true + - name: 'CA_R${i}_SLEW' + label: 'Slew Rate' + index_offset: 0 + advanced: true + servo: + functions: 'Servo' + actuator_testing_values: + min: -1 + max: 1 + default: 0 + per_item_parameters: + - name: 'CA_SV${i}_SLEW' + label: 'Slew Rate' + index_offset: 0 + advanced: true + DEFAULT: + actuator_testing_values: + min: -1 + max: 1 + default: -1 + + rules: + - select_identifier: 'servo-type' # restrict torque based on servo type + apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler'] + items: + # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive. + # By default the scale is set to 1/N, where N is the amount of actuators with an effect on + # the corresponding axis. With that we are using all the available servo moving range. + 0: # Not set + - { 'disabled': True, 'default': 0.0 } # roll + - { 'disabled': True, 'default': 0.0 } # pitch + - { 'disabled': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 1: # Left Aileron + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 2: # Right Aileron + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 3: # Elevator + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 4: # Rudder + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 5: # Left Elevon + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 6: # Right Elevon + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 7: # Left V Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 8: # Right V Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 9: # Left Flap + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 10: # Right Flap + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 11: # Airbrake + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 12: # Custom + - { 'hidden': False, 'default': 0.0 } # roll + - { 'hidden': False, 'default': 0.0 } # pitch + - { 'hidden': False, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 13: # Left A Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 14: # Right A Tail + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 15: # Single Channel Aileron + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 16: # Steering Wheel + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': True, 'min': -1.0, 'max': 1.0, 'default': 0} # spoiler + 17: # Left Spoiler + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler + 18: # Right Spoiler + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 0} # flap + - { 'hidden': False, 'min': -1.0, 'max': 1.0, 'default': 1} # spoiler + + + - select_identifier: 'servo-type-tailsitter' # restrict torque based on servo type for tailsitters + apply_identifiers: ['servo-torque-roll-tailsitter', 'servo-torque-pitch-tailsitter', 'servo-torque-yaw-tailsitter'] + items: + # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive. + # No rules for control surfaces on tailsitter beside elevons. + 5: # Left Elevon (Tailsitter --> wing vertical, so affects pitch and yaw) + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # yaw + 6: # Right Elevon (Tailsitter -->wing vertical, so affects pitch and yaw) + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 0.5 } # pitch + - { 'min': -1.0, 'max': 0.0, 'default': -0.5 } # yaw + + config: + param: CA_AIRFRAME + types: + 0: # Multirotor + type: 'multirotor' + title: 'Multirotor' + actuators: + - actuator_type: 'motor' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true + + 1: # Fixed Wing + title: 'Fixed Wing' + actuators: + - actuator_type: 'motor' + group_label: 'Motors' + count: 'CA_ROTOR_COUNT' + - 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' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + + 2: # Standard VTOL + title: 'Standard VTOL' + 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' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + - 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 Scale' + identifier: 'servo-torque-roll' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + identifier: 'servo-torque-pitch' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + parameters: + - label: 'Lock control surfaces in hover' + name: VT_ELEV_MC_LOCK + + 3: # Tiltrotor VTOL + title: 'Tiltrotor VTOL' + actuators: + - actuator_type: 'motor' + group_label: 'MC Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_TILT' + label: 'Tilted by' + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - actuator_type: 'servo' + group_label: 'Control Surfaces' + count: 'CA_SV_CS_COUNT' + item_label_prefix: 'Surface ${i}' + 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 Scale' + identifier: 'servo-torque-roll' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + identifier: 'servo-torque-pitch' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + identifier: 'servo-torque-yaw' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + parameters: + - label: 'Lock control surfaces in hover' + name: VT_ELEV_MC_LOCK + - actuator_type: 'servo' + group_label: 'Tilt Servos' + count: 'CA_SV_TL_COUNT' + item_label_prefix: 'Tilt ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_TL${i}_MINA' + label: "Angle at Min\nTilt" + - name: 'CA_SV_TL${i}_MAXA' + label: "Angle at Max\nTilt" + - name: 'CA_SV_TL${i}_TD' + label: 'Tilt Direction' + - name: 'CA_SV_TL${i}_CT' + label: 'Use for Control' + + 4: # Tailsitter VTOL + title: 'Tailsitter VTOL' + actuators: + - actuator_type: 'motor' + group_label: 'MC Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - actuator_type: 'servo' + group_label: 'Control Surfaces' + count: 'CA_SV_CS_COUNT' + item_label_prefix: 'Surface ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TYPE' + label: 'Type' + function: 'type' + identifier: 'servo-type-tailsitter' + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Scale' + identifier: 'servo-torque-roll-tailsitter' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + identifier: 'servo-torque-pitch-tailsitter' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + identifier: 'servo-torque-yaw-tailsitter' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + parameters: + - label: 'Lock control surfaces in hover' + name: VT_ELEV_MC_LOCK + + 5: # Rover (Ackermann) + title: 'Rover (Ackermann)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Throttle' + position: [ -1, 0, 0 ] + - actuator_type: 'servo' + instances: + - name: 'Steering' + position: [ 1, 0, 0 ] + + 6: # Rover (Differential) + title: 'Rover (Differential)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Right Motor' + position: [ 0, 1, 0 ] + - name: 'Left Motor' + position: [ 0, -1, 0 ] + + 7: # Motors (6DOF) + actuators: + - actuator_type: 'motor' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + - name: 'CA_ROTOR${i}_KM' + label: "Moment\nCoefficient" + + 8: # Multirotor with Tilt + title: 'Multirotor with Tilt' + actuators: + - actuator_type: 'motor' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_KM' + label: 'Direction CCW' + function: 'spin-dir' + show_as: 'true-if-positive' + - name: 'CA_ROTOR${i}_TILT' + label: 'Tilted by' + - actuator_type: 'servo' + group_label: 'Tilt Servos' + count: 'CA_SV_TL_COUNT' + item_label_prefix: 'Tilt ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_TL${i}_MINA' + label: "Angle at Min\nTilt" + - name: 'CA_SV_TL${i}_MAXA' + label: "Angle at Max\nTilt" + - name: 'CA_SV_TL${i}_TD' + label: 'Tilt Direction' + - name: 'CA_SV_TL${i}_CT' + label: 'Use for Control' + + 9: # Custom + actuators: + - actuator_type: 'motor' + group_label: 'Thrust/Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + extra: + - name: 'CA_ROTOR${i}_AX' + label: 'Axis X' + function: 'axisx' + - name: 'CA_ROTOR${i}_AY' + label: 'Axis Y' + function: 'axisy' + - name: 'CA_ROTOR${i}_AZ' + label: 'Axis Z' + function: 'axisz' + - name: 'CA_ROTOR${i}_CT' + label: "Thrust\nCoefficient" + - name: 'CA_ROTOR${i}_KM' + label: "Moment\nCoefficient" + - actuator_type: 'servo' + group_label: 'Torque' + count: 'CA_SV_CS_COUNT' + item_label_prefix: 'Torque ${i}' + per_item_parameters: + extra: + - name: 'CA_SV_CS${i}_TRQ_R' + label: 'Roll Scale' + - name: 'CA_SV_CS${i}_TRQ_P' + label: 'Pitch Scale' + - name: 'CA_SV_CS${i}_TRQ_Y' + label: 'Yaw Scale' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + - name: 'CA_SV_CS${i}_FLAP' + label: 'Flaps Scale' + advanced: true + identifier: 'servo-scale-flap' + - name: 'CA_SV_CS${i}_SPOIL' + label: 'Spoiler Scale' + advanced: true + identifier: 'servo-scale-spoiler' + + 10: # Helicopter (tail ESC) + actuators: + - actuator_type: 'motor' + count: 1 + item_label_prefix: ['Rotor'] + - actuator_type: 'motor' + item_label_prefix: ['Yaw tail Motor'] + count: 1 + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Collective pitch offset to align least amount of rotor drag' + name: CA_HELI_YAW_CP_O + - label: 'Yaw compensation scale based on collective pitch' + name: CA_HELI_YAW_CP_S + - label: 'Yaw compensation scale based on throttle' + name: CA_HELI_YAW_TH_S + - label: 'Main rotor turns counter-clockwise' + name: CA_HELI_YAW_CCW + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME + + 11: # Helicopter (tail Servo) + actuators: + - actuator_type: 'motor' + count: 1 + item_label_prefix: ['Rotor'] + - actuator_type: 'servo' + item_label_prefix: ['Yaw tail Servo'] + count: 1 + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Yaw compensation scale based on collective pitch' + name: CA_HELI_YAW_CP_S + - label: 'Yaw compensation scale based on throttle' + name: CA_HELI_YAW_TH_S + - label: 'Main rotor turns counter-clockwise' + name: CA_HELI_YAW_CCW + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME + + 12: # Helicopter (Coaxial) + actuators: + - actuator_type: 'motor' + count: 2 + item_label_prefix: ['Clockwise Rotor', 'Counter-clockwise Rotor'] + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME + + 13: # Rover (Mecanum) + title: 'Rover (Mecanum)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Right Motor Front' + position: [ 1, 1, 0 ] + - name: 'Left Motor Front' + position: [ 1, -1, 0 ] + - name: 'Right Motor Back' + position: [ -1, 1, 0 ] + - name: 'Left Motor Back' + position: [ -1, -1, 0 ]