From 07306c4be3ad9ab7114c6d94451cbef9938f7738 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 8 Dec 2021 09:23:33 +0100 Subject: [PATCH] control_allocator: add support for Tailsitter VTOL Signed-off-by: Silvan Fuhrer --- .../airframes/1022_uuv_bluerov2_heavy | 2 +- .../init.d-posix/airframes/1030_plane | 2 +- .../init.d-posix/airframes/1040_standard_vtol | 2 +- .../init.d-posix/airframes/1041_tailsitter | 39 +++++- .../init.d-posix/airframes/1042_tiltrotor | 4 +- .../init.d-posix/airframes/1060_rover | 2 +- .../init.d-posix/airframes/1061_r1_rover | 2 +- .../ActuatorEffectivenessTailsitterVTOL.cpp | 89 +++++++++++++ .../ActuatorEffectivenessTailsitterVTOL.hpp | 81 ++++++++++++ .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../control_allocator/ControlAllocator.cpp | 4 + .../control_allocator/ControlAllocator.hpp | 14 ++- src/modules/control_allocator/module.yaml | 117 +++++++++++------- src/modules/vtol_att_control/standard.cpp | 4 +- src/modules/vtol_att_control/tailsitter.cpp | 36 ++++++ 15 files changed, 340 insertions(+), 60 deletions(-) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy index a212fa18d2..116f08cc24 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy @@ -9,7 +9,7 @@ param set-default CBRK_AIRSPD_CHK 162128 param set-default SYS_CTRL_ALLOC 1 -param set-default CA_AIRFRAME 6 +param set-default CA_AIRFRAME 7 param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 255 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane index 6968455cd6..2d30fa33d0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane @@ -44,7 +44,7 @@ param set-default NAV_DLL_ACT 2 param set-default RWTO_TKOFF 1 #param set-default SYS_CTRL_ALLOC 1 -param set-default CA_AIRFRAME 5 +param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 param set-default CA_ROTOR0_PX 0.3 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol index c0436573a4..0206e9d191 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol @@ -8,7 +8,7 @@ . ${R}etc/init.d/rc.vtol_defaults #param set-default SYS_CTRL_ALLOC 1 -param set-default CA_AIRFRAME 1 +param set-default CA_AIRFRAME 2 param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR0_PX 0.1515 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter index 4fab7d758d..723d8f9f0a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter @@ -7,14 +7,47 @@ . ${R}etc/init.d/rc.vtol_defaults +# param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 4 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 2 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 1 +param set-default CA_ROTOR2_PY -1 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -1 +param set-default CA_ROTOR3_PY 1 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_SV_CS_COUNT 2 +param set-default CA_SV_CS0_TYPE 9 +param set-default CA_SV_CS0_TRQ_P -1 +param set-default CA_SV_CS0_TRQ_Y -1 +param set-default CA_SV_CS1_TYPE 10 +param set-default CA_SV_CS1_TRQ_P -1 +param set-default CA_SV_CS1_TRQ_Y 1 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 0 +param set-default PWM_MAIN_FUNC6 201 +param set-default PWM_MAIN_FUNC7 202 + param set-default FW_L1_PERIOD 12 param set-default FW_MAN_P_MAX 30 param set-default FW_PR_I 0.2 -param set-default FW_PR_P 0.3 +param set-default FW_PR_P 0.2 param set-default FW_PSP_OFF 2 param set-default FW_P_LIM_MAX 32 param set-default FW_P_LIM_MIN -15 -param set-default FW_RR_P 0.3 +param set-default FW_RR_P 0.2 param set-default FW_THR_CRUISE 0.33 param set-default FW_THR_MAX 0.6 param set-default FW_THR_MIN 0.05 @@ -36,6 +69,8 @@ param set-default MPC_XY_VEL_D_ACC 0.1 param set-default NAV_ACC_RAD 5 param set-default NAV_LOITER_RAD 80 +param set-default VT_FW_DIFTHR_EN 1 +param set-default VT_FW_DIFTHR_SC 0.5 param set-default VT_F_TRANS_DUR 1.5 param set-default VT_F_TRANS_THR 0.7 param set-default VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor index 973d42aba3..f2fe9de009 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor @@ -7,8 +7,8 @@ . ${R}etc/init.d/rc.vtol_defaults -#param set-default SYS_CTRL_ALLOC 1 -param set-default CA_AIRFRAME 2 +# param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 3 param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR0_PX 0.1515 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover index d8f9c9aaa4..e8f68f70b9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover @@ -29,7 +29,7 @@ param set-default GND_MAX_ANG 0.6 param set-default GND_WHEEL_BASE 2.0 param set-default SYS_CTRL_ALLOC 1 -param set-default CA_AIRFRAME 3 +param set-default CA_AIRFRAME 5 param set-default CA_R_REV 1 param set-default PWM_MAIN_FUNC1 201 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover index c95a884c5e..3773e6e3d0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover @@ -29,7 +29,7 @@ param set-default GND_MAX_ANG 0.6 param set-default GND_WHEEL_BASE 2.0 param set-default SYS_CTRL_ALLOC 1 -param set-default CA_AIRFRAME 4 +param set-default CA_AIRFRAME 6 param set-default CA_R_REV 3 param set-default PWM_MAIN_FUNC1 101 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp new file mode 100644 index 0000000000..f6dc25d1c7 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessTailsitterVTOL.hpp + * + * Actuator effectiveness for tailsitter VTOL + */ + +#include "ActuatorEffectivenessTailsitterVTOL.hpp" + +using namespace matrix; + +ActuatorEffectivenessTailsitterVTOL::ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent) + : ModuleParams(parent), _mc_rotors(this), _control_surfaces(this) +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} +bool +ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &configuration, bool force) +{ + if (!force) { + return false; + } + + // MC motors + configuration.selected_matrix = 0; + _mc_rotors.enableYawControl(true); //TODO enable yaw with elevons + _mc_rotors.getEffectivenessMatrix(configuration, true); + + // 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 ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + if (_flight_phase == flight_phase) { + return; + } + + ActuatorEffectiveness::setFlightPhase(flight_phase); + + // update stopped motors //TODO: add option to switch off certain motors in FW + switch (flight_phase) { + case FlightPhase::FORWARD_FLIGHT: + _stopped_motors = 0; + 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/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp new file mode 100644 index 0000000000..49ba6d8974 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 ActuatorEffectivenessTailsitterVTOL.hpp + * + * Actuator effectiveness for tailsitter VTOL + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" +#include "ActuatorEffectivenessControlSurfaces.hpp" + +#include +#include + +class ActuatorEffectivenessTailsitterVTOL : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessTailsitterVTOL(ModuleParams *parent); + virtual ~ActuatorEffectivenessTailsitterVTOL() = default; + + 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; + } + + void setFlightPhase(const FlightPhase &flight_phase) override; + + const char *name() const override { return "VTOL Tailsitter"; } + + uint32_t getStoppedMotors() const override { return _stopped_motors; } +protected: + bool _updated{true}; + ActuatorEffectivenessRotors _mc_rotors; + ActuatorEffectivenessControlSurfaces _control_surfaces; + + 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_1)}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index f5919f05fc..c0a1eae0dc 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -46,6 +46,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessStandardVTOL.hpp ActuatorEffectivenessTiltrotorVTOL.cpp ActuatorEffectivenessTiltrotorVTOL.hpp + ActuatorEffectivenessTailsitterVTOL.cpp + ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp ActuatorEffectivenessRoverDifferential.hpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 75d22be801..5eb5e3e27f 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -215,6 +215,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessTiltrotorVTOL(this); break; + case EffectivenessSource::TAILSITTER_VTOL: + tmp = new ActuatorEffectivenessTailsitterVTOL(this); + break; + case EffectivenessSource::ROVER_ACKERMANN: tmp = new ActuatorEffectivenessRoverAckermann(); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 963040df34..e4b7fb6a20 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -136,12 +137,13 @@ private: enum class EffectivenessSource { NONE = -1, MULTIROTOR = 0, - STANDARD_VTOL = 1, - TILTROTOR_VTOL = 2, - ROVER_ACKERMANN = 3, - ROVER_DIFFERENTIAL = 4, - FIXED_WING = 5, - MOTORS_6DOF = 6, + FIXED_WING = 1, + STANDARD_VTOL = 2, + TILTROTOR_VTOL = 3, + TAILSITTER_VTOL = 4, + ROVER_ACKERMANN = 5, + ROVER_DIFFERENTIAL = 6, + MOTORS_6DOF = 7, }; EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index b427ddf570..e9c15a8f32 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -16,12 +16,13 @@ parameters: type: enum values: 0: Multirotor - 1: Standard VTOL (WIP) - 2: Tiltrotor VTOL (WIP) - 3: Rover (Ackermann) - 4: Rover (Differential) - 5: Fixed Wing - 5: Motors (6DOF) + 1: Fixed-wing + 2: Standard VTOL + 3: Tiltrotor VTOL + 4: Tailsitter VTOL + 5: Rover (Ackermann) + 6: Rover (Differential) + 7: Motors (6DOF) default: 0 CA_METHOD: @@ -449,7 +450,36 @@ mixer: function: 'spin-dir' show_as: 'true-if-positive' - 1: # Standard VTOL + 1: # Fixed Wing + actuators: + - actuator_type: 'motor' + group_label: 'Motors' + count: 'CA_ROTOR_COUNT' + per_item_parameters: + standard: + position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] + - actuator_type: 'servo' + 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 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' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + + 2: # Standard VTOL actuators: - actuator_type: 'motor' group_label: 'MC Motors' @@ -487,7 +517,7 @@ mixer: - name: 'CA_SV_CS${i}_TRIM' label: 'Trim' - 2: # Tiltrotor VTOL + 3: # Tiltrotor VTOL actuators: - actuator_type: 'motor' group_label: 'MC Motors' @@ -509,13 +539,13 @@ mixer: function: 'type' identifier: 'servo-type' - name: 'CA_SV_CS${i}_TRQ_R' - label: 'Roll Torque' + label: 'Roll Scale' identifier: 'servo-torque-roll' - name: 'CA_SV_CS${i}_TRQ_P' - label: 'Pitch Torque' + label: 'Pitch Scale' identifier: 'servo-torque-pitch' - name: 'CA_SV_CS${i}_TRQ_Y' - label: 'Yaw Torque' + label: 'Yaw Scale' identifier: 'servo-torque-yaw' - name: 'CA_SV_CS${i}_TRIM' label: 'Trim' @@ -534,7 +564,37 @@ mixer: - name: 'CA_SV_TL${i}_CT' label: 'Use for Control' - 3: # Rover (Ackermann) + 4: # Tailsitter 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' ] + - 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 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' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + + 5: # Rover (Ackermann) actuators: - actuator_type: 'motor' instances: @@ -545,7 +605,7 @@ mixer: - name: 'Steering' position: [ 1., 0, 0 ] - 4: # Rover (Differential) + 6: # Rover (Differential) actuators: - actuator_type: 'motor' instances: @@ -554,36 +614,7 @@ mixer: - name: 'Right Motor' position: [ 0, 1., 0 ] - 5: # Fixed Wing - actuators: - - actuator_type: 'motor' - group_label: 'Motors' - count: 'CA_ROTOR_COUNT' - per_item_parameters: - standard: - position: [ 'CA_ROTOR${i}_PX', 'CA_ROTOR${i}_PY', 'CA_ROTOR${i}_PZ' ] - - actuator_type: 'servo' - 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 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' - - name: 'CA_SV_CS${i}_TRIM' - label: 'Trim' - - 6: # Motors (6DOF) + 7: # Motors (6DOF) actuators: - actuator_type: 'motor' count: 'CA_ROTOR_COUNT' diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index f0c8546975..ea1a9628ee 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -429,11 +429,11 @@ void Standard::fill_actuator_outputs() _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; _thrust_setpoint_0->xyz[0] = 0.f; _thrust_setpoint_0->xyz[1] = 0.f; - _thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_0->xyz[2] = -mc_out[actuator_controls_s::INDEX_THROTTLE]; _thrust_setpoint_1->timestamp = hrt_absolute_time(); _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - _thrust_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_THROTTLE]; _thrust_setpoint_1->xyz[1] = 0.f; _thrust_setpoint_1->xyz[2] = 0.f; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 50bf2943da..f60586d386 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -311,6 +311,31 @@ void Tailsitter::fill_actuator_outputs() auto &mc_out = _actuators_out_0->control; auto &fw_out = _actuators_out_1->control; + _torque_setpoint_0->timestamp = hrt_absolute_time(); + _torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _torque_setpoint_0->xyz[0] = 0.f; + _torque_setpoint_0->xyz[1] = 0.f; + _torque_setpoint_0->xyz[2] = 0.f; + + _torque_setpoint_1->timestamp = hrt_absolute_time(); + _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _torque_setpoint_1->xyz[0] = 0.f; + _torque_setpoint_1->xyz[1] = 0.f; + _torque_setpoint_1->xyz[2] = 0.f; + + _thrust_setpoint_0->timestamp = hrt_absolute_time(); + _thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _thrust_setpoint_0->xyz[0] = 0.f; + _thrust_setpoint_0->xyz[1] = 0.f; + _thrust_setpoint_0->xyz[2] = 0.f; + + _thrust_setpoint_1->timestamp = hrt_absolute_time(); + _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _thrust_setpoint_1->xyz[0] = 0.f; + _thrust_setpoint_1->xyz[1] = 0.f; + _thrust_setpoint_1->xyz[2] = 0.f; + + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; @@ -318,13 +343,22 @@ void Tailsitter::fill_actuator_outputs() if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + // FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation + _thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE]; + /* allow differential thrust if enabled */ if (_params->diff_thrust == 1) { mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; + _torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; } } else { + _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + _torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; + _thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE]; } if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { @@ -334,6 +368,8 @@ void Tailsitter::fill_actuator_outputs() } else { fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH]; + _torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_ROLL]; } _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;