From 6111e9d9dbc7781c9ebee88d46ed1a9c0b991f68 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Tue, 25 Apr 2023 01:31:04 +0100
Subject: [PATCH] Plane: move Stabilize functions into mode

---
 ArduPlane/Attitude.cpp       | 19 ++++++++-----------
 ArduPlane/Plane.h            |  1 -
 ArduPlane/mode.h             |  6 ++++++
 ArduPlane/mode_stabilize.cpp |  7 +++++++
 4 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 0134a187bf..87effd7283 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -213,23 +213,24 @@ float Plane::stabilize_pitch_get_pitch_out()
 
 /*
   this gives the user control of the aircraft in stabilization modes, only used in Stabilize Mode
+  to be moved to mode_stabilize.cpp in future
  */
-void Plane::stabilize_stick_mixing_direct()
+void ModeStabilize::stabilize_stick_mixing_direct()
 {
-    if (!stick_mixing_enabled()) {
+    if (!plane.stick_mixing_enabled()) {
         return;
     }
 #if HAL_QUADPLANE_ENABLED
-    if (!quadplane.allow_stick_mixing()) {
+    if (!plane.quadplane.allow_stick_mixing()) {
         return;
     }
 #endif
     float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
-    aileron = channel_roll->stick_mixing(aileron);
+    aileron = plane.channel_roll->stick_mixing(aileron);
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
 
     float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
-    elevator = channel_pitch->stick_mixing(elevator);
+    elevator = plane.channel_pitch->stick_mixing(elevator);
     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
 }
 
@@ -392,13 +393,9 @@ void Plane::stabilize()
             steering_control.rudder = rudder;
         }
 #endif
-    } else if (control_mode == &mode_acro) {
+    } else if (control_mode == &mode_acro ||
+                control_mode == &mode_stabilize) {
         plane.control_mode->run();
-    } else if (control_mode == &mode_stabilize) {
-        stabilize_roll();
-        stabilize_pitch();
-        stabilize_stick_mixing_direct();
-        stabilize_yaw();
 #if HAL_QUADPLANE_ENABLED
     } else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) {
         // run controlers specific to this mode
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 54e2c05072..39bae83e55 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -862,7 +862,6 @@ private:
     float stabilize_roll_get_roll_out();
     void stabilize_pitch();
     float stabilize_pitch_get_pitch_out();
-    void stabilize_stick_mixing_direct();
     void stabilize_stick_mixing_fbw();
     void stabilize_yaw();
     void calc_nav_yaw_coordinated();
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 46e56c284c..6cd7cb8e38 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -398,6 +398,12 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
+
+    void run() override;
+
+private:
+    void stabilize_stick_mixing_direct();
+
 };
 
 class ModeTraining : public Mode
diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp
index 7414d57506..b73dfb6fc2 100644
--- a/ArduPlane/mode_stabilize.cpp
+++ b/ArduPlane/mode_stabilize.cpp
@@ -7,3 +7,10 @@ void ModeStabilize::update()
     plane.nav_pitch_cd = 0;
 }
 
+void ModeStabilize::run()
+{
+    plane.stabilize_roll();
+    plane.stabilize_pitch();
+    stabilize_stick_mixing_direct();
+    plane.stabilize_yaw();
+}