control_allocator: add support for Tailsitter VTOL

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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

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

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

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

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 SYS_CTRL_ALLOC 1
-param set-default CA_AIRFRAME 2
+param set-default CA_AIRFRAME 3
 
 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 0.1515

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

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

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

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 <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)};
+};

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

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;

src/modules/control_allocator/ControlAllocator.hpp

@@ -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,
+		FIXED_WING = 1,
-		TILTROTOR_VTOL = 2,
+		STANDARD_VTOL = 2,
-		ROVER_ACKERMANN = 3,
+		TILTROTOR_VTOL = 3,
-		ROVER_DIFFERENTIAL = 4,
+		TAILSITTER_VTOL = 4,
-		FIXED_WING = 5,
+		ROVER_ACKERMANN = 5,
-		MOTORS_6DOF = 6,
+		ROVER_DIFFERENTIAL = 6,
+		MOTORS_6DOF = 7,
 	};
 
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};

src/modules/control_allocator/module.yaml

@@ -16,12 +16,13 @@ parameters:
             type: enum
             values:
                 0: Multirotor
-                1: Standard VTOL (WIP)
+                1: Fixed-wing
-                2: Tiltrotor VTOL (WIP)
+                2: Standard VTOL
-                3: Rover (Ackermann)
+                3: Tiltrotor VTOL
-                4: Rover (Differential)
+                4: Tailsitter VTOL
-                5: Fixed Wing
+                5: Rover (Ackermann)
-                5: Motors (6DOF)
+                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
+            7: # Motors (6DOF)
-                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)
                 actuators:
                   - actuator_type: 'motor'
                     count: 'CA_ROTOR_COUNT'

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;
 

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;