AP_NavEKF : Fix bug affecting sonar fusion

10 years ago · 1222559da0
1 changed files with 1 additions and 0 deletions
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@ -3996,6 +3996,7 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
				@@ -3996,6 +3996,7 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
    }
    // Use range finder if 3 or more consecutive good samples. This reduces likelihood of using bad data.
    if (rangeHealth >= 3) {
+        statesAtRngTime = statesAtFlowTime;
        rngMea = rawSonarRange;
        newDataRng = true;
    } else {