diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc index 64f7e9939a..7871be2329 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc @@ -14,19 +14,19 @@ param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 0 -param set-default CA_MC_R_COUNT 4 -param set-default CA_MC_R0_PX 0.1515 -param set-default CA_MC_R0_PY 0.245 -param set-default CA_MC_R0_KM 0.05 -param set-default CA_MC_R1_PX -0.1515 -param set-default CA_MC_R1_PY -0.1875 -param set-default CA_MC_R1_KM 0.05 -param set-default CA_MC_R2_PX 0.1515 -param set-default CA_MC_R2_PY -0.245 -param set-default CA_MC_R2_KM -0.05 -param set-default CA_MC_R3_PX -0.1515 -param set-default CA_MC_R3_PY 0.1875 -param set-default CA_MC_R3_KM -0.05 +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.1515 +param set-default CA_ROTOR0_PY 0.245 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.1515 +param set-default CA_ROTOR1_PY -0.1875 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.1515 +param set-default CA_ROTOR2_PY -0.245 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.1515 +param set-default CA_ROTOR3_PY 0.1875 +param set-default CA_ROTOR3_KM -0.05 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc index 6a20e0f863..39e52f2243 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc @@ -30,26 +30,26 @@ param set-default MNT_MODE_IN 0 param set-default MAV_PROTO_VER 2 param set-default CA_AIRFRAME 0 -param set-default CA_MC_R_COUNT 6 +param set-default CA_ROTOR_COUNT 6 -param set-default CA_MC_R0_PX 0.0 -param set-default CA_MC_R0_PY 1.0 -param set-default CA_MC_R0_KM -0.05 -param set-default CA_MC_R1_PX 0.0 -param set-default CA_MC_R1_PY -1.0 -param set-default CA_MC_R1_KM 0.05 -param set-default CA_MC_R2_PX 0.866025 -param set-default CA_MC_R2_PY -0.5 -param set-default CA_MC_R2_KM -0.05 -param set-default CA_MC_R3_PX -0.866025 -param set-default CA_MC_R3_PY 0.5 -param set-default CA_MC_R3_KM 0.05 -param set-default CA_MC_R4_PX 0.866025 -param set-default CA_MC_R4_PY 0.5 -param set-default CA_MC_R4_KM 0.05 -param set-default CA_MC_R5_PX -0.866025 -param set-default CA_MC_R5_PY -0.5 -param set-default CA_MC_R5_KM -0.05 +param set-default CA_ROTOR0_PX 0.0 +param set-default CA_ROTOR0_PY 1.0 +param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR1_PX 0.0 +param set-default CA_ROTOR1_PY -1.0 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.866025 +param set-default CA_ROTOR2_PY -0.5 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.866025 +param set-default CA_ROTOR3_PY 0.5 +param set-default CA_ROTOR3_KM 0.05 +param set-default CA_ROTOR4_PX 0.866025 +param set-default CA_ROTOR4_PY 0.5 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR5_PX -0.866025 +param set-default CA_ROTOR5_PY -0.5 +param set-default CA_ROTOR5_KM -0.05 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc index 074f91c9aa..e2c2432c92 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc @@ -19,19 +19,19 @@ param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 0 -param set-default CA_MC_R_COUNT 4 -param set-default CA_MC_R0_PX 0.177 -param set-default CA_MC_R0_PY 0.177 -param set-default CA_MC_R0_KM 0.05 -param set-default CA_MC_R1_PX -0.177 -param set-default CA_MC_R1_PY -0.177 -param set-default CA_MC_R1_KM 0.05 -param set-default CA_MC_R2_PX 0.177 -param set-default CA_MC_R2_PY -0.177 -param set-default CA_MC_R2_KM -0.05 -param set-default CA_MC_R3_PX -0.177 -param set-default CA_MC_R3_PY 0.177 -param set-default CA_MC_R3_KM -0.05 +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.177 +param set-default CA_ROTOR0_PY 0.177 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.177 +param set-default CA_ROTOR1_PY -0.177 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.177 +param set-default CA_ROTOR2_PY -0.177 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.177 +param set-default CA_ROTOR3_PY 0.177 +param set-default CA_ROTOR3_KM -0.05 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc index 00ed015524..51185a1ab5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc @@ -15,31 +15,31 @@ param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 0 -param set-default CA_MC_R_COUNT 6 +param set-default CA_ROTOR_COUNT 6 -param set-default CA_MC_R0_PX 0.0 -param set-default CA_MC_R0_PY 0.275 -param set-default CA_MC_R0_KM -0.05 +param set-default CA_ROTOR0_PX 0.0 +param set-default CA_ROTOR0_PY 0.275 +param set-default CA_ROTOR0_KM -0.05 -param set-default CA_MC_R1_PX 0.0 -param set-default CA_MC_R1_PY -0.275 -param set-default CA_MC_R1_KM 0.05 +param set-default CA_ROTOR1_PX 0.0 +param set-default CA_ROTOR1_PY -0.275 +param set-default CA_ROTOR1_KM 0.05 -param set-default CA_MC_R2_PX 0.238 -param set-default CA_MC_R2_PY -0.1375 -param set-default CA_MC_R2_KM -0.05 +param set-default CA_ROTOR2_PX 0.238 +param set-default CA_ROTOR2_PY -0.1375 +param set-default CA_ROTOR2_KM -0.05 -param set-default CA_MC_R3_PX -0.238 -param set-default CA_MC_R3_PY 0.1375 -param set-default CA_MC_R3_KM 0.05 +param set-default CA_ROTOR3_PX -0.238 +param set-default CA_ROTOR3_PY 0.1375 +param set-default CA_ROTOR3_KM 0.05 -param set-default CA_MC_R4_PX 0.238 -param set-default CA_MC_R4_PY 0.1375 -param set-default CA_MC_R4_KM 0.05 +param set-default CA_ROTOR4_PX 0.238 +param set-default CA_ROTOR4_PY 0.1375 +param set-default CA_ROTOR4_KM 0.05 -param set-default CA_MC_R5_PX -0.238 -param set-default CA_MC_R5_PY -0.1375 -param set-default CA_MC_R5_KM -0.05 +param set-default CA_ROTOR5_PX -0.238 +param set-default CA_ROTOR5_PY -0.1375 +param set-default CA_ROTOR5_KM -0.05 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp new file mode 100644 index 0000000000..e8b2b3bea6 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index ad8714eda0..94fc7cac23 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -41,18 +41,36 @@ #pragma once -#include - #include +enum class AllocationMethod { + NONE = -1, + PSEUDO_INVERSE = 0, + SEQUENTIAL_DESATURATION = 1, + AUTO = 2, +}; + +enum class ActuatorType { + MOTORS = 0, + SERVOS, + + COUNT +}; + + class ActuatorEffectiveness { public: ActuatorEffectiveness() = default; virtual ~ActuatorEffectiveness() = default; - static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; - static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES; + static constexpr int NUM_ACTUATORS = 16; + static constexpr int NUM_AXES = 6; + + static constexpr int MAX_NUM_MATRICES = 2; + + using EffectivenessMatrix = matrix::Matrix; + using ActuatorVector = matrix::Vector; enum class FlightPhase { HOVER_FLIGHT = 0, @@ -61,6 +79,31 @@ public: 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]; + + int selected_matrix; + + uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES]; + int num_actuators[(int)ActuatorType::COUNT]; + }; + /** * Set the current flight phase * @@ -71,22 +114,28 @@ public: _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; + } + } + /** * Get the control effectiveness matrix if updated * * @return true if updated and matrix is set */ - virtual bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) = 0; - - /** - * Get the actuator trims - * - * @return Actuator trims - */ - const matrix::Vector &getActuatorTrim() const - { - return _trim; - } + virtual bool getEffectivenessMatrix(Configuration &configuration, bool force) = 0; /** * Get the current flight phase @@ -98,17 +147,26 @@ public: return _flight_phase; } - /** - * Get the number of actuators - */ - virtual int numActuators() const = 0; - /** * Display name */ virtual const char *name() const = 0; + + /** + * 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) {} + + /** + * Get a bitmask of motors to be stopped + */ + virtual uint32_t getStoppedMotors() const { return 0; } + protected: - matrix::Vector _trim; ///< Actuator trim FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp new file mode 100644 index 0000000000..66163891b3 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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); + } + + _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)); + } + + // TODO: enforce limits? + switch (_params[i].type) { + 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::LeftFlaps: + case Type::RightFlaps: + torque.setZero(); + break; + + case Type::LeftAileron: + break; + + case Type::RightAileron: + break; + + case Type::Airbrakes: + torque.setZero(); + break; + + case Type::Custom: + break; + } + } +} + +bool ActuatorEffectivenessControlSurfaces::getEffectivenessMatrix(Configuration &configuration, bool force) +{ + if (!force) { + return false; + } + + for (int i = 0; i < _count; i++) { + configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{}); + } + + return true; +} + +void ActuatorEffectivenessControlSurfaces::applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control, + int first_actuator_idx, + ActuatorVector &actuator_sp) const +{ + for (int i = 0; i < _count; ++i) { + switch (_params[i].type) { + // TODO: check sign + case ActuatorEffectivenessControlSurfaces::Type::LeftFlaps: + actuator_sp(i + first_actuator_idx) += flaps_control; + break; + + case ActuatorEffectivenessControlSurfaces::Type::RightFlaps: + actuator_sp(i + first_actuator_idx) -= flaps_control; + break; + + case ActuatorEffectivenessControlSurfaces::Type::Airbrakes: + actuator_sp(i + first_actuator_idx) += airbrakes_control; + break; + + default: + break; + } + } + +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp new file mode 100644 index 0000000000..58875aad72 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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 + +class ActuatorEffectivenessControlSurfaces : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int MAX_COUNT = 8; + + enum class Type : int32_t { + // This matches with the parameter + Elevator = 1, + Rudder = 2, + LeftElevon = 3, + RightElevon = 4, + LeftVTail = 5, + RightVTail = 6, + LeftFlaps = 7, + RightFlaps = 8, + LeftAileron = 9, + RightAileron = 10, + Airbrakes = 11, + Custom = 12, + }; + + struct Params { + Type type; + + matrix::Vector3f torque; + }; + + ActuatorEffectivenessControlSurfaces(ModuleParams *parent); + virtual ~ActuatorEffectivenessControlSurfaces() = default; + + bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + + const char *name() const override { return "Control Surfaces"; } + + int count() const { return _count; } + + const Params &config(int idx) const { return _params[idx]; } + + void applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control, int first_actuator_idx, + ActuatorVector &actuator_sp) const; + +private: + void updateParams() override; + + struct ParamHandles { + param_t type; + + param_t torque[3]; + }; + ParamHandles _param_handles[MAX_COUNT]; + param_t _count_handle; + + Params _params[MAX_COUNT] {}; + int _count{0}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp deleted file mode 100644 index a9114cc533..0000000000 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/**************************************************************************** - * - * 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 ActuatorEffectivenessMultirotor.hpp - * - * Actuator effectiveness computed from rotors position and orientation - * - * @author Julien Lecoeur - */ - -#include "ActuatorEffectivenessMultirotor.hpp" - -ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(ModuleParams *parent): - ModuleParams(parent) -{ -} - -bool -ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &matrix, - bool force) -{ - if (_updated || force) { - _updated = false; - - // Get multirotor geometry - MultirotorGeometry geometry = {}; - geometry.rotors[0].position_x = _param_ca_mc_r0_px.get(); - geometry.rotors[0].position_y = _param_ca_mc_r0_py.get(); - geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get(); - geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get(); - geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get(); - geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get(); - geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get(); - geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get(); - - geometry.rotors[1].position_x = _param_ca_mc_r1_px.get(); - geometry.rotors[1].position_y = _param_ca_mc_r1_py.get(); - geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get(); - geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get(); - geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get(); - geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get(); - geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get(); - geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get(); - - geometry.rotors[2].position_x = _param_ca_mc_r2_px.get(); - geometry.rotors[2].position_y = _param_ca_mc_r2_py.get(); - geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get(); - geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get(); - geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get(); - geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get(); - geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get(); - geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get(); - - geometry.rotors[3].position_x = _param_ca_mc_r3_px.get(); - geometry.rotors[3].position_y = _param_ca_mc_r3_py.get(); - geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get(); - geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get(); - geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get(); - geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get(); - geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get(); - geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get(); - - geometry.rotors[4].position_x = _param_ca_mc_r4_px.get(); - geometry.rotors[4].position_y = _param_ca_mc_r4_py.get(); - geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get(); - geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get(); - geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get(); - geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get(); - geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get(); - geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get(); - - geometry.rotors[5].position_x = _param_ca_mc_r5_px.get(); - geometry.rotors[5].position_y = _param_ca_mc_r5_py.get(); - geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get(); - geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get(); - geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get(); - geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get(); - geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get(); - geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get(); - - geometry.rotors[6].position_x = _param_ca_mc_r6_px.get(); - geometry.rotors[6].position_y = _param_ca_mc_r6_py.get(); - geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get(); - geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get(); - geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get(); - geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get(); - geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get(); - geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get(); - - geometry.rotors[7].position_x = _param_ca_mc_r7_px.get(); - geometry.rotors[7].position_y = _param_ca_mc_r7_py.get(); - geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get(); - geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get(); - geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get(); - geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get(); - geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get(); - geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get(); - - geometry.num_rotors = _param_ca_mc_r_count.get(); - - _num_actuators = computeEffectivenessMatrix(geometry, matrix); - return true; - } - - return false; -} - -int -ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry, - matrix::Matrix &effectiveness) -{ - int num_actuators = 0; - - effectiveness.setZero(); - - for (int i = 0; i < math::min(NUM_ROTORS_MAX, geometry.num_rotors); i++) { - - // Get rotor axis - matrix::Vector3f axis( - geometry.rotors[i].axis_x, - geometry.rotors[i].axis_y, - geometry.rotors[i].axis_z - ); - - // 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 - matrix::Vector3f position( - geometry.rotors[i].position_x, - geometry.rotors[i].position_y, - geometry.rotors[i].position_z - ); - - // Get coefficients - float ct = geometry.rotors[i].thrust_coef; - float km = geometry.rotors[i].moment_ratio; - - 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) = moment(j); - effectiveness(j + 3, i) = thrust(j); - } - } - - num_actuators = geometry.num_rotors; - - return num_actuators; -} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp deleted file mode 100644 index 678ed40a09..0000000000 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ /dev/null @@ -1,163 +0,0 @@ -/**************************************************************************** - * - * 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 ActuatorEffectivenessMultirotor.hpp - * - * Actuator effectiveness computed from rotors position and orientation - * - * @author Julien Lecoeur - */ - -#pragma once - -#include "ActuatorEffectiveness.hpp" - -#include -#include -#include - -using namespace time_literals; - -class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness -{ -public: - ActuatorEffectivenessMultirotor(ModuleParams *parent); - virtual ~ActuatorEffectivenessMultirotor() = default; - - static constexpr int NUM_ROTORS_MAX = 8; - - struct RotorGeometry { - float position_x; - float position_y; - float position_z; - float axis_x; - float axis_y; - float axis_z; - float thrust_coef; - float moment_ratio; - }; - - struct MultirotorGeometry { - RotorGeometry rotors[NUM_ROTORS_MAX]; - int num_rotors{0}; - }; - - static int computeEffectivenessMatrix(const MultirotorGeometry &geometry, - matrix::Matrix &effectiveness); - - bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; - - int numActuators() const override { return _num_actuators; } - - const char *name() const override { return "Multirotor"; } -private: - bool _updated{true}; - int _num_actuators{0}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_ca_mc_r0_px, - (ParamFloat) _param_ca_mc_r0_py, - (ParamFloat) _param_ca_mc_r0_pz, - (ParamFloat) _param_ca_mc_r0_ax, - (ParamFloat) _param_ca_mc_r0_ay, - (ParamFloat) _param_ca_mc_r0_az, - (ParamFloat) _param_ca_mc_r0_ct, - (ParamFloat) _param_ca_mc_r0_km, - - (ParamFloat) _param_ca_mc_r1_px, - (ParamFloat) _param_ca_mc_r1_py, - (ParamFloat) _param_ca_mc_r1_pz, - (ParamFloat) _param_ca_mc_r1_ax, - (ParamFloat) _param_ca_mc_r1_ay, - (ParamFloat) _param_ca_mc_r1_az, - (ParamFloat) _param_ca_mc_r1_ct, - (ParamFloat) _param_ca_mc_r1_km, - - (ParamFloat) _param_ca_mc_r2_px, - (ParamFloat) _param_ca_mc_r2_py, - (ParamFloat) _param_ca_mc_r2_pz, - (ParamFloat) _param_ca_mc_r2_ax, - (ParamFloat) _param_ca_mc_r2_ay, - (ParamFloat) _param_ca_mc_r2_az, - (ParamFloat) _param_ca_mc_r2_ct, - (ParamFloat) _param_ca_mc_r2_km, - - (ParamFloat) _param_ca_mc_r3_px, - (ParamFloat) _param_ca_mc_r3_py, - (ParamFloat) _param_ca_mc_r3_pz, - (ParamFloat) _param_ca_mc_r3_ax, - (ParamFloat) _param_ca_mc_r3_ay, - (ParamFloat) _param_ca_mc_r3_az, - (ParamFloat) _param_ca_mc_r3_ct, - (ParamFloat) _param_ca_mc_r3_km, - - (ParamFloat) _param_ca_mc_r4_px, - (ParamFloat) _param_ca_mc_r4_py, - (ParamFloat) _param_ca_mc_r4_pz, - (ParamFloat) _param_ca_mc_r4_ax, - (ParamFloat) _param_ca_mc_r4_ay, - (ParamFloat) _param_ca_mc_r4_az, - (ParamFloat) _param_ca_mc_r4_ct, - (ParamFloat) _param_ca_mc_r4_km, - - (ParamFloat) _param_ca_mc_r5_px, - (ParamFloat) _param_ca_mc_r5_py, - (ParamFloat) _param_ca_mc_r5_pz, - (ParamFloat) _param_ca_mc_r5_ax, - (ParamFloat) _param_ca_mc_r5_ay, - (ParamFloat) _param_ca_mc_r5_az, - (ParamFloat) _param_ca_mc_r5_ct, - (ParamFloat) _param_ca_mc_r5_km, - - (ParamFloat) _param_ca_mc_r6_px, - (ParamFloat) _param_ca_mc_r6_py, - (ParamFloat) _param_ca_mc_r6_pz, - (ParamFloat) _param_ca_mc_r6_ax, - (ParamFloat) _param_ca_mc_r6_ay, - (ParamFloat) _param_ca_mc_r6_az, - (ParamFloat) _param_ca_mc_r6_ct, - (ParamFloat) _param_ca_mc_r6_km, - - (ParamFloat) _param_ca_mc_r7_px, - (ParamFloat) _param_ca_mc_r7_py, - (ParamFloat) _param_ca_mc_r7_pz, - (ParamFloat) _param_ca_mc_r7_ax, - (ParamFloat) _param_ca_mc_r7_ay, - (ParamFloat) _param_ca_mc_r7_az, - (ParamFloat) _param_ca_mc_r7_ct, - (ParamFloat) _param_ca_mc_r7_km, - - (ParamInt) _param_ca_mc_r_count - ) -}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp new file mode 100644 index 0000000000..8c8caa1701 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * 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 + */ + +#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); + } + } + + _count_handle = param_find("CA_ROTOR_COUNT"); + + updateParams(); +} + +void ActuatorEffectivenessRotors::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_count_handle, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_rotors = count; + + 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::getEffectivenessMatrix(Configuration &configuration, bool force) +{ + if (_updated || force) { + _updated = false; + + 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; + } + + return false; +} + +int +ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index) +{ + int num_actuators = 0; + + for (int i = 0; i < math::min(NUM_ROTORS_MAX, 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.yaw_disabled) { + 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); + } + } + + return num_actuators; +} + +uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control) +{ + if (!PX4_ISFINITE(tilt_control)) { + 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); + float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f); + 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; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp new file mode 100644 index 0000000000..a60efb0e76 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -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 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 = 8; + + 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 yaw_disabled{false}; + }; + + ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, + bool tilt_support = false); + virtual ~ActuatorEffectivenessRotors() = default; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + static int computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index = 0); + + bool getEffectivenessMatrix(Configuration &configuration, bool force) override; + + const char *name() const override { return "Multirotor"; } + + /** + * 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 enableYawControl(bool enable) { _geometry.yaw_disabled = !enable; } + +private: + void updateParams() override; + + bool _updated{true}; + 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]; + param_t _count_handle; + + Geometry _geometry{}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp similarity index 54% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp rename to src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp index 69b60caf34..603248ab9d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file ActuatorEffectivenessMultirotorTest.cpp + * @file ActuatorEffectivenessRotors.cpp * * Tests for Control Allocation Algorithms * @@ -40,52 +40,55 @@ */ #include -#include +#include "ActuatorEffectivenessRotors.hpp" using namespace matrix; -TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) +TEST(ActuatorEffectivenessRotors, AllZeroCase) { // Quad wide geometry - ActuatorEffectivenessMultirotor::MultirotorGeometry geometry = {}; - geometry.rotors[0].position_x = 1.0f; - geometry.rotors[0].position_y = 1.0f; - geometry.rotors[0].position_z = 0.0f; - geometry.rotors[0].axis_x = 0.0f; - geometry.rotors[0].axis_y = 0.0f; - geometry.rotors[0].axis_z = -1.0f; + 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_x = -1.0f; - geometry.rotors[1].position_y = -1.0f; - geometry.rotors[1].position_z = 0.0f; - geometry.rotors[1].axis_x = 0.0f; - geometry.rotors[1].axis_y = 0.0f; - geometry.rotors[1].axis_z = -1.0f; + 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_x = 1.0f; - geometry.rotors[2].position_y = -1.0f; - geometry.rotors[2].position_z = 0.0f; - geometry.rotors[2].axis_x = 0.0f; - geometry.rotors[2].axis_y = 0.0f; - geometry.rotors[2].axis_z = -1.0f; + 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_x = -1.0f; - geometry.rotors[3].position_y = 1.0f; - geometry.rotors[3].position_z = 0.0f; - geometry.rotors[3].axis_x = 0.0f; - geometry.rotors[3].axis_y = 0.0f; - geometry.rotors[3].axis_z = -1.0f; + 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; - matrix::Matrix effectiveness = - ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry); + 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}, @@ -95,8 +98,38 @@ TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) { 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} }; - matrix::Matrix effectiveness_expected( - expected); + 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/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index beb36b952e..46ed2c12fe 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -31,79 +31,76 @@ * ****************************************************************************/ -/** - * @file ActuatorEffectivenessStandardVTOL.hpp - * - * Actuator effectiveness for standard VTOL - * - * @author Julien Lecoeur - */ - #include "ActuatorEffectivenessStandardVTOL.hpp" +#include -ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() +using namespace matrix; + +ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParams *parent) + : ModuleParams(parent), _mc_rotors(this), _control_surfaces(this) { - setFlightPhase(FlightPhase::HOVER_FLIGHT); } bool -ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix, - bool force) +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configuration, bool force) { - if (!(_updated || force)) { + if (!force) { return false; } - switch (_flight_phase) { - case FlightPhase::HOVER_FLIGHT: { - const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { - {-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.25f, 0.25f, -0.25f, -0.25f, 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.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.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - matrix = matrix::Matrix(standard_vtol); - break; - } + // MC motors + configuration.selected_matrix = 0; + _mc_rotors.getEffectivenessMatrix(configuration, true); + _mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1; - case FlightPhase::FORWARD_FLIGHT: { - const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { - { 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 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.5f, 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.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} - }; - matrix = matrix::Matrix(standard_vtol); - break; - } + // Pusher/Puller + configuration.selected_matrix = 1; - case FlightPhase::TRANSITION_HF_TO_FF: - case FlightPhase::TRANSITION_FF_TO_HF: { - const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { - { -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.25f, 0.25f, -0.25f, -0.25f, 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.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.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - matrix = matrix::Matrix(standard_vtol); - break; - } + for (int i = 0; i < _param_ca_stdvtol_n_p.get(); ++i) { + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f}); } - _updated = false; + // Control Surfaces + configuration.selected_matrix = 1; + _first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; + _control_surfaces.getEffectivenessMatrix(configuration, true); + return true; } -void -ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) +void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp) { - ActuatorEffectiveness::setFlightPhase(flight_phase); - _updated = true; + // apply flaps + if (matrix_index == 1) { + actuator_controls_s actuator_controls_1; + if (_actuator_controls_1_sub.copy(&actuator_controls_1)) { + float control_flaps = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS]; + float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES]; + _control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp); + } + } +} + +void 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 = _mc_motors_mask; + break; + + case FlightPhase::HOVER_FLIGHT: + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: + _stopped_motors = 0; + break; + } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 473e522f1a..88e6c70a18 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -42,25 +42,50 @@ #pragma once #include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" -class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness +#include + +class ActuatorEffectivenessStandardVTOL : public ModuleParams, public ActuatorEffectiveness { public: - ActuatorEffectivenessStandardVTOL(); + ActuatorEffectivenessStandardVTOL(ModuleParams *parent); virtual ~ActuatorEffectivenessStandardVTOL() = default; - bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; - - /** - * Set the current flight phase - * - * @param Flight phase - */ - void setFlightPhase(const FlightPhase &flight_phase) override; - - int numActuators() const override { return 7; } + bool getEffectivenessMatrix(Configuration &configuration, bool force) override; const char *name() const override { return "Standard VTOL"; } -protected: - bool _updated{true}; + + 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 updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp) override; + + void setFlightPhase(const FlightPhase &flight_phase) override; + + uint32_t getStoppedMotors() const override { return _stopped_motors; } + +private: + ActuatorEffectivenessRotors _mc_rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight) + uint32_t _stopped_motors{}; ///< currently stopped motors + + int _first_control_surface_idx{0}; ///< applies to matrix 1 + + uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_stdvtol_n_p ///< number of pushers + ) + }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index d3baed187b..bd72b7398c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -41,76 +41,98 @@ #include "ActuatorEffectivenessTiltrotorVTOL.hpp" -ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() +using namespace matrix; + +ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent) + : ModuleParams(parent), + _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true), + _control_surfaces(this), _tilts(this) { setFlightPhase(FlightPhase::HOVER_FLIGHT); } bool -ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix, - bool force) +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, bool force) { - if (!(_updated || force)) { + if (!_updated && !force) { return false; } - // Trim - float tilt = 0.0f; + // MC motors + configuration.selected_matrix = 0; + _mc_rotors.enableYawControl(!_tilts.hasYawControl()); + _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, _last_tilt_control) + << configuration.num_actuators[(int)ActuatorType::MOTORS]; + _mc_rotors.getEffectivenessMatrix(configuration, true); - switch (_flight_phase) { - case FlightPhase::HOVER_FLIGHT: { - tilt = 0.0f; - break; - } - - case FlightPhase::FORWARD_FLIGHT: { - tilt = 1.5f; - break; - } - - case FlightPhase::TRANSITION_FF_TO_HF: - case FlightPhase::TRANSITION_HF_TO_FF: { - tilt = 1.0f; - break; - } - } - - // Trim: half throttle, tilted motors - _trim(0) = 0.5f; - _trim(1) = 0.5f; - _trim(2) = 0.5f; - _trim(3) = 0.5f; - _trim(4) = tilt; - _trim(5) = tilt; - _trim(6) = tilt; - _trim(7) = tilt; - - // Effectiveness - const float tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = { - {-0.5f * cosf(_trim(4)), 0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), 0.5f * _trim(0) *sinf(_trim(4)), -0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f * cosf(_trim(4)), -0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), -0.5f * _trim(0) *sinf(_trim(4)), 0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.5f * sinf(_trim(4)), 0.5f * sinf(_trim(5)), 0.5f * sinf(_trim(6)), -0.5f * sinf(_trim(7)), -0.5f * _trim(0) *cosf(_trim(4)), 0.5f * _trim(1) *cosf(_trim(5)), 0.5f * _trim(2) *cosf(_trim(6)), -0.5f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.25f * sinf(_trim(4)), 0.25f * sinf(_trim(5)), 0.25f * sinf(_trim(6)), 0.25f * sinf(_trim(7)), 0.25f * _trim(0) *cosf(_trim(4)), 0.25f * _trim(1) *cosf(_trim(5)), 0.25f * _trim(2) *cosf(_trim(6)), 0.25f * _trim(3) *cosf(_trim(7)), 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.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - matrix = matrix::Matrix(tiltrotor_vtol); - - // Temporarily disable a few controls (WIP) - for (size_t j = 4; j < 8; j++) { - matrix(3, j) = 0.0f; - matrix(4, j) = 0.0f; - matrix(5, j) = 0.0f; - } + // Control Surfaces + configuration.selected_matrix = 1; + _first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; + _control_surfaces.getEffectivenessMatrix(configuration, true); + // Tilts + configuration.selected_matrix = 0; + _first_tilt_idx = configuration.num_actuators_matrix[configuration.selected_matrix]; + _tilts.updateYawSign(_mc_rotors.geometry()); + _tilts.getEffectivenessMatrix(configuration, true); _updated = false; return true; } -void -ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) +void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp) { + // apply flaps + if (matrix_index == 1) { + actuator_controls_s actuator_controls_1; + + if (_actuator_controls_1_sub.copy(&actuator_controls_1)) { + float control_flaps = -1.f; // For tilt-rotors INDEX_FLAPS is set as combined tilt. TODO: fix this + float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES]; + _control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp); + } + } + + // apply tilt + if (matrix_index == 0) { + actuator_controls_s actuator_controls_1; + + if (_actuator_controls_1_sub.copy(&actuator_controls_1)) { + float control_tilt = actuator_controls_1.control[4] * 2.f - 1.f; + + if (fabsf(control_tilt - _last_tilt_control) > 0.01f && PX4_ISFINITE(_last_tilt_control)) { + _updated = true; + } + + for (int i = 0; i < _tilts.count(); ++i) { + if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) { + actuator_sp(i + _first_tilt_idx) += control_tilt; + } + } + + _last_tilt_control = control_tilt; + } + } +} + +void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + if (_flight_phase == flight_phase) { + return; + } + ActuatorEffectiveness::setFlightPhase(flight_phase); - _updated = true; + // update stopped motors + switch (flight_phase) { + case FlightPhase::FORWARD_FLIGHT: + _stopped_motors = _nontilted_motors; + break; + + case FlightPhase::HOVER_FLIGHT: + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: + _stopped_motors = 0; + break; + } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index a947ad32b5..ab808b5cc3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -42,25 +42,51 @@ #pragma once #include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" +#include "ActuatorEffectivenessTilts.hpp" -class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness +#include +#include + +class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness { public: - ActuatorEffectivenessTiltrotorVTOL(); + ActuatorEffectivenessTiltrotorVTOL(ModuleParams *parent); virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; - bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; + bool getEffectivenessMatrix(Configuration &configuration, bool force) 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; + } - /** - * Set the current flight phase - * - * @param Flight phase - */ void setFlightPhase(const FlightPhase &flight_phase) override; - int numActuators() const override { return 10; } + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp) override; const char *name() const override { return "VTOL Tiltrotor"; } + + uint32_t getStoppedMotors() const override { return _stopped_motors; } protected: bool _updated{true}; + ActuatorEffectivenessRotors _mc_rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + ActuatorEffectivenessTilts _tilts; + + uint32_t _nontilted_motors{}; ///< motors that are not tiltable + uint32_t _stopped_motors{}; ///< currently stopped motors + + int _first_control_surface_idx{0}; ///< applies to matrix 1 + int _first_tilt_idx{0}; ///< applies to matrix 0 + + float _last_tilt_control{NAN}; + + uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp new file mode 100644 index 0000000000..9491541fc9 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * 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); + + _yaw_torque[i] = 0.f; + } +} + +bool ActuatorEffectivenessTilts::getEffectivenessMatrix(Configuration &configuration, bool force) +{ + if (!force) { + return false; + } + + for (int i = 0; i < _count; i++) { + Vector3f torque{}; + + if (_params[i].control == Control::Yaw) { + torque(2) = _yaw_torque[i]; + } + + configuration.addActuator(ActuatorType::SERVOS, torque, Vector3f{}); + } + + return true; +} + +void ActuatorEffectivenessTilts::updateYawSign(const ActuatorEffectivenessRotors::Geometry &geometry) +{ + 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) { + continue; + } + + // 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 + _yaw_torque[tilt_index] = 1.f; + + } else if (rotated_pos(1) > 0.01f) { + _yaw_torque[tilt_index] = -1.f; + } + } +} + +bool ActuatorEffectivenessTilts::hasYawControl() const +{ + for (int i = 0; i < _count; i++) { + if (_params[i].control == Control::Yaw) { + return true; + } + } + + return false; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp new file mode 100644 index 0000000000..ce3b1d2aa0 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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, + }; + 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 getEffectivenessMatrix(Configuration &configuration, bool force) override; + + const char *name() const override { return "Tilts"; } + + int count() const { return _count; } + + const Params &config(int idx) const { return _params[idx]; } + + void updateYawSign(const ActuatorEffectivenessRotors::Geometry &geometry); + + bool hasYawControl() const; + +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}; + + float _yaw_torque[MAX_COUNT] {}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 50eec6edcf..152f181ef8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -32,9 +32,14 @@ ############################################################################ px4_add_library(ActuatorEffectiveness + ActuatorEffectiveness.cpp ActuatorEffectiveness.hpp - ActuatorEffectivenessMultirotor.cpp - ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessControlSurfaces.cpp + ActuatorEffectivenessControlSurfaces.hpp + ActuatorEffectivenessTilts.cpp + ActuatorEffectivenessTilts.hpp + ActuatorEffectivenessRotors.cpp + ActuatorEffectivenessRotors.hpp ActuatorEffectivenessStandardVTOL.cpp ActuatorEffectivenessStandardVTOL.hpp ActuatorEffectivenessTiltrotorVTOL.cpp @@ -46,7 +51,6 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D target_link_libraries(ActuatorEffectiveness PRIVATE mathlib - ControlAllocation ) -# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 75f1c31151..9b22194803 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -71,14 +71,16 @@ #include +#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp" + class ControlAllocation { public: ControlAllocation(); virtual ~ControlAllocation() = default; - static constexpr uint8_t NUM_ACTUATORS = 16; - static constexpr uint8_t NUM_AXES = 6; + static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS; + static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES; typedef matrix::Vector ActuatorVector; @@ -93,8 +95,6 @@ public: /** * Allocate control setpoint to actuators - * - * @param control_setpoint Desired control setpoint vector (input) */ virtual void allocate() = 0; @@ -190,6 +190,8 @@ public: */ void clipActuatorSetpoint(matrix::Vector &actuator) const; + void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); } + /** * Normalize the actuator setpoint between minimum and maximum values. * @@ -207,6 +209,8 @@ public: int numConfiguredActuators() const { return _num_actuators; } 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 diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index ea78ed391f..e9d07eee5e 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -122,7 +122,4 @@ ControlAllocationPseudoInverse::allocate() // Allocate _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); - - // Clip - clipActuatorSetpoint(_actuator_sp); } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index 1d2f097da9..d934e17a80 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -53,9 +53,9 @@ public: ControlAllocationPseudoInverse() = default; virtual ~ControlAllocationPseudoInverse() = default; - virtual void allocate() override; - virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim, int num_actuators) override; + void allocate() override; + void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators) override; protected: matrix::Matrix _mix; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp index aba3cf8fd0..41dbf9de42 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -59,6 +59,7 @@ TEST(ControlAllocationTest, AllZeroCase) method.setEffectivenessMatrix(effectiveness, actuator_trim, 16); method.setControlSetpoint(control_sp); method.allocate(); + method.clipActuatorSetpoint(); actuator_sp = method.getActuatorSetpoint(); control_allocated_expected = method.getAllocatedControl(); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 5132579a7f..336f0303c2 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -60,9 +60,6 @@ ControlAllocationSequentialDesaturation::allocate() mixAirmodeDisabled(); break; } - - // Clip - clipActuatorSetpoint(_actuator_sp); } void ControlAllocationSequentialDesaturation::desaturateActuators( diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 90c1797ffd..688a745c2b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -59,7 +59,10 @@ ControlAllocator::ControlAllocator() : ControlAllocator::~ControlAllocator() { - delete _control_allocation; + for (int i = 0; i < ActuatorEffectiveness::MAX_NUM_MATRICES; ++i) { + delete _control_allocation[i]; + } + delete _actuator_effectiveness; perf_free(_loop_perf); @@ -86,69 +89,85 @@ ControlAllocator::parameters_updated() { // Allocation method & effectiveness source // Do this first: in case a new method is loaded, it will be configured below - update_effectiveness_source(); - update_allocation_method(); + bool updated = update_effectiveness_source(); + update_allocation_method(updated); // must be called after update_effectiveness_source() - if (_control_allocation == nullptr) { + if (_num_control_allocation == 0) { return; } - _control_allocation->updateParameters(); + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->updateParameters(); + } + update_effectiveness_matrix_if_needed(true); } void -ControlAllocator::update_allocation_method() +ControlAllocator::update_allocation_method(bool force) { - AllocationMethod method = (AllocationMethod)_param_ca_method.get(); + AllocationMethod configured_method = (AllocationMethod)_param_ca_method.get(); - if (_allocation_method_id != method) { + if (!_actuator_effectiveness) { + PX4_ERR("_actuator_effectiveness null"); + return; + } - // Save current state - matrix::Vector actuator_sp; + if (_allocation_method_id != configured_method || force) { - if (_control_allocation != nullptr) { - actuator_sp = _control_allocation->getActuatorSetpoint(); + 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; } - // try to instanciate new allocation method - ControlAllocation *tmp = nullptr; + _num_control_allocation = _actuator_effectiveness->numMatrices(); - switch (method) { - case AllocationMethod::PSEUDO_INVERSE: - tmp = new ControlAllocationPseudoInverse(); - break; + AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES]; + _actuator_effectiveness->getDesiredAllocationMethod(desired_methods); - case AllocationMethod::SEQUENTIAL_DESATURATION: - tmp = new ControlAllocationSequentialDesaturation(); - break; + for (int i = 0; i < _num_control_allocation; ++i) { + AllocationMethod method = configured_method; - default: - PX4_ERR("Unknown allocation method"); - break; + 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]->setActuatorSetpoint(actuator_sp[i]); + } } - // Replace previous method with new one - if (tmp == nullptr) { - // It did not work, forget about it - PX4_ERR("Control allocation init failed"); - _param_ca_method.set((int)_allocation_method_id); - - } else { - // Swap allocation methods - delete _control_allocation; - _control_allocation = tmp; - - // Save method id - _allocation_method_id = method; - - // Configure new allocation method - _control_allocation->setActuatorSetpoint(actuator_sp); - } + _allocation_method_id = configured_method; } } -void +bool ControlAllocator::update_effectiveness_source() { EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get(); @@ -161,15 +180,15 @@ ControlAllocator::update_effectiveness_source() switch (source) { case EffectivenessSource::NONE: case EffectivenessSource::MULTIROTOR: - tmp = new ActuatorEffectivenessMultirotor(this); + tmp = new ActuatorEffectivenessRotors(this); break; case EffectivenessSource::STANDARD_VTOL: - tmp = new ActuatorEffectivenessStandardVTOL(); + tmp = new ActuatorEffectivenessStandardVTOL(this); break; case EffectivenessSource::TILTROTOR_VTOL: - tmp = new ActuatorEffectivenessTiltrotorVTOL(); + tmp = new ActuatorEffectivenessTiltrotorVTOL(this); break; default: @@ -191,7 +210,11 @@ ControlAllocator::update_effectiveness_source() // Save source id _effectiveness_source_id = source; } + + return true; } + + return false; } void @@ -216,7 +239,7 @@ ControlAllocator::Run() parameters_updated(); } - if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) { + if (_num_control_allocation == 0 || _actuator_effectiveness == nullptr) { return; } @@ -281,18 +304,35 @@ ControlAllocator::Run() update_effectiveness_matrix_if_needed(); - // Set control setpoint vector - matrix::Vector c; - c(0) = _torque_sp(0); - c(1) = _torque_sp(1); - c(2) = _torque_sp(2); - c(3) = _thrust_sp(0); - c(4) = _thrust_sp(1); - c(5) = _thrust_sp(2); - _control_allocation->setControlSetpoint(c); + // 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); - // Do allocation - _control_allocation->allocate(); + if (_num_control_allocation > 1) { + _vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint); + _vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_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]; + 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->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp); + _control_allocation[i]->clipActuatorSetpoint(); + } // Publish actuator setpoint and allocator status publish_actuator_controls(); @@ -309,26 +349,61 @@ ControlAllocator::Run() void ControlAllocator::update_effectiveness_matrix_if_needed(bool force) { - matrix::Matrix effectiveness; + ActuatorEffectiveness::Configuration config{}; - if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness, force)) { + if (!force && hrt_elapsed_time(&_last_effectiveness_update) < 100_ms) { // rate-limit updates + return; + } - const matrix::Vector &trim = _actuator_effectiveness->getActuatorTrim(); + if (_actuator_effectiveness->getEffectivenessMatrix(config, force)) { + _last_effectiveness_update = hrt_absolute_time(); - // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) - matrix::Vector actuator_max = _control_allocation->getActuatorMax(); - matrix::Vector actuator_min = _control_allocation->getActuatorMin(); + memcpy(_control_allocation_selection_indexes, config.matrix_selection_indexes, + sizeof(_control_allocation_selection_indexes)); - for (size_t j = 0; j < NUM_ACTUATORS; j++) { - if (actuator_min(j) >= actuator_max(j)) { - for (size_t i = 0; i < NUM_AXES; i++) { - effectiveness(i, j) = 0.0f; + // Get the minimum and maximum depending on type and configuration + ActuatorEffectiveness::ActuatorVector minimum[ActuatorEffectiveness::MAX_NUM_MATRICES]; + ActuatorEffectiveness::ActuatorVector maximum[ActuatorEffectiveness::MAX_NUM_MATRICES]; + int actuator_idx = 0; + int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; + + 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) { + PX4_ERR("Too many actuators"); + break; } + + int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; + + if ((ActuatorType)actuator_type == ActuatorType::MOTORS) { + 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; + } + + } 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; } } - // Assign control effectiveness matrix - _control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators()); + for (int i = 0; i < _num_control_allocation; ++i) { + _control_allocation[i]->setActuatorMin(minimum[i]); + _control_allocation[i]->setActuatorMax(maximum[i]); + + // Assign control effectiveness matrix + int total_num_actuators = config.num_actuators_matrix[i]; + _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], total_num_actuators); + } } } @@ -338,8 +413,10 @@ ControlAllocator::publish_control_allocator_status() control_allocator_status_s control_allocator_status{}; control_allocator_status.timestamp = hrt_absolute_time(); + // TODO: handle multiple matrices & disabled motors (?) + // Allocated control - const matrix::Vector &allocated_control = _control_allocation->getAllocatedControl(); + const matrix::Vector &allocated_control = _control_allocation[0]->getAllocatedControl(); control_allocator_status.allocated_torque[0] = allocated_control(0); control_allocator_status.allocated_torque[1] = allocated_control(1); control_allocator_status.allocated_torque[2] = allocated_control(2); @@ -348,7 +425,7 @@ ControlAllocator::publish_control_allocator_status() control_allocator_status.allocated_thrust[2] = allocated_control(5); // Unallocated control - matrix::Vector unallocated_control = _control_allocation->getControlSetpoint() - allocated_control; + matrix::Vector unallocated_control = _control_allocation[0]->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); @@ -363,11 +440,11 @@ ControlAllocator::publish_control_allocator_status() unallocated_control(5)).norm_squared() < 1e-6f); // Actuator saturation - const matrix::Vector &actuator_sp = _control_allocation->getActuatorSetpoint(); - const matrix::Vector &actuator_min = _control_allocation->getActuatorMin(); - const matrix::Vector &actuator_max = _control_allocation->getActuatorMax(); + const matrix::Vector &actuator_sp = _control_allocation[0]->getActuatorSetpoint(); + const matrix::Vector &actuator_min = _control_allocation[0]->getActuatorMin(); + const matrix::Vector &actuator_max = _control_allocation[0]->getActuatorMax(); - for (size_t i = 0; i < NUM_ACTUATORS; i++) { + 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; @@ -386,17 +463,57 @@ ControlAllocator::publish_actuator_controls() actuator_motors.timestamp = hrt_absolute_time(); actuator_motors.timestamp_sample = _timestamp_sample; - const matrix::Vector &actuator_sp = _control_allocation->getActuatorSetpoint(); - matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint( - actuator_sp); + actuator_servos_s actuator_servos; + actuator_servos.timestamp = actuator_motors.timestamp; + actuator_servos.timestamp_sample = _timestamp_sample; - for (int i = 0; i < _control_allocation->numConfiguredActuators(); i++) { - actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN; + 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(); + + // 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); - // TODO: servos + // 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); + } } int ControlAllocator::task_spawn(int argc, char *argv[]) @@ -439,6 +556,10 @@ int ControlAllocator::print_status() case AllocationMethod::SEQUENTIAL_DESATURATION: PX4_INFO("Method: Sequential desaturation"); break; + + case AllocationMethod::AUTO: + PX4_INFO("Method: Auto"); + break; } // Print current airframe @@ -447,11 +568,20 @@ int ControlAllocator::print_status() } // Print current effectiveness matrix - if (_control_allocation != nullptr) { - const matrix::Matrix &effectiveness = _control_allocation->getEffectivenessMatrix(); - PX4_INFO("Effectiveness.T ="); + 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("Configured actuators: %i", _control_allocation->numConfiguredActuators()); + 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()); } // Print perf diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index d42e7f7795..4ec106f2a5 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -42,7 +42,7 @@ #pragma once #include -#include +#include #include #include @@ -61,8 +61,6 @@ #include #include #include -#include -#include #include #include #include @@ -72,6 +70,11 @@ class ControlAllocator : public ModuleBase, public ModuleParams, public px4::WorkItem { public: + static constexpr int NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; + static constexpr int NUM_AXES = ControlAllocation::NUM_AXES; + + using ActuatorVector = ActuatorEffectiveness::ActuatorVector; + ControlAllocator(); virtual ~ControlAllocator(); @@ -92,9 +95,6 @@ public: bool init(); - static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; - static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES; - private: /** @@ -102,8 +102,8 @@ private: */ void parameters_updated(); - void update_allocation_method(); - void update_effectiveness_source(); + void update_allocation_method(bool force); + bool update_effectiveness_source(); void update_effectiveness_matrix_if_needed(bool force = false); @@ -111,14 +111,10 @@ private: void publish_actuator_controls(); - enum class AllocationMethod { - NONE = -1, - PSEUDO_INVERSE = 0, - SEQUENTIAL_DESATURATION = 1, - }; - AllocationMethod _allocation_method_id{AllocationMethod::NONE}; - ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations + 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, @@ -130,10 +126,16 @@ private: 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::Publication _control_allocator_status_pub{ORB_ID(control_allocator_status)}; /**< actuator setpoint publication */ @@ -142,15 +144,11 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; - // float _battery_scale_factor{1.0f}; - // float _airspeed_scale_factor{1.0f}; - perf_counter_t _loop_perf; /**< loop duration performance counter */ hrt_abstime _last_run{0}; @@ -159,7 +157,8 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_ca_airframe, - (ParamInt) _param_ca_method + (ParamInt) _param_ca_method, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d7de3439ad..5c630d5c9c 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -1,4 +1,6 @@ __max_num_mc_motors: &max_num_mc_motors 8 +__max_num_servos: &max_num_servos 8 +__max_num_tilts: &max_num_tilts 4 module_name: Control Allocation @@ -22,30 +24,50 @@ parameters: description: short: Control allocation method long: | - Selects the algorithm and desaturation method + 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 - default: 1 + 2: Automatic + default: 2 - # MC rotors - CA_MC_R_COUNT: + # Motor parameters + CA_R_REV: + description: + short: Reversible motors + long: | + Configure motors to be 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 + default: 0 + + # (MC) Rotors + CA_ROTOR_COUNT: description: short: Total number of rotors type: enum values: - 0: 0 Motors - 1: 1 Motor - 2: 2 Motors - 3: 3 Motors - 4: 4 Motors - 5: 5 Motors - 6: 6 Motors - 7: 7 Motors - 8: 8 Motors + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' default: 0 - CA_MC_R${i}_PX: + CA_ROTOR${i}_PX: description: short: Position of rotor ${i} along X body axis type: float @@ -56,7 +78,7 @@ parameters: min: -100 max: 100 default: 0.0 - CA_MC_R${i}_PY: + CA_ROTOR${i}_PY: description: short: Position of rotor ${i} along Y body axis type: float @@ -67,7 +89,7 @@ parameters: min: -100 max: 100 default: 0.0 - CA_MC_R${i}_PZ: + CA_ROTOR${i}_PZ: description: short: Position of rotor ${i} along Z body axis type: float @@ -79,7 +101,7 @@ parameters: max: 100 default: 0.0 - CA_MC_R${i}_AX: + 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). @@ -90,7 +112,7 @@ parameters: min: -100 max: 100 default: 0.0 - CA_MC_R${i}_AY: + 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). @@ -101,7 +123,7 @@ parameters: min: -100 max: 100 default: 0.0 - CA_MC_R${i}_AZ: + 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). @@ -113,7 +135,7 @@ parameters: max: 100 default: -1.0 - CA_MC_R${i}_CT: + CA_ROTOR${i}_CT: description: short: Thrust coefficient of rotor ${i} long: | @@ -128,7 +150,7 @@ parameters: max: 100 default: 6.5 - CA_MC_R${i}_KM: + CA_ROTOR${i}_KM: description: short: Moment coefficient of rotor ${i} long: | @@ -143,6 +165,163 @@ parameters: 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: + 1: Elevator + 2: Rudder + 3: Left Elevon + 4: Right Elevon + 5: Left V-Tail + 6: Right V-Tail + 7: Left Flaps + 8: Right Flaps + 9: Left Aileron + 10: Right Aileron + 11: Airbrakes + 12: Custom + num_instances: *max_num_servos + instance_start: 0 + default: 1 + + 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 + + # 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 + values: + 0: 'None' + 1: 'Yaw' + 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' + num_instances: *max_num_tilts + instance_start: 0 + default: 0 + + # Standard VTOL + CA_STDVTOL_N_P: + description: + short: Number of fixed wing (pusher/puller) motors + type: enum + values: + 1: '1' + 2: '2' + 3: '3' + 4: '4' + default: 1 + # Mixer mixer: @@ -153,6 +332,12 @@ mixer: min: 0 max: 1 default_is_nan: true + per_item_parameters: + - name: 'CA_R_REV' + label: 'Reversible' + show_as: 'bitset' + index_offset: 0 + advanced: true servo: functions: 'Servo' actuator_testing_values: @@ -165,42 +350,125 @@ mixer: 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'] + items: + # TODO + 1: # Elevator + - { 'hidden': True, 'default': 0.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + 2: # Rudder + - { 'hidden': True, 'default': 0.0 } # roll + - { 'hidden': True, 'default': 0.0 } # pitch + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # yaw + 3: # Left elevon + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll + - { 'min': -1.0, 'max': 0.0, 'default': -1.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + 4: # Right elevon + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll + - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # pitch + - { 'hidden': True, 'default': 0.0 } # yaw + config: param: CA_AIRFRAME types: 0: # Multirotor - type: "multirotor" + type: 'multirotor' actuators: - actuator_type: 'motor' - count: 'CA_MC_R_COUNT' + count: 'CA_ROTOR_COUNT' per_item_parameters: standard: - position: [ 'CA_MC_R${i}_PX', 'CA_MC_R${i}_PY', 'CA_MC_R${i}_PZ' ] + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] extra: - - name: 'CA_MC_R${i}_KM' + - name: 'CA_ROTOR${i}_KM' label: 'Direction CCW' function: 'spin-dir' show_as: 'true-if-positive' - 2: # Tiltrotor VTOL example + 1: # Standard VTOL actuators: - actuator_type: 'motor' - instances: - - name: 'Front Left Motor' - position: [ 0.3, -0.3, 0 ] - - name: 'Front Right Motor' - position: [ 0.3, 0.3, 0 ] - - name: 'Rear Motor' - position: [ -0.75, 0.3, 0 ] + 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: 'motor' + group_label: 'FW Motors' + count: 'CA_STDVTOL_N_P' + item_label_prefix: 'FW Motor ${i}' - actuator_type: 'servo' - instances: - - name: 'Front Left Tilt' - position: [ 0.3, -0.3, 0 ] - - name: 'Front Right Tilt' - position: [ 0.3, 0.3, 0 ] - - name: 'Left Elevon' - position: [ 0, -0.3, 0 ] - - name: 'Right Elevon' - position: [ 0, 0.3, 0 ] + 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' + + 2: # 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' + - 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 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' + - 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' +