Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: utility: add thread safe ring buffer duplicate

This commit is contained in:
Peter Hall 2020-02-02 16:13:12 +00:00 committed by Andrew Tridgell
parent 1ee14ffa75
commit 2bd6d4674b
1 changed files with 173 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

173
libraries/AP_HAL/utility/RingBuffer.h
View File

@ -2,6 +2,7 @@
#include <atomic>
#include <stdint.h>
#include <AP_HAL/AP_HAL.h>
/*
* Circular buffer of bytes.
@ -93,6 +94,7 @@ private:
/*
ring buffer class for objects of fixed size
!!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
*/
template <class T>
class ObjectBuffer {
@ -109,27 +111,32 @@ public:
}
// 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 @@ public:
}
// 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 @@ public:
/*
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 @@ public:
/*
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 @@ public:
* 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 @@ public:
/*
* 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 @@ public:
/*
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 @@ public:
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 @@ public:
}
// 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 @@ private:
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 T>
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