Plane: Quadplane: move modes fully to run function

ArduPlane/Attitude.cpp

@@ -392,22 +392,6 @@ void Plane::stabilize()
             }
             steering_control.rudder = rudder;
         }
-#endif
-    } else if (control_mode == &mode_acro ||
-                control_mode == &mode_stabilize) {
-        plane.control_mode->run();
-#if HAL_QUADPLANE_ENABLED
-    } else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) {
-        // run controlers specific to this mode
-        plane.control_mode->run();
-
-        // we also stabilize using fixed wing surfaces
-        if (plane.control_mode->mode_number() == Mode::Number::QACRO) {
-            plane.mode_acro.run();
-        } else {
-            stabilize_roll();
-            stabilize_pitch();
-        }
 #endif
     } else {
         plane.control_mode->run();

ArduPlane/mode_qacro.cpp

@@ -32,6 +32,13 @@ void ModeQAcro::update()
  */
 void ModeQAcro::run()
 {
+    const uint32_t now = AP_HAL::millis();
+    if (quadplane.tailsitter.in_vtol_transition(now)) {
+        // Tailsitters in FW pull up phase of VTOL transition run FW controllers
+        Mode::run();
+        return;
+    }
+
     if (quadplane.throttle_wait) {
         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control->set_throttle_out(0, true, 0);
@@ -64,6 +71,9 @@ void ModeQAcro::run()
         // output pilot's throttle without angle boost
         attitude_control->set_throttle_out(throttle_out, false, 10.0f);
     }
+
+    // Stabilize with fixed wing surfaces
+    plane.mode_acro.run();
 }
 
 #endif

ArduPlane/mode_qautotune.cpp

@@ -21,9 +21,20 @@ void ModeQAutotune::update()
 
 void ModeQAutotune::run()
 {
+    const uint32_t now = AP_HAL::millis();
+    if (quadplane.tailsitter.in_vtol_transition(now)) {
+        // Tailsitters in FW pull up phase of VTOL transition run FW controllers
+        Mode::run();
+        return;
+    }
+
 #if QAUTOTUNE_ENABLED
     quadplane.qautotune.run();
 #endif
+
+    // Stabilize with fixed wing surfaces
+    plane.stabilize_roll();
+    plane.stabilize_pitch();
 }
 
 void ModeQAutotune::_exit()

ArduPlane/mode_qhover.cpp

@@ -24,6 +24,13 @@ void ModeQHover::update()
  */
 void ModeQHover::run()
 {
+    const uint32_t now = AP_HAL::millis();
+    if (quadplane.tailsitter.in_vtol_transition(now)) {
+        // Tailsitters in FW pull up phase of VTOL transition run FW controllers
+        Mode::run();
+        return;
+    }
+
     if (quadplane.throttle_wait) {
         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control->set_throttle_out(0, true, 0);
@@ -32,6 +39,10 @@ void ModeQHover::run()
     } else {
         quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
     }
+
+    // Stabilize with fixed wing surfaces
+    plane.stabilize_roll();
+    plane.stabilize_pitch();
 }
 
 #endif

ArduPlane/mode_qloiter.cpp

@@ -28,6 +28,13 @@ void ModeQLoiter::update()
 // run quadplane loiter controller
 void ModeQLoiter::run()
 {
+    const uint32_t now = AP_HAL::millis();
+    if (quadplane.tailsitter.in_vtol_transition(now)) {
+        // Tailsitters in FW pull up phase of VTOL transition run FW controllers
+        Mode::run();
+        return;
+    }
+
     if (quadplane.throttle_wait) {
         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control->set_throttle_out(0, true, 0);
@@ -35,6 +42,10 @@ void ModeQLoiter::run()
         pos_control->relax_z_controller(0);
         loiter_nav->clear_pilot_desired_acceleration();
         loiter_nav->init_target();
+
+        // Stabilize with fixed wing surfaces
+        plane.stabilize_roll();
+        plane.stabilize_pitch();
         return;
     }
     if (!quadplane.motors->armed()) {
@@ -45,7 +56,6 @@ void ModeQLoiter::run()
         loiter_nav->soften_for_landing();
     }
 
-    const uint32_t now = AP_HAL::millis();
     if (now - quadplane.last_loiter_ms > 500) {
         loiter_nav->clear_pilot_desired_acceleration();
         loiter_nav->init_target();
@@ -112,6 +122,10 @@ void ModeQLoiter::run()
         quadplane.set_climb_rate_cms(quadplane.get_pilot_desired_climb_rate_cms());
     }
     quadplane.run_z_controller();
+
+    // Stabilize with fixed wing surfaces
+    plane.stabilize_roll();
+    plane.stabilize_pitch();
 }
 
 #endif

ArduPlane/mode_qrtl.cpp

@@ -86,6 +86,13 @@ void ModeQRTL::update()
  */
 void ModeQRTL::run()
 {
+    const uint32_t now = AP_HAL::millis();
+    if (quadplane.tailsitter.in_vtol_transition(now)) {
+        // Tailsitters in FW pull up phase of VTOL transition run FW controllers
+        Mode::run();
+        return;
+    }
+
     switch (submode) {
         case SubMode::climb: {
             // request zero velocity
@@ -165,6 +172,10 @@ void ModeQRTL::run()
             break;
         }
     }
+
+    // Stabilize with fixed wing surfaces
+    plane.stabilize_roll();
+    plane.stabilize_pitch();
 }
 
 /*

ArduPlane/mode_qstabilize.cpp

@@ -40,15 +40,28 @@ void ModeQStabilize::update()
 // quadplane stabilize mode
 void ModeQStabilize::run()
 {
+    const uint32_t now = AP_HAL::millis();
+    if (quadplane.tailsitter.in_vtol_transition(now)) {
+        // Tailsitters in FW pull up phase of VTOL transition run FW controllers
+        Mode::run();
+        return;
+    }
+
     // special check for ESC calibration in QSTABILIZE
     if (quadplane.esc_calibration != 0) {
         quadplane.run_esc_calibration();
+        plane.stabilize_roll();
+        plane.stabilize_pitch();
         return;
     }
 
     // normal QSTABILIZE mode
     float pilot_throttle_scaled = quadplane.get_pilot_throttle();
     quadplane.hold_stabilize(pilot_throttle_scaled);
+
+    // Stabilize with fixed wing surfaces
+    plane.stabilize_roll();
+    plane.stabilize_pitch();
 }
 
 // set the desired roll and pitch for a tailsitter