From 349f15260163a21d332dcb7a2109bf62b333d8d0 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sun, 14 Aug 2022 19:33:38 +0200
Subject: [PATCH] Helicopter: throttle spoolup upon arming

Uses COM_SPOOLUP_TIME to slowly ramp the throttle and allow the
tailrotor to compensate in a coordinated way based on the yaw
compesation from throttle, see CA_HELI_YAW_TH_S.

This coordinated spoolup is necessary to avoid unsafe yaw twitches
because of the heli rotating until the correct compensation kicks in
through the feedback controller.
---
 .../ActuatorEffectivenessHelicopter.cpp       | 19 +++++++++++++++++++
 .../ActuatorEffectivenessHelicopter.hpp       | 10 ++++++++++
 2 files changed, 29 insertions(+)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index 461436e80f..ccc7e7eeb1 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -35,6 +35,7 @@
 #include <lib/mathlib/mathlib.h>
 
 using namespace matrix;
+using namespace time_literals;
 
 ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent)
 	: ModuleParams(parent)
@@ -59,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.spoolup_time = param_find("COM_SPOOLUP_TIME");
 
 	updateParams();
 }
@@ -90,6 +92,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);
+	param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
 }
 
 bool
@@ -124,6 +127,22 @@ 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
+	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) {
+		throttle *= spoolup_progress;
+	}
+
+	// actuator mapping
 	actuator_sp(0) = throttle;
 	actuator_sp(1) = control_sp(ControlAxis::YAW)
 			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index ea5ffbb42b..9cb81c905b 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -37,6 +37,9 @@
 
 #include <px4_platform_common/module_params.h>
 
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+
 class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness
 {
 public:
@@ -56,6 +59,7 @@ public:
 		float pitch_curve[NUM_CURVE_POINTS];
 		float yaw_collective_pitch_scale;
 		float yaw_throttle_scale;
+		float spoolup_time;
 	};
 
 	ActuatorEffectivenessHelicopter(ModuleParams *parent);
@@ -84,10 +88,16 @@ private:
 		param_t pitch_curve[NUM_CURVE_POINTS];
 		param_t yaw_collective_pitch_scale;
 		param_t yaw_throttle_scale;
+		param_t spoolup_time;
 	};
 	ParamHandles _param_handles{};
 
 	Geometry _geometry{};
 
 	int _first_swash_plate_servo_index{};
+
+	// Throttle spoolup state
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	bool _armed{false};
+	uint64_t _armed_time{0};
 };