From e346190e6386ed2a7e68a7d992d0ab170e0499a7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 14 Jul 2022 17:41:54 +0200
Subject: [PATCH] helicopter: use tail motor & add CA_HELI_YAW_CP_S

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

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index 41fc850954..bd2bff61c1 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -57,6 +57,8 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 		_param_handles.pitch_curve[i] = param_find(buffer);
 	}
 
+	_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
+
 	updateParams();
 }
 
@@ -84,6 +86,8 @@ void ActuatorEffectivenessHelicopter::updateParams()
 		param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]);
 		param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]);
 	}
+
+	param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale);
 }
 
 bool
@@ -97,8 +101,8 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura
 	// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
 
-	// Tail (yaw) servo
-	configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
+	// Tail (yaw) motor
+	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
 
 	// N swash plate servos
 	_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
@@ -134,6 +138,8 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 
 	actuator_sp(0) = throttle;
 
+	actuator_sp(1) = control_sp(ControlAxis::YAW) + collective_pitch * _geometry.yaw_collective_pitch_scale;
+
 	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
 		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
 				+ cosf(_geometry.swash_plate_servos[i].angle) * pitch_cmd * _geometry.swash_plate_servos[i].arm_length
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index 3438f017a5..e4b3e52878 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -54,6 +54,7 @@ public:
 		int num_swash_plate_servos{0};
 		float throttle_curve[NUM_CURVE_POINTS];
 		float pitch_curve[NUM_CURVE_POINTS];
+		float yaw_collective_pitch_scale;
 	};
 
 	ActuatorEffectivenessHelicopter(ModuleParams *parent);
@@ -80,6 +81,7 @@ private:
 		param_t num_swash_plate_servos;
 		param_t throttle_curve[NUM_CURVE_POINTS];
 		param_t pitch_curve[NUM_CURVE_POINTS];
+		param_t yaw_collective_pitch_scale;
 	};
 	ParamHandles _param_handles{};
 
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index 0159ab1113..e6c60194c4 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -440,6 +440,19 @@ parameters:
             min: 0
             max: 1
             default: [0.05, 0.15, 0.25, 0.35, 0.45]
+        CA_HELI_YAW_CP_S:
+          description:
+            short: Yaw scaling by collective pitch
+            long: |
+              This allows to add a proportional factor of the collective pitch command to the yaw command.
+
+              tail_output += CA_HELI_YAW_CP_S * collective_pitch
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: -2
+          max: 2
+          default: 0.0
 
         # Others
         CA_FAILURE_MODE:
@@ -876,8 +889,8 @@ mixer:
                   - actuator_type: 'motor'
                     count: 1
                     item_label_prefix: ['Rotor']
-                  - actuator_type: 'servo'
-                    item_label_prefix: ['Yaw tail servo']
+                  - actuator_type: 'motor'
+                    item_label_prefix: ['Yaw tail Motor']
                     count: 1
                   - actuator_type: 'servo'
                     group_label: 'Swash plate servos'