Plane: clean up qacro

ArduPlane/mode_qacro.cpp

@@ -3,12 +3,24 @@
 
 bool ModeQAcro::_enter()
 {
-    //return false;
+    plane.throttle_allows_nudging = false;
-    return plane.mode_qstabilize._enter();
+    plane.auto_navigation_mode = false;
+    if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) {
+        plane.control_mode = plane.previous_mode;
+    } else {
+        plane.auto_throttle_mode = false;
+        plane.auto_state.vtol_mode = true;
+    }
+
+    return true;
 }
 
 void ModeQAcro::update()
 {
-    plane.mode_qstabilize.update();
+    // get nav_roll and nav_pitch from multicopter attitude controller
+    Vector3f att_target = plane.quadplane.attitude_control->get_att_target_euler_cd();
+    plane.nav_pitch_cd = att_target.y;
+    plane.nav_roll_cd = att_target.x;
+    return;
 }
 

ArduPlane/mode_qstabilize.cpp

@@ -17,13 +17,6 @@ bool ModeQStabilize::_enter()
 
 void ModeQStabilize::update()
 {
-    if (plane.quadplane.tailsitter_active() && (plane.control_mode == &plane.mode_qacro)) {
-        // get nav_roll and nav_pitch from multicopter attitude controller
-        Vector3f att_target = plane.quadplane.attitude_control->get_att_target_euler_cd();
-        plane.nav_pitch_cd = att_target.y;
-        plane.nav_roll_cd = att_target.x;
-        return;
-    }
     // set nav_roll and nav_pitch using sticks
     int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max);
     float pitch_input = plane.channel_pitch->norm_input();

ArduPlane/quadplane.cpp

@@ -1869,9 +1869,12 @@ void QuadPlane::control_run(void)
     switch (plane.control_mode->mode_number()) {
     case Mode::Number::QACRO:
         control_qacro();
-        // QACRO uses only the multicopter controller
+        if (!is_tailsitter()) {
-        // so skip the Plane attitude control calls below
+            // also stabilize using fixed wing surfaces
+            plane.stabilize_acro(plane.get_speed_scaler());
+        }
         return;
+        break;
     case Mode::Number::QSTABILIZE:
         control_stabilize();
         break;

ArduPlane/quadplane.h

@@ -35,6 +35,7 @@ public:
     friend class ModeQRTL;
     friend class ModeQStabilize;
     friend class ModeQAutotune;
+    friend class ModeQAcro;
     
     QuadPlane(AP_AHRS_NavEKF &_ahrs);