From 2254252afffb9a535f000dbc88e7424a548d2b1c Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 24 Apr 2023 13:28:55 +0100
Subject: [PATCH] Plane: move mode manual reset into run function removing
 manual early return

---
 ArduPlane/Attitude.cpp    | 21 ++++-----------------
 ArduPlane/mode.cpp        | 13 +++++++++++++
 ArduPlane/mode.h          |  5 +++++
 ArduPlane/mode_manual.cpp |  4 ++++
 4 files changed, 26 insertions(+), 17 deletions(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index ba45e8735e..77e41ba2fe 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -346,13 +346,6 @@ void Plane::stabilize_yaw()
  */
 void Plane::stabilize()
 {
-    if (control_mode == &mode_manual) {
-        // reset steering controls
-        steer_state.locked_course = false;
-        steer_state.locked_course_err = 0;
-        return;
-    }
-
     uint32_t now = AP_HAL::millis();
 #if HAL_QUADPLANE_ENABLED
     if (quadplane.available()) {
@@ -361,19 +354,13 @@ void Plane::stabilize()
 #endif
 
     if (now - last_stabilize_ms > 2000) {
-        // if we haven't run the rate controllers for 2 seconds then
-        // reset the integrators
-        rollController.reset_I();
-        pitchController.reset_I();
-        yawController.reset_I();
-
-        // and reset steering controls
-        steer_state.locked_course = false;
-        steer_state.locked_course_err = 0;
+        // if we haven't run the rate controllers for 2 seconds then reset
+        control_mode->reset_controllers();
     }
     last_stabilize_ms = now;
 
-    if (control_mode == &mode_training) {
+    if (control_mode == &mode_training ||
+            control_mode == &mode_manual) {
         plane.control_mode->run();
 #if AP_SCRIPTING_ENABLED
     } else if (nav_scripting_active()) {
diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp
index bcb40c9d34..f8291b1696 100644
--- a/ArduPlane/mode.cpp
+++ b/ArduPlane/mode.cpp
@@ -212,3 +212,16 @@ void Mode::run()
     plane.stabilize_pitch();
     plane.stabilize_yaw();
 }
+
+// Reset rate and steering controllers
+void Mode::reset_controllers()
+{
+    // reset integrators
+    plane.rollController.reset_I();
+    plane.pitchController.reset_I();
+    plane.yawController.reset_I();
+
+    // reset steering controls
+    plane.steer_state.locked_course = false;
+    plane.steer_state.locked_course_err = 0;
+}
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 0ff3095917..a4cb940cd1 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -79,6 +79,9 @@ public:
     // returns true if the vehicle can be armed in this mode
     bool pre_arm_checks(size_t buflen, char *buffer) const;
 
+    // Reset rate and steering controllers
+    void reset_controllers();
+
     //
     // methods that sub classes should override to affect movement of the vehicle in this mode
     //
@@ -355,6 +358,8 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
+
+    void run() override;
 };
 
 
diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp
index 68535f9618..d04767f377 100644
--- a/ArduPlane/mode_manual.cpp
+++ b/ArduPlane/mode_manual.cpp
@@ -11,3 +11,7 @@ void ModeManual::update()
     plane.nav_pitch_cd = ahrs.pitch_sensor;
 }
 
+void ModeManual::run()
+{
+    reset_controllers();
+}