From a3605b4bc88778e4a1518857c7f946303216829d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 12 May 2022 21:49:44 -0400 Subject: [PATCH] navigator: mission_block relax yaw acceptance if heading not good for control --- src/modules/navigator/mission_block.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 03be49bcc5..f8ab99dc16 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -367,7 +367,8 @@ MissionBlock::is_mission_item_reached() /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < _navigator->get_yaw_threshold() - || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading)) { + || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading) + || !_navigator->get_local_position()->heading_good_for_control) { _waypoint_yaw_reached = true; }