AP_NavEKF2: accept extnav at up to 50hz

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;
     }
 

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;
     }
 

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