AP_NavEKF2: Add data buffers for range finder data

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -752,7 +752,45 @@ bool NavEKF2_core::RecallBaro()
     }
 }
 
+// store baro in a history array
+void NavEKF2_core::StoreRange()
+{
+    if (rangeStoreIndex >= OBS_BUFFER_LENGTH) {
+        rangeStoreIndex = 0;
+    }
+    storedRange[rangeStoreIndex] = rangeDataNew;
+    rangeStoreIndex += 1;
+}
 
+// return newest un-used range finder data that has fallen behind the fusion time horizon
+// if no un-used data is available behind the fusion horizon, return false
+bool NavEKF2_core::RecallRange()
+{
+    range_elements dataTemp;
+    range_elements dataTempZero;
+    dataTempZero.time_ms = 0;
+    uint32_t temp_ms = 0;
+    uint8_t bestIndex = 0;
+    for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
+        dataTemp = storedRange[i];
+        // find a measurement older than the fusion time horizon that we haven't checked before
+        if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
+            // Find the most recent non-stale measurement that meets the time horizon criteria
+            if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
+                rangeDataDelayed = dataTemp;
+                temp_ms = dataTemp.time_ms;
+                bestIndex = i;
+            }
+        }
+    }
+    if (temp_ms != 0) {
+        // zero the time stamp for that piece of data so we won't use it again
+        storedRange[bestIndex]=dataTempZero;
+        return true;
+    } else {
+        return false;
+    }
+}
 
 /********************************************************
 *                Air Speed Measurements                 *

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -367,6 +367,11 @@ private:
         uint32_t    time_ms;     // 1
     };
 
+    struct range_elements {
+        float       rng;         // 0
+        uint32_t    time_ms;     // 1
+    };
+
     struct tas_elements {
         float       tas;         // 0
         uint32_t    time_ms;     // 1
@@ -445,6 +450,13 @@ private:
     // return true if data found
     bool RecallBaro();
 
+    // store range finder data
+    void StoreRange();
+
+    // recall range finder data at the fusion time horizon
+    // return true if data found
+    bool RecallRange();
+
     // store magnetometer data
     void StoreMag();
 
@@ -645,6 +657,7 @@ private:
     gps_elements storedGPS[OBS_BUFFER_LENGTH];      // GPS data buffer
     mag_elements storedMag[OBS_BUFFER_LENGTH];      // Magnetometer data buffer
     baro_elements storedBaro[OBS_BUFFER_LENGTH];    // Baro data buffer
+    range_elements storedRange[OBS_BUFFER_LENGTH];  // Rang finder data buffer
     tas_elements storedTAS[OBS_BUFFER_LENGTH];      // TAS data buffer
     output_elements *storedOutput;  // output state buffer [imu_buffer_length]
     Vector3f correctedDelAng;       // delta angles about the xyz body axes corrected for errors (rad)
@@ -737,6 +750,9 @@ private:
     baro_elements baroDataNew;      // Baro data at the current time horizon
     baro_elements baroDataDelayed;  // Baro data at the fusion time horizon
     uint8_t baroStoreIndex;         // Baro data storage index
+    range_elements rangeDataNew;    // Range finder data at the current time horizon
+    range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
+    uint8_t rangeStoreIndex;        // Range finder data storage index
     tas_elements tasDataNew;        // TAS data at the current time horizon
     tas_elements tasDataDelayed;    // TAS data at the fusion time horizon
     uint8_t tasStoreIndex;          // TAS data storage index