AP_NavEKF2: Rework measurement buffer refactor

These changes were pair coded an tested by Siddharth Purohit and Paul Riseborough

Fix indexing errors
Move buffer code into a separate file
Split observer and IMU/output buffers and remove duplicate sample time
Optimise observation buffer search
Reduce maximum allowed fusion age to 100 msec

libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h

@@ -0,0 +1,190 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// EKF Buffer models
+
+// this buffer model is to be used for observation buffers,
+// the data is pushed into buffer like any standard ring buffer
+// return is based on the sample time provided
+template <typename element_type>
+class obs_ring_buffer_t
+{
+public:
+    struct element_t{
+        element_type element;
+    } *buffer;
+
+    // initialise buffer, returns false when allocation has failed
+    bool init(uint32_t size)
+    {
+        buffer = new element_t[size];
+        if(buffer == NULL)
+        {
+            return false;
+        }
+        memset(buffer,0,size*sizeof(element_t));
+        _size = size;
+        _head = 0;
+        _tail = 0;
+        _new_data = false;
+        return true;
+    }
+
+    /*
+     * Searches through a ring buffer and return the newest data that is older than the
+     * time specified by sample_time_ms
+     * Zeros old data so it cannot not be used again
+     * Returns false if no data can be found that is less than 100msec old
+    */
+
+    bool recall(element_type &element,uint32_t sample_time)
+    {
+        if(!_new_data) {
+            return false;
+        }
+        bool success = false;
+        uint8_t tail = _tail, bestIndex;
+
+        if(_head == tail) {
+            if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
+                // if head is equal to tail just check if the data is unused and within time horizon window
+                if (((sample_time - buffer[tail].element.time_ms) < 100)) {
+                    bestIndex = tail;
+                    success = true;
+                    _new_data = false;
+                }
+            }
+        } else {
+            while(_head != tail) {
+                // find a measurement older than the fusion time horizon that we haven't checked before
+                if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
+                    // Find the most recent non-stale measurement that meets the time horizon criteria
+                    if (((sample_time - buffer[tail].element.time_ms) < 100)) {
+                        bestIndex = tail;
+                        success = true;
+                    }
+                } else if(buffer[tail].element.time_ms > sample_time){
+                    break;
+                }
+                tail = (tail+1)%_size;
+            }
+        }
+
+        if (success) {
+            element = buffer[bestIndex].element;
+            _tail = (bestIndex+1)%_size;
+            //make time zero to stop using it again, 
+            //resolves corner case of reusing the element when head == tail
+            buffer[bestIndex].element.time_ms = 0;
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    /*
+     * Writes data and timestamp to a Ring buffer and advances indices that
+     * define the location of the newest and oldest data
+    */
+    inline void push(element_type element)
+    {
+        // Advance head to next available index
+        _head = (_head+1)%_size;
+        // New data is written at the head
+        buffer[_head].element = element;
+        _new_data = true;
+    }
+    // writes the same data to all elements in the ring buffer
+    inline void reset_history(element_type element, uint32_t sample_time) {
+        for (uint8_t index=0; index<_size; index++) {
+            buffer[index].element = element;
+        }
+    }
+
+    // zeroes all data in the ring buffer
+    inline void reset() {
+        _head = 0;
+        _tail = 0;
+        _new_data = false;
+        memset(buffer,0,_size*sizeof(element_t));
+    }
+
+private:
+    uint8_t _size,_head,_tail,_new_data;
+};
+
+
+// Folowing buffer model is for IMU data,
+// it achieves a distance of sample size
+// between youngest and oldest
+template <typename element_type>
+class imu_ring_buffer_t
+{
+public:
+    struct element_t{
+        element_type element;
+    } *buffer;
+
+    // initialise buffer, returns false when allocation has failed
+    bool init(uint32_t size)
+    {
+        buffer = new element_t[size];
+        if(buffer == NULL)
+        {
+            return false;
+        }
+        memset(buffer,0,size*sizeof(element_t));
+        _size = size;
+        _youngest = 0;
+        _oldest = 0;
+        return true;
+    }
+    /*
+     * Writes data to a Ring buffer and advances indices that
+     * define the location of the newest and oldest data
+    */
+    inline void push_youngest_element(element_type element)
+    {
+        // push youngest to the buffer
+        _youngest = (_youngest+1)%_size;
+        buffer[_youngest].element = element;
+        // set oldest data index
+        _oldest = (_youngest+1)%_size;
+    }
+
+    // retrieve the oldest data from the ring buffer tail
+    inline element_type pop_oldest_element() {
+        element_type ret = buffer[_oldest].element;
+        return ret;
+    }
+
+    // writes the same data to all elements in the ring buffer
+    inline void reset_history(element_type element) {
+        for (uint8_t index=0; index<_size; index++) {
+            buffer[index].element = element;
+        }
+    }
+
+    // zeroes all data in the ring buffer
+    inline void reset() {
+        _youngest = 0;
+        _oldest = 0;
+        memset(buffer,0,_size*sizeof(element_t));
+    }
+
+    // retrieves data from the ring buffer at a specified index
+    inline element_type& operator[](uint32_t index) {
+        return buffer[index].element;
+    }
+
+    // returns the index for the ring buffer oldest data
+    inline uint8_t get_oldest_index(){
+        return _oldest;
+    }
+
+    // returns the index for the ring buffer youngest data
+    inline uint8_t get_youngest_index(){
+        return _youngest;
+    }
+private:
+    uint8_t _size,_oldest,_youngest;
+};

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -71,14 +71,14 @@ void NavEKF2_core::readRangeFinder(void)
             rangeDataNew.rng = max(storedRngMeas[midIndex],rngOnGnd);
             rngValidMeaTime_ms = imuSampleTime_ms;
             // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
-            storedRange.push(rangeDataNew,imuSampleTime_ms);
+            storedRange.push(rangeDataNew);
         } else if (!takeOffDetected) {
             // before takeoff we assume on-ground range value if there is no data
             rangeDataNew.time_ms = imuSampleTime_ms;
             rangeDataNew.rng = rngOnGnd;
             rngValidMeaTime_ms = imuSampleTime_ms;
             // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
-            storedRange.push(rangeDataNew,imuSampleTime_ms);
+            storedRange.push(rangeDataNew);
         }
     }
 }
@@ -131,7 +131,7 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
         // Prevent time delay exceeding age of oldest IMU data in the buffer
         ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms);
         // Save data to buffer
-        storedOF.push(ofDataNew, ofDataNew.time_ms);
+        storedOF.push(ofDataNew);
         // Check for data at the fusion time horizon
         flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
     }
@@ -213,7 +213,7 @@ void NavEKF2_core::readMagData()
         consistentMagData = _ahrs->get_compass()->consistent();
 
         // save magnetometer measurement to buffer to be fused later
-        storedMag.push(magDataNew, magDataNew.time_ms);
+        storedMag.push(magDataNew);
     }
 }
 
@@ -296,7 +296,7 @@ void NavEKF2_core::readIMUData()
         // Time stamp the data
         imuDataDownSampledNew.time_ms = imuSampleTime_ms;
         // Write data to the FIFO IMU buffer
-        storedIMU.push(imuDataDownSampledNew, imuSampleTime_ms);
+        storedIMU.push_youngest_element(imuDataDownSampledNew);
         // zero the accumulated IMU data and quaternion
         imuDataDownSampledNew.delAng.zero();
         imuDataDownSampledNew.delVel.zero();
@@ -314,7 +314,7 @@ void NavEKF2_core::readIMUData()
     }
 
     // extract the oldest available data from the FIFO buffer
-    imuDataDelayed = storedIMU.pop();
+    imuDataDelayed = storedIMU.pop_oldest_element();
     float minDT = 0.1f*dtEkfAvg;
     imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT);
     imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT);
@@ -419,7 +419,7 @@ void NavEKF2_core::readGpsData()
             if (validOrigin) {
                 gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
                 gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
-                storedGPS.push(gpsDataNew, gpsDataNew.time_ms);
+                storedGPS.push(gpsDataNew);
                 // declare GPS available for use
                 gpsNotAvailable = false;
             }
@@ -532,7 +532,7 @@ void NavEKF2_core::readBaroData()
         baroDataNew.time_ms = max(baroDataNew.time_ms,imuDataDelayed.time_ms);
 
         // save baro measurement to buffer to be fused later
-        storedBaro.push(baroDataNew,baroDataNew.time_ms);
+        storedBaro.push(baroDataNew);
     }
 }
 
@@ -564,7 +564,7 @@ void NavEKF2_core::readAirSpdData()
         // Correct for the average intersampling delay due to the filter update rate
         tasDataNew.time_ms -= localFilterTimeStep_ms/2;
         newDataTas = true;
-        storedTAS.push(tasDataNew, tasDataNew.time_ms);
+        storedTAS.push(tasDataNew);
         storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
     } else {
         newDataTas = false;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -254,6 +254,15 @@ void NavEKF2_core::InitialiseVariables()
     imuDataDownSampledNew.delVelDT = 0.0f;
     runUpdates = false;
     framesSincePredict = 0;
+
+    // zero data buffers
+    storedIMU.reset();
+    storedGPS.reset();
+    storedMag.reset();
+    storedBaro.reset();
+    storedTAS.reset();
+    storedRange.reset();
+    storedOutput.reset();
 }
 
 // Initialise the states from accelerometer and magnetometer data (if present)
@@ -273,7 +282,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
     dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate();
     dtEkfAvg = min(0.01f,dtIMUavg);
     readIMUData();
-    storedIMU.reset_history(imuDataNew, imuSampleTime_ms);
+    storedIMU.reset_history(imuDataNew);
     imuDataDelayed = imuDataNew;
 
     // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
@@ -580,11 +589,11 @@ void  NavEKF2_core::calcOutputStatesFast() {
 
     // store the output in the FIFO buffer if this is a filter update step
     if (runUpdates) {
-        storedOutput[storedIMU.get_head()] = outputDataNew;
+        storedOutput[storedIMU.get_youngest_index()] = outputDataNew;
     }
 
     // extract data at the fusion time horizon from the FIFO buffer
-    outputDataDelayed = storedOutput[storedIMU.get_tail()];
+    outputDataDelayed = storedOutput[storedIMU.get_oldest_index()];
 
     // compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
 

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -31,6 +31,7 @@
 #include "AP_NavEKF2.h"
 #include <stdio.h>
 #include <AP_Math/vectorN.h>
+#include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
 
 // GPS pre-flight check bit locations
 #define MASK_GPS_NSATS      (1<<0)
@@ -42,119 +43,6 @@
 #define MASK_GPS_VERT_SPD   (1<<6)
 #define MASK_GPS_HORIZ_SPD  (1<<7)
 
-
-template <typename element_type>
-class timed_ring_buffer_t
-{
-public:
-    struct element_t{
-        element_type element;
-        uint32_t sample_time;
-    } *buffer;
-
-    bool init(uint32_t size)
-    {
-        buffer = new element_t[size];
-        memset(buffer,0,_size*sizeof(element_t));
-        if(buffer == NULL)
-        {
-            return false;
-        }
-        _size = size;
-        _head = 0;
-        _tail = 0;
-        return true;
-    }
-
-    void sorted_store(element_type element, uint32_t sample_time)
-    {
-        uint8_t head = _head;    
-        //will drop the element if older than tail i.e. recently fetched data
-        while(head != _tail) {
-            if(buffer[(head - 1)%_size].sample_time < sample_time) {
-                buffer[head].element = element;
-            } else {
-                buffer[head] = buffer[(head - 1)%_size];
-                head = (head-1)%_size;
-            }
-        }
-        _head = (_head+1)%_size;
-        return;
-    }
-
-    bool recall(element_type &element,uint32_t sample_time)
-    {
-        bool success = false;
-        uint8_t tail = _tail, bestIndex;
-        while (_head != tail) {
-            // find a measurement older than the fusion time horizon that we haven't checked before
-            if (buffer[tail].sample_time != 0 && buffer[tail].sample_time <= sample_time) {
-                // Find the most recent non-stale measurement that meets the time horizon criteria
-                if (((sample_time - buffer[tail].sample_time) < 500)) {
-                    bestIndex = tail;
-                    success = true;
-                }
-            } else if(buffer[tail].sample_time > sample_time){
-                break;
-            }
-            tail = (tail+1)%_size;
-        }
-        if (success) {
-            // zero the time stamp for that piece of data so we won't use it again
-            element = buffer[bestIndex].element;
-            element.time_ms = buffer[bestIndex].sample_time;
-            _tail=(bestIndex+1)%_size;
-            return true;
-        } else {
-            return false;
-        }
-    }
-
-    inline void push(element_type element, uint32_t sample_time)
-    {
-        buffer[_head].element = element;
-        buffer[_head].sample_time = sample_time;
-        _head = (_head+1)%_size;
-    }
-
-    inline void push(element_type element)
-    {
-        buffer[_head].element = element;
-        _head = (_head+1)%_size;
-    }
-
-    inline element_type pop() {
-        element_type ret = buffer[_tail].element;
-        if(_head != _tail) {
-            _tail = (_tail+1)%_size;
-        }
-        return ret;
-    }
-
-    inline void reset_history(element_type element, uint32_t sample_time) {
-        _head = (_tail+1)%_size;
-        buffer[_tail].sample_time = sample_time;
-        buffer[_tail].element = element;
-    }
-
-    inline void reset() {
-        _head=_tail=0;
-        memset(buffer,0,_size*sizeof(element_t));
-    }
-
-    inline element_type& operator[](uint32_t index) {
-        return buffer[index].element;
-    }
-    inline uint8_t get_tail(){
-        return _tail;
-    }
-    inline uint8_t get_head(){
-        return _head;
-    }
-private:
-    uint8_t _size,_head,_tail;
-};
-
 class AP_AHRS;
 
 class NavEKF2_core
@@ -750,13 +638,13 @@ private:
     Matrix24 KH;                    // intermediate result used for covariance updates
     Matrix24 KHP;                   // intermediate result used for covariance updates
     Matrix24 P;                     // covariance matrix
-    timed_ring_buffer_t<imu_elements> storedIMU;      // IMU data buffer
-    timed_ring_buffer_t<gps_elements> storedGPS;      // GPS data buffer
-    timed_ring_buffer_t<mag_elements> storedMag;      // Magnetometer data buffer
-    timed_ring_buffer_t<baro_elements> storedBaro;    // Baro data buffer
-    timed_ring_buffer_t<tas_elements> storedTAS;      // TAS data buffer
-    timed_ring_buffer_t<range_elements> storedRange;
-    timed_ring_buffer_t<output_elements> storedOutput;// output state buffer
+    imu_ring_buffer_t<imu_elements> storedIMU;      // IMU data buffer
+    obs_ring_buffer_t<gps_elements> storedGPS;      // GPS data buffer
+    obs_ring_buffer_t<mag_elements> storedMag;      // Magnetometer data buffer
+    obs_ring_buffer_t<baro_elements> storedBaro;    // Baro data buffer
+    obs_ring_buffer_t<tas_elements> storedTAS;      // TAS data buffer
+    obs_ring_buffer_t<range_elements> storedRange;
+    imu_ring_buffer_t<output_elements> storedOutput;// output state buffer
     Vector3f correctedDelAng;       // delta angles about the xyz body axes corrected for errors (rad)
     Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
     Vector3f correctedDelVel;       // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
@@ -918,7 +806,7 @@ private:
     float lastInnovation;
 
     // variables added for optical flow fusion
-    timed_ring_buffer_t<of_elements> storedOF;    // OF data buffer
+    obs_ring_buffer_t<of_elements> storedOF;    // OF data buffer
     of_elements ofDataNew;          // OF data at the current time horizon
     of_elements ofDataDelayed;      // OF data at the fusion time horizon
     uint8_t ofStoreIndex;           // OF data storage index