From d103eebf9157a16fdf4ffd22ff5588e396306ba6 Mon Sep 17 00:00:00 2001
From: bnsgeyer <bnsgeyer@aol.com>
Date: Thu, 29 Feb 2024 23:09:14 -0500
Subject: [PATCH] AC_AttitudeControl: Tradheli- fix inverted mode collective
 handling

---
 .../AC_AttitudeControl_Heli.cpp               | 32 ++++++++++++++++---
 .../AC_AttitudeControl_Heli.h                 |  6 ++--
 2 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
index 21cf2ffae7..98b5df48f9 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
@@ -539,12 +539,27 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
     _throttle_in = throttle_in;
     update_althold_lean_angle_max(throttle_in);
 
-    if (_inverted_flight) {
-        throttle_in = 1.0 - throttle_in;
+    if (_transition_count > 0) {
+        _transition_count -= 1;
+    } else {
+        _transition_count = 0;
+    }
+    float throttle_out = 0.0f;
+    if (_transition_count > 0) {
+        if ((_ahrs.roll_sensor >= -3000 && _ahrs.roll_sensor <= 3000) || _ahrs.roll_sensor >= 15000 || _ahrs.roll_sensor <= -15000) {
+            throttle_out = (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid()) / cosf(radians(_ahrs.roll_sensor * 0.01f)) + ((AP_MotorsHeli&)_motors).get_coll_mid();
+        } else if ((_ahrs.roll_sensor > 3000 && _ahrs.roll_sensor < 15000) || (_ahrs.roll_sensor > -15000 && _ahrs.roll_sensor < -3000)) {
+            float scale_factor = cosf(radians(_ahrs.roll_sensor * 0.01f)) / cosf(radians(30.0f));
+            throttle_out = scale_factor * (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid())/ cosf(radians(30.0f)) + ((AP_MotorsHeli&)_motors).get_coll_mid();
+        }
+    } else if (_inverted_flight) {
+        throttle_out = 1.0f - throttle_in;
+    } else {
+        throttle_out = throttle_in;
     }
 
     _motors.set_throttle_filter_cutoff(filter_cutoff);
-    _motors.set_throttle(throttle_in);
+    _motors.set_throttle(throttle_out);
     // Clear angle_boost for logging purposes
     _angle_boost = 0.0f;
 }
@@ -574,4 +589,13 @@ void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate)
     _pid_rate_pitch.set_notch_sample_rate(sample_rate);
     _pid_rate_yaw.set_notch_sample_rate(sample_rate);
 #endif
-}
\ No newline at end of file
+}
+
+// enable/disable inverted flight
+void AC_AttitudeControl_Heli::set_inverted_flight(bool inverted)
+{
+    if (_inverted_flight != inverted) {
+        _transition_count = AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME * AP::scheduler().get_filtered_loop_rate_hz();
+    }
+    _inverted_flight = inverted;
+}
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
index d978d62f34..974a404dee 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
@@ -28,6 +28,7 @@
 #define AC_ATTITUDE_HELI_RATE_Y_FF_FILTER          20.0f
 #define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT    300
 #define AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD   ToRad(30.0f)
+#define AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME    3.0f
 
 class AC_AttitudeControl_Heli : public AC_AttitudeControl {
 public:
@@ -82,9 +83,7 @@ public:
     void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;
     
     // enable/disable inverted flight
-    void set_inverted_flight(bool inverted) override {
-        _inverted_flight = inverted;
-    }
+    void set_inverted_flight(bool inverted) override;
 
     // set the PID notch sample rates
     void set_notch_sample_rate(float sample_rate) override;
@@ -103,6 +102,7 @@ private:
 
     // true in inverted flight mode
     bool _inverted_flight;
+    uint16_t _transition_count;
 
     // Integrate vehicle rate into _att_error_rot_vec_rad
     void integrate_bf_rate_error_to_angle_errors();