mirror of https://github.com/ArduPilot/ardupilot
AP_HAL: added ByteBuffer and ObjectBuffer in RingBuffer
much better API than old macros
This commit is contained in:
parent
cf170b2aa7
commit
ec6a679482
|
@ -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];
|
||||
}
|
|
@ -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;
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue