From 7090e634cc18ebceaab01cc33a8d2abf3b0c0dfc Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Tue, 16 Nov 2021 08:04:35 +0100 Subject: [PATCH] mission_block: always accept yaw in MC takeoff --- src/modules/navigator/mission_block.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 6d6ba8da38..eaaec07da1 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -343,6 +343,14 @@ MissionBlock::is_mission_item_reached() _waypoint_yaw_reached = true; } + // Always accept yaw during takeoff + // TODO: Ideally Navigator would handle a yaw reset and adjust its yaw setpoint, making the + // following no longer necessary. + // FlightTaskAuto is currently also ignoring the yaw setpoint during takeoff and thus "handling" it. + if (_mission_item.nav_cmd == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { + _waypoint_yaw_reached = true; + } + /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && (_navigator->get_yaw_timeout() >= FLT_EPSILON) &&