diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index f403424b678c..dd0965c01c39 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/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();
 }
@@ -102,6 +103,22 @@ 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
+		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;
+		_geometry.linearize_servos = 1;
+
+	}  else {
+		// handle any undefined behaviour if disabled
+		_geometry.max_servo_height = 0.f;
+		_geometry.inverse_max_servo_throw = 0.f;
+		_geometry.linearize_servos = 0;
+	}
 }
 
 bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
@@ -162,6 +179,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);
@@ -174,6 +196,21 @@ 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_servo_height * input;
+
+	if (!PX4_ISFINITE(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/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index 439e0f4ab9ce..57d1de2c919b 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -64,6 +64,9 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe
 		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);
@@ -84,6 +87,7 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();
+	float getLinearServoOutput(float input) const;
 
 	void updateParams() override;
 
@@ -114,6 +118,7 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe
 		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 d085704df403..ff2b79886391 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -528,6 +528,18 @@ parameters:
               which is mostly the case when the main rotor turns counter-clockwise.
           type: boolean
           default: 0
+        CA_MAX_SVO_THROW:
+          description:
+            short:  Defines max/min throw of servo for linearization
+            long: |
+              Defines min/max throw of servo. Used to linearize mechanical output of servo attached to a 4-servo swashplate. Setting to zero disables feature.
+          type: float
+          decimal: 3
+          unit: deg
+          increment: 0.1
+          min: 0
+          max: 75
+          default: 0.0
 
         # Others
         CA_FAILURE_MODE: