From 937d27f8eef89efbd9c56ec1bb12371a3c2be4c0 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 1 Nov 2022 17:53:36 +0100
Subject: [PATCH] Helicopter: add offset for yaw compensation based on
 collective pitch

---
 .../ActuatorEffectivenessHelicopter.cpp       |  4 +++-
 .../ActuatorEffectivenessHelicopter.hpp       |  2 ++
 src/modules/control_allocator/module.yaml     | 19 ++++++++++++++++++-
 3 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index f5a7aaedbb..9faae22ca4 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -61,6 +61,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 	}
 
 	_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
+	_param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O");
 	_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");
@@ -95,6 +96,7 @@ void ActuatorEffectivenessHelicopter::updateParams()
 	}
 
 	param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale);
+	param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset);
 	param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale);
 	param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
 	int32_t yaw_ccw = 0;
@@ -142,7 +144,7 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	actuator_sp(0) = mainMotorEnaged() ? throttle : NAN;
 
 	actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign
-			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
+			 + fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale
 			 + throttle * _geometry.yaw_throttle_scale;
 
 	// Saturation check for yaw
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index 3cd64b2f36..fc66e0044a 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -60,6 +60,7 @@ public:
 		float throttle_curve[NUM_CURVE_POINTS];
 		float pitch_curve[NUM_CURVE_POINTS];
 		float yaw_collective_pitch_scale;
+		float yaw_collective_pitch_offset;
 		float yaw_throttle_scale;
 		float yaw_sign;
 		float spoolup_time;
@@ -109,6 +110,7 @@ private:
 		param_t throttle_curve[NUM_CURVE_POINTS];
 		param_t pitch_curve[NUM_CURVE_POINTS];
 		param_t yaw_collective_pitch_scale;
+		param_t yaw_collective_pitch_offset;
 		param_t yaw_throttle_scale;
 		param_t yaw_ccw;
 		param_t spoolup_time;
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index 59d49b0afa..e48570dc2b 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -476,7 +476,24 @@ parameters:
               This allows to add a proportional factor of the collective pitch command to the yaw command.
               A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.
 
-              tail_output += CA_HELI_YAW_CP_S * collective_pitch
+              tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: -2
+          max: 2
+          default: 0.0
+        CA_HELI_YAW_CP_O:
+          description:
+            short: Offset for yaw compensation based on collective pitch
+            long: |
+              This allows to specify which collective pitch command results in the least amount of rotor drag.
+              This is used to increase the accuracy of the yaw drag torque compensation based on collective pitch
+              by aligning the lowest rotor drag with zero compensation.
+              For symmetric profile blades this is the command that results in exactly 0° collective blade angle.
+              For lift profile blades this is typically a command resulting in slightly negative collective blade angle.
+
+              tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
           type: float
           decimal: 3
           increment: 0.1