Tailsitter: only support single matrix and mc rate controller, and CA only

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

ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert

@@ -8,10 +8,10 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
-param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_MC_CS_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter

@@ -9,12 +9,12 @@
 
 param set-default MAV_TYPE 20
 
-# param set-default SYS_CTRL_ALLOC 1
+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_PY 1
 param set-default CA_ROTOR0_KM 0.05
 param set-default CA_ROTOR1_PX -1
 param set-default CA_ROTOR1_PY -1
@@ -26,22 +26,10 @@ 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 5
-param set-default CA_SV_CS0_TRQ_P 0.5
-param set-default CA_SV_CS0_TRQ_Y -0.5
-param set-default CA_SV_CS1_TYPE 6
-param set-default CA_SV_CS1_TRQ_P 0.5
-param set-default CA_SV_CS1_TRQ_Y 0.5
-
 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 PWM_MAIN_REV 96 # invert both elevons
 
 param set-default FW_L1_PERIOD 12
 param set-default FW_MAN_P_MAX 30
@@ -69,15 +57,29 @@ param set-default MPC_XY_VEL_P_ACC 3
 param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1
 
-param set-default NAV_ACC_RAD 5
-
-param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.5
+param set-default VT_FW_DIFTHR_EN 7
+param set-default VT_FW_DIFTHR_S_P 1
+param set-default VT_FW_DIFTHR_S_R 1
+param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0
 
 param set-default WV_EN 0
 
+#uncomment the following block to enable elevons (otherwise only motors are used for attitdue control)
+# param set-default CA_SV_CS_COUNT 2
+# param set-default CA_SV_CS0_TYPE 5
+# param set-default CA_SV_CS0_TRQ_P 0.5
+# param set-default CA_SV_CS0_TRQ_Y -0.5
+# param set-default CA_SV_CS1_TYPE 6
+# param set-default CA_SV_CS1_TRQ_P 0.5
+# param set-default CA_SV_CS1_TRQ_Y 0.5
+# param set-default PWM_MAIN_FUNC6 201
+# param set-default PWM_MAIN_FUNC7 202
+# param set-default PWM_MAIN_REV 96 # invert both elevons
+# param set-default VT_FW_DIFTHR_EN 1
+
+
 set MIXER_FILE etc/mixers-sitl/quad_x_vtol.main.mix
 set MIXER custom

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -18,11 +18,11 @@
 . ${R}etc/init.d/rc.vtol_defaults
 
 param set UAVCAN_ENABLE 0
-param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_MC_CS_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
 

ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol

@@ -34,7 +34,7 @@ param set-default MC_YAWRATE_P 0.22
 param set-default MC_YAWRATE_I 0.02
 
 param set-default VT_IDLE_PWM_MC  1080
-param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_MC_CS_LOCK 0
 param set-default VT_MOT_ID 12
 param set-default VT_TYPE 0
 

ROMFS/px4fmu_common/init.d/airframes/13002_firefly6

@@ -44,7 +44,7 @@ param set-default VT_FW_MOT_OFFID 56
 param set-default VT_TILT_MC 0.08
 param set-default VT_TILT_TRANS 0.5
 param set-default VT_TILT_FW 0.9
-param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_MC_CS_LOCK 0
 param set-default VT_TYPE 1
 
 set MIXER firefly6

ROMFS/px4fmu_common/init.d/airframes/13012_convergence

@@ -57,14 +57,14 @@ param set-default VT_B_TRANS_DUR  1
 param set-default VT_F_TRANS_DUR  1.2
 param set-default VT_F_TR_OL_TM   4
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.17
+param set-default VT_FW_DIFTHR_S_Y 0.17
 param set-default VT_FW_MOT_OFFID 3
 param set-default VT_IDLE_PWM_MC 1200
 param set-default VT_MOT_ID 123
 param set-default VT_TILT_TRANS   0.45
 param set-default VT_TRANS_MIN_TM 1.2
 param set-default VT_TRANS_P2_DUR 1.3
-param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_MC_CS_LOCK 0
 param set-default VT_TYPE 1
 
 set MIXER vtol_convergence
@@ -74,4 +74,4 @@ then
 	set PWM_OUT 1234
 else
 	set PWM_OUT 3456
-fi	
+fi

ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark

@@ -86,7 +86,7 @@ param set-default VT_ARSP_BLEND 10
 param set-default VT_ARSP_TRANS 21
 param set-default VT_B_DEC_MSS 1.5
 param set-default VT_B_TRANS_DUR 12
-param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_MC_CS_LOCK 0
 param set-default VT_FWD_THRUST_SC 1.2
 param set-default VT_FW_MOT_OFFID 5678
 param set-default VT_F_TR_OL_TM 8

ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter

@@ -21,7 +21,7 @@
 
 param set-default MAV_TYPE 19
 
-param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_MC_CS_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 

ROMFS/px4fmu_common/init.d/airframes/13201_swan_quad_tailsitter

@@ -0,0 +1,39 @@
+#!/bin/sh
+#
+# @name Swan K1 Quad Tailsitter VTOL
+#
+# @type VTOL Quad Tailsitter
+# @class VTOL
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default MAV_TYPE 20
+
+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 0.15
+param set-default CA_ROTOR0_PY 0.23
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.23
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.23
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.23
+param set-default CA_ROTOR3_KM -0.05
+
+#output allocation
+#min pwm (1100)
+#controller tuning
+
+param set-default VT_MC_CS_LOCK 0
+
+param set-default VT_TYPE 0
+
+param set-default SENS_EN_MS4525DO 1

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -130,6 +130,7 @@ px4_add_romfs_files(
 	13030_generic_vtol_quad_tiltrotor
 	13050_generic_vtol_octo
 	13200_generic_vtol_tailsitter
+	13201_swan_quad_tailsitter
 
 	# [14000, 14999] Tri Y
 	14001_tri_y_yaw+

src/lib/parameters/param_translation.cpp

@@ -255,5 +255,23 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2022-07-18: translate VT_ELEV_MC_LOCK->VT_MC_CS_LOCK
+	{
+		if (strcmp("VT_ELEV_MC_LOCK", node->name) == 0) {
+			strcpy(node->name, "VT_MC_CS_LOCK");
+			PX4_INFO("copying %s -> %s", "VT_ELEV_MC_LOCK", "VT_MC_CS_LOCK");
+			return true;
+		}
+	}
+
+	// 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y
+	{
+		if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) {
+			strcpy(node->name, "VT_FW_DIFTHR_S_Y");
+			PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y");
+			return true;
+		}
+	}
+
 	return false;
 }

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -61,7 +61,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
 	// Control Surfaces
-	configuration.selected_matrix = 1;
+	configuration.selected_matrix = 0; // for tailsitter we only use one matrix, as opposed to the other VTOL types
 	_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
 	const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
 

src/modules/control_allocator/module.yaml

@@ -644,7 +644,7 @@ mixer:
                             label: 'Trim'
                     parameters:
                       - label: 'Lock control surfaces in hover'
-                        name: VT_ELEV_MC_LOCK
+                        name: VT_MC_CS_LOCK
 
             3: # Tiltrotor VTOL
                 title: 'Tiltrotor VTOL'
@@ -685,7 +685,7 @@ mixer:
                             label: 'Trim'
                     parameters:
                       - label: 'Lock control surfaces in hover'
-                        name: VT_ELEV_MC_LOCK
+                        name: VT_MC_CS_LOCK
                   - actuator_type: 'servo'
                     group_label: 'Tilt Servos'
                     count: 'CA_SV_TL_COUNT'
@@ -738,7 +738,7 @@ mixer:
                             label: 'Trim'
                     parameters:
                       - label: 'Lock control surfaces in hover'
-                        name: VT_ELEV_MC_LOCK
+                        name: VT_MC_CS_LOCK
 
             5: # Rover (Ackermann)
                 title: 'Rover (Ackermann)'

src/modules/ekf2/EKF2.cpp

@@ -532,6 +532,7 @@ void EKF2::Run()
 
 				_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
 						vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+				_is_tailsitter_in_fw = is_fixed_wing && vehicle_status.is_vtol_tailsitter;
 			}
 		}
 
@@ -667,6 +668,40 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
 		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+
+		/* get current rotation matrix and euler angles from control state quaternions */
+		matrix::Dcmf R = matrix::Quatf(att.q);
+
+		if (_is_tailsitter_in_fw) {
+			/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
+			 * */
+			matrix::Dcmf R_adapted = R;		//modified rotation matrix
+
+			/* move z to x */
+			R_adapted(0, 0) = R(0, 2);
+			R_adapted(1, 0) = R(1, 2);
+			R_adapted(2, 0) = R(2, 2);
+
+			/* move x to z */
+			R_adapted(0, 2) = R(0, 0);
+			R_adapted(1, 2) = R(1, 0);
+			R_adapted(2, 2) = R(2, 0);
+
+			/* change direction of pitch (convert to right handed system) */
+			R_adapted(0, 0) = -R_adapted(0, 0);
+			R_adapted(1, 0) = -R_adapted(1, 0);
+			R_adapted(2, 0) = -R_adapted(2, 0);
+
+			/* fill in new attitude data */
+			R = R_adapted;
+
+			const matrix::Eulerf euler_angles(R);
+
+			const matrix::Quatf quat_new(matrix::Eulerf(euler_angles.phi(), euler_angles.theta(), euler_angles.psi()));
+
+			quat_new.copyTo(att.q);
+		}
+
 		_attitude_pub.publish(att);
 
 	}  else if (_replay_mode) {

src/modules/ekf2/EKF2.hpp

@@ -183,6 +183,8 @@ private:
 		}
 	}
 
+	bool _is_tailsitter_in_fw{false};
+
 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
 	 */

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -151,6 +151,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 
 					_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
 					_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
+					// TODO rotate thrust prior to publication for tailsitter?
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
@@ -319,20 +320,20 @@ void FixedwingAttitudeControl::Run()
 			 * */
 			matrix::Dcmf R_adapted = R;		//modified rotation matrix
 
-			/* move z to x */
-			R_adapted(0, 0) = R(0, 2);
-			R_adapted(1, 0) = R(1, 2);
-			R_adapted(2, 0) = R(2, 2);
+			// /* move z to x */
+			// R_adapted(0, 0) = R(0, 2);
+			// R_adapted(1, 0) = R(1, 2);
+			// R_adapted(2, 0) = R(2, 2);
 
-			/* move x to z */
-			R_adapted(0, 2) = R(0, 0);
-			R_adapted(1, 2) = R(1, 0);
-			R_adapted(2, 2) = R(2, 0);
+			// /* move x to z */
+			// R_adapted(0, 2) = R(0, 0);
+			// R_adapted(1, 2) = R(1, 0);
+			// R_adapted(2, 2) = R(2, 0);
 
-			/* change direction of pitch (convert to right handed system) */
-			R_adapted(0, 0) = -R_adapted(0, 0);
-			R_adapted(1, 0) = -R_adapted(1, 0);
-			R_adapted(2, 0) = -R_adapted(2, 0);
+			// /* change direction of pitch (convert to right handed system) */
+			// R_adapted(0, 0) = -R_adapted(0, 0);
+			// R_adapted(1, 0) = -R_adapted(1, 0);
+			// R_adapted(2, 0) = -R_adapted(2, 0);
 
 			/* fill in new attitude data */
 			R = R_adapted;
@@ -562,10 +563,10 @@ void FixedwingAttitudeControl::Run()
 
 					_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
 
-					/* add in manual rudder control in manual modes */
-					if (_vcontrol_mode.flag_control_manual_enabled) {
-						_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
-					}
+					// /* add in manual rudder control in manual modes */
+					// if (_vcontrol_mode.flag_control_manual_enabled) {
+					// 	_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
+					// }
 
 					if (!PX4_ISFINITE(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
@@ -598,10 +599,23 @@ void FixedwingAttitudeControl::Run()
 				 */
 				_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
 				_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
-				_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
+
+				/* add yaw rate setpoint from sticks in Stabilized mode */
+				if (_vcontrol_mode.flag_control_manual_enabled && _vcontrol_mode.flag_control_attitude_enabled) {
+					_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate() + _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
+				}
 
 				_rates_sp.timestamp = hrt_absolute_time();
 
+				// rotate output of FW attitude controller to body frame for a tailsitter
+				if (_vehicle_status.is_vtol_tailsitter) {
+					const float tmp = _rates_sp.roll;
+					_rates_sp.roll = _rates_sp.yaw;
+					_rates_sp.yaw = -tmp;
+					_rates_sp.thrust_body[2] = -_rates_sp.thrust_body[0];
+					_rates_sp.thrust_body[0] = 0.f;
+				}
+
 				_rate_sp_pub.publish(_rates_sp);
 
 			} else {

src/modules/mc_att_control/mc_att_control.hpp

@@ -153,7 +153,8 @@ private:
 		(ParamFloat<px4::params::MPC_MANTHR_MIN>)   _param_mpc_manthr_min,      /**< minimum throttle for stabilized */
 		(ParamFloat<px4::params::MPC_THR_MAX>)      _param_mpc_thr_max,         /**< maximum throttle for stabilized */
 		(ParamFloat<px4::params::MPC_THR_HOVER>)    _param_mpc_thr_hover,       /**< throttle at stationary hover */
-		(ParamInt<px4::params::MPC_THR_CURVE>)      _param_mpc_thr_curve        /**< throttle curve behavior */
+		(ParamInt<px4::params::MPC_THR_CURVE>)      _param_mpc_thr_curve,        /**< throttle curve behavior */
+		(ParamInt<px4::params::MC_ATT_VTOL_WAY>) _param_mc_att_vtol_way
 	)
 };
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -164,7 +164,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
 	attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2);
 
 	/* modify roll/pitch only if we're a VTOL */
-	if (_vtol) {
+	if (_vtol && _param_mc_att_vtol_way.get()) {
 		// Construct attitude setpoint rotation matrix. Modify the setpoints for roll
 		// and pitch such that they reflect the user's intention even if a large yaw error
 		// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
@@ -303,10 +303,12 @@ MulticopterAttitudeControl::Run()
 
 		const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode);
 
-		// vehicle is a tailsitter in transition mode
 		const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
 
-		bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
+		const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing
+
+		bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition
+				    || run_mc_att_c_in_fw_tailsitter);
 
 		if (run_att_ctrl) {
 

src/modules/mc_att_control/mc_att_control_params.c

@@ -158,3 +158,11 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f);
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MC_MAN_TILT_TAU, 0.0f);
+
+/**
+ * Generate manual attitude sp VTOL way
+ *
+ * @boolean
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_INT32(MC_ATT_VTOL_WAY, 1);

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -209,8 +209,9 @@ MulticopterRateControl::Run()
 		// run the rate controller
 		if (_vehicle_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
 
-			// reset integral if disarmed
-			if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			// reset integral if disarmed or we do not run the controller (VTOLs not in hover except tailsitter)
+			if (!_vehicle_control_mode.flag_armed || (_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+					&& !_vehicle_status.is_vtol_tailsitter)) {
 				_rate_control.resetIntegral();
 			}
 

src/modules/vtol_att_control/standard.cpp

@@ -341,9 +341,9 @@ void Standard::fill_actuator_outputs()
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
 
 		// FW out = 0, other than roll and pitch depending on elevon lock
-		fw_out[actuator_controls_s::INDEX_ROLL]         = _param_vt_elev_mc_lock.get() ? 0 :
+		fw_out[actuator_controls_s::INDEX_ROLL]         = _param_vt_mc_cs_lock.get() ? 0 :
 				fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH]        = _param_vt_elev_mc_lock.get() ? 0 :
+		fw_out[actuator_controls_s::INDEX_PITCH]        = _param_vt_mc_cs_lock.get() ? 0 :
 				fw_in[actuator_controls_s::INDEX_PITCH];
 		fw_out[actuator_controls_s::INDEX_YAW]          = 0;
 		fw_out[actuator_controls_s::INDEX_THROTTLE]     = _pusher_throttle;

src/modules/vtol_att_control/tailsitter.cpp

@@ -44,7 +44,7 @@
 
 #include <uORB/topics/landing_gear.h>
 
-#define PITCH_TRANSITION_FRONT_P1 -1.1f	// pitch angle to switch to TRANSITION_P2
+#define PITCH_TRANSITION_FRONT_P1 -0.5f	// pitch angle to switch to TRANSITION_P2 (28°)
 #define PITCH_TRANSITION_BACK -0.25f	// pitch angle to switch to MC
 
 using namespace matrix;
@@ -206,7 +206,15 @@ void Tailsitter::update_transition_state()
 
 			// the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint,
 			// the yaw setpoint and zero roll since we want wings level transition
-			_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
+
+			const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing
+
+			if (run_mc_att_c_in_fw_tailsitter) {
+				_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, yaw_sp);
+
+			} else {
+				_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
+			}
 
 			// attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
 			// multirotor frame
@@ -285,7 +293,60 @@ void Tailsitter::waiting_on_tecs()
 
 void Tailsitter::update_fw_state()
 {
-	VtolType::update_fw_state();
+	if (_flag_idle_mc) {
+		_flag_idle_mc = !set_idle_fw();
+	}
+
+	const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing
+
+	// copy virtual attitude setpoint to real attitude setpoint
+	if (run_mc_att_c_in_fw_tailsitter) {
+		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
+	} else {
+		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+	}
+
+	check_quadchute_condition();
+
+	// printf("""""""""""""\n");
+	// printf("roll sp: %f\n", (double)_v_att_sp->roll_body);
+	// printf("pitch sp: (raw) %f\n", (double)_v_att_sp->pitch_body);
+	// printf("yaw_sp: %f\n", (double)_v_att_sp->yaw_body);
+
+	// // rotate setpoint 90° down
+	// _v_att_sp->roll_body = _v_att_sp->roll_body;
+	// _v_att_sp->pitch_body = _v_att_sp->pitch_body - M_PI_2_F;
+	// _v_att_sp->yaw_body = _v_att_sp->yaw_body;
+	// Quatf q(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+	// q.copyTo(_v_att_sp->q_d);
+
+	// // calculate rotation axis for transition.
+	// _q_trans_start = Quatf(_v_att->q);
+	// Vector3f z = -_q_trans_start.dcm_z();
+	// _trans_rot_axis = z.cross(Vector3f(0, 0, -1));
+
+	// _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, M_PI_2_F));
+
+	// // as heading setpoint we choose the heading given by the direction the vehicle points
+	// float yaw_sp = atan2f(z(1), z(0));
+
+	// // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint,
+	// // the yaw setpoint and zero roll since we want wings level transition
+	// _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
+
+
+	// const Eulerf euler_sp(_q_trans_sp);
+	// _v_att_sp->roll_body = euler_sp.phi();
+	// _v_att_sp->pitch_body = euler_sp.theta();
+	// _v_att_sp->yaw_body = euler_sp.psi();
+
+
+	// printf("roll sp: %f\n", (double)_v_att_sp->roll_body);
+	// printf("pitch sp: (rotated) %f\n", (double)_v_att_sp->pitch_body);
+	// printf("yaw_sp: %f\n", (double)_v_att_sp->yaw_body);
+
+	// _q_trans_sp.copyTo(_v_att_sp->q_d);
 
 }
 
@@ -295,10 +356,8 @@ void Tailsitter::update_fw_state()
 void Tailsitter::fill_actuator_outputs()
 {
 	auto &mc_in = _actuators_mc_in->control;
-	auto &fw_in = _actuators_fw_in->control;
 
 	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;
@@ -306,50 +365,25 @@ void Tailsitter::fill_actuator_outputs()
 	_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_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH];
-	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW];
-
 	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 (_param_vt_fw_difthr_en.get()) {
-			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
-			_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
-		}
+		_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_r.get();
+		_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ;
+		_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_y.get();
 
 	} else {
 		_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL];
 		_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH];
 		_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW];
-
-		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];
 	}
 
+	_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE];
+
 	// Landing Gear
 	if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
@@ -358,19 +392,6 @@ void Tailsitter::fill_actuator_outputs()
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
 	}
 
-	if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
-		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
-		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
-
-	} 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;
-	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
-
 	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
 }

src/modules/vtol_att_control/tiltrotor.cpp

@@ -495,9 +495,9 @@ void Tiltrotor::fill_actuator_outputs()
 		_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
 
 		/* allow differential thrust if enabled */
-		if (_param_vt_fw_difthr_en.get()) {
-			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
-			_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
+		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
+			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
+			_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
 		}
 
 	} else {
@@ -518,7 +518,7 @@ void Tiltrotor::fill_actuator_outputs()
 	// Fixed wing output
 	fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
 
-	if (_param_vt_elev_mc_lock.get()  && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_param_vt_mc_cs_lock.get()  && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
 		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
 		fw_out[actuator_controls_s::INDEX_YAW]   = 0;

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -364,7 +364,7 @@ VtolAttitudeControl::Run()
 			// vehicle is in fw mode
 			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
 
-			if (fw_att_sp_updated) {
+			if (mc_att_sp_updated || fw_att_sp_updated) {
 				_vtol_type->update_fw_state();
 				_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
 			}

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(VT_TYPE, 0);
  * @boolean
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 1);
+PARAM_DEFINE_INT32(VT_MC_CS_LOCK, 1);
 
 /**
  * Duration of a front transition
@@ -286,29 +286,65 @@ PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);
 PARAM_DEFINE_INT32(VT_MOT_ID, 0);
 
 /**
- * Differential thrust in forwards flight.
+ * Differential thrust in forward flight
  *
- * Set to 1 to enable differential thrust in fixed-wing flight.
+ * Enable differential thrust seperately for roll, pitch, yaw in forward (fixed-wing) mode.
+ * The effectiveness of differential thrust around the corresponding axis can be
+ * tuned by setting VT_FW_DIFTHR_S_R / VT_FW_DIFTHR_S_P / VT_FW_DIFTHR_S_Y.
  *
  * @min 0
- * @max 1
- * @decimal 0
+ * @max 7
+ * @bit 0 Yaw
+ * @bit 1 Roll
+ * @bit 2 Pitch
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
 
 /**
- * Differential thrust scaling factor
+ * Roll differential thrust factor in forward flight
  *
- * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight.
+ * Maps the roll control output in forward flight to the actuator differential thrust output.
+ *
+ * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
  *
  * @min 0.0
- * @max 1.0
+ * @max 2.0
  * @decimal 2
  * @increment 0.1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_R, 1.f);
+
+/**
+ * Pitch differential thrust factor in forward flight
+ *
+ * Maps the pitch control output in forward flight to the actuator differential thrust output.
+ *
+ * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f);
+
+/**
+ * Yaw differential thrust factor in forward flight
+ *
+ * Maps the yaw control output in forward flight to the actuator differential thrust output.
+ *
+ * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f);
 
 /**
  * Backtransition deceleration setpoint to pitch feedforward gain.

src/modules/vtol_att_control/vtol_type.h

@@ -97,6 +97,13 @@ enum class pwm_limit_type {
 	TYPE_MAXIMUM
 };
 
+// enum for bitmask of VT_FW_DIFTHR_EN parameter options
+enum class VtFwDifthrEnBits : int32_t {
+	YAW_BIT = (1 << 0),
+	ROLL_BIT = (1 << 1),
+	PITCH_BIT = (1 << 2),
+};
+
 class VtolAttitudeControl;
 
 class VtolType:  public ModuleParams
@@ -274,7 +281,7 @@ protected:
 	float _dt{0.0025f}; // time step [s]
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
-					(ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
+					(ParamBool<px4::params::VT_MC_CS_LOCK>) _param_vt_mc_cs_lock,
 					(ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,
 					(ParamFloat<px4::params::VT_FW_ALT_ERR>) _param_vt_fw_alt_err,
 					(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
@@ -291,8 +298,10 @@ protected:
 					(ParamBool<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
 					(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
 					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
-					(ParamBool<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
-					(ParamFloat<px4::params::VT_FW_DIFTHR_SC>) _param_vt_fw_difthr_sc,
+					(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
+					(ParamFloat<px4::params::VT_FW_DIFTHR_S_P>) _param_vt_fw_difthr_s_p,
+					(ParamFloat<px4::params::VT_FW_DIFTHR_S_R>) _param_vt_fw_difthr_s_r,
+					(ParamFloat<px4::params::VT_FW_DIFTHR_S_Y>) _param_vt_fw_difthr_s_y,
 					(ParamFloat<px4::params::VT_B_DEC_FF>) _param_vt_b_dec_ff,
 					(ParamFloat<px4::params::VT_B_DEC_I>) _param_vt_b_dec_i,
 					(ParamFloat<px4::params::VT_B_DEC_MSS>) _param_vt_b_dec_mss,