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

@@ -0,0 +1,110 @@
+#!/bin/sh
+#
+# @name Standard VTOL
+#
+# @type Standard VTOL
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+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
+param set-default FW_PR_FF 0.2
+param set-default FW_PR_P 0.9
+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_FF 0.1
+param set-default FW_RR_P 0.3
+param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_MAX 0.6
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_ALT_TC 2
+param set-default FW_T_CLMB_MAX 8
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 2.7
+param set-default FW_T_SINK_MIN 2.2
+param set-default FW_T_TAS_TC 2
+
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_YAW_P 1.6
+
+param set-default MIS_TAKEOFF_ALT 10
+
+param set-default MPC_ACC_HOR_MAX 2
+param set-default MPC_XY_P 0.8
+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 NAV_LOITER_RAD 80
+
+param set-default VT_FWD_THRUST_EN 4
+param set-default VT_F_TRANS_THR 0.75
+param set-default VT_MOT_ID 1234
+param set-default VT_FW_MOT_OFFID 1234
+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
+param set-default CA_ACT3_MIN 0.0
+param set-default CA_ACT0_MAX 1.0
+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
+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 skip

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

@@ -67,6 +67,7 @@ px4_add_romfs_files(
 	1042_tiltrotor
 	1043_standard_vtol_drop
 	1043_standard_vtol_drop.post
+	1050_standard_vtol_ctrlalloc
 	1060_rover
 	1061_r1_rover
 	1062_tf-r1

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

platforms/posix/cmake/sitl_target.cmake

@@ -144,6 +144,7 @@ set(models
 	solo
 	standard_vtol
 	standard_vtol_drop
+	standard_vtol_ctrlalloc
 	tailsitter
 	techpod
 	tiltrotor

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[])