control_allocator: Added linearization feature for 4 servo swash plates to prevent binding

src/modules/control_allocator/ActuatorEffectiveness/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_servo_throw = param_find("CA_MAX_SVO_THROW");
 
 	updateParams();
 }
@@ -102,6 +104,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_servo_throw_deg = 0.f;
+	param_get(_param_handles.max_servo_throw, &max_servo_throw_deg);
+	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;
 }
 
 bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration,
@@ -162,6 +172,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);
@@ -174,6 +189,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;

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,8 @@ class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffe
 		param_t yaw_throttle_scale;
 		param_t yaw_ccw;
 		param_t spoolup_time;
+		param_t linearize_servos;
+		param_t max_servo_throw;
 	};
 	ParamHandles _param_handles{};
 

src/modules/control_allocator/module.yaml

@@ -528,6 +528,25 @@ parameters:
               which is mostly the case when the main rotor turns counter-clockwise.
           type: boolean
           default: 0
+        CA_LIN_SERVO:
+          description:
+            short: linearize servo output
+            long: |
+              Linearize the servo's mechanical output attached to a 4-servos swashplate. Requires setting the servo range of motion using CA_MAX_SVO_THROW.
+          type: boolean
+          default: 0
+        CA_MAX_SVO_THROW:
+          description:
+            short: Defines max/min throw of servo.
+            long: |
+              Defines min/max 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:
@@ -1092,6 +1111,10 @@ mixer:
                         name: CA_HELI_YAW_CCW
                       - label: 'Throttle spoolup time'
                         name: COM_SPOOLUP_TIME
+                      - label: 'Linearize servos'
+                        name: CA_LIN_SERVO
+                      - label: 'Min/max servo throw in degrees'
+                        name: CA_MAX_SVO_THROW
 
             11: # Helicopter (tail Servo)
               actuators:
@@ -1121,6 +1144,10 @@ mixer:
                       name: CA_HELI_YAW_CCW
                     - label: 'Throttle spoolup time'
                       name: COM_SPOOLUP_TIME
+                    - label: 'Linearize servos'
+                      name: CA_LIN_SERVO
+                    - label: 'Min/max servo throw in degrees'
+                      name: CA_MAX_SVO_THROW
 
             12: # Helicopter (Coaxial)
                 actuators: