diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 212dcaa8b1..776de4b08b 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -173,8 +173,7 @@ void Plane::update_loiter(uint16_t radius) } if (loiter.start_time_ms != 0 && - quadplane.available() && - quadplane.guided_mode != 0) { + quadplane.guided_mode_enabled()) { if (!auto_state.vtol_loiter) { auto_state.vtol_loiter = true; // reset loiter start time, so we don't consider the point @@ -202,8 +201,7 @@ void Plane::update_loiter(uint16_t radius) // starting a loiter in GUIDED means we just reached the target point gcs_send_mission_item_reached_message(0); } - if (quadplane.available() && - quadplane.guided_mode != 0) { + if (quadplane.guided_mode_enabled()) { quadplane.guided_start(); } } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 6cadfd8a95..c4938dfcfa 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2037,3 +2037,18 @@ void QuadPlane::afs_terminate(void) motors->output(); } } + +/* + return true if we should do guided mode loitering using VTOL motors + */ +bool QuadPlane::guided_mode_enabled(void) +{ + if (!available()) { + return false; + } + // only use quadplane guided when in AUTO or GUIDED mode + if (plane.control_mode != GUIDED && plane.control_mode != AUTO) { + return false; + } + return guided_mode != 0; +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index f1c29c7113..bedb0204ec 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -347,6 +347,7 @@ private: void tilt_compensate(float *thrust, uint8_t num_motors); void afs_terminate(void); + bool guided_mode_enabled(void); public: void motor_test_output();