From ea4589ea94c278a67cf88bcccd1c0bc32009844b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Mon, 8 Jun 2020 08:43:42 +1000
Subject: [PATCH] AP_NavEKF2: Reset yaw estimator bias states at start of
 flight

Enables yaw bias to be learned when sitting stationary on ground.
---
 libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
index fa45ef1e55..27852859f1 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
@@ -406,6 +406,9 @@ void NavEKF2_core::detectFlight()
         EKFGSF_yaw_reset_request_ms = 0;
         EKFGSF_yaw_reset_count = 0;
         EKFGSF_run_filterbank = true;
+        Vector3f gyroBias;
+        getGyroBias(gyroBias);
+        yawEstimator->setGyroBias(gyroBias);
     }
 
     // store current on-ground  and in-air status for next time