From b1d3d5d9a37ddb56593fe3f1889f0a6dd4d1067c Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Thu, 16 Oct 2014 16:03:37 +1100
Subject: [PATCH] AP_NavEKF : Don't update focal length scale factor at low
 speeds

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 3804da3aad..de7cf6e3fc 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3674,8 +3674,8 @@ void NavEKF::SetFlightAndFusionModes()
     } else {
         inhibitGndState = false;
     }
-    // Don't update focal length offset state if there is no range finder
-    if (!useRngFinder()) {
+    // Don't update focal length offset state if there is no range finder or flying at low velocity
+    if (!useRngFinder() || !highGndSpdStage2) {
         fScaleInhibit = true;
     } else {
         fScaleInhibit = false;