AP_HAL: added ObjectArray template

this is a ring buffer that supports indexing for efficient handling of
queue peeking and manipulation

libraries/AP_HAL/utility/RingBuffer.h

@@ -87,8 +87,7 @@ template <class T>
 class ObjectBuffer {
 public:
     ObjectBuffer(uint32_t _size) {
-        size = _size;
+        buffer = new ByteBuffer((_size * sizeof(T))+1);
-        buffer = new ByteBuffer((size * sizeof(T))+1);
     }
     ~ObjectBuffer(void) {
         delete buffer;
@@ -161,6 +160,124 @@ public:
     
 private:
     ByteBuffer *buffer = nullptr;
-    uint32_t size = 0;
+};
+    
+
+
+/*
+  ring buffer class for objects of fixed size with pointer
+  access. Note that this is not thread safe, buf offers efficient
+  array-like access
+ */
+template <class T>
+class ObjectArray {
+public:
+    ObjectArray(uint16_t _size) {
+        size = _size;
+        head = count = 0;
+        buffer = new T[size];
+    }
+    ~ObjectArray(void) {
+        delete[] buffer;
+    }
+
+    // return number of objects available to be read
+    uint16_t available(void) const {
+        return count;
+    }
+
+    // return number of objects that could be written
+    uint16_t space(void) const {
+        return size - count;
+    }
+
+    // true is available() == 0
+    bool empty(void) const {
+        return count == 0;
+    }
+
+    // push one object
+    bool push(const T &object) {
+        if (space() == 0) {
+            return false;
+        }
+        buffer[(head+count)%size] = object;
+        count++;
+        return true;
+    }
+   
+    /*
+      throw away an object
+     */
+    bool pop(void) {
+        if (empty()) {
+            return false;
+        }
+        head = (head+1) % size;
+        count--;
+        return true;
+    }
+
+    /*
+      pop earliest object off the queue
+     */
+    bool pop(T &object) {
+        if (empty()) {
+            return false;
+        }
+        object = buffer[head];
+        return pop();
+    }
+
+    
+    /*
+     * push_force() is semantically equivalent to:
+     *   if (!push(t)) { pop(); push(t); }
+     */
+    bool push_force(const T &object) {
+        if (space() == 0) {
+            pop();
+        }
+        return push(object);
+    }
+
+    /*
+      remove the Nth element from the array. First element is zero
+     */
+    bool remove(uint16_t n) {
+        if (n >= count) {
+            return false;
+        }
+        if (n == count-1) {
+            // remove last element
+            count--;
+            return true;
+        }
+        if (n == 0) {
+            // remove first element
+            return pop();
+        }
+        // take advantage of the [] operator for simple shift of the array elements
+        for (uint16_t i=n; i<count-1; i++) {
+            *(*this)[i] = *(*this)[i+1];
+        }
+        count--;
+        return true;
+    }
+    
+    // allow array indexing, based on current head. Returns a pointer
+    // to the object or NULL
+    T * operator[](uint16_t i) {
+        if (i >= count) {
+            return nullptr;
+        }
+        return &buffer[(head+i)%size];
+    }
+    
+private:
+    T *buffer;
+    uint16_t size;  // total buffer size
+    uint16_t count; // number in buffer now
+    uint16_t head;  // first element
 };