AP_NavEKF : Don't update focal length scale factor at low speeds

10 years ago · b1d3d5d9a3
1 changed files with 2 additions and 2 deletions
--- 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
+    // Don't update focal length offset state if there is no range finder or flying at low velocity
-    if (!useRngFinder()) {
+    if (!useRngFinder() || !highGndSpdStage2) {
        fScaleInhibit = true;
    } else {
        fScaleInhibit = false;