闫刚 px4 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 &timestamp, 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();