From 6c106550595b23ea8dc4c0482519ae28e0a260e9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 8 Jun 2020 12:13:13 +0900
Subject: [PATCH] AP_NavEKF2: accept extnav at up to 50hz

---
 libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 4 ++--
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp         | 4 ++--
 libraries/AP_NavEKF2/AP_NavEKF2_core.h           | 1 +
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index d59a6b7bd2..c2037f16f8 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -894,7 +894,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
 {
     // limit update rate to maximum allowed by sensor buffers and fusion process
     // don't try to write to buffer until the filter has been initialised
-    if ((timeStamp_ms - extNavMeasTime_ms) < 70) {
+    if ((timeStamp_ms - extNavMeasTime_ms) < 20) {
         return;
     } else {
         extNavMeasTime_ms = timeStamp_ms;
@@ -1018,7 +1018,7 @@ void NavEKF2_core::writeDefaultAirSpeed(float airspeed)
 
 void NavEKF2_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
 {
-    if ((timeStamp_ms - extNavVelMeasTime_ms) < 70) {
+    if ((timeStamp_ms - extNavVelMeasTime_ms) < 20) {
         return;
     }
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 601126b2e4..ebf1a09b87 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -101,7 +101,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     if(!storedRangeBeacon.init(imu_buffer_length)) {
         return false;
     }
-    if(!storedExtNav.init(OBS_BUFFER_LENGTH)) {
+    if(!storedExtNav.init(EXTNAV_BUFFER_LENGTH)) {
         return false;
     }
     if(!storedIMU.init(imu_buffer_length)) {
@@ -110,7 +110,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     if(!storedOutput.init(imu_buffer_length)) {
         return false;
     }
-    if(!storedExtNavVel.init(OBS_BUFFER_LENGTH)) {
+    if(!storedExtNavVel.init(EXTNAV_BUFFER_LENGTH)) {
        return false;
     }
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index ffec68cce6..79bd26f236 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -848,6 +848,7 @@ private:
     // Must be larger than the time period defined by IMU_BUFFER_LENGTH
     static const uint32_t OBS_BUFFER_LENGTH = 5;
     static const uint32_t FLOW_BUFFER_LENGTH = 15;
+    static const uint32_t EXTNAV_BUFFER_LENGTH = 15;
 
     // Variables
     bool statesInitialised;         // boolean true when filter states have been initialised