From f32d931117486e84f073e57b59e16264a288dcda Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 16 Aug 2022 17:04:10 +0200
Subject: [PATCH] helicopter: add yaw sign parameter & expose settings in UI

---
 .../ActuatorEffectivenessHelicopter.cpp       | 58 ++++++++++++++-----
 .../ActuatorEffectivenessHelicopter.hpp       |  4 ++
 src/modules/control_allocator/module.yaml     | 21 ++++++-
 3 files changed, 67 insertions(+), 16 deletions(-)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index ccc7e7eeb1..dfb17578d6 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -60,6 +60,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");
+	_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
 	_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
 
 	updateParams();
@@ -93,6 +94,9 @@ 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);
 	param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
+	int32_t yaw_ccw = 0;
+	param_get(_param_handles.yaw_ccw, &yaw_ccw);
+	_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
 }
 
 bool
@@ -127,7 +131,24 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 			       _geometry.throttle_curve) * throttleSpoolupProgress();
 	const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
 
-	// throttle spoolup
+	// actuator mapping
+	actuator_sp(0) = throttle;
+	actuator_sp(1) = control_sp(ControlAxis::YAW)
+			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
+			 + throttle * _geometry.yaw_throttle_scale;
+	actuator_sp(1) *= _geometry.yaw_sign;
+
+	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
+		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
+				+ control_sp(ControlAxis::PITCH) * cosf(_geometry.swash_plate_servos[i].angle) *
+				_geometry.swash_plate_servos[i].arm_length
+				- control_sp(ControlAxis::ROLL) * sinf(_geometry.swash_plate_servos[i].angle) *
+				_geometry.swash_plate_servos[i].arm_length;
+	}
+}
+
+float ActuatorEffectivenessHelicopter::throttleSpoolupProgress()
+{
 	vehicle_status_s vehicle_status;
 
 	if (_vehicle_status_sub.update(&vehicle_status)) {
@@ -139,20 +160,27 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	const float spoolup_progress = time_since_arming / _geometry.spoolup_time;
 
 	if (_armed && spoolup_progress < 1.f) {
-		throttle *= spoolup_progress;
+		return spoolup_progress;
 	}
 
-	// actuator mapping
-	actuator_sp(0) = throttle;
-	actuator_sp(1) = control_sp(ControlAxis::YAW)
-			 + 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
-				+ control_sp(ControlAxis::PITCH) * cosf(_geometry.swash_plate_servos[i].angle) *
-				_geometry.swash_plate_servos[i].arm_length
-				- control_sp(ControlAxis::ROLL) * sinf(_geometry.swash_plate_servos[i].angle) *
-				_geometry.swash_plate_servos[i].arm_length;
-	}
+	return 1.f;
+}
+
+float ActuatorEffectivenessHelicopter::throttleSpoolupProgress()
+{
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+		_armed_time = vehicle_status.armed_time;
+	}
+
+	const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f;
+	const float spoolup_progress = time_since_arming / _geometry.spoolup_time;
+
+	if (_armed && spoolup_progress < 1.f) {
+		return spoolup_progress;
+	}
+
+	return 1.f;
 }
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index 9cb81c905b..c66fb52906 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -59,6 +59,7 @@ public:
 		float pitch_curve[NUM_CURVE_POINTS];
 		float yaw_collective_pitch_scale;
 		float yaw_throttle_scale;
+		float yaw_sign;
 		float spoolup_time;
 	};
 
@@ -75,6 +76,8 @@ public:
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp) override;
 private:
+	float throttleSpoolupProgress();
+
 	void updateParams() override;
 
 	struct ParamHandlesSwashPlate {
@@ -88,6 +91,7 @@ private:
 		param_t pitch_curve[NUM_CURVE_POINTS];
 		param_t yaw_collective_pitch_scale;
 		param_t yaw_throttle_scale;
+		param_t yaw_ccw;
 		param_t spoolup_time;
 	};
 	ParamHandles _param_handles{};
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index 3f05a0b73a..d3c11d911b 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -435,11 +435,13 @@ parameters:
                 short: Collective pitch curve at position ${i}
                 long: |
                   Defines the collective pitch at the interval position ${i} for a given thrust setpoint.
+
+                  Use negative values if the swash plate needs to move down to provide upwards thrust.
             type: float
             decimal: 3
             increment: 0.1
             num_instances: 5
-            min: 0
+            min: -1
             max: 1
             default: [0.05, 0.15, 0.25, 0.35, 0.45]
         CA_HELI_YAW_CP_S:
@@ -468,6 +470,13 @@ parameters:
           min: -2
           max: 2
           default: 0.0
+        CA_HELI_YAW_CCW:
+          description:
+            short: Change yaw direction
+            long: |
+              By default, positive thrust of the tail rotor is expected to rotate the vehicle clockwise. Set this parameter to true if the tail rotor provides thrust in the other direciton.
+          type: boolean
+          default: 0
 
         # Others
         CA_FAILURE_MODE:
@@ -916,3 +925,13 @@ mixer:
                             label: 'Angle'
                           - name: 'CA_SP0_ARM_L${i}'
                             label: 'Arm Length (relative)'
+                    parameters:
+                      - label: 'Yaw compensation scale based on collective pitch'
+                        name: CA_HELI_YAW_CP_S
+                      - label: 'Yaw compensation scale based on throttle'
+                        name: CA_HELI_YAW_TH_S
+                      - label: 'Yaw direction is CCW'
+                        name: CA_HELI_YAW_CCW
+                      - label: 'Throttle spoolup time'
+                        name: COM_SPOOLUP_TIME
+