From 0416354bd9e78b3c40ece5e2c43e3a254afe34d2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Mar 2023 17:48:04 +0200 Subject: [PATCH 1/9] Helicopter: add collective pitch offset to Actuator UI parameters --- src/modules/control_allocator/module.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 605d11fda048..c80a5fa15b76 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -1079,6 +1079,8 @@ mixer: - name: 'CA_SV_CS${i}_TRIM' label: 'Trim' parameters: + - label: 'Collective pitch offset to align least amount of rotor drag' + name: CA_HELI_YAW_CP_O - label: 'Yaw compensation scale based on collective pitch' name: CA_HELI_YAW_CP_S - label: 'Yaw compensation scale based on throttle' From 3fe15f9c91fe0f698f6c9491c96b73666d509713 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Mar 2023 16:41:41 +0200 Subject: [PATCH 2/9] ActuatorEffectivenessHelicopter: spacing --- .../ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 8d3401a6f8f4..a47a1645ac24 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -104,8 +104,7 @@ void ActuatorEffectivenessHelicopter::updateParams() _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; } -bool -ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, +bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { @@ -221,7 +220,6 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) { - // Note: the values '-1', '1' and '0' are just to indicate a negative, // positive or no saturation to the rate controller. The actual magnitude is not used. if (_saturation_flags.roll_pos) { From 62411eab752cec5d2129e5fbac50312eebfe6273 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Mar 2023 16:48:30 +0200 Subject: [PATCH 3/9] control_allocator: allow for only 2 swash plate servos This is required to support fixed pitch propeller helicopters that have no collective but only cyclic pitch with two degrees of freedom and hence only two servos. The amount of thrust in the body z axis is then controlled using the motor speed which makes particularly sense on coaxial helicopters that need to control yaw with changing motor speeds already. --- src/modules/control_allocator/module.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index c80a5fa15b76..97c9fbe8dc05 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -416,6 +416,7 @@ parameters: short: Number of swash plates servos type: enum values: + 2: '2' 3: '3' 4: '4' default: 3 From 0d023b1fcfadaf7c303a13ff6b5453b2c2b0c692 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Mar 2023 16:43:09 +0200 Subject: [PATCH 4/9] control_allocator: add coaxial helicopter effectiveness It's now just a copy of the helicopter such that changes get well visible in the history. --- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 252 ++++++++++++++++++ ...ActuatorEffectivenessHelicopterCoaxial.hpp | 132 +++++++++ .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../control_allocator/ControlAllocator.cpp | 4 + .../control_allocator/ControlAllocator.hpp | 2 + src/modules/control_allocator/module.yaml | 21 ++ 6 files changed, 413 insertions(+) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp new file mode 100644 index 000000000000..e36709adf43d --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -0,0 +1,252 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ActuatorEffectivenessHelicopterCoaxial.hpp" +#include + +using namespace matrix; +using namespace time_literals; + +ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent) + : ModuleParams(parent) +{ + for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i); + _param_handles.swash_plate_servos[i].angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i); + _param_handles.swash_plate_servos[i].arm_length = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i); + _param_handles.swash_plate_servos[i].trim = param_find(buffer); + } + + _param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT"); + + for (int i = 0; i < NUM_CURVE_POINTS; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i); + _param_handles.throttle_curve[i] = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i); + _param_handles.pitch_curve[i] = param_find(buffer); + } + + _param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S"); + _param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O"); + _param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S"); + _param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW"); + _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); +} + +void ActuatorEffectivenessHelicopterCoaxial::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + float angle_deg{}; + param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg); + _geometry.swash_plate_servos[i].angle = math::radians(angle_deg); + param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length); + param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim); + } + + for (int i = 0; i < NUM_CURVE_POINTS; ++i) { + param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]); + param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]); + } + + param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale); + param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset); + param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale); + param_get(_param_handles.spoolup_time, &_geometry.spoolup_time); + int32_t yaw_ccw = 0; + param_get(_param_handles.yaw_ccw, &yaw_ccw); + _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; +} + +bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // As the allocation is non-linear, we use updateSetpoint() instead of the matrix + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); + + // Tail (yaw) motor + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); + + // N swash plate servos + _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{}); + configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim; + } + + return true; +} + +void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + _saturation_flags = {}; + + // throttle/collective pitch curve + const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), + _geometry.throttle_curve) * throttleSpoolupProgress(); + const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve); + + // actuator mapping + actuator_sp(0) = mainMotorEnaged() ? throttle : NAN; + + actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign + + fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale + + throttle * _geometry.yaw_throttle_scale; + + // Saturation check for yaw + if (actuator_sp(1) < actuator_min(1)) { + setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); + + } else if (actuator_sp(1) > actuator_max(1)) { + setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + } + + for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { + float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch + + control_sp(ControlAxis::PITCH) * pitch_coeff + - control_sp(ControlAxis::ROLL) * roll_coeff + + _geometry.swash_plate_servos[i].trim; + + // Saturation check for roll & pitch + if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos); + + } else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg); + } + } +} + +bool ActuatorEffectivenessHelicopterCoaxial::mainMotorEnaged() +{ + manual_control_switches_s manual_control_switches; + + if (_manual_control_switches_sub.update(&manual_control_switches)) { + _main_motor_engaged = manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_NONE + || manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_ON; + } + + return _main_motor_engaged; +} + +float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f; + const float spoolup_progress = time_since_arming / _geometry.spoolup_time; + + if (_armed && spoolup_progress < 1.f) { + return spoolup_progress; + } + + return 1.f; +} + + +void ActuatorEffectivenessHelicopterCoaxial::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag) +{ + if (coeff > 0.f) { + // A positive change in given axis will increase saturation + positive_flag = true; + + } else if (coeff < 0.f) { + // A negative change in given axis will increase saturation + negative_flag = true; + } +} + +void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_saturation_flags.roll_pos) { + status.unallocated_torque[0] = 1.f; + + } else if (_saturation_flags.roll_neg) { + status.unallocated_torque[0] = -1.f; + } + + if (_saturation_flags.pitch_pos) { + status.unallocated_torque[1] = 1.f; + + } else if (_saturation_flags.pitch_neg) { + status.unallocated_torque[1] = -1.f; + } + + if (_saturation_flags.yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_saturation_flags.yaw_neg) { + status.unallocated_torque[2] = -1.f; + } + + if (_saturation_flags.thrust_pos) { + status.unallocated_thrust[2] = 1.f; + + } else if (_saturation_flags.thrust_neg) { + status.unallocated_thrust[2] = -1.f; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp new file mode 100644 index 000000000000..57f36b2db993 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include + +#include +#include +#include + +class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4; + static constexpr int NUM_CURVE_POINTS = 5; + + struct SwashPlateGeometry { + float angle; + float arm_length; + float trim; + }; + + struct Geometry { + SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + int num_swash_plate_servos{0}; + float throttle_curve[NUM_CURVE_POINTS]; + float pitch_curve[NUM_CURVE_POINTS]; + float yaw_collective_pitch_scale; + float yaw_collective_pitch_offset; + float yaw_throttle_scale; + float yaw_sign; + float spoolup_time; + }; + + ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent); + virtual ~ActuatorEffectivenessHelicopterCoaxial() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Helicopter"; } + + + const Geometry &geometry() const { return _geometry; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; +private: + float throttleSpoolupProgress(); + bool mainMotorEnaged(); + + void updateParams() override; + + struct SaturationFlags { + bool roll_pos; + bool roll_neg; + bool pitch_pos; + bool pitch_neg; + bool yaw_pos; + bool yaw_neg; + bool thrust_pos; + bool thrust_neg; + }; + static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag); + + struct ParamHandlesSwashPlate { + param_t angle; + param_t arm_length; + param_t trim; + }; + struct ParamHandles { + ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + param_t num_swash_plate_servos; + param_t throttle_curve[NUM_CURVE_POINTS]; + param_t pitch_curve[NUM_CURVE_POINTS]; + param_t yaw_collective_pitch_scale; + param_t yaw_collective_pitch_offset; + param_t yaw_throttle_scale; + param_t yaw_ccw; + param_t spoolup_time; + }; + ParamHandles _param_handles{}; + + Geometry _geometry{}; + + int _first_swash_plate_servo_index{}; + SaturationFlags _saturation_flags; + + // Throttle spoolup state + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; + + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; + bool _main_motor_engaged{true}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 18c0679d1067..994b566270f1 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -44,6 +44,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessFixedWing.hpp ActuatorEffectivenessHelicopter.cpp ActuatorEffectivenessHelicopter.hpp + ActuatorEffectivenessHelicopterCoaxial.cpp + ActuatorEffectivenessHelicopterCoaxial.hpp ActuatorEffectivenessMCTilt.cpp ActuatorEffectivenessMCTilt.hpp ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2b08e8a32790..0fad1b5f62a1 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -266,6 +266,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS); break; + case EffectivenessSource::HELICOPTER_COAXIAL: + tmp = new ActuatorEffectivenessHelicopterCoaxial(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 d04d1a5bf729..25904f05515e 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -157,6 +158,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam CUSTOM = 9, HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, + HELICOPTER_COAXIAL = 12, }; enum class FailureMode { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 97c9fbe8dc05..2c506cc4e270 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -29,6 +29,7 @@ parameters: 9: Custom 10: Helicopter (tail ESC) 11: Helicopter (tail Servo) + 12: Helicopter (Coaxial) default: 0 CA_METHOD: @@ -1119,3 +1120,23 @@ mixer: name: CA_HELI_YAW_CCW - label: 'Throttle spoolup time' name: COM_SPOOLUP_TIME + + 12: # Helicopter (Coaxial) + actuators: + - actuator_type: 'motor' + count: 2 + item_label_prefix: ['Clockwise Rotor', 'Counter-clockwise Rotor'] + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME From 01e3af1bd6b58944ee3b9a0fa74162cdbecfab0f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 28 Mar 2023 14:33:48 +0200 Subject: [PATCH 5/9] HelicopterCoaxial: adjust for coaxial allocation --- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 74 ++++--------------- ...ActuatorEffectivenessHelicopterCoaxial.hpp | 15 ---- 2 files changed, 16 insertions(+), 73 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index e36709adf43d..4ea7c956d379 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -51,19 +51,6 @@ ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(M } _param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT"); - - for (int i = 0; i < NUM_CURVE_POINTS; ++i) { - char buffer[17]; - snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i); - _param_handles.throttle_curve[i] = param_find(buffer); - snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i); - _param_handles.pitch_curve[i] = param_find(buffer); - } - - _param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S"); - _param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O"); - _param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S"); - _param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW"); _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); updateParams(); @@ -90,18 +77,7 @@ void ActuatorEffectivenessHelicopterCoaxial::updateParams() param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim); } - for (int i = 0; i < NUM_CURVE_POINTS; ++i) { - param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]); - param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]); - } - - param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale); - param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset); - param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale); param_get(_param_handles.spoolup_time, &_geometry.spoolup_time); - int32_t yaw_ccw = 0; - param_get(_param_handles.yaw_ccw, &yaw_ccw); - _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; } bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration, @@ -112,10 +88,8 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio } // As the allocation is non-linear, we use updateSetpoint() instead of the matrix - configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); - - // Tail (yaw) motor - configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Clockwise rotor + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Counter-clockwise rotor // N swash plate servos _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; @@ -135,32 +109,28 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector _saturation_flags = {}; // throttle/collective pitch curve - const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), - _geometry.throttle_curve) * throttleSpoolupProgress(); - const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve); + const float throttle = -control_sp(ControlAxis::THRUST_Z) * throttleSpoolupProgress(); + const float yaw = control_sp(ControlAxis::YAW); // actuator mapping - actuator_sp(0) = mainMotorEnaged() ? throttle : NAN; + actuator_sp(0) = throttle - yaw; // Clockwise + actuator_sp(1) = throttle + yaw; // Counter-clockwise - actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign - + fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale - + throttle * _geometry.yaw_throttle_scale; + // Saturation check for yaw TODO check saturation + // if (actuator_sp(1) < actuator_min(1)) { + // setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); - // Saturation check for yaw - if (actuator_sp(1) < actuator_min(1)) { - setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); - - } else if (actuator_sp(1) > actuator_max(1)) { - setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); - } + // } else if (actuator_sp(1) > actuator_max(1)) { + // setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + // } for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; - actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch - + control_sp(ControlAxis::PITCH) * pitch_coeff - - control_sp(ControlAxis::ROLL) * roll_coeff - + _geometry.swash_plate_servos[i].trim; + actuator_sp(_first_swash_plate_servo_index + i) = + + control_sp(ControlAxis::PITCH) * pitch_coeff + - control_sp(ControlAxis::ROLL) * roll_coeff + + _geometry.swash_plate_servos[i].trim; // Saturation check for roll & pitch if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) { @@ -174,18 +144,6 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector } } -bool ActuatorEffectivenessHelicopterCoaxial::mainMotorEnaged() -{ - manual_control_switches_s manual_control_switches; - - if (_manual_control_switches_sub.update(&manual_control_switches)) { - _main_motor_engaged = manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_NONE - || manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_ON; - } - - return _main_motor_engaged; -} - float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress() { vehicle_status_s vehicle_status; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index 57f36b2db993..d5316bf498c8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -46,7 +46,6 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua public: static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4; - static constexpr int NUM_CURVE_POINTS = 5; struct SwashPlateGeometry { float angle; @@ -57,12 +56,6 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua struct Geometry { SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; int num_swash_plate_servos{0}; - float throttle_curve[NUM_CURVE_POINTS]; - float pitch_curve[NUM_CURVE_POINTS]; - float yaw_collective_pitch_scale; - float yaw_collective_pitch_offset; - float yaw_throttle_scale; - float yaw_sign; float spoolup_time; }; @@ -83,7 +76,6 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: float throttleSpoolupProgress(); - bool mainMotorEnaged(); void updateParams() override; @@ -107,12 +99,6 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua struct ParamHandles { ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; param_t num_swash_plate_servos; - param_t throttle_curve[NUM_CURVE_POINTS]; - param_t pitch_curve[NUM_CURVE_POINTS]; - param_t yaw_collective_pitch_scale; - param_t yaw_collective_pitch_offset; - param_t yaw_throttle_scale; - param_t yaw_ccw; param_t spoolup_time; }; ParamHandles _param_handles{}; @@ -128,5 +114,4 @@ class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public Actua uint64_t _armed_time{0}; uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; - bool _main_motor_engaged{true}; }; From 258007ed3f10b83c06fd858b49efda7af5858d5d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 4 Apr 2023 17:53:50 +0200 Subject: [PATCH 6/9] HelicopterCoaxial: handle yaw saturation This had to be done for the integrators to work at all. --- .../ActuatorEffectivenessHelicopterCoaxial.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 4ea7c956d379..827714cd53b6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -116,13 +116,13 @@ void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector actuator_sp(0) = throttle - yaw; // Clockwise actuator_sp(1) = throttle + yaw; // Counter-clockwise - // Saturation check for yaw TODO check saturation - // if (actuator_sp(1) < actuator_min(1)) { - // setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); + // Saturation check for yaw + if ((actuator_sp(0) < actuator_min(0)) || (actuator_sp(1) > actuator_max(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); - // } else if (actuator_sp(1) > actuator_max(1)) { - // setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); - // } + } else if ((actuator_sp(0) > actuator_max(0)) || (actuator_sp(1) < actuator_min(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + } for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; From de42ca92460ca79a54af9a319767f2da43b4c412 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 6 Apr 2023 15:14:38 +0200 Subject: [PATCH 7/9] HelicopterCoaxial: correct constraining for minimum 2 swash plate servos --- .../ActuatorEffectivenessHelicopterCoaxial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 827714cd53b6..abb8351449a3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -67,7 +67,7 @@ void ActuatorEffectivenessHelicopterCoaxial::updateParams() return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain((int)count, 2, NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; From ff3458bb8378982871df66bfc825ad2cf8df0296 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 6 Apr 2023 20:13:03 +0200 Subject: [PATCH 8/9] HelicopterCoaxial: only publish unallocated thrust in the saturation case --- .../ActuatorEffectivenessHelicopterCoaxial.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index abb8351449a3..213df1e0ad46 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -185,6 +185,9 @@ void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_in } else if (_saturation_flags.roll_neg) { status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; } if (_saturation_flags.pitch_pos) { @@ -192,6 +195,9 @@ void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_in } else if (_saturation_flags.pitch_neg) { status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; } if (_saturation_flags.yaw_pos) { @@ -199,6 +205,9 @@ void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_in } else if (_saturation_flags.yaw_neg) { status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; } if (_saturation_flags.thrust_pos) { @@ -206,5 +215,8 @@ void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_in } else if (_saturation_flags.thrust_neg) { status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; } } From d97fcef4091f69e7220f3b3d9c4e69bb0fb26b72 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Sep 2023 17:31:38 +0200 Subject: [PATCH 9/9] ActuatorEffectivenessHelicopterCoaxial: fix copy paste error in saturation logic --- .../ActuatorEffectivenessHelicopterCoaxial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 213df1e0ad46..0c06f5963f08 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -217,6 +217,6 @@ void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_in status.unallocated_thrust[2] = -1.f; } else { - status.unallocated_torque[2] = 0.f; + status.unallocated_thrust[2] = 0.f; } }