From 63663303de96b54ce9852dd58b39818f886949ac Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Thu, 26 Sep 2024 14:24:53 +0900 Subject: [PATCH] Copter: Keep FIXED mode when WP_YAW_BEHAVIOR is NONE --- ArduCopter/mode_auto.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 821cf9a5a3..f897b660c2 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -449,7 +449,7 @@ bool ModeAuto::wp_start(const Location& dest_loc) // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw.mode() != AutoYaw::Mode::ROI) { + if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == WP_YAW_BEHAVIOR_NONE)) { auto_yaw.set_mode_to_default(false); } @@ -1804,7 +1804,7 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw.mode() != AutoYaw::Mode::ROI) { + if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == WP_YAW_BEHAVIOR_NONE)) { auto_yaw.set_mode_to_default(false); }