From e293866245a4b7b9625ca713b7b61399f101c566 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 14 Aug 2021 19:52:04 +0100
Subject: [PATCH] plane: Qacro: move functions to ModeQAcro

---
 ArduPlane/mode_qacro.cpp | 51 ++++++++++++++++++++++++++++++++++++++++
 ArduPlane/quadplane.cpp  | 51 ----------------------------------------
 2 files changed, 51 insertions(+), 51 deletions(-)

diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp
index a9cdd96846..201ba71fa8 100644
--- a/ArduPlane/mode_qacro.cpp
+++ b/ArduPlane/mode_qacro.cpp
@@ -6,6 +6,16 @@ bool ModeQAcro::_enter()
     return plane.mode_qstabilize._enter();
 }
 
+/*
+  init QACRO mode
+ */
+void ModeQAcro::init()
+{
+    quadplane.throttle_wait = false;
+    quadplane.transition_state = QuadPlane::TRANSITION_DONE;
+    attitude_control->relax_attitude_controllers();
+}
+
 void ModeQAcro::update()
 {
     // get nav_roll and nav_pitch from multicopter attitude controller
@@ -15,3 +25,44 @@ void ModeQAcro::update()
     return;
 }
 
+
+/*
+  control QACRO mode
+ */
+void ModeQAcro::run()
+{
+    if (quadplane.throttle_wait) {
+        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
+        attitude_control->set_throttle_out(0, true, 0);
+        quadplane.relax_attitude_control();
+    } else {
+        quadplane.check_attitude_relax();
+
+        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+        // convert the input to the desired body frame rate
+        float target_roll = 0;
+        float target_pitch = plane.channel_pitch->norm_input() * quadplane.acro_pitch_rate * 100.0f;
+        float target_yaw = 0;
+        if (quadplane.tailsitter.enabled()) {
+            // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
+            target_roll =  plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0f;
+            target_yaw  = -plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
+        } else {
+            target_roll = plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
+            target_yaw  = plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0;
+        }
+
+        float throttle_out = quadplane.get_pilot_throttle();
+
+        // run attitude controller
+        if (plane.g.acro_locking) {
+            attitude_control->input_rate_bf_roll_pitch_yaw_3(target_roll, target_pitch, target_yaw);
+        } else {
+            attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw);
+        }
+
+        // output pilot's throttle without angle boost
+        attitude_control->set_throttle_out(throttle_out, false, 10.0f);
+    }
+}
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index f17bd861e4..2689b60647 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -1053,16 +1053,6 @@ void QuadPlane::check_attitude_relax(void)
     last_att_control_ms = now;
 }
 
-/*
-  init QACRO mode
- */
-void ModeQAcro::init()
-{
-    quadplane.throttle_wait = false;
-    quadplane.transition_state = QuadPlane::TRANSITION_DONE;
-    attitude_control->relax_attitude_controllers();
-}
-
 // init quadplane hover mode 
 void ModeQHover::init()
 {
@@ -1191,47 +1181,6 @@ float QuadPlane::get_pilot_land_throttle(void) const
     return constrain_float(throttle_in, 0, 1);
 }
 
-/*
-  control QACRO mode
- */
-void ModeQAcro::run()
-{
-    if (quadplane.throttle_wait) {
-        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
-        attitude_control->set_throttle_out(0, true, 0);
-        quadplane.relax_attitude_control();
-    } else {
-        quadplane.check_attitude_relax();
-
-        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-
-        // convert the input to the desired body frame rate
-        float target_roll = 0;
-        float target_pitch = plane.channel_pitch->norm_input() * quadplane.acro_pitch_rate * 100.0f;
-        float target_yaw = 0;
-        if (quadplane.tailsitter.enabled()) {
-            // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
-            target_roll =  plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0f;
-            target_yaw  = -plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
-        } else {
-            target_roll = plane.channel_roll->norm_input() * quadplane.acro_roll_rate * 100.0f;
-            target_yaw  = plane.channel_rudder->norm_input() * quadplane.acro_yaw_rate * 100.0;
-        }
-
-        float throttle_out = quadplane.get_pilot_throttle();
-
-        // run attitude controller
-        if (plane.g.acro_locking) {
-            attitude_control->input_rate_bf_roll_pitch_yaw_3(target_roll, target_pitch, target_yaw);
-        } else {
-            attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw);
-        }
-
-        // output pilot's throttle without angle boost
-        attitude_control->set_throttle_out(throttle_out, false, 10.0f);
-    }
-}
-
 /*
   control QHOVER mode
  */