From baf2b4a3d802c339e22d02765b732bd5f6db0a64 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 14 Aug 2021 19:59:06 +0100
Subject: [PATCH] Plane: Qstabilize: mode functions to ModeQStabilize

---
 ArduPlane/mode_qstabilize.cpp | 20 ++++++++++++++++++++
 ArduPlane/quadplane.cpp       | 21 ---------------------
 2 files changed, 20 insertions(+), 21 deletions(-)

diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp
index 553e78c1b4..4cc3efe83b 100644
--- a/ArduPlane/mode_qstabilize.cpp
+++ b/ArduPlane/mode_qstabilize.cpp
@@ -10,6 +10,12 @@ bool ModeQStabilize::_enter()
     return true;
 }
 
+// init quadplane stabilize mode 
+void ModeQStabilize::init()
+{
+    quadplane.throttle_wait = false;
+}
+
 void ModeQStabilize::update()
 {
     // set nav_roll and nav_pitch using sticks
@@ -38,6 +44,20 @@ void ModeQStabilize::update()
     }
 }
 
+// quadplane stabilize mode
+void ModeQStabilize::run()
+{
+    // special check for ESC calibration in QSTABILIZE
+    if (quadplane.esc_calibration != 0) {
+        quadplane.run_esc_calibration();
+        return;
+    }
+
+    // normal QSTABILIZE mode
+    float pilot_throttle_scaled = quadplane.get_pilot_throttle();
+    quadplane.hold_stabilize(pilot_throttle_scaled);
+}
+
 // set the desired roll and pitch for a tailsitter
 void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const float pitch_input)
 {
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index b8392b7756..536cb7463e 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -845,13 +845,6 @@ void QuadPlane::run_esc_calibration(void)
     }
 }
 
-
-// init quadplane stabilize mode 
-void ModeQStabilize::init()
-{
-    quadplane.throttle_wait = false;
-}
-
 /*
   when doing a forward transition of a tilt-vectored quadplane we use
   euler angle control to maintain good yaw. This updates the yaw
@@ -997,20 +990,6 @@ void QuadPlane::hold_stabilize(float throttle_in)
     }
 }
 
-// quadplane stabilize mode
-void ModeQStabilize::run()
-{
-    // special check for ESC calibration in QSTABILIZE
-    if (quadplane.esc_calibration != 0) {
-        quadplane.run_esc_calibration();
-        return;
-    }
-
-    // normal QSTABILIZE mode
-    float pilot_throttle_scaled = quadplane.get_pilot_throttle();
-    quadplane.hold_stabilize(pilot_throttle_scaled);
-}
-
 // run the multicopter Z controller
 void QuadPlane::run_z_controller(void)
 {