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) &&