From 35d96d57f9e13c672fe62b3231128ae03eb26628 Mon Sep 17 00:00:00 2001
From: Ted <86834177+TedObrien@users.noreply.github.com>
Date: Mon, 24 Feb 2025 12:17:11 +0000
Subject: [PATCH] control_allocator: Added linearization feature for 4 servo
 swash plates to prevent binding (#23961)

* control_allocator: Added linearization feature for heli swashplates to help prevent servo binding

* Apply suggestions from code review

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

* update description of CA_LIN_SERVO parameter

* update variable name

* add missing semi-colon

* fix variable referenced before assignment

* add missing indentation

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

* removed param unnecessary param

* removed whitespace

* remove CA_LIN_SERVO and enable feature if CA_MAX_SERVO_THROW > 0 plus Update param description.

* remove CA_MAX_SVO_THROW from actuators tab to avoid confusion during standard swashplate setup.

* added comment and fixed spelling mistake

* fix spelling mistake

* fix formatting

* reduce CA_MAX_SVO_THROW short description length to stop test failure

* ActuatorEffectivenessHelicopter: clarfification suggestions for servo linearization feature

* remove NAN check.

---------

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
---
 .../ActuatorEffectivenessHelicopter.cpp       | 32 +++++++++++++++++++
 .../ActuatorEffectivenessHelicopter.hpp       |  5 +++
 src/modules/control_allocator/module.yaml     | 14 ++++++++
 3 files changed, 51 insertions(+)

diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index beacc7007b..967706a6d8 100644
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -65,6 +65,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 	_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
 	_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
 	_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
+	_param_handles.max_servo_throw = param_find("CA_MAX_SVO_THROW");
 
 	updateParams();
 }
@@ -101,6 +102,21 @@ void ActuatorEffectivenessHelicopter::updateParams()
 	int32_t yaw_ccw = 0;
 	param_get(_param_handles.yaw_ccw, &yaw_ccw);
 	_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
+	float max_servo_throw_deg = 0.f;
+	param_get(_param_handles.max_servo_throw, &max_servo_throw_deg);
+
+	if (max_servo_throw_deg > 0.f) {
+		// linearization feature enabled
+		_geometry.linearize_servos = 1;
+		const float max_servo_throw = math::radians(max_servo_throw_deg);
+		_geometry.max_servo_height = sinf(max_servo_throw);
+		_geometry.inverse_max_servo_throw = 1.f / max_servo_throw;
+
+	} else {
+		// handle any undefined behaviour if disabled
+		_geometry.linearize_servos = 0;
+		_geometry.max_servo_height = _geometry.inverse_max_servo_throw = 0.f;
+	}
 }
 
 bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
@@ -168,6 +184,11 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 				- control_sp(ControlAxis::ROLL) * roll_coeff
 				+ _geometry.swash_plate_servos[i].trim;
 
+		// Apply linearization to the actuator setpoint if enabled
+		if (_geometry.linearize_servos) {
+			actuator_sp(_first_swash_plate_servo_index + i) = getLinearServoOutput(actuator_sp(_first_swash_plate_servo_index + i));
+		}
+
 		// 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);
@@ -180,6 +201,17 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	}
 }
 
+float ActuatorEffectivenessHelicopter::getLinearServoOutput(float input) const
+{
+	input = math::constrain(input, -1.f, 1.f);
+
+	// make sure a the maximal input of [-1,1] maps to the maximal vertical deflection the servo can reach of sin(CA_MAX_SVO_THROW)
+	float servo_height = _geometry.max_servo_height * input;
+
+	// mulitply by 1 over max arm roation in radians to normalise
+	return _geometry.inverse_max_servo_throw * asinf(servo_height);
+}
+
 bool ActuatorEffectivenessHelicopter::mainMotorEnaged()
 {
 	manual_control_switches_s manual_control_switches;
diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index 669bb725e6..78df3abec1 100644
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -66,6 +66,9 @@ public:
 		float yaw_throttle_scale;
 		float yaw_sign;
 		float spoolup_time;
+		int linearize_servos;
+		float max_servo_height;
+		float inverse_max_servo_throw;
 	};
 
 	ActuatorEffectivenessHelicopter(ModuleParams *parent, ActuatorType tail_actuator_type);
@@ -86,6 +89,7 @@ public:
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();
+	float getLinearServoOutput(float input) const;
 
 	void updateParams() override;
 
@@ -116,6 +120,7 @@ private:
 		param_t yaw_throttle_scale;
 		param_t yaw_ccw;
 		param_t spoolup_time;
+		param_t max_servo_throw;
 	};
 	ParamHandles _param_handles{};
 
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index b7fe1eb019..c43d66a51c 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -564,6 +564,20 @@ parameters:
           min: 0
           max: 10
           default: 0.0
+        CA_MAX_SVO_THROW:
+          description:
+            short: Throw angle of swashplate servo at maximum commands for linearization
+            long: |
+              Used to linearize mechanical output of swashplate servos to avoid axis coupling and binding with 4 servo redundancy.
+              This requires a symmetric setup where the servo horn is exactly centered with a 0 command.
+              Setting to zero disables feature.
+          type: float
+          decimal: 1
+          unit: deg
+          increment: 0.1
+          min: 0
+          max: 75
+          default: 0.0
 
         # Others
         CA_FAILURE_MODE: