From 23ea84bf3287d8388ba89e87147ee66af9aaab8f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 15 Feb 2022 17:40:51 +0000 Subject: [PATCH] copter: fix condition yaw early completion --- ArduCopter/mode.h | 2 ++ ArduCopter/mode_auto.cpp | 8 +++----- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 994b522240..bae3ed679b 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -234,6 +234,8 @@ public: void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds); + bool fixed_yaw_slew_finished() { return is_zero(_fixed_yaw_offset_cd); } + private: float look_ahead_yaw(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 56954e18cf..6b74938358 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1871,13 +1871,11 @@ bool ModeAuto::verify_within_distance() // verify_yaw - return true if we have reached the desired heading bool ModeAuto::verify_yaw() { - // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) - if (auto_yaw.mode() != AUTO_YAW_FIXED) { - auto_yaw.set_mode(AUTO_YAW_FIXED); - } + // make sure still in fixed yaw mode, the waypoint controller often retakes control of yaw as it executes a new waypoint command + auto_yaw.set_mode(AUTO_YAW_FIXED); // check if we are within 2 degrees of the target heading - return (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200); + return auto_yaw.fixed_yaw_slew_finished() && (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200); } // verify_nav_wp - check if we have reached the next way point