diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 798ed97ff4..ba45e8735e 100644 --- a/ArduPlane/Attitude.cpp +++ b/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(); diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index d1c69de4e3..b74a434384 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/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 diff --git a/ArduPlane/mode_qautotune.cpp b/ArduPlane/mode_qautotune.cpp index d74cf792b5..28bf977128 100644 --- a/ArduPlane/mode_qautotune.cpp +++ b/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() diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index 13c1a4c4e6..1fe4848296 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/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 diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 26ef2bf214..e39ad63559 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/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 diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 75d82b4822..c11b1ba100 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/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(); } /* diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 2c8da2d968..39f168fd73 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/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