From babdb3625ac65e71c8c6986be6708235540f14e9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 26 Sep 2023 18:56:28 +1000
Subject: [PATCH] Plane: moved assign tilt to the run() function

this prevents double calling and fixes qhover
---
 ArduPlane/mode_qhover.cpp     | 1 +
 ArduPlane/mode_qstabilize.cpp | 7 ++-----
 ArduPlane/quadplane.cpp       | 6 +++++-
 3 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp
index 1fe4848296..fac2147f93 100644
--- a/ArduPlane/mode_qhover.cpp
+++ b/ArduPlane/mode_qhover.cpp
@@ -37,6 +37,7 @@ void ModeQHover::run()
         quadplane.relax_attitude_control();
         pos_control->relax_z_controller(0);
     } else {
+        plane.quadplane.assign_tilt_to_fwd_thr();
         quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
     }
 
diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp
index 0122147a35..659d674736 100644
--- a/ArduPlane/mode_qstabilize.cpp
+++ b/ArduPlane/mode_qstabilize.cpp
@@ -35,11 +35,6 @@ void ModeQStabilize::update()
         plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max;
         plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
     }
-
-    if (plane.control_mode->mode_number() == Mode::Number::QSTABILIZE) {
-        // protect against this function running twice
-        plane.quadplane.assign_tilt_to_fwd_thr();
-    }
 }
 
 // quadplane stabilize mode
@@ -52,6 +47,8 @@ void ModeQStabilize::run()
         return;
     }
 
+    plane.quadplane.assign_tilt_to_fwd_thr();
+
     // special check for ESC calibration in QSTABILIZE
     if (quadplane.esc_calibration != 0) {
         quadplane.run_esc_calibration();
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 339dd7cf3f..93d062d38d 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2972,6 +2972,9 @@ QuadPlane::ActiveFwdThr QuadPlane::get_vfwd_method(void) const
     return ActiveFwdThr::NONE;
 }
 
+/*
+  map from pitch tilt to fwd throttle when enabled
+ */
 void QuadPlane::assign_tilt_to_fwd_thr(void) {
 
     const auto fwd_thr_active = get_vfwd_method();
@@ -2991,7 +2994,8 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) {
     if (is_positive(fwd_tilt_range_cd)) {
         // rate limit the forward tilt change to slew between the motor good and motor failed
         // value over 10 seconds
-        const float fwd_pitch_lim_cd_tgt = plane.quadplane.pos_control->get_fwd_pitch_is_limited() ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim;
+        const bool fwd_limited = plane.quadplane.pos_control->is_active_xy() and plane.quadplane.pos_control->get_fwd_pitch_is_limited();
+        const float fwd_pitch_lim_cd_tgt = fwd_limited ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim;
         const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt;
         q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max);
         // Don't let the forward pitch limit be more than the forward pitch demand before limiting to