diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 4acf350caa..919a2be778 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -73,6 +73,15 @@ public: static constexpr int NUM_ACTUATORS = 16; static constexpr int NUM_AXES = 6; + enum ControlAxis { + ROLL = 0, + PITCH, + YAW, + THRUST_X, + THRUST_Y, + THRUST_Z + }; + static constexpr int MAX_NUM_MATRICES = 2; using EffectivenessMatrix = matrix::Matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp new file mode 100644 index 0000000000..41fc850954 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ActuatorEffectivenessHelicopter.hpp" +#include + +using namespace matrix; + +ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent) + : ModuleParams(parent) +{ + for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i); + _param_handles.swash_plate_servos[i].angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i); + _param_handles.swash_plate_servos[i].arm_length = param_find(buffer); + } + + _param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT"); + + for (int i = 0; i < NUM_CURVE_POINTS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i); + _param_handles.throttle_curve[i] = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i); + _param_handles.pitch_curve[i] = param_find(buffer); + } + + updateParams(); +} + +void ActuatorEffectivenessHelicopter::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + float angle_deg{}; + param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg); + _geometry.swash_plate_servos[i].angle = math::radians(angle_deg); + param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length); + } + + for (int i = 0; i < NUM_CURVE_POINTS; ++i) { + param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]); + param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]); + } +} + +bool +ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // As the allocation is non-linear, we use updateSetpoint() instead of the matrix + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); + + // Tail (yaw) servo + configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{}); + + // N swash plate servos + _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{}); + } + + return true; +} + +void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp) +{ + // Find index to use for curves + float thrust_cmd = -control_sp(ControlAxis::THRUST_Z); + float num_intervals = NUM_CURVE_POINTS - 1; + // We access idx + 1 below, so max legal index is (size - 2) + int idx = math::constrain((int)(thrust_cmd * num_intervals), 0, NUM_CURVE_POINTS - 2); + + // Local throttle curve gradient and offset + float tg = (_geometry.throttle_curve[idx + 1] - _geometry.throttle_curve[idx]) * num_intervals; + float to = (_geometry.throttle_curve[idx]) - (tg * idx / num_intervals); + float throttle = math::constrain(tg * thrust_cmd + to, 0.0f, 1.0f); + + // Local pitch curve gradient and offset + float pg = (_geometry.pitch_curve[idx + 1] - _geometry.pitch_curve[idx]) * num_intervals; + float po = (_geometry.pitch_curve[idx]) - (pg * idx / num_intervals); + float collective_pitch = math::constrain((pg * thrust_cmd + po), -0.5f, 0.5f); + + float roll_cmd = control_sp(ControlAxis::ROLL); + float pitch_cmd = control_sp(ControlAxis::PITCH); + + actuator_sp(0) = throttle; + + for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { + actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch + + cosf(_geometry.swash_plate_servos[i].angle) * pitch_cmd * _geometry.swash_plate_servos[i].arm_length + - sinf(_geometry.swash_plate_servos[i].angle) * roll_cmd * _geometry.swash_plate_servos[i].arm_length; + } + +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp new file mode 100644 index 0000000000..3438f017a5 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include + +class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4; + static constexpr int NUM_CURVE_POINTS = 5; + + struct SwashPlateGeometry { + float angle; + float arm_length; + }; + + struct Geometry { + SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + int num_swash_plate_servos{0}; + float throttle_curve[NUM_CURVE_POINTS]; + float pitch_curve[NUM_CURVE_POINTS]; + }; + + ActuatorEffectivenessHelicopter(ModuleParams *parent); + virtual ~ActuatorEffectivenessHelicopter() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Helicopter"; } + + + const Geometry &geometry() const { return _geometry; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp) override; +private: + void updateParams() override; + + struct ParamHandlesSwashPlate { + param_t angle; + param_t arm_length; + }; + struct ParamHandles { + ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + param_t num_swash_plate_servos; + param_t throttle_curve[NUM_CURVE_POINTS]; + param_t pitch_curve[NUM_CURVE_POINTS]; + }; + ParamHandles _param_handles{}; + + Geometry _geometry{}; + + int _first_swash_plate_servo_index{}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 2dbceae013..e99d718550 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -40,6 +40,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessCustom.hpp ActuatorEffectivenessFixedWing.cpp ActuatorEffectivenessFixedWing.hpp + ActuatorEffectivenessHelicopter.cpp + ActuatorEffectivenessHelicopter.hpp ActuatorEffectivenessMCTilt.cpp ActuatorEffectivenessMCTilt.hpp ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index b1bd1475da..70e7e8b695 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -254,6 +254,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessCustom(this); break; + case EffectivenessSource::HELICOPTER: + tmp = new ActuatorEffectivenessHelicopter(this); + break; + default: PX4_ERR("Unknown airframe"); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 033a43156b..2942dd9fab 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -148,6 +149,7 @@ private: MOTORS_6DOF = 7, MULTIROTOR_WITH_TILT = 8, CUSTOM = 9, + HELICOPTER = 10, }; EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 273967dfae..c1f64e35cd 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -27,6 +27,7 @@ parameters: 7: Motors (6DOF) 8: Multirotor with Tilt 9: Custom + 10: Helicopter default: 0 CA_METHOD: @@ -378,6 +379,62 @@ parameters: instance_start: 0 default: 0 + # helicopter + CA_SP0_COUNT: + description: + short: Number of swash plates servos + type: enum + values: + 3: '3' + 4: '4' + default: 3 + CA_SP0_ANG${i}: + description: + short: Angle for swash plate servo ${i} + type: float + decimal: 0 + increment: 10 + unit: deg + num_instances: 4 + min: 0 + max: 360 + default: [0, 140, 220, 0] + CA_SP0_ARM_L${i}: + description: + short: Arm length for swash plate servo ${i} + long: | + This is relative to the other arm lengths. + type: float + decimal: 3 + increment: 0.1 + num_instances: 4 + min: 0 + max: 10 + default: 1.0 + CA_HELI_THR_C${i}: + description: + short: Throttle curve at position ${i} + long: | + Defines the output throttle at the interval position ${i}. + type: float + decimal: 3 + increment: 0.1 + num_instances: 5 + min: 0 + max: 1 + default: [0, 0.3, 0.6, 0.8, 1] + CA_HELI_PITCH_C${i}: + description: + short: Collective pitch curve at position ${i} + long: | + Defines the collective pitch at the interval position ${i} for a given thrust setpoint. + type: float + decimal: 3 + increment: 0.1 + num_instances: 5 + min: 0 + max: 1 + default: [0.05, 0.15, 0.25, 0.35, 0.45] # Mixer mixer: @@ -767,3 +824,20 @@ mixer: - name: 'CA_SV_CS${i}_TRIM' label: 'Trim' + 10: # Helicopter + actuators: + - actuator_type: 'motor' + count: 1 + item_label_prefix: ['Rotor'] + - actuator_type: 'servo' + item_label_prefix: ['Yaw tail servo'] + count: 1 + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)'