From b3b373e074d94b3ab9def733a094719d9fa48f26 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 27 Mar 2023 16:43:09 +0200 Subject: [PATCH] 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 0000000000..e36709adf4 --- /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 0000000000..57f36b2db9 --- /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 18c0679d10..994b566270 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 2b08e8a327..0fad1b5f62 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 d04d1a5bf7..25904f0551 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 @@ private: 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 97c9fbe8dc..2c506cc4e2 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