From 9ba6f4efb70d944e0a96432caaed00533e30f3cd Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 9 Aug 2022 13:19:55 +0200
Subject: [PATCH] Helicopter: add yaw compensation from throttle
 CA_HELI_YAW_TH_S

---
 .../ActuatorEffectivenessHelicopter.cpp           |  5 ++++-
 .../ActuatorEffectivenessHelicopter.hpp           |  2 ++
 src/modules/control_allocator/module.yaml         | 15 ++++++++++++++-
 3 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index 801e941446..9927a14374 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -58,6 +58,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 	}
 
 	_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
+	_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
 
 	updateParams();
 }
@@ -88,6 +89,7 @@ void ActuatorEffectivenessHelicopter::updateParams()
 	}
 
 	param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale);
+	param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale);
 }
 
 bool
@@ -138,7 +140,8 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 
 	actuator_sp(0) = throttle;
 	actuator_sp(1) = control_sp(ControlAxis::YAW)
-			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale;
+			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
+			 + throttle * _geometry.yaw_throttle_scale;
 
 	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
 		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index e4b3e52878..ea5ffbb42b 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -55,6 +55,7 @@ public:
 		float throttle_curve[NUM_CURVE_POINTS];
 		float pitch_curve[NUM_CURVE_POINTS];
 		float yaw_collective_pitch_scale;
+		float yaw_throttle_scale;
 	};
 
 	ActuatorEffectivenessHelicopter(ModuleParams *parent);
@@ -82,6 +83,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_throttle_scale;
 	};
 	ParamHandles _param_handles{};
 
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index 2e85aeacc7..3f05a0b73a 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -444,7 +444,7 @@ parameters:
             default: [0.05, 0.15, 0.25, 0.35, 0.45]
         CA_HELI_YAW_CP_S:
           description:
-            short: Yaw scaling by collective pitch
+            short: Scale for yaw compensation based on collective pitch
             long: |
               This allows to add a proportional factor of the collective pitch command to the yaw command.
 
@@ -455,6 +455,19 @@ parameters:
           min: -2
           max: 2
           default: 0.0
+        CA_HELI_YAW_TH_S:
+          description:
+            short: Scale for yaw compensation based on throttle
+            long: |
+              This allows to add a proportional factor of the throttle command to the yaw command.
+
+              tail_output += CA_HELI_YAW_TH_S * throttle
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: -2
+          max: 2
+          default: 0.0
 
         # Others
         CA_FAILURE_MODE: