dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

control_allocator: add support for Tailsitter VTOL 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-12-08 09:23:33 +01:00 committed by Daniel Agar
parent 0568cff299
commit 07306c4be3
15 changed files with 340 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
View File

@ -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

39
ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
View File

@ -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

4
ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover
View File

@ -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

89
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp Normal file
View File

@ -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;
}
}

81
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp Normal file
View File

@ -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 <uORB/topics/actuator_controls.h>
#include <uORB/Subscription.hpp>
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)};
};

2
src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
View File

@ -46,6 +46,8 @@ px4_add_library(ActuatorEffectiveness
ActuatorEffectivenessStandardVTOL.hpp
ActuatorEffectivenessTiltrotorVTOL.cpp
ActuatorEffectivenessTiltrotorVTOL.hpp
ActuatorEffectivenessTailsitterVTOL.cpp
ActuatorEffectivenessTailsitterVTOL.hpp
ActuatorEffectivenessRoverAckermann.hpp
ActuatorEffectivenessRoverAckermann.cpp
ActuatorEffectivenessRoverDifferential.hpp

4
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -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;

14
src/modules/control_allocator/ControlAllocator.hpp
View File

@ -45,6 +45,7 @@
#include <ActuatorEffectivenessRotors.hpp>
#include <ActuatorEffectivenessStandardVTOL.hpp>
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
#include <ActuatorEffectivenessTailsitterVTOL.hpp>
#include <ActuatorEffectivenessRoverAckermann.hpp>
#include <ActuatorEffectivenessRoverDifferential.hpp>
#include <ActuatorEffectivenessFixedWing.hpp>
@ -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};

117
src/modules/control_allocator/module.yaml
View File

@ -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'

4
src/modules/vtol_att_control/standard.cpp
View File

@ -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;

36
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -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;