From deabe9a38da47a730f6be0930e4cbba7b29713f6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 17 Feb 2023 10:07:58 +0100 Subject: [PATCH] Navigator: accept yaw immedietaly if the flag heading_good_for_control is not set Signed-off-by: Silvan Fuhrer --- 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 84edcc0e56..e6ff8107f6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -414,7 +414,8 @@ MissionBlock::is_mission_item_reached_or_completed() if (_waypoint_position_reached && !_waypoint_yaw_reached) { if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) { + && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw)) + && _navigator->get_local_position()->heading_good_for_control) { const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading);