diff --git a/libraries/AP_HAL/utility/RingBuffer.h b/libraries/AP_HAL/utility/RingBuffer.h index 9d7de54e0e852..77fa9de0da742 100644 --- a/libraries/AP_HAL/utility/RingBuffer.h +++ b/libraries/AP_HAL/utility/RingBuffer.h @@ -2,6 +2,7 @@ #include #include +#include /* * Circular buffer of bytes. @@ -93,6 +94,7 @@ class ByteBuffer { /* ring buffer class for objects of fixed size + !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! */ template class ObjectBuffer { @@ -109,27 +111,32 @@ class ObjectBuffer { } // Discards the buffer content, emptying it. + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! void clear(void) { buffer->clear(); } // return number of objects available to be read from the front of the queue + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! uint32_t available(void) const { return buffer->available() / sizeof(T); } // return number of objects that could be written to the back of the queue + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! uint32_t space(void) const { return buffer->space() / sizeof(T); } // true is available() == 0 + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool empty(void) const { return buffer->empty(); } // push one object onto the back of the queue + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool push(const T &object) { if (buffer->space() < sizeof(T)) { return false; @@ -138,6 +145,7 @@ class ObjectBuffer { } // push N objects onto the back of the queue + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool push(const T *object, uint32_t n) { if (buffer->space() < n*sizeof(T)) { return false; @@ -148,6 +156,7 @@ class ObjectBuffer { /* throw away an object from the front of the queue */ + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool pop(void) { return buffer->advance(sizeof(T)); } @@ -155,6 +164,7 @@ class ObjectBuffer { /* pop earliest object off the front of the queue */ + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool pop(T &object) { if (buffer->available() < sizeof(T)) { return false; @@ -167,6 +177,7 @@ class ObjectBuffer { * push_force() is semantically equivalent to: * if (!push(t)) { pop(); push(t); } */ + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool push_force(const T &object) { if (buffer->space() < sizeof(T)) { buffer->advance(sizeof(T)); @@ -177,6 +188,7 @@ class ObjectBuffer { /* * push_force() N objects */ + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool push_force(const T *object, uint32_t n) { uint32_t _space = buffer->space(); if (_space < sizeof(T)*n) { @@ -188,6 +200,7 @@ class ObjectBuffer { /* peek copies an object out from the front of the queue without advancing the read pointer */ + // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!! bool peek(T &object) { return buffer->peekbytes((uint8_t*)&object, sizeof(T)) == sizeof(T); } @@ -196,6 +209,7 @@ class ObjectBuffer { return a pointer to first contiguous array of available objects. Return nullptr if none available */ + // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!! const T *readptr(uint32_t &n) { uint32_t avail_bytes = 0; const T *ret = (const T *)buffer->readptr(avail_bytes); @@ -207,12 +221,14 @@ class ObjectBuffer { } // advance the read pointer (discarding objects) + // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!! bool advance(uint32_t n) { return buffer->advance(n * sizeof(T)); } /* update the object at the front of the queue (the one that would be fetched by pop()) */ + // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!! bool update(const T &object) { return buffer->update((uint8_t*)&object, sizeof(T)); } @@ -221,7 +237,164 @@ class ObjectBuffer { ByteBuffer *buffer = nullptr; }; +/* + Thread safe ring buffer class for objects of fixed size + !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + */ +template +class ObjectBuffer_TS { +public: + ObjectBuffer_TS(uint32_t _size) { + // we set size to 1 more than requested as the byte buffer + // gives one less byte than requested. We round up to a full + // multiple of the object size so that we always get aligned + // elements, which makes the readptr() method possible + buffer = new ByteBuffer(((_size+1) * sizeof(T))); + } + ~ObjectBuffer_TS(void) { + delete buffer; + } + + // Discards the buffer content, emptying it. + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + void clear(void) + { + WITH_SEMAPHORE(sem); + buffer->clear(); + } + + // return number of objects available to be read from the front of the queue + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + uint32_t available(void) { + WITH_SEMAPHORE(sem); + return buffer->available() / sizeof(T); + } + + // return number of objects that could be written to the back of the queue + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + uint32_t space(void) { + WITH_SEMAPHORE(sem); + return buffer->space() / sizeof(T); + } + + // true is available() == 0 + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool empty(void) { + WITH_SEMAPHORE(sem); + return buffer->empty(); + } + + // push one object onto the back of the queue + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool push(const T &object) { + WITH_SEMAPHORE(sem); + if (buffer->space() < sizeof(T)) { + return false; + } + return buffer->write((uint8_t*)&object, sizeof(T)) == sizeof(T); + } + + // push N objects onto the back of the queue + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool push(const T *object, uint32_t n) { + WITH_SEMAPHORE(sem); + if (buffer->space() < n*sizeof(T)) { + return false; + } + return buffer->write((uint8_t*)object, n*sizeof(T)) == n*sizeof(T); + } + + /* + throw away an object from the front of the queue + */ + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool pop(void) { + WITH_SEMAPHORE(sem); + return buffer->advance(sizeof(T)); + } + + /* + pop earliest object off the front of the queue + */ + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool pop(T &object) { + WITH_SEMAPHORE(sem); + if (buffer->available() < sizeof(T)) { + return false; + } + return buffer->read((uint8_t*)&object, sizeof(T)) == sizeof(T); + } + + /* + * push_force() is semantically equivalent to: + * if (!push(t)) { pop(); push(t); } + */ + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool push_force(const T &object) { + WITH_SEMAPHORE(sem); + if (buffer->space() < sizeof(T)) { + buffer->advance(sizeof(T)); + } + return push(object); + } + + /* + * push_force() N objects + */ + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool push_force(const T *object, uint32_t n) { + WITH_SEMAPHORE(sem); + uint32_t _space = buffer->space(); + if (_space < sizeof(T)*n) { + buffer->advance(sizeof(T)*(n-_space)); + } + return push(object, n); + } + + /* + peek copies an object out from the front of the queue without advancing the read pointer + */ + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool peek(T &object) { + WITH_SEMAPHORE(sem); + return buffer->peekbytes((uint8_t*)&object, sizeof(T)) == sizeof(T); + } + + /* + return a pointer to first contiguous array of available + objects. Return nullptr if none available + */ + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + const T *readptr(uint32_t &n) { + WITH_SEMAPHORE(sem); + uint32_t avail_bytes = 0; + const T *ret = (const T *)buffer->readptr(avail_bytes); + if (!ret || avail_bytes < sizeof(T)) { + return nullptr; + } + n = avail_bytes / sizeof(T); + return ret; + } + + // advance the read pointer (discarding objects) + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool advance(uint32_t n) { + WITH_SEMAPHORE(sem); + return buffer->advance(n * sizeof(T)); + } + /* update the object at the front of the queue (the one that would + be fetched by pop()) */ + // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!! + bool update(const T &object) { + WITH_SEMAPHORE(sem); + return buffer->update((uint8_t*)&object, sizeof(T)); + } + +private: + ByteBuffer *buffer = nullptr; + HAL_Semaphore sem; +}; /* ring buffer class for objects of fixed size with pointer diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index f0470385e692d..71e22a586bfc4 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -78,7 +78,7 @@ struct AP_Param::param_override *AP_Param::param_overrides = nullptr; uint16_t AP_Param::num_param_overrides = 0; uint16_t AP_Param::num_read_only = 0; -ObjectBuffer AP_Param::save_queue{30}; +ObjectBuffer_TS AP_Param::save_queue{30}; bool AP_Param::registered_save_handler; // we need a dummy object for the parameter save callback diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index f38eb89f06197..86f84aac91330 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -666,7 +666,7 @@ class AP_Param AP_Param *param; bool force_save; }; - static ObjectBuffer save_queue; + static ObjectBuffer_TS save_queue; static bool registered_save_handler; // background function for saving parameters