control_allocator: Fixes for standard VTOL frames

- Add 'actuatorType()' to ActuatorEffectiveness
- Map outputs to uORB topics by type
- Fix incorrect number of outputs in StandardVTOL
- Fix 1050_standard_vtol_ctrlalloc airframe file

ROMFS/px4fmu_common/init.d-posix/airframes/1050_standard_vtol_ctrlalloc

@@ -6,7 +6,13 @@
 #
 
 . ${R}etc/init.d/rc.vtol_defaults
-. ${R}etc/init.d/rc.ctrlalloc
+
+param set-default SYS_CTRL_ALLOC 1
+
+param set-default VM_MASS 1.5
+param set-default VM_INERTIA_XX 0.03
+param set-default VM_INERTIA_YY 0.03
+param set-default VM_INERTIA_ZZ 0.05
 
 param set-default FW_L1_PERIOD 12
 param set-default FW_MAN_P_MAX 30
@@ -49,7 +55,9 @@ param set-default VT_B_TRANS_DUR 8
 param set-default VT_TYPE 2
 
 param set-default CA_AIRFRAME 1
+param set-default CA_METHOD 1
 
+# VTOL Motors
 param set-default CA_ACT0_MIN 0.0
 param set-default CA_ACT1_MIN 0.0
 param set-default CA_ACT2_MIN 0.0
@@ -59,9 +67,11 @@ param set-default CA_ACT1_MAX 1.0
 param set-default CA_ACT2_MAX 1.0
 param set-default CA_ACT3_MAX 1.0
 
+# Throttle
 param set-default CA_ACT4_MIN 0.0
 param set-default CA_ACT4_MAX 1.0
 
+# Ailerons/Ruddervator
 param set-default CA_ACT5_MIN -1.0
 param set-default CA_ACT5_MAX 1.0
 param set-default CA_ACT6_MIN -1.0
@@ -69,8 +79,32 @@ param set-default CA_ACT6_MAX 1.0
 param set-default CA_ACT7_MIN -1.0
 param set-default CA_ACT7_MAX 1.0
 
+param set-default CA_MC_R0_PX 0.177
+param set-default CA_MC_R0_PY 0.177
+param set-default CA_MC_R0_CT 6.5
+param set-default CA_MC_R0_KM 0.05
+param set-default CA_MC_R1_PX -0.177
+param set-default CA_MC_R1_PY -0.177
+param set-default CA_MC_R1_CT 6.5
+param set-default CA_MC_R1_KM 0.05
+param set-default CA_MC_R2_PX 0.177
+param set-default CA_MC_R2_PY -0.177
+param set-default CA_MC_R2_CT 6.5
+param set-default CA_MC_R2_KM -0.05
+param set-default CA_MC_R3_PX -0.177
+param set-default CA_MC_R3_PY 0.177
+param set-default CA_MC_R3_CT 6.5
+param set-default CA_MC_R3_KM -0.05
+
+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 105 # Setting pusher to Motor5 for now
+param set-default PWM_MAIN_FUNC6 201
+param set-default PWM_MAIN_FUNC7 202
+param set-default PWM_MAIN_FUNC8 203
+
 set MAV_TYPE 22
 
-#set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix
-#set MIXER custom
-set MIXER direct
+set MIXER skip

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -15,6 +15,22 @@ ekf2 start &
 #                        End Estimator group selection                        #
 ###############################################################################
 
+if param compare SYS_CTRL_ALLOC 1
+then
+	#
+	# Start Control Allocator
+	#
+	control_allocator start
+
+	#
+	# Disable hover thrust estimator and prearming
+	# These features are currently incompatible with control allocation
+	#
+	# TODO: fix
+	#
+	param set MPC_USE_HTE 0
+fi
+
 airspeed_selector start
 
 vtol_att_control start

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -44,6 +44,7 @@
 #include <ControlAllocation/ControlAllocation.hpp>
 
 #include <matrix/matrix/math.hpp>
+#include <mixer_module/output_functions.hpp>
 #include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ActuatorEffectiveness
@@ -104,6 +105,11 @@ public:
 	 */
 	virtual int numActuators() const = 0;
 
+	/**
+	 * Get the function of an actuator
+	 */
+	virtual int actuatorFunction(uint8_t idx) const = 0;
+
 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -78,6 +78,8 @@ public:
 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
 
 	int numActuators() const override { return _num_actuators; }
+
+	int actuatorFunction(uint8_t idx) const override { return (int)OutputFunction::Motor1 + idx; }
 private:
 	bool _updated{true};
 	int _num_actuators{0};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -58,7 +58,15 @@ public:
 	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
-	int numActuators() const override { return 7; }
+	int numActuators() const override { return 8; }
+
+	int actuatorFunction(uint8_t idx) const override { return (int)_actuator_functions[idx]; }
+
 protected:
+	OutputFunction _actuator_functions[NUM_ACTUATORS] = {
+		OutputFunction::Motor1, OutputFunction::Motor2, OutputFunction::Motor3, OutputFunction::Motor4,
+		OutputFunction::Motor5, OutputFunction::Servo1, OutputFunction::Servo2, OutputFunction::Servo3
+	};
+
 	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -59,6 +59,8 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	int numActuators() const override { return 10; }
+
+	int actuatorFunction(uint8_t idx) const { return (int)OutputFunction::Motor1 + idx; }; /// TODO: This is wrong.  Someone familiar with the tiltrotor should fix this.
 protected:
 	bool _updated{true};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -436,21 +436,35 @@ ControlAllocator::publish_control_allocator_status()
 void
 ControlAllocator::publish_legacy_actuator_controls()
 {
-	actuator_motors_s actuator_motors;
-	actuator_motors.timestamp = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
+
+	actuator_motors_s actuator_motors {};
+	actuator_motors.timestamp = now;
 	actuator_motors.timestamp_sample = _timestamp_sample;
 
+	actuator_servos_s actuator_servos {};
+	actuator_servos.timestamp = now;
+	actuator_servos.timestamp_sample = _timestamp_sample;
+
 	matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
 	matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
 				actuator_sp);
 
-	for (size_t i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
-		actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
+	for (size_t i = 0; i < NUM_ACTUATORS; i++) {
+		const int func = _actuator_effectiveness->actuatorFunction(i);
+		const float control = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
+
+		if (func >= (int)OutputFunction::Motor1 && func <= (int)OutputFunction::MotorMax) {
+			actuator_motors.control[func - (int)OutputFunction::Motor1] = control;
+		}
+
+		if (func >= (int)OutputFunction::Servo1 && func <= (int)OutputFunction::ServoMax) {
+			actuator_servos.control[func - (int)OutputFunction::Servo1] = control;
+		}
 	}
 
 	_actuator_motors_pub.publish(actuator_motors);
-
-	// TODO: servos
+	_actuator_servos_pub.publish(actuator_servos);
 }
 
 int ControlAllocator::task_spawn(int argc, char *argv[])