Coaxial Helicopter Support

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -104,8 +104,7 @@ void ActuatorEffectivenessHelicopter::updateParams()
 	_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
 }
 
-bool
-ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
+bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
 {
 	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
@@ -221,7 +220,6 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit
 
 void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
 {
-
 	// Note: the values '-1', '1' and '0' are just to indicate a negative,
 	// positive or no saturation to the rate controller. The actual magnitude is not used.
 	if (_saturation_flags.roll_pos) {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp

@@ -0,0 +1,222 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "ActuatorEffectivenessHelicopterCoaxial.hpp"
+#include <lib/mathlib/mathlib.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(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);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
+		_param_handles.swash_plate_servos[i].trim = param_find(buffer);
+	}
+
+	_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
+	_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
+
+	updateParams();
+}
+
+void ActuatorEffectivenessHelicopterCoaxial::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, 2, 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);
+		param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim);
+	}
+
+	param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
+}
+
+bool ActuatorEffectivenessHelicopterCoaxial::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{}); // Clockwise rotor
+	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Counter-clockwise rotor
+
+	// 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{});
+		configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim;
+	}
+
+	return true;
+}
+
+void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	_saturation_flags = {};
+
+	// throttle/collective pitch curve
+	const float throttle = -control_sp(ControlAxis::THRUST_Z) * throttleSpoolupProgress();
+	const float yaw = control_sp(ControlAxis::YAW);
+
+	// actuator mapping
+	actuator_sp(0) = throttle - yaw; // Clockwise
+	actuator_sp(1) = throttle + yaw; // Counter-clockwise
+
+	// Saturation check for yaw
+	if ((actuator_sp(0) < actuator_min(0)) || (actuator_sp(1) > actuator_max(1))) {
+		setSaturationFlag(1.f, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
+
+	} else if ((actuator_sp(0) > actuator_max(0)) || (actuator_sp(1) < actuator_min(1))) {
+		setSaturationFlag(1.f, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
+	}
+
+	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
+		float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
+		float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
+		actuator_sp(_first_swash_plate_servo_index + i) =
+			+ control_sp(ControlAxis::PITCH) * pitch_coeff
+			- control_sp(ControlAxis::ROLL) * roll_coeff
+			+ _geometry.swash_plate_servos[i].trim;
+
+		// Saturation check for roll & pitch
+		if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) {
+			setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg);
+			setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos);
+
+		} else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) {
+			setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos);
+			setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg);
+		}
+	}
+}
+
+float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress()
+{
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+		_armed_time = vehicle_status.armed_time;
+	}
+
+	const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f;
+	const float spoolup_progress = time_since_arming / _geometry.spoolup_time;
+
+	if (_armed && spoolup_progress < 1.f) {
+		return spoolup_progress;
+	}
+
+	return 1.f;
+}
+
+
+void ActuatorEffectivenessHelicopterCoaxial::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag)
+{
+	if (coeff > 0.f) {
+		// A positive change in given axis will increase saturation
+		positive_flag = true;
+
+	} else if (coeff < 0.f) {
+		// A negative change in given axis will increase saturation
+		negative_flag = true;
+	}
+}
+
+void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_saturation_flags.roll_pos) {
+		status.unallocated_torque[0] = 1.f;
+
+	} else if (_saturation_flags.roll_neg) {
+		status.unallocated_torque[0] = -1.f;
+
+	} else {
+		status.unallocated_torque[0] = 0.f;
+	}
+
+	if (_saturation_flags.pitch_pos) {
+		status.unallocated_torque[1] = 1.f;
+
+	} else if (_saturation_flags.pitch_neg) {
+		status.unallocated_torque[1] = -1.f;
+
+	} else {
+		status.unallocated_torque[1] = 0.f;
+	}
+
+	if (_saturation_flags.yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_saturation_flags.yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
+	}
+
+	if (_saturation_flags.thrust_pos) {
+		status.unallocated_thrust[2] = 1.f;
+
+	} else if (_saturation_flags.thrust_neg) {
+		status.unallocated_thrust[2] = -1.f;
+
+	} else {
+		status.unallocated_thrust[2] = 0.f;
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/manual_control_switches.h>
+
+class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public ActuatorEffectiveness
+{
+public:
+
+	static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4;
+
+	struct SwashPlateGeometry {
+		float angle;
+		float arm_length;
+		float trim;
+	};
+
+	struct Geometry {
+		SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
+		int num_swash_plate_servos{0};
+		float spoolup_time;
+	};
+
+	ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent);
+	virtual ~ActuatorEffectivenessHelicopterCoaxial() = 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, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
+private:
+	float throttleSpoolupProgress();
+
+	void updateParams() override;
+
+	struct SaturationFlags {
+		bool roll_pos;
+		bool roll_neg;
+		bool pitch_pos;
+		bool pitch_neg;
+		bool yaw_pos;
+		bool yaw_neg;
+		bool thrust_pos;
+		bool thrust_neg;
+	};
+	static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag);
+
+	struct ParamHandlesSwashPlate {
+		param_t angle;
+		param_t arm_length;
+		param_t trim;
+	};
+	struct ParamHandles {
+		ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
+		param_t num_swash_plate_servos;
+		param_t spoolup_time;
+	};
+	ParamHandles _param_handles{};
+
+	Geometry _geometry{};
+
+	int _first_swash_plate_servo_index{};
+	SaturationFlags _saturation_flags;
+
+	// Throttle spoolup state
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	bool _armed{false};
+	uint64_t _armed_time{0};
+
+	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
+};

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

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

src/modules/control_allocator/ControlAllocator.cpp

@@ -266,6 +266,10 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS);
 			break;
 
+		case EffectivenessSource::HELICOPTER_COAXIAL:
+			tmp = new ActuatorEffectivenessHelicopterCoaxial(this);
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;

src/modules/control_allocator/ControlAllocator.hpp

@@ -53,6 +53,7 @@
 #include <ActuatorEffectivenessCustom.hpp>
 #include <ActuatorEffectivenessUUV.hpp>
 #include <ActuatorEffectivenessHelicopter.hpp>
+#include <ActuatorEffectivenessHelicopterCoaxial.hpp>
 
 #include <ControlAllocation.hpp>
 #include <ControlAllocationPseudoInverse.hpp>
@@ -157,6 +158,7 @@ class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParam
 		CUSTOM = 9,
 		HELICOPTER_TAIL_ESC = 10,
 		HELICOPTER_TAIL_SERVO = 11,
+		HELICOPTER_COAXIAL = 12,
 	};
 
 	enum class FailureMode {