diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index 9962462ea8..370d458a68 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -3,12 +3,24 @@ bool ModeQAcro::_enter() { - //return false; - return plane.mode_qstabilize._enter(); + plane.throttle_allows_nudging = false; + 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; } diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 65387805e4..089769974a 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/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(); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 23f80705b6..3c51606d52 100644 --- a/ArduPlane/quadplane.cpp +++ b/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 - // so skip the Plane attitude control calls below + if (!is_tailsitter()) { + // also stabilize using fixed wing surfaces + plane.stabilize_acro(plane.get_speed_scaler()); + } return; + break; case Mode::Number::QSTABILIZE: control_stabilize(); break; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index a32a65c77b..c727d1a9ca 100644 --- a/ArduPlane/quadplane.h +++ b/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);