From f13718d4827078877110aeb83cdc762d04701e7a Mon Sep 17 00:00:00 2001
From: TedObrien <tedobr1998@gmail.com>
Date: Thu, 14 Nov 2024 11:56:14 +0000
Subject: [PATCH] control_allocator: Added linearization feature for heli
 swashplates to help prevent servo binding

---
 .../ActuatorEffectivenessHelicopter.cpp       | 31 +++++++++++++++++++
 .../ActuatorEffectivenessHelicopter.hpp       |  7 +++++
 src/modules/control_allocator/module.yaml     | 28 +++++++++++++++++
 3 files changed, 66 insertions(+)

diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index beacc7007b..ca425cd110 100644
--- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -65,6 +65,8 @@ 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.linearize_servos = param_find("CA_LIN_SERVO");
+	_param_handles.max_sevo_throw = param_find("CA_MAX_SVO_THROW");
 
 	updateParams();
 }
@@ -101,6 +103,14 @@ 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;
+	int32_t linearize_servos = 0;
+	param_get(_param_handles.linearize_servos, &linearize_servos);
+	_geometry.linearize_servos = (linearize_servos != 0);
+	float max_sevo_throw = 0.f;
+	param_get(_param_handles.max_sevo_throw, &max_sevo_throw);
+	max_sevo_throw *=  M_PI_F / 180.0f;  //converting deg to rad
+	_geometry.max_sevo_height = sinf(max_sevo_throw);
+	_geometry.inverse_max_servo_throw = 1/max_sevo_throw;
 }
 
 bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
@@ -168,6 +178,11 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 				- control_sp(ControlAxis::ROLL) * roll_coeff
 				+ _geometry.swash_plate_servos[i].trim;
 
+		// Apply linearzsation 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 +195,22 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	}
 }
 
+float ActuatorEffectivenessHelicopter::getLinearServoOutput(float input) const
+{
+
+	input = math::constrain(input, -1.0f, 1.0f);
+
+	//servo output is calculated by normalizing input to arm rotation of CA_MAX_SVO_THROW degrees as full input for a linear throw
+	float svo_height = _geometry.max_sevo_height * input;
+
+	if (std::isnan(svo_height)) {
+		svo_height = 0.0f;
+	}
+
+	// mulitply by 1 over max arm roation in radians to normalise
+	return _geometry.inverse_max_servo_throw * asinf(svo_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..c6b99407c0 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_sevo_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,9 @@ private:
 		param_t yaw_throttle_scale;
 		param_t yaw_ccw;
 		param_t spoolup_time;
+		param_t linearize_servos;
+		param_t max_sevo_throw;
+		param_t inverse_max_servo_throw;
 	};
 	ParamHandles _param_handles{};
 
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index b7fe1eb019..90b51a9b29 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -564,6 +564,25 @@ parameters:
           min: 0
           max: 10
           default: 0.0
+        CA_LIN_SERVO:
+          description:
+            short: linearize servo output
+            long: |
+              This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation. Should only be necessary with specific swashplate setups using 4 servos and requires specific setup to work properly. The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. If the spline on the servo control horn is not allowing you to get the servo arm perpendicular to the shaft, the use the servo trim parameters to make them perpendicular to the shaft. Leveling the swashplate can only be done using the linkages. set your linkages so that the swashplate in its mid position will give you required collective for hover.
+          type: boolean
+          default: 0
+        CA_MAX_SVO_THROW:
+          description:
+            short: Defines max/min throw of servo.
+            long: |
+              Defines max/min throw of servo when linearizing servo output. Only used when CA_LIN_SERVO is enabled.
+          type: float
+          decimal: 3
+          unit: deg
+          increment: 0.1
+          min: 5
+          max: 75
+          default: 50.0
 
         # Others
         CA_FAILURE_MODE:
@@ -1128,6 +1147,10 @@ mixer:
                         name: CA_HELI_YAW_CCW
                       - label: 'Throttle spoolup time'
                         name: COM_SPOOLUP_TIME
+                      - label: 'linearize servos'
+                        name: CA_LIN_SERVO
+                      - label: 'max/min throw of servo in degrees'
+                        name: CA_MAX_SVO_THROW
 
             11: # Helicopter (tail Servo)
               actuators:
@@ -1157,6 +1180,11 @@ mixer:
                       name: CA_HELI_YAW_CCW
                     - label: 'Throttle spoolup time'
                       name: COM_SPOOLUP_TIME
+                    - label: 'linearize servos'
+                      name: CA_LIN_SERVO
+                    - label: 'max/min throw of servo in degrees'
+                      name: CA_MAX_SVO_THROW
+
 
             12: # Helicopter (Coaxial)
                 actuators: