From 7a71cbb9bfcb6221eef7fcec81a9e107036d3297 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 3 Aug 2020 18:07:47 +0200 Subject: [PATCH] FlightTask Auto: use yaw setpoint from Navigator if disable_weather_vane is true When disable_weather_vane is true, the vehicle is aligning for the transition waypoint (VTOL) and thus the yaw setpoint from Navigator should be used. Signed-off-by: Silvan Fuhrer --- src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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;