control_allocator: add parameter based ActuatorEffectivenessCustom

- this allows creating a custom actuator effectiveness entirely from
parameters of the form CA_ACTn_TRQ_{R,P,Y} and CA_ACTn_THR_{X,Y,Z}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -44,7 +44,6 @@
 #include <ControlAllocation/ControlAllocation.hpp>
 
 #include <matrix/matrix/math.hpp>
-#include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ActuatorEffectiveness
 {
@@ -55,23 +54,6 @@ 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
 	 *
@@ -84,20 +66,7 @@ public:
 	 *
 	 * @return Actuator trims
 	 */
-	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
+	const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const { return _trim; }
-	{
-		return _trim;
-	}
-
-	/**
-	 * Get the current flight phase
-	 *
-	 * @return Flight phase
-	 */
-	const FlightPhase &getFlightPhase() const
-	{
-		return _flight_phase;
-	}
 
 	/**
 	 * Get the number of actuators
@@ -106,5 +75,4 @@ public:
 
 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

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -54,78 +54,33 @@ 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();
 
-		geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
+		for (int n = 0; n < NUM_ROTORS_MAX; n++) {
-		geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
+			char buffer[17];
-		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();
 
-		geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
+			for (int i = 0; i < 3; i++) {
-		geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
+				char axis_char = 'X' + i;
-		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();
 
-		geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
+				// CA_MC_Rn_P{X,Y,Z}
-		geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
+				sprintf(buffer, "CA_MC_R%u_P%c", n, axis_char);
-		geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
+				param_get(param_find(buffer), &geometry.rotors[n].position(i));
-		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();
 
-		geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
+				// CA_MC_Rn_A{X,Y,Z}
-		geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
+				sprintf(buffer, "CA_MC_R%u_A%c", n, axis_char);
-		geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
+				param_get(param_find(buffer), &geometry.rotors[n].axis(i));
-		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[5].position_x = _param_ca_mc_r5_px.get();
+			geometry.rotors[n].axis.normalize();
-		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();
 
-		geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
+			// CA_MC_Rn_CT
-		geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
+			sprintf(buffer, "CA_MC_R%u_CT", n);
-		geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
+			param_get(param_find(buffer), &geometry.rotors[n].thrust_coef);
-		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();
 
-		geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
+			// CA_MC_Rn_KM
-		geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
+			sprintf(buffer, "CA_MC_R%u_KM", n);
-		geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
+			param_get(param_find(buffer), &geometry.rotors[n].moment_ratio);
-		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;
@@ -145,11 +100,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 	for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
 
 		// Get rotor axis
-		matrix::Vector3f axis(
+		matrix::Vector3f axis{geometry.rotors[i].axis};
-			geometry.rotors[i].axis_x,
-			geometry.rotors[i].axis_y,
-			geometry.rotors[i].axis_z
-		);
 
 		// Normalize axis
 		float axis_norm = axis.norm();
@@ -163,11 +114,7 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}
 
 		// Get rotor position
-		matrix::Vector3f position(
+		const matrix::Vector3f position{geometry.rotors[i].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;
@@ -178,10 +125,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom
 		}
 
 		// Compute thrust generated by this rotor
-		matrix::Vector3f thrust = ct * axis;
+		const matrix::Vector3f thrust{ct * axis};
 
 		// Compute moment generated by this rotor
-		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
+		const 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 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -57,20 +57,16 @@ public:
 
 	static constexpr int NUM_ROTORS_MAX = 8;
 
-	typedef struct {
+	struct RotorGeometry {
-		float position_x;
+		matrix::Vector3f position{};
-		float position_y;
+		matrix::Vector3f axis{};
-		float position_z;
+		float thrust_coef{0.f};
-		float axis_x;
+		float moment_ratio{0.f};
-		float axis_y;
+	};
-		float axis_z;
-		float thrust_coef;
-		float moment_ratio;
-	} RotorGeometry;
 
-	typedef struct {
+	struct MultirotorGeometry {
 		RotorGeometry rotors[NUM_ROTORS_MAX];
-	} MultirotorGeometry;
+	};
 
 	static int computeEffectivenessMatrix(const MultirotorGeometry &geometry,
 					      matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness);
@@ -81,78 +77,4 @@ public:
 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 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -41,15 +41,40 @@
 
 #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;
 	}
@@ -99,11 +124,3 @@ 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 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -43,22 +43,30 @@
 
 #include "ActuatorEffectiveness.hpp"
 
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
 class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessStandardVTOL();
+	ActuatorEffectivenessStandardVTOL() = default;
 	virtual ~ActuatorEffectivenessStandardVTOL() = 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 7; }
 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/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -41,14 +41,40 @@
 
 #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;
 	}
@@ -106,11 +132,3 @@ 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 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -43,22 +43,30 @@
 
 #include "ActuatorEffectiveness.hpp"
 
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
 class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
 {
 public:
-	ActuatorEffectivenessTiltrotorVTOL();
+	ActuatorEffectivenessTiltrotorVTOL() = default;
 	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; }
 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

@@ -1,52 +0,0 @@
-############################################################################
-#
-#   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,22 +31,45 @@
 #
 ############################################################################
 
-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

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

src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

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

src/modules/control_allocator/ControlAllocator.cpp

@@ -212,6 +212,10 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessTiltrotorVTOL();
 			break;
 
+		case EffectivenessSource::CUSTOM:
+			tmp = new ActuatorEffectivenessCustom(this);
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;
@@ -260,34 +264,6 @@ 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);
@@ -512,6 +488,10 @@ 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,14 +41,15 @@
 
 #pragma once
 
-#include <ActuatorEffectiveness.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectiveness.hpp>
-#include <ActuatorEffectivenessMultirotor.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp>
-#include <ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp>
-#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp>
+#include <ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp>
 
-#include <ControlAllocation.hpp>
+#include <ControlAllocation/ControlAllocation.hpp>
-#include <ControlAllocationPseudoInverse.hpp>
+#include <ControlAllocation/ControlAllocationPseudoInverse.hpp>
-#include <ControlAllocationSequentialDesaturation.hpp>
+#include <ControlAllocation/ControlAllocationSequentialDesaturation.hpp>
 
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
@@ -68,7 +69,6 @@
 #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,10 +124,11 @@ private:
 	ControlAllocation *_control_allocation{nullptr}; 	///< class for control allocation calculations
 
 	enum class EffectivenessSource {
-		NONE = -1,
+		NONE           = -1,
-		MULTIROTOR = 0,
+		MULTIROTOR     = 0,
-		STANDARD_VTOL = 1,
+		STANDARD_VTOL  = 1,
 		TILTROTOR_VTOL = 2,
+		CUSTOM         = 3,
 	};
 
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
@@ -147,7 +148,6 @@ 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,6 +50,7 @@
  * @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

@@ -0,0 +1,72 @@
+__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