diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp new file mode 100644 index 0000000000..910caea55b --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ActuatorEffectivenessCustom.hpp" + +ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent): + ModuleParams(parent) +{ +} + +float getScaleParameter(uint8_t n) +{ + // CA_ACTn_SC: scale + char buffer[17]; + sprintf(buffer, "CA_ACT%u_SC", n); + param_t param_handle = param_find(buffer); + + if (param_handle == PARAM_INVALID) { + PX4_ERR("invalid parameter %s", buffer); + return 0.f; + } + + float scale = 1.f; + + if (param_get(param_handle, &scale) != PX4_OK) { + PX4_ERR("param_get %s failed", buffer); + return scale; + } + + return 0.f; +} + +bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix &matrix, + bool force) +{ + // TODO: + // - airspeed scaling + // - battery scaling + // - publish motors and servos (handle ranges) + // - disable individual actuators + // - feedback from output module (actual failures) + // - VTOL transitions (cleanly blend in and out?) + // - eg disable all downward thrust + // - eg disable all control surfaces + // - blend in/out higher level by adjusting max? then completely purge from effectiveness once off + if (_updated || force) { + _updated = false; + + int num_actuators = 0; + _effectiveness.zero(); + + for (int n = 0; n < NUM_ACTUATORS; n++) { + // CA_ACTn_FUNC: look up function + char buffer[17]; + sprintf(buffer, "CA_ACT%u_FUNC", n); + int32_t ctrl_func = 0; + param_get(param_find(buffer), &ctrl_func); + + switch (ctrl_func) { + case 0: + // disabled + break; + + case 1: { + // 1: custom thrust (parameters) + matrix::Vector3f axis{}; + matrix::Vector3f position{}; + float thrust_coef = 0.f; + float moment_ratio = 0.f; + + for (int i = 0; i < 3; i++) { + char axis_char = 'X' + i; + + // CA_ACTn_P{X,Y,Z} + sprintf(buffer, "CA_ACT%u_P%c", n, axis_char); + + if (param_get(param_find(buffer), &position(i)) != PX4_OK) { + PX4_ERR("unable to get %s", buffer); + } + + // CA_ACTn_A_{X,Y,Z} + sprintf(buffer, "CA_ACT%u_A%c", n, axis_char); + + if (param_get(param_find(buffer), &axis(i)) != PX4_OK) { + PX4_ERR("unable to get %s", buffer); + } + } + + // CA_ACTn_CT TODO + sprintf(buffer, "CA_ACT%u_CT", n); + param_get(param_find(buffer), &thrust_coef); + + // CA_ACTn_KM TODO + sprintf(buffer, "CA_ACT%u_KM", n); + param_get(param_find(buffer), &moment_ratio); + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get coefficients + float ct = thrust_coef; + float km = moment_ratio; + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + const matrix::Vector3f thrust{ct * axis}; + + // Compute moment generated by this rotor + const 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, n) = moment(j); + _effectiveness(j + 3, n) = thrust(j); + } + } + + break; + + case 2: { + // 2: custom torque (parameters) + + // CA_ACTn_TRQ_R + // CA_ACTn_TRQ_P + // CA_ACTn_TRQ_Y + char torque_str[3][17]; + sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n); + sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n); + sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n); + + for (int i = 0; i < 3; i++) { + // CA_ACTn_TRQ_{R,P,Y} + param_get(param_find(torque_str[i]), &_effectiveness(i, n)); + } + } + break; + + case 100: { + // 100: Aileron, scaled (CA_ACTn_SC) roll torque + _effectiveness(0, n) = getScaleParameter(n); + } + break; + + case 101: { + // 101: Elevator, scaled (CA_ACTn_SC) pitch torque + _effectiveness(1, n) = getScaleParameter(n); + } + break; + + case 102: { + // 102: Rudder, scaled (CA_ACTn_SC) yaw torque + _effectiveness(2, n) = getScaleParameter(n); + } + break; + + case 103: { + // 103: Throttle, scaled (CA_ACTn_SC) X thrust + _effectiveness(4, n) = getScaleParameter(n); + } + break; + + case 104: { + // 104: Elevon(+), scaled (CA_ACTn_SC) roll & pitch torque + float scale = getScaleParameter(n); + _effectiveness(0, n) = 0.5f * scale; + _effectiveness(1, n) = 0.5f * scale; + } + break; + + case 105: { + // 105: Elevon(+), scaled (CA_ACTn_SC) -roll & pitch torque + float scale = getScaleParameter(n); + _effectiveness(0, n) = -0.5f * scale; + _effectiveness(1, n) = 0.5f * scale; + } + break; + + case 106: { + // 106: V-Tail (+), scaled (CA_ACTn_SC) pitch & yaw torque + float scale = getScaleParameter(n); + _effectiveness(1, n) = 0.5f * scale; + _effectiveness(2, n) = 0.5f * scale; + } + break; + + case 107: { + // 107: V-Tail (-), scaled (CA_ACTn_SC) pitch & -yaw torque + float scale = getScaleParameter(n); + _effectiveness(1, n) = 0.5f * scale; + _effectiveness(2, n) = -0.5f * scale; + } + break; + + default: + PX4_ERR("%s invalid function %d", buffer, n); + break; + } + + if (_effectiveness.col(n).longerThan(0.f)) { + num_actuators = n + 1; + } + } + + _effectiveness.transpose().print(); + + _num_actuators = num_actuators; + matrix = _effectiveness; + return true; + } + + return false; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp new file mode 100644 index 0000000000..a6c0d48f40 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessCustom.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 ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessCustom(ModuleParams *parent); + virtual ~ActuatorEffectivenessCustom() = default; + + bool getEffectivenessMatrix(matrix::Matrix &matrix, bool force) override; + + int numActuators() const override { return _num_actuators; } +private: + matrix::Matrix _effectiveness{}; + + int _num_actuators{0}; + + bool _updated{true}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 50eec6edcf..ad97661ffc 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 @@ -33,6 +33,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.hpp + ActuatorEffectivenessCustom.cpp + ActuatorEffectivenessCustom.hpp ActuatorEffectivenessMultirotor.cpp ActuatorEffectivenessMultirotor.hpp ActuatorEffectivenessStandardVTOL.cpp diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 06a0ff017a..35474e1cd2 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 @@ -49,4 +49,6 @@ px4_add_module( ControlAllocation mixer px4_work_queue + MODULE_CONFIG + module.yaml ) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 156147595f..787550cd14 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -51,8 +51,7 @@ using namespace time_literals; ControlAllocator::ControlAllocator() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) + WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc) { parameters_updated(); } @@ -212,6 +211,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessTiltrotorVTOL(); break; + case EffectivenessSource::CUSTOM: + tmp = new ActuatorEffectivenessCustom(this); + break; + default: PX4_ERR("Unknown airframe"); break; @@ -512,6 +515,10 @@ int ControlAllocator::print_status() case EffectivenessSource::TILTROTOR_VTOL: PX4_INFO("EffectivenessSource: Tiltrotor VTOL"); break; + + case EffectivenessSource::CUSTOM: + PX4_INFO("EffectivenessSource: Custom"); + break; } // Print current effectiveness matrix diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index b72e5bb17a..21fde7712f 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -42,6 +42,7 @@ #pragma once #include +#include #include #include #include @@ -61,8 +62,6 @@ #include #include #include -#include -#include #include #include #include @@ -124,10 +123,11 @@ private: ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations enum class EffectivenessSource { - NONE = -1, - MULTIROTOR = 0, - STANDARD_VTOL = 1, + NONE = -1, + MULTIROTOR = 0, + STANDARD_VTOL = 1, TILTROTOR_VTOL = 2, + CUSTOM = 3, }; EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; @@ -146,16 +146,14 @@ 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 */ + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; hrt_abstime _last_run{0}; hrt_abstime _timestamp_sample{0}; diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index c1384d4414..e8e0e342c2 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -50,6 +50,7 @@ * @value 0 Multirotor * @value 1 Standard VTOL (WIP) * @value 2 Tiltrotor VTOL (WIP) + * @value 3 Custom (CA_ACTn* torque and thrust parameters) * @group Control Allocation */ PARAM_DEFINE_INT32(CA_AIRFRAME, 0); diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml new file mode 100644 index 0000000000..2b565faf8e --- /dev/null +++ b/src/modules/control_allocator/module.yaml @@ -0,0 +1,198 @@ +__max_num_config_instances: &max_num_config_instances 16 + +module_name: control_allocator + +parameters: + - group: Control Allocator + definitions: + CA_ACT${i}_FUNC: + description: + short: actuator ${i} function + long: actuator ${i} function + type: enum + values: + 0: disabled + 1: custom thrust (parameters) + 2: custom torque (parameters) + 100: Aileron + 101: Elevator + 102: Rudder + 103: Throttle + 104: Elevon (+) + 105: Elevon (-) + 106: V-Tail (+) + 107: V-Tail (-) + num_instances: *max_num_config_instances + instance_start: 0 + default: 0 + + CA_ACT${i}_PX: + description: + short: actuator ${i} position X + long: Actuator ${i} position along X body axis + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_PY: + description: + short: actuator ${i} position Y + long: Actuator ${i} position along Y body axis + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_PZ: + description: + short: actuator ${i} position Z + long: Actuator ${i} position along Z body axis + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_THR_X: + description: + short: actuator ${i} thrust x + long: Actuator ${i} thrust x + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_THR_Y: + description: + short: actuator ${i} thrust y + long: Actuator ${i} thrust y + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_THR_Z: + description: + short: actuator ${i} thrust z + long: Actuator ${i} thrust z + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_AX: + description: + short: actuator ${i} axis of thrust vector, X body axis component + long: Actuator ${i} axis of thrust vector, X body axis component + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_AY: + description: + short: actuator ${i} axis of thrust vector, Y body axis component + long: Actuator ${i} axis of thrust vector, Y body axis component + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_AZ: + description: + short: actuator ${i} axis of thrust vector, Z body axis component + long: Actuator ${i} axis of thrust vector, Z body axis component + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_CT: + description: + short: actuator ${i} thrust coefficient + long: The thrust coefficient if defined as Thrust = CT * u^2, + where u (with value between CA_ACT0_MIN and CA_ACT0_MAX) + is the output signal sent to the motor controller. + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_KM: + description: + short: actuator ${i} moment coefficient + long: The moment coefficient if defined as Torque = KM * Thrust + Use a positive value for a rotor with CCW rotation. + Use a negative value for a rotor with CW rotation. + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_TRQ_R: + description: + short: actuator ${i} roll torque + long: Actuator ${i} roll torque + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_TRQ_P: + description: + short: actuator ${i} pitch torque + long: Actuator ${i} pitch torque + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_TRQ_Y: + description: + short: actuator ${i} yaw torque + long: Actuator ${i} yaw torque + type: float + decimal: 2 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 0.0 + + CA_ACT${i}_SC: + description: + short: actuator ${i} scale + long: Actuator ${i} scale + type: float + decimal: 2 + minimum: -1.0 + maximum: 1.0 + reboot_required: false + num_instances: *max_num_config_instances + instance_start: 0 + default: 1.0