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,8 @@
 #include <ControlAllocation/ControlAllocation.hpp>
 
 #include <matrix/matrix/math.hpp>
+#include <mixer_module/output_functions.hpp>
+#include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ActuatorEffectiveness
 {
@@ -54,6 +56,23 @@ public:
 	static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
 	static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
 
+	enum class FlightPhase {
+		HOVER_FLIGHT = 0,
+		FORWARD_FLIGHT = 1,
+		TRANSITION_HF_TO_FF = 2,
+		TRANSITION_FF_TO_HF = 3
+	};
+
+	/**
+	 * Set the current flight phase
+	 *
+	 * @param Flight phase
+	 */
+	virtual void setFlightPhase(const FlightPhase &flight_phase)
+	{
+		_flight_phase = flight_phase;
+	}
+
 	/**
 	 * Get the control effectiveness matrix if updated
 	 *
@@ -66,13 +85,32 @@ public:
 	 *
 	 * @return Actuator trims
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const { return _trim; }
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
+	{
+		return _trim;
+	}
+
+	/**
+	 * Get the current flight phase
+	 *
+	 * @return Flight phase
+	 */
+	const FlightPhase &getFlightPhase() const
+	{
+		return _flight_phase;
+	}
 
 	/**
 	 * Get the number of actuators
 	 */
 	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/ActuatorEffectivenessCustom.cpp

@@ -1,85 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ActuatorEffectivenessCustom.hpp"
-
-ActuatorEffectivenessCustom::ActuatorEffectivenessCustom(ModuleParams *parent):
-	ModuleParams(parent)
-{
-}
-
-bool ActuatorEffectivenessCustom::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
-		bool force)
-{
-	if (_updated || force) {
-		_updated = false;
-
-		int num_actuators = 0;
-
-		for (int n = 0; n < NUM_ACTUATORS; n++) {
-			// CA_ACTn_TRQ_R
-			// CA_ACTn_TRQ_P
-			// CA_ACTn_TRQ_Y
-			char torque_str[3][17];
-			sprintf(torque_str[0], "CA_ACT%u_TRQ_R", n);
-			sprintf(torque_str[1], "CA_ACT%u_TRQ_P", n);
-			sprintf(torque_str[2], "CA_ACT%u_TRQ_Y", n);
-
-			// CA_ACTn_THR_X
-			// CA_ACTn_THR_Y
-			// CA_ACTn_THR_Z
-			char thrust_str[3][17];
-			sprintf(thrust_str[0], "CA_ACT%u_THR_X", n);
-			sprintf(thrust_str[1], "CA_ACT%u_THR_Y", n);
-			sprintf(thrust_str[2], "CA_ACT%u_THR_Z", n);
-
-			for (int i = 0; i < 3; i++) {
-				// CA_ACTn_TRQ_{R,P,Y}
-				param_get(param_find(torque_str[i]), &_matrix(n, i));
-
-				// CA_ACTn_THR_{X,Y,Z}
-				param_get(param_find(thrust_str[i]), &_matrix(n, i + 3));
-			}
-
-			if (_matrix.row(n).longerThan(0.f)) {
-				num_actuators++;
-			}
-		}
-
-		_num_actuators = num_actuators;
-		matrix = _matrix;
-		return true;
-	}
-
-	return false;
-}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ActuatorEffectivenessCustom.hpp
- *
- * Actuator effectiveness computed from rotors position and orientation
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#pragma once
-
-#include "ActuatorEffectiveness.hpp"
-
-#include <px4_platform_common/module_params.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-
-using namespace time_literals;
-
-class ActuatorEffectivenessCustom: public ModuleParams, public ActuatorEffectiveness
-{
-public:
-	ActuatorEffectivenessCustom(ModuleParams *parent);
-	virtual ~ActuatorEffectivenessCustom() = default;
-
-	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
-
-	int numActuators() const override { return _num_actuators; }
-private:
-
-	matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _matrix{};
-
-	int _num_actuators{0};
-
-	bool _updated{true};
-};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -54,33 +54,78 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix<float, NU
 		_updated = false;
 
 		// Get multirotor geometry
-		MultirotorGeometry geometry{};
+		MultirotorGeometry geometry = {};
+		geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
+		geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
+		geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
+		geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
+		geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
+		geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
+		geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
+		geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
 
-		for (int n = 0; n < NUM_ROTORS_MAX; n++) {
-			char buffer[17];
+		geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
+		geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
+		geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
+		geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
+		geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
+		geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
+		geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
+		geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
 
-			for (int i = 0; i < 3; i++) {
-				char axis_char = 'X' + i;
+		geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
+		geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
+		geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
+		geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
+		geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
+		geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
+		geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
+		geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
 
-				// CA_MC_Rn_P{X,Y,Z}
-				sprintf(buffer, "CA_MC_R%u_P%c", n, axis_char);
-				param_get(param_find(buffer), &geometry.rotors[n].position(i));
+		geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
+		geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
+		geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
+		geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
+		geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
+		geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
+		geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
+		geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
 
-				// CA_MC_Rn_A{X,Y,Z}
-				sprintf(buffer, "CA_MC_R%u_A%c", n, axis_char);
-				param_get(param_find(buffer), &geometry.rotors[n].axis(i));
-			}
+		geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
+		geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
+		geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
+		geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
+		geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
+		geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
+		geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
+		geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
 
-			geometry.rotors[n].axis.normalize();
+		geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
+		geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
+		geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
+		geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
+		geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
+		geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
+		geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
+		geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
 
-			// CA_MC_Rn_CT
-			sprintf(buffer, "CA_MC_R%u_CT", n);
-			param_get(param_find(buffer), &geometry.rotors[n].thrust_coef);
+		geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
+		geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
+		geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
+		geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
+		geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
+		geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
+		geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
+		geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
 
-			// CA_MC_Rn_KM
-			sprintf(buffer, "CA_MC_R%u_KM", n);
-			param_get(param_find(buffer), &geometry.rotors[n].moment_ratio);
-		}
+		geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
+		geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
+		geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
+		geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
+		geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
+		geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
+		geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
+		geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
 
 		_num_actuators = computeEffectivenessMatrix(geometry, matrix);
 		return true;
@@ -100,7 +145,11 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
 
 		// Get rotor axis
-		matrix::Vector3f axis{geometry.rotors[i].axis};
+		matrix::Vector3f axis(
+			geometry.rotors[i].axis_x,
+			geometry.rotors[i].axis_y,
+			geometry.rotors[i].axis_z
+		);
 
 		// Normalize axis
 		float axis_norm = axis.norm();
@@ -114,7 +163,11 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}
 
 		// Get rotor position
-		const matrix::Vector3f position{geometry.rotors[i].position};
+		matrix::Vector3f position(
+			geometry.rotors[i].position_x,
+			geometry.rotors[i].position_y,
+			geometry.rotors[i].position_z
+		);
 
 		// Get coefficients
 		float ct = geometry.rotors[i].thrust_coef;
@@ -125,10 +178,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}
 
 		// Compute thrust generated by this rotor
-		const matrix::Vector3f thrust{ct * axis};
+		matrix::Vector3f thrust = ct * axis;
 
 		// Compute moment generated by this rotor
-		const matrix::Vector3f moment{ct * position.cross(axis) - ct *km * axis};
+		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
 
 		// Fill corresponding items in effectiveness matrix
 		for (size_t j = 0; j < 3; j++) {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -57,16 +57,20 @@ public:
 
 	static constexpr int NUM_ROTORS_MAX = 8;
 
-	struct RotorGeometry {
-		matrix::Vector3f position{};
-		matrix::Vector3f axis{};
-		float thrust_coef{0.f};
-		float moment_ratio{0.f};
-	};
+	typedef struct {
+		float position_x;
+		float position_y;
+		float position_z;
+		float axis_x;
+		float axis_y;
+		float axis_z;
+		float thrust_coef;
+		float moment_ratio;
+	} RotorGeometry;
 
-	struct MultirotorGeometry {
+	typedef struct {
 		RotorGeometry rotors[NUM_ROTORS_MAX];
-	};
+	} MultirotorGeometry;
 
 	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
@@ -74,7 +78,83 @@ 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};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
+		(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,
+		(ParamFloat<px4::params::CA_MC_R0_PZ>) _param_ca_mc_r0_pz,
+		(ParamFloat<px4::params::CA_MC_R0_AX>) _param_ca_mc_r0_ax,
+		(ParamFloat<px4::params::CA_MC_R0_AY>) _param_ca_mc_r0_ay,
+		(ParamFloat<px4::params::CA_MC_R0_AZ>) _param_ca_mc_r0_az,
+		(ParamFloat<px4::params::CA_MC_R0_CT>) _param_ca_mc_r0_ct,
+		(ParamFloat<px4::params::CA_MC_R0_KM>) _param_ca_mc_r0_km,
+
+		(ParamFloat<px4::params::CA_MC_R1_PX>) _param_ca_mc_r1_px,
+		(ParamFloat<px4::params::CA_MC_R1_PY>) _param_ca_mc_r1_py,
+		(ParamFloat<px4::params::CA_MC_R1_PZ>) _param_ca_mc_r1_pz,
+		(ParamFloat<px4::params::CA_MC_R1_AX>) _param_ca_mc_r1_ax,
+		(ParamFloat<px4::params::CA_MC_R1_AY>) _param_ca_mc_r1_ay,
+		(ParamFloat<px4::params::CA_MC_R1_AZ>) _param_ca_mc_r1_az,
+		(ParamFloat<px4::params::CA_MC_R1_CT>) _param_ca_mc_r1_ct,
+		(ParamFloat<px4::params::CA_MC_R1_KM>) _param_ca_mc_r1_km,
+
+		(ParamFloat<px4::params::CA_MC_R2_PX>) _param_ca_mc_r2_px,
+		(ParamFloat<px4::params::CA_MC_R2_PY>) _param_ca_mc_r2_py,
+		(ParamFloat<px4::params::CA_MC_R2_PZ>) _param_ca_mc_r2_pz,
+		(ParamFloat<px4::params::CA_MC_R2_AX>) _param_ca_mc_r2_ax,
+		(ParamFloat<px4::params::CA_MC_R2_AY>) _param_ca_mc_r2_ay,
+		(ParamFloat<px4::params::CA_MC_R2_AZ>) _param_ca_mc_r2_az,
+		(ParamFloat<px4::params::CA_MC_R2_CT>) _param_ca_mc_r2_ct,
+		(ParamFloat<px4::params::CA_MC_R2_KM>) _param_ca_mc_r2_km,
+
+		(ParamFloat<px4::params::CA_MC_R3_PX>) _param_ca_mc_r3_px,
+		(ParamFloat<px4::params::CA_MC_R3_PY>) _param_ca_mc_r3_py,
+		(ParamFloat<px4::params::CA_MC_R3_PZ>) _param_ca_mc_r3_pz,
+		(ParamFloat<px4::params::CA_MC_R3_AX>) _param_ca_mc_r3_ax,
+		(ParamFloat<px4::params::CA_MC_R3_AY>) _param_ca_mc_r3_ay,
+		(ParamFloat<px4::params::CA_MC_R3_AZ>) _param_ca_mc_r3_az,
+		(ParamFloat<px4::params::CA_MC_R3_CT>) _param_ca_mc_r3_ct,
+		(ParamFloat<px4::params::CA_MC_R3_KM>) _param_ca_mc_r3_km,
+
+		(ParamFloat<px4::params::CA_MC_R4_PX>) _param_ca_mc_r4_px,
+		(ParamFloat<px4::params::CA_MC_R4_PY>) _param_ca_mc_r4_py,
+		(ParamFloat<px4::params::CA_MC_R4_PZ>) _param_ca_mc_r4_pz,
+		(ParamFloat<px4::params::CA_MC_R4_AX>) _param_ca_mc_r4_ax,
+		(ParamFloat<px4::params::CA_MC_R4_AY>) _param_ca_mc_r4_ay,
+		(ParamFloat<px4::params::CA_MC_R4_AZ>) _param_ca_mc_r4_az,
+		(ParamFloat<px4::params::CA_MC_R4_CT>) _param_ca_mc_r4_ct,
+		(ParamFloat<px4::params::CA_MC_R4_KM>) _param_ca_mc_r4_km,
+
+		(ParamFloat<px4::params::CA_MC_R5_PX>) _param_ca_mc_r5_px,
+		(ParamFloat<px4::params::CA_MC_R5_PY>) _param_ca_mc_r5_py,
+		(ParamFloat<px4::params::CA_MC_R5_PZ>) _param_ca_mc_r5_pz,
+		(ParamFloat<px4::params::CA_MC_R5_AX>) _param_ca_mc_r5_ax,
+		(ParamFloat<px4::params::CA_MC_R5_AY>) _param_ca_mc_r5_ay,
+		(ParamFloat<px4::params::CA_MC_R5_AZ>) _param_ca_mc_r5_az,
+		(ParamFloat<px4::params::CA_MC_R5_CT>) _param_ca_mc_r5_ct,
+		(ParamFloat<px4::params::CA_MC_R5_KM>) _param_ca_mc_r5_km,
+
+		(ParamFloat<px4::params::CA_MC_R6_PX>) _param_ca_mc_r6_px,
+		(ParamFloat<px4::params::CA_MC_R6_PY>) _param_ca_mc_r6_py,
+		(ParamFloat<px4::params::CA_MC_R6_PZ>) _param_ca_mc_r6_pz,
+		(ParamFloat<px4::params::CA_MC_R6_AX>) _param_ca_mc_r6_ax,
+		(ParamFloat<px4::params::CA_MC_R6_AY>) _param_ca_mc_r6_ay,
+		(ParamFloat<px4::params::CA_MC_R6_AZ>) _param_ca_mc_r6_az,
+		(ParamFloat<px4::params::CA_MC_R6_CT>) _param_ca_mc_r6_ct,
+		(ParamFloat<px4::params::CA_MC_R6_KM>) _param_ca_mc_r6_km,
+
+		(ParamFloat<px4::params::CA_MC_R7_PX>) _param_ca_mc_r7_px,
+		(ParamFloat<px4::params::CA_MC_R7_PY>) _param_ca_mc_r7_py,
+		(ParamFloat<px4::params::CA_MC_R7_PZ>) _param_ca_mc_r7_pz,
+		(ParamFloat<px4::params::CA_MC_R7_AX>) _param_ca_mc_r7_ax,
+		(ParamFloat<px4::params::CA_MC_R7_AY>) _param_ca_mc_r7_ay,
+		(ParamFloat<px4::params::CA_MC_R7_AZ>) _param_ca_mc_r7_az,
+		(ParamFloat<px4::params::CA_MC_R7_CT>) _param_ca_mc_r7_ct,
+		(ParamFloat<px4::params::CA_MC_R7_KM>) _param_ca_mc_r7_km
+	)
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,40 +41,15 @@
 
 #include "ActuatorEffectivenessStandardVTOL.hpp"
 
+ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
+{
+	setFlightPhase(FlightPhase::HOVER_FLIGHT);
+}
+
 bool
 ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
 		bool force)
 {
-	vehicle_status_s vehicle_status;
-
-	if (_vehicle_status_sub.update(&vehicle_status)) {
-
-		FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
-
-		// Check if the current flight phase is HOVER or FIXED_WING
-		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			flight_phase = FlightPhase::HOVER_FLIGHT;
-
-		} else {
-			flight_phase = FlightPhase::FORWARD_FLIGHT;
-		}
-
-		// Special cases for VTOL in transition
-		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
-			if (vehicle_status.in_transition_to_fw) {
-				flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
-
-			} else {
-				flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
-			}
-		}
-
-		if (flight_phase != _flight_phase) {
-			_flight_phase = flight_phase;
-			_updated = true;
-		}
-	}
-
 	if (!(_updated || force)) {
 		return false;
 	}
@@ -124,3 +99,11 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
+
+void
+ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+	_updated = true;
+
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,30 +43,30 @@
 
 #include "ActuatorEffectiveness.hpp"
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_status.h>
-
 class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessStandardVTOL() = default;
+	ActuatorEffectivenessStandardVTOL();
 	virtual ~ActuatorEffectivenessStandardVTOL() = default;
 
 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
 
-	int numActuators() const override { return 7; }
+	/**
+	 * Set the current flight phase
+	 *
+	 * @param Flight phase
+	 */
+	void setFlightPhase(const FlightPhase &flight_phase) override;
+
+	int numActuators() const override { return 8; }
+
+	int actuatorFunction(uint8_t idx) const override { return (int)_actuator_functions[idx]; }
+
 protected:
-
-	enum class FlightPhase {
-		HOVER_FLIGHT = 0,
-		FORWARD_FLIGHT = 1,
-		TRANSITION_HF_TO_FF = 2,
-		TRANSITION_FF_TO_HF = 3
+	OutputFunction _actuator_functions[NUM_ACTUATORS] = {
+		OutputFunction::Motor1, OutputFunction::Motor2, OutputFunction::Motor3, OutputFunction::Motor4,
+		OutputFunction::Motor5, OutputFunction::Servo1, OutputFunction::Servo2, OutputFunction::Servo3
 	};
 
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
-
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
 	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,40 +41,14 @@
 
 #include "ActuatorEffectivenessTiltrotorVTOL.hpp"
 
+ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
+{
+	setFlightPhase(FlightPhase::HOVER_FLIGHT);
+}
 bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix,
 		bool force)
 {
-	vehicle_status_s vehicle_status;
-
-	if (_vehicle_status_sub.update(&vehicle_status)) {
-
-		FlightPhase flight_phase{FlightPhase::HOVER_FLIGHT};
-
-		// Check if the current flight phase is HOVER or FIXED_WING
-		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			flight_phase = FlightPhase::HOVER_FLIGHT;
-
-		} else {
-			flight_phase = FlightPhase::FORWARD_FLIGHT;
-		}
-
-		// Special cases for VTOL in transition
-		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
-			if (vehicle_status.in_transition_to_fw) {
-				flight_phase = FlightPhase::TRANSITION_HF_TO_FF;
-
-			} else {
-				flight_phase = FlightPhase::TRANSITION_FF_TO_HF;
-			}
-		}
-
-		if (flight_phase != _flight_phase) {
-			_flight_phase = flight_phase;
-			_updated = true;
-		}
-	}
-
 	if (!(_updated || force)) {
 		return false;
 	}
@@ -132,3 +106,11 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix<float,
 	_updated = false;
 	return true;
 }
+
+void
+ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
+{
+	ActuatorEffectiveness::setFlightPhase(flight_phase);
+
+	_updated = true;
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,30 +43,24 @@
 
 #include "ActuatorEffectiveness.hpp"
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_status.h>
-
 class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessTiltrotorVTOL() = default;
+	ActuatorEffectivenessTiltrotorVTOL();
 	virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
 
 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
 
+	/**
+	 * Set the current flight phase
+	 *
+	 * @param Flight phase
+	 */
+	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:
-
-	enum class FlightPhase {
-		HOVER_FLIGHT = 0,
-		FORWARD_FLIGHT = 1,
-		TRANSITION_HF_TO_FF = 2,
-		TRANSITION_FF_TO_HF = 3
-	};
-
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
-
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
 	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -0,0 +1,52 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(ActuatorEffectiveness
+	ActuatorEffectiveness.hpp
+	ActuatorEffectivenessMultirotor.cpp
+	ActuatorEffectivenessMultirotor.hpp
+	ActuatorEffectivenessStandardVTOL.cpp
+	ActuatorEffectivenessStandardVTOL.hpp
+	ActuatorEffectivenessTiltrotorVTOL.cpp
+	ActuatorEffectivenessTiltrotorVTOL.hpp
+)
+
+target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(ActuatorEffectiveness
+	PRIVATE
+		mathlib
+		ControlAllocation
+)
+
+# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)

src/modules/control_allocator/CMakeLists.txt

@@ -31,45 +31,22 @@
 #
 ############################################################################
 
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+add_subdirectory(ActuatorEffectiveness)
+add_subdirectory(ControlAllocation)
+
 px4_add_module(
 	MODULE modules__control_allocator
 	MAIN control_allocator
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
-	INCLUDES
-		${CMAKE_CURRENT_SOURCE_DIR}
 	SRCS
-		ActuatorEffectiveness/ActuatorEffectiveness.hpp
-		ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
-		ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
-		ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp
-		ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp
-		ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
-		ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
-		ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
-		ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
-
-		ControlAllocation/ControlAllocation.cpp
-		ControlAllocation/ControlAllocation.hpp
-		ControlAllocation/ControlAllocationPseudoInverse.cpp
-		ControlAllocation/ControlAllocationPseudoInverse.hpp
-		ControlAllocation/ControlAllocationSequentialDesaturation.cpp
-		ControlAllocation/ControlAllocationSequentialDesaturation.hpp
-
 		ControlAllocator.cpp
 		ControlAllocator.hpp
 	DEPENDS
 		mathlib
+		ActuatorEffectiveness
+		ControlAllocation
+		mixer
 		px4_work_queue
-	MODULE_CONFIG
-		module.yaml
 )
-
-px4_add_library(ControlAllocation_testing
-	ControlAllocation/ControlAllocation.cpp
-	ControlAllocation/ControlAllocationPseudoInverse.cpp
-)
-target_link_libraries(ControlAllocation_testing PRIVATE mathlib)
-
-px4_add_unit_gtest(SRC ControlAllocation/ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation_testing)
-# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)

src/modules/control_allocator/ControlAllocation/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(ControlAllocation
+	ControlAllocation.cpp
+	ControlAllocation.hpp
+	ControlAllocationPseudoInverse.cpp
+	ControlAllocationPseudoInverse.hpp
+	ControlAllocationSequentialDesaturation.cpp
+	ControlAllocationSequentialDesaturation.hpp
+)
+target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(ControlAllocation PRIVATE mathlib)
+
+px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -70,6 +70,7 @@
 #pragma once
 
 #include <matrix/matrix/math.hpp>
+#include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ControlAllocation
 {
@@ -80,7 +81,7 @@ public:
 	static constexpr uint8_t NUM_ACTUATORS = 16;
 	static constexpr uint8_t NUM_AXES = 6;
 
-	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
+	typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
 
 	enum ControlAxis {
 		ROLL = 0,

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

@@ -40,8 +40,7 @@
  */
 
 #include <gtest/gtest.h>
-
-#include "ControlAllocationPseudoInverse.hpp"
+#include <ControlAllocationPseudoInverse.hpp>
 
 using namespace matrix;
 

src/modules/control_allocator/ControlAllocator.cpp

@@ -212,10 +212,6 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessTiltrotorVTOL();
 			break;
 
-		case EffectivenessSource::CUSTOM:
-			tmp = new ActuatorEffectivenessCustom(this);
-			break;
-
 		default:
 			PX4_ERR("Unknown airframe");
 			break;
@@ -264,6 +260,34 @@ ControlAllocator::Run()
 		return;
 	}
 
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+
+		ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
+
+		// Check if the current flight phase is HOVER or FIXED_WING
+		if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
+
+		} else {
+			flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
+		}
+
+		// Special cases for VTOL in transition
+		if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
+			if (vehicle_status.in_transition_to_fw) {
+				flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
+
+			} else {
+				flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
+			}
+		}
+
+		// Forward to effectiveness source
+		_actuator_effectiveness->setFlightPhase(flight_phase);
+	}
+
 	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 	const hrt_abstime now = hrt_absolute_time();
 	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@@ -412,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[])
@@ -488,10 +526,6 @@ int ControlAllocator::print_status()
 	case EffectivenessSource::TILTROTOR_VTOL:
 		PX4_INFO("EffectivenessSource: Tiltrotor VTOL");
 		break;
-
-	case EffectivenessSource::CUSTOM:
-		PX4_INFO("EffectivenessSource: Custom");
-		break;
 	}
 
 	// Print current effectiveness matrix

src/modules/control_allocator/ControlAllocator.hpp

@@ -41,15 +41,14 @@
 
 #pragma once
 
-#include <ActuatorEffectiveness/ActuatorEffectiveness.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp>
-#include <ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp>
+#include <ActuatorEffectiveness.hpp>
+#include <ActuatorEffectivenessMultirotor.hpp>
+#include <ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 
-#include <ControlAllocation/ControlAllocation.hpp>
-#include <ControlAllocation/ControlAllocationPseudoInverse.hpp>
-#include <ControlAllocation/ControlAllocationSequentialDesaturation.hpp>
+#include <ControlAllocation.hpp>
+#include <ControlAllocationPseudoInverse.hpp>
+#include <ControlAllocationSequentialDesaturation.hpp>
 
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -69,6 +68,7 @@
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_actuator_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
 
 class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::WorkItem
 {
@@ -124,11 +124,10 @@ private:
 	ControlAllocation *_control_allocation{nullptr}; 	///< class for control allocation calculations
 
 	enum class EffectivenessSource {
-		NONE           = -1,
-		MULTIROTOR     = 0,
-		STANDARD_VTOL  = 1,
+		NONE = -1,
+		MULTIROTOR = 0,
+		STANDARD_VTOL = 1,
 		TILTROTOR_VTOL = 2,
-		CUSTOM         = 3,
 	};
 
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
@@ -148,6 +147,7 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};				/**< airspeed subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	matrix::Vector3f _torque_sp;
 	matrix::Vector3f _thrust_sp;

src/modules/control_allocator/control_allocator_params.c

@@ -50,7 +50,6 @@
  * @value 0 Multirotor
  * @value 1 Standard VTOL (WIP)
  * @value 2 Tiltrotor VTOL (WIP)
- * @value 3 Custom (CA_ACTn* torque and thrust parameters)
  * @group Control Allocation
  */
 PARAM_DEFINE_INT32(CA_AIRFRAME, 0);

src/modules/control_allocator/module.yaml

@@ -1,72 +0,0 @@
-__max_num_config_instances: &max_num_config_instances 16
-
-module_name: control_allocator
-
-parameters:
-    - group: Control Allocator
-      definitions:
-        CA_ACT${i}_TRQ_R:
-            description:
-                short: actuator ${i} roll
-                long: Actuator ${i} roll torque
-            type: float
-            decimal: 2
-            reboot_required: false
-            num_instances: *max_num_config_instances
-            instance_start: 0
-            default: 0.0
-
-        CA_ACT${i}_TRQ_P:
-            description:
-                short: actuator ${i} pitch
-                long: Actuator ${i} pitch torque
-            type: float
-            decimal: 2
-            reboot_required: false
-            num_instances: *max_num_config_instances
-            instance_start: 0
-            default: 0.0
-
-        CA_ACT${i}_TRQ_Y:
-            description:
-                short: actuator ${i} yaw
-                long: Actuator ${i} yaw torque
-            type: float
-            decimal: 2
-            reboot_required: false
-            num_instances: *max_num_config_instances
-            instance_start: 0
-            default: 0.0
-
-        CA_ACT${i}_THR_X:
-            description:
-                short: actuator ${i} thrust x
-                long: Actuator ${i} thrust x
-            type: float
-            decimal: 2
-            reboot_required: false
-            num_instances: *max_num_config_instances
-            instance_start: 0
-            default: 0.0
-
-        CA_ACT${i}_THR_Y:
-            description:
-                short: actuator ${i} thrust y
-                long: Actuator ${i} thrust y
-            type: float
-            decimal: 2
-            reboot_required: false
-            num_instances: *max_num_config_instances
-            instance_start: 0
-            default: 0.0
-
-        CA_ACT${i}_THR_Z:
-            description:
-                short: actuator ${i} thrust y
-                long: Actuator ${i} thrust y
-            type: float
-            decimal: 2
-            reboot_required: false
-            num_instances: *max_num_config_instances
-            instance_start: 0
-            default: 0.0