AP_HAL: added ByteBuffer and ObjectBuffer in RingBuffer

much better API than old macros

libraries/AP_HAL/utility/RingBuffer.cpp

@@ -0,0 +1,131 @@
+#include "RingBuffer.h"
+#include <stdlib.h>
+#include <string.h>
+
+/*
+  implement a simple ringbuffer of bytes
+ */
+
+ByteBuffer::ByteBuffer(uint32_t _size)
+{
+    size = _size;
+    buf = new uint8_t[size];
+    head = tail = 0;
+}
+
+ByteBuffer::~ByteBuffer(void)
+{
+    delete [] buf;
+}
+
+uint32_t ByteBuffer::available(void) const
+{
+    uint32_t _tail;
+    return ((head > (_tail=tail))? (size - head) + _tail: _tail - head);
+}
+
+uint32_t ByteBuffer::space(void) const
+{
+    uint32_t _head;
+    return (((_head=head) > tail)?(_head - tail) - 1:((size - tail) + _head) - 1);
+}
+
+bool ByteBuffer::empty(void) const
+{
+    return head == tail;
+}
+
+uint32_t ByteBuffer::write(const uint8_t *data, uint32_t len)
+{
+    if (len > space()) {
+        len = space();
+    }
+    if (len == 0) {
+        return 0;
+    }
+    if (tail+len <= size) {
+        // perform as single memcpy
+        memcpy(&buf[tail], data, len);
+        tail = (tail + len) % size;
+        return len;
+    }
+
+    // perform as two memcpy calls
+    uint32_t n = size - tail;
+    if (n > len) {
+        n = len;
+    }
+    memcpy(&buf[tail], data, n);
+    tail = (tail + n) % size;
+    data += n;
+    n = len - n;
+    if (n > 0) {
+        memcpy(&buf[tail], data, n);
+        tail = (tail + n) % size;
+    }
+    return len;
+}
+
+bool ByteBuffer::advance(uint32_t n)
+{
+    if (n > available()) {
+        return false;
+    }
+    head = (head + n) % size;
+    return true;
+}
+
+uint32_t ByteBuffer::read(uint8_t *data, uint32_t len)
+{
+    if (len > available()) {
+        len = available();
+    }
+    if (len == 0) {
+        return 0;
+    }
+    uint32_t n;
+    const uint8_t *b = readptr(n);
+    if (n > len) {
+        n = len;
+    }
+
+    // perform first memcpy
+    memcpy(data, b, n);
+    advance(n);
+    data += n;
+
+    if (len > n) {
+        // possible second memcpy
+        uint32_t n2;
+        b = readptr(n2);
+        if (n2 > len-n) {
+            n2 = len-n;
+        }
+        memcpy(data, b, n2);
+        advance(n2);
+    }
+    return len;
+}
+
+/*
+  return a pointer to a contiguous read buffer
+ */
+const uint8_t *ByteBuffer::readptr(uint32_t &available_bytes)
+{
+    available_bytes = available();
+    if (available_bytes == 0) {
+        return nullptr;
+    }
+    if (head+available_bytes > size) {
+        available_bytes = size - head;
+    }
+    return &buf[head];
+}
+
+int16_t ByteBuffer::peek(uint32_t ofs) const
+{
+    if (ofs >= available()) {
+        return -1;
+    }
+    return buf[(head+ofs)%size];
+}

libraries/AP_HAL/utility/RingBuffer.h

@@ -1,8 +1,11 @@
-#ifndef __AP_HAL_UTILITY_RINGBUFFER_H__
-#define __AP_HAL_UTILITY_RINGBUFFER_H__
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
 
 /*
-  common ring buffer handling macros
+  old style ring buffer handling macros
 
   These macros assume a ring buffer like this:
 
@@ -19,4 +22,73 @@
 #define BUF_ADVANCETAIL(buf, n) buf##_tail = (buf##_tail + n) % buf##_size
 #define BUF_ADVANCEHEAD(buf, n) buf##_head = (buf##_head + n) % buf##_size
 
-#endif // __AP_HAL_UTILITY_RINGBUFFER_H__
+
+/*
+  new style buffers
+ */
+class ByteBuffer {
+public:
+    ByteBuffer(uint32_t size);
+    ~ByteBuffer(void);
+    
+    uint32_t available(void) const;
+    uint32_t space(void) const;
+    bool empty(void) const;
+    uint32_t write(const uint8_t *data, uint32_t len);
+    uint32_t read(uint8_t *data, uint32_t len);
+    uint32_t get_size(void) const { return size; }
+    bool advance(uint32_t n);
+    const uint8_t *readptr(uint32_t &available_bytes);
+    int16_t peek(uint32_t ofs) const;
+    
+private:
+    uint8_t *buf = nullptr;
+    uint32_t size = 0;
+
+    // head is where the next available data is. tail is where new
+    // data is written
+    volatile uint32_t head = 0;
+    volatile uint32_t tail = 0;
+};
+
+/*
+  ring buffer class for objects of fixed size
+ */
+template <class T>
+class ObjectBuffer {
+public:
+    ObjectBuffer(uint32_t _size) {
+        size = _size;
+        buffer = new ByteBuffer((size * sizeof(T))+1);
+    }
+    ~ObjectBuffer(void) {
+        delete buffer;
+    }
+
+    uint32_t available(void) const {
+        return buffer->available() / sizeof(T);
+    }
+    uint32_t space(void) const {
+        return buffer->space() / sizeof(T);
+    }
+    bool empty(void) const {
+        return buffer->empty();
+    }
+    bool push(const T &object) {
+        if (buffer->space() < sizeof(T)) {
+            return false;
+        }
+        return buffer->write((uint8_t*)&object, sizeof(T)) == sizeof(T);
+    }
+    bool pop(T &object) {
+        if (buffer->available() < sizeof(T)) {
+            return false;
+        }
+        return buffer->read((uint8_t*)&object, sizeof(T)) == sizeof(T);
+    }
+
+private:
+    ByteBuffer *buffer = nullptr;
+    uint32_t size = 0;
+};
+