From 6bb0096b9d9ab3066317ad3c52fffe6dd35003a1 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 12 Feb 2023 13:38:37 +0000
Subject: [PATCH] Plane: move training stabilize function to training mode

---
 ArduPlane/Attitude.cpp | 26 +++++++++++++-------------
 ArduPlane/Plane.h      |  1 -
 ArduPlane/mode.h       |  3 +++
 3 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 54580f8cab..bb2a262033 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -338,34 +338,34 @@ void Plane::stabilize_yaw()
 /*
   a special stabilization function for training mode
  */
-void Plane::stabilize_training()
+void ModeTraining::run()
 {
-    const float rexpo = roll_in_expo(false);
-    const float pexpo = pitch_in_expo(false);
-    if (training_manual_roll) {
+    const float rexpo = plane.roll_in_expo(false);
+    const float pexpo = plane.pitch_in_expo(false);
+    if (plane.training_manual_roll) {
         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
     } else {
         // calculate what is needed to hold
-        stabilize_roll();
-        if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
-            (nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
+        plane.stabilize_roll();
+        if ((plane.nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
+            (plane.nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
             // allow user to get out of the roll
             SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
         }
     }
 
-    if (training_manual_pitch) {
+    if (plane.training_manual_pitch) {
         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
     } else {
-        stabilize_pitch();
-        if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
-            (nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
+        plane.stabilize_pitch();
+        if ((plane.nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
+            (plane.nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
             // allow user to get back to level
             SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
         }
     }
 
-    stabilize_yaw();
+    plane.stabilize_yaw();
 }
 
 
@@ -594,7 +594,7 @@ void Plane::stabilize()
     last_stabilize_ms = now;
 
     if (control_mode == &mode_training) {
-        stabilize_training();
+        plane.control_mode->run();
 #if AP_SCRIPTING_ENABLED
     } else if (nav_scripting_active()) {
         // scripting is in control of roll and pitch rates and throttle
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index e1c0c1cf5c..090d8f9a10 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -868,7 +868,6 @@ private:
     void stabilize_stick_mixing_direct();
     void stabilize_stick_mixing_fbw();
     void stabilize_yaw();
-    void stabilize_training();
     void stabilize_acro();
     void stabilize_acro_quaternion();
     void calc_nav_yaw_coordinated();
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index c6a0681ec0..e94d93dd70 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -391,6 +391,9 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
+
+    void run() override;
+
 };
 
 class ModeInitializing : public Mode