control_allocator: add helicopter mixer

Same logic as the existing mixer.
Untested.

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -73,6 +73,15 @@ public:
 	static constexpr int NUM_ACTUATORS = 16;
 	static constexpr int NUM_AXES = 6;
 
+	enum ControlAxis {
+		ROLL = 0,
+		PITCH,
+		YAW,
+		THRUST_X,
+		THRUST_Y,
+		THRUST_Z
+	};
+
 	static constexpr int MAX_NUM_MATRICES = 2;
 
 	using EffectivenessMatrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>;

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -0,0 +1,143 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 "ActuatorEffectivenessHelicopter.hpp"
+#include <lib/mathlib/mathlib.h>
+
+using namespace matrix;
+
+ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent)
+	: ModuleParams(parent)
+{
+	for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i);
+		_param_handles.swash_plate_servos[i].angle = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i);
+		_param_handles.swash_plate_servos[i].arm_length = param_find(buffer);
+	}
+
+	_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
+
+	for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i);
+		_param_handles.throttle_curve[i] = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_HELI_PITCH_C%u", i);
+		_param_handles.pitch_curve[i] = param_find(buffer);
+	}
+
+	updateParams();
+}
+
+void ActuatorEffectivenessHelicopter::updateParams()
+{
+	ModuleParams::updateParams();
+
+	int32_t count = 0;
+
+	if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) {
+		PX4_ERR("param_get failed");
+		return;
+	}
+
+	_geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX);
+
+	for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
+		float angle_deg{};
+		param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg);
+		_geometry.swash_plate_servos[i].angle = math::radians(angle_deg);
+		param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length);
+	}
+
+	for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
+		param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]);
+		param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]);
+	}
+}
+
+bool
+ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
+		EffectivenessUpdateReason external_update)
+{
+	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+		return false;
+	}
+
+	// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
+	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
+
+	// Tail (yaw) servo
+	configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
+
+	// N swash plate servos
+	_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
+
+	for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
+		configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{});
+	}
+
+	return true;
+}
+
+void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp)
+{
+	// Find index to use for curves
+	float thrust_cmd = -control_sp(ControlAxis::THRUST_Z);
+	float num_intervals = NUM_CURVE_POINTS - 1;
+	// We access idx + 1 below, so max legal index is (size - 2)
+	int idx = math::constrain((int)(thrust_cmd * num_intervals), 0, NUM_CURVE_POINTS - 2);
+
+	// Local throttle curve gradient and offset
+	float tg = (_geometry.throttle_curve[idx + 1] - _geometry.throttle_curve[idx]) * num_intervals;
+	float to = (_geometry.throttle_curve[idx]) - (tg * idx / num_intervals);
+	float throttle = math::constrain(tg * thrust_cmd + to, 0.0f, 1.0f);
+
+	// Local pitch curve gradient and offset
+	float pg = (_geometry.pitch_curve[idx + 1] - _geometry.pitch_curve[idx]) * num_intervals;
+	float po = (_geometry.pitch_curve[idx]) - (pg * idx / num_intervals);
+	float collective_pitch = math::constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
+
+	float roll_cmd = control_sp(ControlAxis::ROLL);
+	float pitch_cmd = control_sp(ControlAxis::PITCH);
+
+	actuator_sp(0) = throttle;
+
+	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
+		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
+				+ cosf(_geometry.swash_plate_servos[i].angle) * pitch_cmd * _geometry.swash_plate_servos[i].arm_length
+				- sinf(_geometry.swash_plate_servos[i].angle) * roll_cmd * _geometry.swash_plate_servos[i].arm_length;
+	}
+
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ActuatorEffectiveness.hpp"
+
+#include <px4_platform_common/module_params.h>
+
+class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+
+	static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4;
+	static constexpr int NUM_CURVE_POINTS = 5;
+
+	struct SwashPlateGeometry {
+		float angle;
+		float arm_length;
+	};
+
+	struct Geometry {
+		SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
+		int num_swash_plate_servos{0};
+		float throttle_curve[NUM_CURVE_POINTS];
+		float pitch_curve[NUM_CURVE_POINTS];
+	};
+
+	ActuatorEffectivenessHelicopter(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessHelicopter() = default;
+
+	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+
+	const char *name() const override { return "Helicopter"; }
+
+
+	const Geometry &geometry() const { return _geometry; }
+
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp) override;
+private:
+	void updateParams() override;
+
+	struct ParamHandlesSwashPlate {
+		param_t angle;
+		param_t arm_length;
+	};
+	struct ParamHandles {
+		ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
+		param_t num_swash_plate_servos;
+		param_t throttle_curve[NUM_CURVE_POINTS];
+		param_t pitch_curve[NUM_CURVE_POINTS];
+	};
+	ParamHandles _param_handles{};
+
+	Geometry _geometry{};
+
+	int _first_swash_plate_servo_index{};
+};

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -40,6 +40,8 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessCustom.hpp
 	ActuatorEffectivenessFixedWing.cpp
 	ActuatorEffectivenessFixedWing.hpp
+	ActuatorEffectivenessHelicopter.cpp
+	ActuatorEffectivenessHelicopter.hpp
 	ActuatorEffectivenessMCTilt.cpp
 	ActuatorEffectivenessMCTilt.hpp
 	ActuatorEffectivenessMultirotor.cpp

src/modules/control_allocator/ControlAllocator.cpp

@@ -254,6 +254,10 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessCustom(this);
 			break;
 
+		case EffectivenessSource::HELICOPTER:
+			tmp = new ActuatorEffectivenessHelicopter(this);
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;

src/modules/control_allocator/ControlAllocator.hpp

@@ -51,6 +51,7 @@
 #include <ActuatorEffectivenessFixedWing.hpp>
 #include <ActuatorEffectivenessMCTilt.hpp>
 #include <ActuatorEffectivenessCustom.hpp>
+#include <ActuatorEffectivenessHelicopter.hpp>
 
 #include <ControlAllocation.hpp>
 #include <ControlAllocationPseudoInverse.hpp>
@@ -148,6 +149,7 @@ private:
 		MOTORS_6DOF = 7,
 		MULTIROTOR_WITH_TILT = 8,
 		CUSTOM = 9,
+		HELICOPTER = 10,
 	};
 
 	EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};

src/modules/control_allocator/module.yaml

@@ -27,6 +27,7 @@ parameters:
                 7: Motors (6DOF)
                 8: Multirotor with Tilt
                 9: Custom
+                10: Helicopter
             default: 0
 
         CA_METHOD:
@@ -378,6 +379,62 @@ parameters:
             instance_start: 0
             default: 0
 
+        # helicopter
+        CA_SP0_COUNT:
+            description:
+                short: Number of swash plates servos
+            type: enum
+            values:
+                3: '3'
+                4: '4'
+            default: 3
+        CA_SP0_ANG${i}:
+            description:
+                short: Angle for swash plate servo ${i}
+            type: float
+            decimal: 0
+            increment: 10
+            unit: deg
+            num_instances: 4
+            min: 0
+            max: 360
+            default: [0, 140, 220, 0]
+        CA_SP0_ARM_L${i}:
+            description:
+                short: Arm length for swash plate servo ${i}
+                long: |
+                  This is relative to the other arm lengths.
+            type: float
+            decimal: 3
+            increment: 0.1
+            num_instances: 4
+            min: 0
+            max: 10
+            default: 1.0
+        CA_HELI_THR_C${i}:
+            description:
+                short: Throttle curve at position ${i}
+                long: |
+                  Defines the output throttle at the interval position ${i}.
+            type: float
+            decimal: 3
+            increment: 0.1
+            num_instances: 5
+            min: 0
+            max: 1
+            default: [0, 0.3, 0.6, 0.8, 1]
+        CA_HELI_PITCH_C${i}:
+            description:
+                short: Collective pitch curve at position ${i}
+                long: |
+                  Defines the collective pitch at the interval position ${i} for a given thrust setpoint.
+            type: float
+            decimal: 3
+            increment: 0.1
+            num_instances: 5
+            min: 0
+            max: 1
+            default: [0.05, 0.15, 0.25, 0.35, 0.45]
 
 # Mixer
 mixer:
@@ -767,3 +824,20 @@ mixer:
                           - name: 'CA_SV_CS${i}_TRIM'
                             label: 'Trim'
 
+            10: # Helicopter
+                actuators:
+                  - actuator_type: 'motor'
+                    count: 1
+                    item_label_prefix: ['Rotor']
+                  - actuator_type: 'servo'
+                    item_label_prefix: ['Yaw tail servo']
+                    count: 1
+                  - actuator_type: 'servo'
+                    group_label: 'Swash plate servos'
+                    count: 'CA_SP0_COUNT'
+                    per_item_parameters:
+                        extra:
+                          - name: 'CA_SP0_ANG${i}'
+                            label: 'Angle'
+                          - name: 'CA_SP0_ARM_L${i}'
+                            label: 'Arm Length (relative)'