control_allocator: generic motor configuration for standard vtols

This adds the pusher/puller to the standard motors and makes the axis
configurable.

ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol

@@ -10,7 +10,7 @@
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2
 
-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.1515
 param set-default CA_ROTOR0_PY 0.245
 param set-default CA_ROTOR0_KM 0.05
@@ -23,6 +23,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.1515
 param set-default CA_ROTOR3_PY 0.1875
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
 
 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1

ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil

@@ -62,7 +62,7 @@ param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_TYPE 2
 
 param set-default CA_AIRFRAME 2
-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.15
 param set-default CA_ROTOR0_PY 0.15
 param set-default CA_ROTOR1_PX -0.15
@@ -73,6 +73,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.15
 param set-default CA_ROTOR3_PY 0.15
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
 
 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS1_TRQ_R 0.5

ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard

@@ -25,7 +25,7 @@
 . ${R}etc/init.d/rc.vtol_defaults
 
 param set-default CA_AIRFRAME 2
-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.15
 param set-default CA_ROTOR0_PY 0.15
 param set-default CA_ROTOR1_PX -0.15
@@ -36,6 +36,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.15
 param set-default CA_ROTOR3_PY 0.15
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
 param set-default CA_SV_CS_COUNT 4
 param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS0_TRQ_R -0.5

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -49,7 +49,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
 	}
 
 	// motors
-	_motors.enableYawControl(false);
+	_motors.enablePropellerTorque(false);
 	const bool motors_added_successfully = _motors.addActuators(configuration);
 
 	// Torque

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -51,7 +51,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	}
 
 	// Motors
-	_rotors.enableYawControl(false);
+	_rotors.enablePropellerTorque(false);
 	const bool rotors_added_successfully = _rotors.addActuators(configuration);
 
 	// Control Surfaces

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -51,7 +51,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 	}
 
 	// MC motors
-	_mc_rotors.enableYawControl(!_tilts.hasYawControl());
+	_mc_rotors.enablePropellerTorque(!_tilts.hasYawControl());
 	const bool rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
 	// Tilts

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp

@@ -185,10 +185,18 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
 		float ct = geometry.rotors[i].thrust_coef;
 		float km = geometry.rotors[i].moment_ratio;
 
-		if (geometry.yaw_disabled) {
+		if (geometry.propeller_torque_disabled) {
 			km = 0.f;
 		}
 
+		if (geometry.propeller_torque_disabled_non_upwards) {
+			bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f;
+
+			if (!upwards) {
+				km = 0.f;
+			}
+		}
+
 		if (fabsf(ct) < FLT_EPSILON) {
 			continue;
 		}
@@ -239,3 +247,18 @@ Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_di
 	Vector3f axis{0.f, 0.f, -1.f};
 	return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
 }
+
+uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
+{
+	uint32_t upwards_motors = 0;
+
+	for (int i = 0; i < _geometry.num_rotors; ++i) {
+		const Vector3f &axis = _geometry.rotors[i].axis;
+
+		if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) {
+			upwards_motors |= 1u << i;
+		}
+	}
+
+	return upwards_motors;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp

@@ -73,7 +73,8 @@ public:
 	struct Geometry {
 		RotorGeometry rotors[NUM_ROTORS_MAX];
 		int num_rotors{0};
-		bool yaw_disabled{false};
+		bool propeller_torque_disabled{false};
+		bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors
 	};
 
 	ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
@@ -113,7 +114,11 @@ public:
 	 */
 	static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction);
 
-	void enableYawControl(bool enable) { _geometry.yaw_disabled = !enable; }
+	void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; }
+
+	void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; }
+
+	uint32_t getUpwardsMotors() const;
 
 private:
 	void updateParams() override;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -37,7 +37,7 @@
 using namespace matrix;
 
 ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL(ModuleParams *parent)
-	: ModuleParams(parent), _mc_rotors(this), _control_surfaces(this)
+	: ModuleParams(parent), _rotors(this), _control_surfaces(this)
 {
 }
 
@@ -49,17 +49,11 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 		return false;
 	}
 
-	// MC motors
+	// Motors
 	configuration.selected_matrix = 0;
-	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
+	_rotors.enablePropellerTorqueNonUpwards(false);
-	_mc_motors_mask = (1u << _mc_rotors.geometry().num_rotors) - 1;
+	const bool mc_rotors_added_successfully = _rotors.addActuators(configuration);
-
+	_mc_motors_mask = _rotors.getUpwardsMotors();
-	// Pusher/Puller
-	configuration.selected_matrix = 1;
-
-	for (int i = 0; i < _param_ca_stdvtol_n_p.get(); ++i) {
-		configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f});
-	}
 
 	// Control Surfaces
 	configuration.selected_matrix = 1;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -80,7 +80,7 @@ public:
 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
 
 private:
-	ActuatorEffectivenessRotors _mc_rotors;
+	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 
 	uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight)
@@ -90,8 +90,4 @@ private:
 
 	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_0)};
 
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_STDVTOL_N_P>) _param_ca_stdvtol_n_p ///< number of pushers
-	)
-
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -56,7 +56,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
 
 	// MC motors
 	configuration.selected_matrix = 0;
-	_mc_rotors.enableYawControl(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors
+	_mc_rotors.enablePropellerTorque(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
 
 	// Control Surfaces

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -60,7 +60,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 
 	// MC motors
 	configuration.selected_matrix = 0;
-	_mc_rotors.enableYawControl(!_tilts.hasYawControl());
+	_mc_rotors.enablePropellerTorque(!_tilts.hasYawControl());
 
 	// Update matrix with tilts in vertical position when update is triggered by a manual
 	// configuration (parameter) change. This is to make sure the normalization

src/modules/control_allocator/module.yaml

@@ -370,18 +370,6 @@ parameters:
             instance_start: 0
             default: 0
 
-        # Standard VTOL
-        CA_STDVTOL_N_P:
-            description:
-                short: Number of fixed wing (pusher/puller) motors
-            type: enum
-            values:
-                1: '1'
-                2: '2'
-                3: '3'
-                4: '4'
-            default: 1
-
 
 # Mixer
 mixer:
@@ -525,7 +513,7 @@ mixer:
                 title: 'Standard VTOL'
                 actuators:
                   - actuator_type: 'motor'
-                    group_label: 'MC Motors'
+                    group_label: 'Motors'
                     count: 'CA_ROTOR_COUNT'
                     per_item_parameters:
                         standard:
@@ -535,10 +523,15 @@ mixer:
                             label: 'Direction CCW'
                             function: 'spin-dir'
                             show_as: 'true-if-positive'
-                  - actuator_type: 'motor'
+                          - name: 'CA_ROTOR${i}_AX'
-                    group_label: 'FW Motors'
+                            label: 'Axis X'
-                    count: 'CA_STDVTOL_N_P'
+                            function: 'axisx'
-                    item_label_prefix: 'FW Motor ${i}'
+                          - name: 'CA_ROTOR${i}_AY'
+                            label: 'Axis Y'
+                            function: 'axisy'
+                          - name: 'CA_ROTOR${i}_AZ'
+                            label: 'Axis Z'
+                            function: 'axisz'
                   - actuator_type: 'servo'
                     group_label: 'Control Surfaces'
                     count: 'CA_SV_CS_COUNT'

src/modules/vtol_att_control/standard.cpp

@@ -427,13 +427,13 @@ void Standard::fill_actuator_outputs()
 
 	_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[0] = fw_out[actuator_controls_s::INDEX_THROTTLE];
 	_thrust_setpoint_0->xyz[1] = 0.f;
 	_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_out[actuator_controls_s::INDEX_THROTTLE];
+	_thrust_setpoint_1->xyz[0] = 0.f;
 	_thrust_setpoint_1->xyz[1] = 0.f;
 	_thrust_setpoint_1->xyz[2] = 0.f;