From 9ec1faf3f95a8dbb519e5adaf3a5f70ff05547af Mon Sep 17 00:00:00 2001
From: Peter Hall <iampete@hotmail.co.uk>
Date: Sun, 23 Jan 2022 16:24:02 +0000
Subject: [PATCH] Plane: tailsitter: rate limit second half of FW transtion

---
 ArduPlane/tailsitter.cpp | 23 ++++++++++++++++++++++-
 ArduPlane/tailsitter.h   |  4 ++++
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp
index 6789b8260a..678bd29bae 100644
--- a/ArduPlane/tailsitter.cpp
+++ b/ArduPlane/tailsitter.cpp
@@ -777,6 +777,12 @@ void Tailsitter_Transition::update()
     case TRANSITION_ANGLE_WAIT_FW: {
         if (tailsitter.transition_fw_complete()) {
             transition_state = TRANSITION_DONE;
+            if (hal.util->get_soft_armed()) {
+                fw_limit_start_ms = now;
+                fw_limit_initial_pitch = constrain_float(quadplane.ahrs.pitch_sensor,-8500,8500);
+                plane.nav_pitch_cd = fw_limit_initial_pitch;
+                plane.nav_roll_cd = 0;
+            }
             break;
         }
         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
@@ -867,7 +873,21 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na
 
     } else if (transition_state == TRANSITION_DONE) {
         // still in FW, reset transition starting point
-        force_transistion_complete();
+        vtol_transition_start_ms = now;
+        vtol_transition_initial_pitch = constrain_float(plane.nav_pitch_cd,-8500,8500);
+
+        // rate limit initial pitch down
+        if (fw_limit_start_ms != 0) {
+            const float pitch_limit_cd = fw_limit_initial_pitch - (now - fw_limit_start_ms) * tailsitter.transition_rate_fw * 0.1;
+            if ((pitch_limit_cd <= 0) || (nav_pitch_cd >= pitch_limit_cd)) {
+                // never limit past 0, never limit to a smaller pitch angle
+                fw_limit_start_ms = 0;
+            } else {
+                nav_pitch_cd = pitch_limit_cd;
+                nav_roll_cd = 0;
+                allow_stick_mixing = false;
+            }
+        }
     }
 }
 
@@ -917,6 +937,7 @@ void Tailsitter_Transition::force_transistion_complete()
     transition_state = TRANSITION_DONE;
     vtol_transition_start_ms = AP_HAL::millis();
     vtol_transition_initial_pitch = constrain_float(plane.nav_pitch_cd,-8500,8500);
+    fw_limit_start_ms = 0;
 }
 
 MAV_VTOL_STATE Tailsitter_Transition::get_mav_vtol_state() const
diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h
index bb58e29890..1ab5b711aa 100644
--- a/ArduPlane/tailsitter.h
+++ b/ArduPlane/tailsitter.h
@@ -179,6 +179,10 @@ private:
     uint32_t vtol_limit_start_ms;
     float vtol_limit_initial_pitch;
 
+    // for rate limit of FW flight
+    uint32_t fw_limit_start_ms;
+    float fw_limit_initial_pitch;
+
     // for transition to FW flight
     uint32_t fw_transition_start_ms;
     float fw_transition_initial_pitch;