From e09ff842187553a4aaeed98da077b27eb0a2dc3b Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 24 Oct 2014 20:24:32 +1100 Subject: [PATCH] AP_NavEKF : Don't estimate focal length scale factor without reliable velocity --- 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 360a66eaf6..2dd88c7be0 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3689,8 +3689,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 GPS, or we are flying slowly + if (!useRngFinder() || _fusionModeGPS == 3 || gndSpdSq < 5.0f) { fScaleInhibit = true; } else { fScaleInhibit = false;