AC_AttitudeControl: Tradheli- fix inverted mode collective handling

bnsgeyer · Mar 1, 2024 · 6ea06ac · IamPete1 · Mar 1, 2024 · 6ea06ac
1 parent 1f7efca
commit 6ea06ac
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 3 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
@@ -318,6 +318,8 @@ AC_AttitudeControl_Heli::AC_AttitudeControl_Heli(AP_AHRS_View &ahrs, const AP_Mu
     _flags_heli.leaky_i = true;
     _flags_heli.flybar_passthrough = false;
     _flags_heli.tail_passthrough = false;
+    // initialize transition variable for inverted flight transition
+     _transition_count = 0;
 #if AP_FILTER_ENABLED
     set_notch_sample_rate(AP::scheduler().get_loop_rate_hz());
 #endif
@@ -539,12 +541,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;
 }

diff --git 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:
@@ -83,6 +84,9 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
 
     // enable/disable inverted flight
     void set_inverted_flight(bool inverted) override {
+        if (_inverted_flight != inverted) {
+            _transition_count = AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME * 400.0f;
+        }
         _inverted_flight = inverted;
     }
 
@@ -103,6 +107,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
 
     // 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();