闫刚 px4 ringbuffer深入讲解
文章目录
- 资源
- 简介
- ringbuffer
- 搜索Firmware中所有用到ringbuffer的源代码
- 源代码分析
- 分析ringbufer在rm3100中的使用
- 2. lib/ecl/EKF/RingBuffer.h
- 查询所有使用RingBuffer位置
- 分析RingBuffer代码
- RingBuffer使用例子
资源
px4_ringbuffer.md
简介
在drivers/device/ringbuffer.h中的ringbuffer比较常见,在sensor中主要使用乒乓缓冲,异步读取sensor数据
在ecl/EKF/RingBuffer.h中RingBuffer主要和传感器相关,和时间戳绑定
ringbuffer
drivers/device/ringbuffer.h
搜索Firmware中所有用到ringbuffer的源代码
yangang@yangang-ubuntu:~/work/px4_proj/Firmware/src$ grep -rn "ringbuffer.h"
drivers/camera_capture/camera_capture.hpp:41:#include <drivers/device/ringbuffer.h>
drivers/optical_flow/px4flow/px4flow.cpp:43:#include <drivers/device/ringbuffer.h>
drivers/distance_sensor/sf1xx/sf1xx.cpp:69:#include <drivers/device/ringbuffer.h>
drivers/distance_sensor/mb12xx/mb12xx.cpp:59:#include <drivers/device/ringbuffer.h>
drivers/distance_sensor/vl53lxx/vl53lxx.cpp:46:#include <drivers/device/ringbuffer.h>
drivers/imu/bma180/bma180.cpp:68:#include <drivers/device/ringbuffer.h>
drivers/irlock/irlock.cpp:46:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/ist8308/ist8308.cpp:71:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/bmm150/bmm150.hpp:32:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/lsm303agr/LSM303AGR.hpp:40:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/lis3mdl/lis3mdl.h:45:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/rm3100/rm3100.h:45:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/ist8310/ist8310.cpp:71:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/hmc5883/HMC5883.hpp:62:#include <drivers/device/ringbuffer.h>
drivers/magnetometer/qmc5883/qmc5883.cpp:69:#include <drivers/device/ringbuffer.h>
modules/mavlink/mavlink_main.h:62:#include <drivers/device/ringbuffer.h>
lib/drivers/airspeed/airspeed.cpp:52:#include <drivers/device/ringbuffer.h>
lib/drivers/device/ringbuffer.h:35: * @file ringbuffer.h
lib/drivers/device/ringbuffer.cpp:40:#include "ringbuffer.h"
源代码分析
namespace ringbuffer __EXPORT
{
class RingBuffer
{
public:
//构造函数, num_items: 节点的数量, item_size:节点内存大小
RingBuffer(unsigned num_items, size_t item_size);
//析够函数
virtual ~RingBuffer();
//此接口写入ringbuffer的数据是1个内存地址,val_size = 0是1个c++的函数默认参的写法
bool put(const void *val, size_t val_size = 0);
//此接口不同宽度的变量的值到ringbuffer中,uint8_t, uint16_t, uint32_t,float, double
bool put(int8_t val);
bool put(uint8_t val);
bool put(int16_t val);
bool put(uint16_t val);
bool put(int32_t val);
bool put(uint32_t val);
bool put(int64_t val);
bool put(uint64_t val);
bool put(float val);
bool put(double val);
//force目的是缓冲区满了,直接覆盖缓存的头部,这个是环形缓冲的特点
bool force(const void *val, size_t val_size = 0);
bool force(int8_t val);
bool force(uint8_t val);
bool force(int16_t val);
bool force(uint16_t val);
bool force(int32_t val);
bool force(uint32_t val);
bool force(int64_t val);
bool force(uint64_t val);
bool force(float val);
bool force(double val);
//从ringbuffer中获取数据,下面是从ringbuffer的尾部获取不同的宽度的数据
bool get(void *val, size_t val_size = 0);
bool get(int8_t &val);
bool get(uint8_t &val);
bool get(int16_t &val);
bool get(uint16_t &val);
bool get(int32_t &val);
bool get(uint32_t &val);
bool get(int64_t &val);
bool get(uint64_t &val);
bool get(float &val);
bool get(double &val);
unsigned space(void);
unsigned count(void);
//查询ringbuffer是否空
bool empty();
//查询ringbuffer是否满
bool full();
//查询ringbuffer内的数量的大小
unsigned size();
//清空缓冲
void flush();
//重新分配ringbufer的大小, 缓冲区指向新的内存地址
bool resize(unsigned new_size);
//打印ringbuffer的状态
void print_info(const char *name);
private:
// ringbuffer的item的数量
unsigned _num_items;
// ringbuffer每个item的大小
const size_t _item_size;
// ringbuffer的缓冲大小
char *_buf;
// ringbuffer的头位置
volatile unsigned _head; /**< insertion point in _item_size units */
// ringbuffer的尾位置
volatile unsigned _tail; /**< removal point in _item_size units */
unsigned _next(unsigned index);
/* we don't want this class to be copied */
RingBuffer(const RingBuffer &);
RingBuffer operator=(const RingBuffer &);
};
} // namespace ringbuffer
当写入数据为0, 或者当读大于item长度的时候, 都修改成item的大小
bool
RingBuffer::put(const void *val, size_t val_size)
{
unsigned next = _next(_head);
if (next != _tail) {
// 当写入数据为0, 或者当读大于item长度的时候, 都修改成item的大小
if ((val_size == 0) || (val_size > _item_size)) {
val_size = _item_size;
}
memcpy(&_buf[_head * _item_size], val, val_size);
_head = next;
return true;
} else {
return false;
}
}
分析ringbufer在rm3100中的使用
./src/drivers/magnetometer/rm3100/rm3100.cpp
//创建ringbuffer对象,数量是2个,也称为乒乓缓冲,大小是sensor_mag_s消息的大小
RM3100::init()
->_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s));
//往ringbufer中写1个item, 使用的是force方法
RM3100::collect()
->sensor_mag_s new_mag_report;
-> _reports->force(&new_mag_report);
//从ringbuffer中获取1个item,使用的get方法
RM3100::read(struct file *file_pointer, char *buffer, size_t buffer_len)
-> sensor_mag_s *mag_buf = reinterpret_cast<sensor_mag_s *>(buffer);
if (_reports->get(mag_buf)) {
//打印ringbufer状态
RM3100::print_info()
-> _reports->print_info("report queue");
//分析mavlink_log
2. lib/ecl/EKF/RingBuffer.h
查询所有使用RingBuffer位置
yangang@yangang-ubuntu:~/work/px4_proj/Firmware/src$ grep -rn "RingBuffer.h"
lib/ecl/EKF/RingBuffer.h:35: * @file RingBuffer.h
lib/ecl/EKF/estimator_interface.h:46:#include "RingBuffer.h"
lib/ecl/tools/format.sh:5:EKF/RingBuffer.h
分析RingBuffer代码
template <typename data_type>
class RingBuffer {
public:
//默认构造1个大小的ringbuffer
RingBuffer() {
if (allocate(1)) {
// initialize with one empty sample
data_type d = {};
push(d);
}
}
~RingBuffer() { delete[] _buffer; }
RingBuffer(const RingBuffer &) = delete;
RingBuffer &operator=(const RingBuffer &) = delete;
RingBuffer(RingBuffer &&) = delete;
RingBuffer &operator=(RingBuffer &&) = delete;
//分配item数量
bool allocate(uint8_t size) {
if (_buffer != nullptr) {
delete[] _buffer;
}
_buffer = new data_type[size];
if (_buffer == nullptr) {
return false;
}
_size = size;
_head = 0;
_tail = 0;
for (uint8_t index = 0; index < _size; index++) {
_buffer[index] = {};
}
_first_write = true;
return true;
}
void unallocate() {
delete[] _buffer;
_buffer = nullptr;
}
//往ringbuffer中写1个item
void push(const data_type &sample) {
uint8_t head_new = _head;
if (!_first_write) {
head_new = (_head + 1) % _size;
}
_buffer[head_new] = sample;
_head = head_new;
//如果缓冲区满子自动覆盖缓冲区的头部
if (_head == _tail && !_first_write) {
_tail = (_tail + 1) % _size;
} else {
_first_write = false;
}
}
//获取rinbuffer的长度
uint8_t get_length() const { return _size; }
//通过数组下标的形式,获取缓冲区某个对象
data_type &operator[](const uint8_t index) { return _buffer[index]; }
//获取最新的数据, 并没有更新tail位置
const data_type &get_newest() { return _buffer[_head]; }
//获取最旧的数据
const data_type &get_oldest() { return _buffer[_tail]; }
//获取最老数据的索引
uint8_t get_oldest_index() const { return _tail; }
//按照时间戳获取离时间戳最近的往前1个数据,类似最数据的时间戳是500, 1000, 如果给的时间戳是1001,
//那么会取到时间戳是1000的数据
bool pop_first_older_than(const uint64_t ×tamp, data_type *sample) {
// start looking from newest observation data
for (uint8_t i = 0; i < _size; i++) {
int index = (_head - i);
index = index < 0 ? _size + index : index;
if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < (uint64_t)1e5) {
*sample = _buffer[index];
if (index == _head) {
_tail = _head;
_first_write = true;
} else {
_tail = (index + 1) % _size;
}
_buffer[index].time_us = 0;
return true;
}
if (index == _tail) {
return false;
}
}
return false;
}
//获取整个对象+动态内存buffer的大小
int get_total_size() { return sizeof(*this) + sizeof(data_type) * _size; }
private:
//缓冲区
data_type *_buffer{nullptr};
//头指针初始化为0
uint8_t _head{0};
//尾指针初始化为0
uint8_t _tail{0};
//缓冲区的大小
uint8_t _size{0};
bool _first_write{true};
};
RingBuffer使用例子
//初始化
EKF/estimator_interface.h:529: RingBuffer<gpsSample> _gps_buffer;
//分配内存大小
EKF/estimator_interface.cpp:180: _gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);
//写入数据
EKF/estimator_interface.cpp:229: _gps_buffer.push(gps_sample_new);
//读取数据,不更新尾指针
EKF/control.cpp:91: const gpsSample &gps_init = _gps_buffer.get_newest();
//从ringbuffer提取数据,也就是更新了tail指针
EKF/control.cpp:95: _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
//销毁ringbuffer对象, 释放内存
EKF/estimator_interface.cpp:552: _gps_buffer.unallocate();