Control Allocation: Tiltrotor: pass only thrust magnitude instead of 3D thrust to allocator

Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in
MC), pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
by the allocator directly.

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

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -212,6 +212,17 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
 			effectiveness(j, i + actuator_start_index) = moment(j);
 			effectiveness(j + 3, i + actuator_start_index) = thrust(j);
 		}
+
+		if (geometry.three_dimensional_thrust_disabled) {
+			// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
+			// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
+			// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
+			// by the allocator directly.
+
+			effectiveness(0 + 3, i + actuator_start_index) = 0.f;
+			effectiveness(1 + 3, i + actuator_start_index) = 0.f;
+			effectiveness(2 + 3, i + actuator_start_index) = ct;
+		}
 	}
 
 	return num_actuators;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -75,6 +75,7 @@ public:
 		int num_rotors{0};
 		bool propeller_torque_disabled{false};
 		bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors
+		bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt
 	};
 
 	ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
@@ -120,6 +121,8 @@ public:
 
 	void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; }
 
+	void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
+
 	uint32_t getUpwardsMotors() const;
 
 private:

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -61,6 +61,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// MC motors
 	configuration.selected_matrix = 0;
 	_mc_rotors.enablePropellerTorque(!_tilts.hasYawControl());
+	_mc_rotors.enableThreeDimensionalThrust(false);
 
 	// Update matrix with tilts in vertical position when update is triggered by a manual
 	// configuration (parameter) change. This is to make sure the normalization
@@ -130,6 +131,18 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				}
 			}
 		}
+
+		// in FW directly use throttle sp
+		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
+
+			actuator_controls_s actuator_controls_0;
+
+			if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
+				for (int i = 0; i < _first_tilt_idx; ++i) {
+					actuator_sp(i) = actuator_controls_0.control[actuator_controls_s::INDEX_THROTTLE];
+				}
+			}
+		}
 	}
 }
 

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -95,4 +95,5 @@ protected:
 	float _last_tilt_control{NAN};
 
 	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
+	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
 };

src/modules/vtol_att_control/tiltrotor.cpp

@@ -514,8 +514,11 @@ void Tiltrotor::fill_actuator_outputs()
 		// for the legacy mixing system pubish FW throttle on the MC output
 		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
 
-		// FW thrust is allocated on mc_thrust_sp[0] for tiltrotor with dynamic control allocation
-		_thrust_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE];
+		// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
+		// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
+		// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
+		// by the allocator directly.
+		_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
 
 		/* allow differential thrust if enabled */
 		if (_params->diff_thrust == 1) {
@@ -525,9 +528,9 @@ void Tiltrotor::fill_actuator_outputs()
 
 	} else {
 
+		// see comment above for passing magnitude of thrust, not 3D thrust
 		mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
-		_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
-		_thrust_setpoint_0->xyz[0] = -_thrust_setpoint_0->xyz[2] * sinf(_tilt_control * M_PI_2_F);
+		_thrust_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
 	}
 
 	// Landing gear