From 67ae40e92273286d41088968afe4d83209021bdf Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 16 Jul 2020 16:10:22 +0200 Subject: [PATCH] FlightTaskAuto: fix yaw reset issue during takeoff During takeoff, Navigator is sending a constant yaw value. However, there is always a reset at 1.5m of the yaw estimate in EKF2 that is not handled by Navigator that produces a glitch in the rate controller. Given that in FlightTask, the yaw is already computed and properly corrected in case of an estimator reset event, we just ignore the yaw value sent by navigator during takeoff. --- src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index dbcda481bf..ce5261fa5b 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -277,7 +277,11 @@ bool FlightTaskAuto::_evaluateTriplets() _yaw_setpoint = NAN; } else { - if (_sub_triplet_setpoint.get().current.yaw_valid) { + if (_type != WaypointType::takeoff + && _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 + // TODO: fix in navigator _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw; } else {