diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index ce5261fa5b..2c5855533b 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -277,10 +277,12 @@ bool FlightTaskAuto::_evaluateTriplets() _yaw_setpoint = NAN; } else { - if (_type != WaypointType::takeoff + if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane) && _sub_triplet_setpoint.get().current.yaw_valid) { // Use the yaw computed in Navigator except during takeoff because - // Navigator is not handling the yaw reset properly + // Navigator is not handling the yaw reset properly. + // But: use if from Navigator during takeoff if disable_weather_vane is true, + // because we're then aligning to the transition waypoint. // TODO: fix in navigator _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;