diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index adcbf35002..29a32d1d4e 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3259,6 +3259,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) position control. */ if (!in_vtol_mode() || + !transition->allow_weathervane() || !motors->armed() || (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) || plane.control_mode == &plane.mode_qstabilize || #if QAUTOTUNE_ENABLED diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 678bd29bae..7ee5b99920 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -960,4 +960,10 @@ MAV_VTOL_STATE Tailsitter_Transition::get_mav_vtol_state() const return MAV_VTOL_STATE_UNDEFINED; } +// only allow to weathervane once transition is complete and desired pitch has been reached +bool Tailsitter_Transition::allow_weathervane() +{ + return !tailsitter.in_vtol_transition() && (vtol_limit_start_ms == 0); +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index 1ab5b711aa..30dc780d96 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -163,6 +163,8 @@ public: bool set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) override; + bool allow_weathervane() override; + private: enum { diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index adc8106bf0..0016780077 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -52,6 +52,8 @@ public: virtual bool set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) { return false; } + virtual bool allow_weathervane() { return true; } + protected: // refences for convenience