control_allocator: Added linearization feature for heli swashplates t…

…o help prevent servo 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_sevo_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_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,
@@ -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,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;

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_sevo_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,9 @@ 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_sevo_throw;
+		param_t inverse_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: |
+              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:
@@ -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: 'max/min throw of servo in degrees'
+                        name: CA_MAX_SVO_THROW
 
             11: # Helicopter (tail Servo)
               actuators:
@@ -1121,6 +1144,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: