AP_NavEKF: added common EKF buffer classes

libraries/AP_NavEKF/EKF_Buffer.cpp

@@ -0,0 +1,199 @@
+/*
+  common EKF Buffer class. This handles the storage buffers for EKF data to
+  bring it onto the fusion time horizon
+*/
+
+#include "EKF_Buffer.h"
+#include <stdlib.h>
+#include <string.h>
+
+// constructor
+ekf_ring_buffer::ekf_ring_buffer(uint8_t _elsize) :
+    elsize(_elsize)
+{}
+
+bool ekf_ring_buffer::init(uint8_t size)
+{
+    buffer = calloc(size, elsize);
+    if (buffer == nullptr) {
+        return false;
+    }
+    _size = size;
+    _head = 0;
+    _tail = 0;
+    _new_data = false;
+    return true;
+}
+
+/*
+  get buffer offset for an index
+ */
+void *ekf_ring_buffer::get_offset(uint8_t idx) const
+{
+    return (void*)(((uint8_t*)buffer)+idx*uint32_t(elsize));
+}
+
+/*
+  get a reference to the timestamp for an index
+ */
+uint32_t &ekf_ring_buffer::time_ms(uint8_t idx)
+{
+    EKF_obs_element_t *el = (EKF_obs_element_t *)get_offset(idx);
+    return el->time_ms;
+}
+
+/*
+  Search 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 ekf_ring_buffer::recall(void *element,uint32_t sample_time)
+{
+    if (!_new_data) {
+        return false;
+    }
+    bool success = false;
+    uint8_t tail = _tail, bestIndex;
+
+    if (_head == tail) {
+        if (time_ms(tail) != 0 && time_ms(tail) <= sample_time) {
+            // if head is equal to tail just check if the data is unused and within time horizon window
+            if (((sample_time - time_ms(tail)) < 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 (time_ms(tail) != 0 && time_ms(tail) <= sample_time) {
+                // Find the most recent non-stale measurement that meets the time horizon criteria
+                if (((sample_time - time_ms(tail)) < 100)) {
+                    bestIndex = tail;
+                    success = true;
+                }
+            } else if (time_ms(tail) > sample_time){
+                break;
+            }
+            tail = (tail+1) % _size;
+        }
+    }
+
+    if (!success) {
+        return false;
+    }
+
+    memcpy(element, get_offset(bestIndex), elsize);
+    _tail = (bestIndex+1) % _size;
+    // make time zero to stop using it again,
+    // resolves corner case of reusing the element when head == tail
+    time_ms(bestIndex) = 0;
+    return true;
+}
+
+/*
+ * Writes data and timestamp to a Ring buffer and advances indices that
+ * define the location of the newest and oldest data
+ */
+void ekf_ring_buffer::push(const void *element)
+{
+    if (buffer == nullptr) {
+        return;
+    }
+    // Advance head to next available index
+    _head = (_head+1) % _size;
+    // New data is written at the head
+    memcpy(get_offset(_head), element, elsize);
+    _new_data = true;
+}
+
+
+// zeroes all data in the ring buffer
+void ekf_ring_buffer::reset()
+{
+    _head = 0;
+    _tail = 0;
+    _new_data = false;
+    memset((void *)buffer,0,_size*uint32_t(elsize));
+}
+
+////////////////////////////////////////////////////
+/*
+  IMU buffer operations implemented separately due to different
+  semantics
+*/
+
+// constructor
+ekf_imu_buffer::ekf_imu_buffer(uint8_t _elsize) :
+    elsize(_elsize)
+{}
+
+/*
+  get buffer offset for an index
+ */
+void *ekf_imu_buffer::get_offset(uint8_t idx) const
+{
+    return (void*)(((uint8_t*)buffer)+idx*uint32_t(elsize));
+}
+
+// initialise buffer, returns false when allocation has failed
+bool ekf_imu_buffer::init(uint32_t size)
+{
+    buffer = calloc(size, elsize);
+    if (buffer == nullptr) {
+        return false;
+    }
+    _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
+*/
+void ekf_imu_buffer::push_youngest_element(const void *element)
+{
+    if (!buffer) {
+        return;
+    }
+    // push youngest to the buffer
+    _youngest = (_youngest+1) % _size;
+    memcpy(get_offset(_youngest), element, elsize);
+    // set oldest data index
+    _oldest = (_youngest+1) % _size;
+    if (_oldest == 0) {
+        _filled = true;
+    }
+}
+
+// retrieve the oldest data from the ring buffer tail
+void ekf_imu_buffer::get_oldest_element(void *element)
+{
+    memcpy(element, get_offset(_oldest), elsize);
+}
+
+// writes the same data to all elements in the ring buffer
+void ekf_imu_buffer::reset_history(const void *element)
+{
+    for (uint8_t index=0; index<_size; index++) {
+        memcpy(get_offset(index), element, elsize);
+    }
+}
+
+// zeroes all data in the ring buffer
+void ekf_imu_buffer::reset()
+{
+    _youngest = 0;
+    _oldest = 0;
+    memset(buffer, 0, _size*uint32_t(elsize));
+}
+
+// retrieves data from the ring buffer at a specified index
+void *ekf_imu_buffer::get(uint8_t index) const
+{
+    return get_offset(index);
+}

libraries/AP_NavEKF/EKF_Buffer.h

@@ -0,0 +1,197 @@
+/*
+  common EKF buffer classes. These handles the storage buffers for EKF
+  data to bring it onto the fusion time horizon
+*/
+
+#include <stdint.h>
+#include <type_traits>
+
+typedef struct {
+    // measurement timestamp (msec)
+    uint32_t    time_ms;
+} EKF_obs_element_t;
+
+// this class 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
+class ekf_ring_buffer
+{
+public:
+    ekf_ring_buffer(uint8_t elsize);
+
+    // initialise buffer, returns false when allocation has failed
+    bool init(uint8_t size);
+
+    /*
+     * 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(void *element, uint32_t sample_time);
+
+    /*
+     * Writes data and timestamp to a Ring buffer and advances indices that
+     * define the location of the newest and oldest data
+    */
+    void push(const void *element);
+
+    // zeroes all data in the ring buffer
+    void reset();
+
+private:
+    const uint8_t elsize;
+    void *buffer;
+    uint8_t _size, _head, _tail, _new_data;
+
+    uint32_t &time_ms(uint8_t idx);
+    void *get_offset(uint8_t idx) const;
+};
+
+/*
+  template class for more convenient type handling
+ */
+template <typename element_type>
+class EKF_obs_buffer_t : ekf_ring_buffer
+{
+    static_assert(
+        std::is_base_of<EKF_obs_element_t, element_type>::value,
+        "must be a descendant of EKF_obs_element_t"
+    );
+public:
+    EKF_obs_buffer_t() :
+        ekf_ring_buffer(sizeof(element_type))
+        {}
+
+    bool init(uint8_t size) {
+        return ekf_ring_buffer::init(size);
+    }
+
+    bool recall(element_type &element,uint32_t sample_time) {
+        return ekf_ring_buffer::recall(&element, sample_time);
+    }
+
+    void push(element_type element) {
+        return ekf_ring_buffer::push(&element);
+    }
+
+    void reset() {
+        return ekf_ring_buffer::reset();
+    }
+};
+
+
+/*
+  ring buffer for IMU data,
+*/
+class ekf_imu_buffer
+{
+public:
+    ekf_imu_buffer(uint8_t elsize);
+    
+    // initialise buffer, returns false when allocation has failed
+    bool init(uint32_t size);
+
+    /*
+      Writes data to a Ring buffer and advances indices that
+      define the location of the newest and oldest data
+    */
+    void push_youngest_element(const void *element);
+
+    // return true if the buffer has been filled at least once
+    bool is_filled(void) const {
+        return _filled;
+    }
+    
+    // retrieve the oldest data from the ring buffer tail
+    void get_oldest_element(void *element);
+
+    // writes the same data to all elements in the ring buffer
+    void reset_history(const void *element);
+
+    // zeroes all data in the ring buffer
+    void reset();
+
+    // retrieves data from the ring buffer at a specified index
+    void *get(uint8_t index) const;
+
+    // returns the index for the ring buffer oldest data
+    uint8_t get_oldest_index() const {
+        return _oldest;
+    }
+
+    // returns the index for the ring buffer youngest data
+    uint8_t get_youngest_index() const {
+        return _youngest;
+    }
+
+protected:
+    const uint8_t elsize;
+    void *buffer;
+    uint8_t _size,_oldest,_youngest;
+    bool _filled;
+
+    void *get_offset(uint8_t idx) const;
+};
+
+/*
+  template class for more convenient type handling
+ */
+template <typename element_type>
+class EKF_IMU_buffer_t : ekf_imu_buffer
+{
+public:
+    EKF_IMU_buffer_t() :
+        ekf_imu_buffer(sizeof(element_type))
+        {}
+
+    bool init(uint8_t size) {
+        return ekf_imu_buffer::init(size);
+    }
+
+    /*
+      Writes data to a Ring buffer and advances indices that
+     define the location of the newest and oldest data
+    */
+    void push_youngest_element(element_type element) {
+        return ekf_imu_buffer::push_youngest_element(&element);
+    }
+
+    // return true if the buffer has been filled at least once
+    bool is_filled(void) const {
+        return ekf_imu_buffer::is_filled();
+    }
+    
+    // retrieve the oldest data from the ring buffer tail
+    element_type get_oldest_element() {
+        element_type ret;
+        ekf_imu_buffer::get_oldest_element(&ret);
+        return ret;
+    }
+
+    // writes the same data to all elements in the ring buffer
+    void reset_history(element_type element) {
+        ekf_imu_buffer::reset_history(&element);
+    }
+
+    // zeroes all data in the ring buffer
+    void reset() {
+        ekf_imu_buffer::reset();
+    }
+
+    // retrieves data from the ring buffer at a specified index
+    element_type& operator[](uint32_t index) {
+        element_type *ret = (element_type *)ekf_imu_buffer::get(index);
+        return *ret;
+    }
+
+    // returns the index for the ring buffer oldest data
+    uint8_t get_oldest_index() {
+        return ekf_imu_buffer::get_oldest_index();
+    }
+
+    // returns the index for the ring buffer youngest data
+    inline uint8_t get_youngest_index() {
+        return ekf_imu_buffer::get_youngest_index();
+    }
+};