move EKF2 RingBuffer to lib so it can be used by other modules

src/lib/ringbuffer/CMakeLists.txt

@@ -38,3 +38,4 @@ px4_add_library(ringbuffer
 target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer)
+px4_add_unit_gtest(SRC TimestampedRingBufferTest.cpp)

src/modules/ekf2/EKF/RingBuffer.h → src/lib/ringbuffer/TimestampedRingBuffer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2015-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2015-2026 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,31 +32,31 @@
  ****************************************************************************/
 
 /**
- * @file RingBuffer.h
+ * @file TimestampedRingBuffer.h
  * @author Roman Bapst <bapstroman@gmail.com>
- * Template RingBuffer.
+ * @brief Template ring buffer for timestamped samples (requires data_type::time_us).
+ * Note: This is not the same as `Ringbuffer` (byte FIFO) in `src/lib/ringbuffer/Ringbuffer.hpp`.
  */
 
-#ifndef EKF_RINGBUFFER_H
-#define EKF_RINGBUFFER_H
+#pragma once
 
 #include <inttypes.h>
 #include <cstdio>
 #include <cstring>
 
 template <typename data_type>
-class RingBuffer
+class TimestampedRingBuffer
 {
 public:
-	explicit RingBuffer(size_t size) { allocate(size); }
-	RingBuffer() = delete;
-	~RingBuffer() { delete[] _buffer; }
+	explicit TimestampedRingBuffer(size_t size) { allocate(size); }
+	TimestampedRingBuffer() = delete;
+	~TimestampedRingBuffer() { delete[] _buffer; }
 
 	// no copy, assignment, move, move assignment
-	RingBuffer(const RingBuffer &) = delete;
-	RingBuffer &operator=(const RingBuffer &) = delete;
-	RingBuffer(RingBuffer &&) = delete;
-	RingBuffer &operator=(RingBuffer &&) = delete;
+	TimestampedRingBuffer(const TimestampedRingBuffer &) = delete;
+	TimestampedRingBuffer &operator=(const TimestampedRingBuffer &) = delete;
+	TimestampedRingBuffer(TimestampedRingBuffer &&) = delete;
+	TimestampedRingBuffer &operator=(TimestampedRingBuffer &&) = delete;
 
 	bool allocate(uint8_t size)
 	{
@@ -191,5 +191,3 @@ class RingBuffer
 
 	bool _first_write{true};
 };
-
-#endif // !EKF_RINGBUFFER_H

src/modules/ekf2/test/test_EKF_ringbuffer.cpp → src/lib/ringbuffer/TimestampedRingBufferTest.cpp

@@ -33,24 +33,24 @@
 
 #include <gtest/gtest.h>
 #include <math.h>
-#include "EKF/ekf.h"
+#include "TimestampedRingBuffer.hpp"
 
 struct sample {
 	uint64_t time_us;
 	float data[3];
 };
 
 
-class EkfRingBufferTest : public ::testing::Test
+class TimestampedRingBufferTest : public ::testing::Test
 {
 public:
 
 	sample _x, _y, _z;
-	RingBuffer<sample> *_buffer{nullptr};
+	TimestampedRingBuffer<sample> *_buffer{nullptr};
 
 	void SetUp() override
 	{
-		_buffer = new RingBuffer<sample>(3);
+		_buffer = new TimestampedRingBuffer<sample>(3);
 		_x.time_us = 1000000;
 		_x.data[0] = _x.data[1] = _x.data[2] = 1.5f;
 
@@ -67,15 +67,15 @@ class EkfRingBufferTest : public ::testing::Test
 	}
 };
 
-TEST_F(EkfRingBufferTest, goodInitialisation)
+TEST_F(TimestampedRingBufferTest, goodInitialisation)
 {
 	// WHEN: buffer was allocated
 	// THEN: allocation should have succeed
 	ASSERT_EQ(true, _buffer->allocate(3));
 
 }
 
-TEST_F(EkfRingBufferTest, badInitialisation)
+TEST_F(TimestampedRingBufferTest, badInitialisation)
 {
 	// WHEN: buffer allocation input is bad
 	// THEN: allocation should fail
@@ -85,7 +85,7 @@ TEST_F(EkfRingBufferTest, badInitialisation)
 	// ASSERT_EQ(false, _buffer->allocate(0));
 }
 
-TEST_F(EkfRingBufferTest, orderOfSamples)
+TEST_F(TimestampedRingBufferTest, orderOfSamples)
 {
 	ASSERT_EQ(true, _buffer->allocate(3));
 	// GIVEN: allocated buffer
@@ -103,7 +103,7 @@ TEST_F(EkfRingBufferTest, orderOfSamples)
 	EXPECT_EQ(_y.time_us, _buffer->get_newest().time_us);
 }
 
-TEST_F(EkfRingBufferTest, popSample)
+TEST_F(TimestampedRingBufferTest, popSample)
 {
 	ASSERT_EQ(true, _buffer->allocate(3));
 	_buffer->push(_x);
@@ -128,7 +128,7 @@ TEST_F(EkfRingBufferTest, popSample)
 	// TODO: When changing the order of popping sample it does not behave as expected, fix this
 }
 
-TEST_F(EkfRingBufferTest, askingForTooNewSample)
+TEST_F(TimestampedRingBufferTest, askingForTooNewSample)
 {
 	ASSERT_EQ(true, _buffer->allocate(3));
 	_buffer->push(_x);
@@ -142,7 +142,7 @@ TEST_F(EkfRingBufferTest, askingForTooNewSample)
 	EXPECT_EQ(false, _buffer->pop_first_older_than(_y.time_us + 100000, &pop));
 }
 
-TEST_F(EkfRingBufferTest, reallocateBuffer)
+TEST_F(TimestampedRingBufferTest, reallocateBuffer)
 {
 	ASSERT_EQ(true, _buffer->allocate(5));
 	_buffer->push(_x);

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp

@@ -43,7 +43,7 @@
 //  WelfordMean for rate
 
 #include "../../common.h"
-#include "../../RingBuffer.h"
+#include <lib/ringbuffer/TimestampedRingBuffer.hpp>
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 
@@ -95,7 +95,7 @@ class AuxGlobalPosition : public ModuleParams
 	};
 
 	estimator_aid_source2d_s _aid_src_aux_global_position{};
-	RingBuffer<AuxGlobalPositionSample> _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
+	TimestampedRingBuffer<AuxGlobalPositionSample> _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
 	uint64_t _time_last_buffer_push{0};
 
 	enum class Ctrl : uint8_t {

src/modules/ekf2/EKF/ekf.cpp

@@ -425,7 +425,7 @@ void Ekf::updateParameters()
 }
 
 template<typename T>
-static void printRingBuffer(const char *name, RingBuffer<T> *rb)
+static void printRingBuffer(const char *name, TimestampedRingBuffer<T> *rb)
 {
 	if (rb) {
 		printf("%s: %d/%d entries (%d/%d Bytes) (%zu Bytes per entry)\n",

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -128,7 +128,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 
 	// Allocate the required buffer size if not previously done
 	if (_mag_buffer == nullptr) {
-		_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
+		_mag_buffer = new TimestampedRingBuffer<magSample>(_obs_buffer_length);
 
 		if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
 			delete _mag_buffer;
@@ -167,7 +167,7 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample, const bool pp
 
 	// Allocate the required buffer size if not previously done
 	if (_gps_buffer == nullptr) {
-		_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);
+		_gps_buffer = new TimestampedRingBuffer<gnssSample>(_obs_buffer_length);
 
 		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
 			delete _gps_buffer;
@@ -216,7 +216,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 
 	// Allocate the required buffer size if not previously done
 	if (_baro_buffer == nullptr) {
-		_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
+		_baro_buffer = new TimestampedRingBuffer<baroSample>(_obs_buffer_length);
 
 		if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
 			delete _baro_buffer;
@@ -255,7 +255,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 
 	// Allocate the required buffer size if not previously done
 	if (_airspeed_buffer == nullptr) {
-		_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
+		_airspeed_buffer = new TimestampedRingBuffer<airspeedSample>(_obs_buffer_length);
 
 		if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
 			delete _airspeed_buffer;
@@ -293,7 +293,7 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 
 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
+		_range_buffer = new TimestampedRingBuffer<sensor::rangeSample>(_obs_buffer_length);
 
 		if (_range_buffer == nullptr || !_range_buffer->valid()) {
 			delete _range_buffer;
@@ -332,7 +332,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 
 	// Allocate the required buffer size if not previously done
 	if (_flow_buffer == nullptr) {
-		_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
+		_flow_buffer = new TimestampedRingBuffer<flowSample>(_imu_buffer_length);
 
 		if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
 			delete _flow_buffer;
@@ -370,7 +370,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 
 	// Allocate the required buffer size if not previously done
 	if (_ext_vision_buffer == nullptr) {
-		_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
+		_ext_vision_buffer = new TimestampedRingBuffer<extVisionSample>(_obs_buffer_length);
 
 		if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
 			delete _ext_vision_buffer;
@@ -410,7 +410,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 
 	// Allocate the required buffer size if not previously done
 	if (_auxvel_buffer == nullptr) {
-		_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
+		_auxvel_buffer = new TimestampedRingBuffer<auxVelSample>(_obs_buffer_length);
 
 		if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
 			delete _auxvel_buffer;
@@ -447,7 +447,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 
 	// Allocate the required buffer size if not previously done
 	if (_system_flag_buffer == nullptr) {
-		_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
+		_system_flag_buffer = new TimestampedRingBuffer<systemFlagUpdate>(_obs_buffer_length);
 
 		if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
 			delete _system_flag_buffer;
@@ -483,7 +483,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 
 		// Allocate the required buffer size if not previously done
 		if (_drag_buffer == nullptr) {
-			_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
+			_drag_buffer = new TimestampedRingBuffer<dragSample>(_obs_buffer_length);
 
 			if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
 				delete _drag_buffer;

src/modules/ekf2/EKF/estimator_interface.h

@@ -62,7 +62,7 @@
 #endif
 
 #include "common.h"
-#include "RingBuffer.h"
+#include <lib/ringbuffer/TimestampedRingBuffer.hpp>
 #include "imu_down_sampler/imu_down_sampler.hpp"
 #include "output_predictor/output_predictor.h"
 
@@ -375,15 +375,15 @@ class EstimatorInterface
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	RingBuffer<sensor::rangeSample> *_range_buffer {nullptr};
+	TimestampedRingBuffer<sensor::rangeSample> *_range_buffer {nullptr};
 	uint64_t _time_last_range_buffer_push{0};
 
 	sensor::SensorRangeFinder _range_sensor{};
 	RangeFinderConsistencyCheck _rng_consistency_check;
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	RingBuffer<flowSample> 	*_flow_buffer {nullptr};
+	TimestampedRingBuffer<flowSample> 	*_flow_buffer {nullptr};
 
 	flowSample _flow_sample_delayed{};
 
@@ -403,7 +403,7 @@ class EstimatorInterface
 	float _local_origin_alt{NAN};
 
 #if defined(CONFIG_EKF2_GNSS)
-	RingBuffer<gnssSample> *_gps_buffer {nullptr};
+	TimestampedRingBuffer<gnssSample> *_gps_buffer {nullptr};
 	uint64_t _time_last_gps_buffer_push{0};
 
 	gnssSample _gps_sample_delayed{};
@@ -429,7 +429,7 @@ class EstimatorInterface
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
-	RingBuffer<dragSample> *_drag_buffer {nullptr};
+	TimestampedRingBuffer<dragSample> *_drag_buffer {nullptr};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 #endif // CONFIG_EKF2_DRAG_FUSION
 
@@ -444,29 +444,29 @@ class EstimatorInterface
 
 	// data buffer instances
 	static constexpr uint8_t kBufferLengthDefault = 12;
-	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
+	TimestampedRingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	RingBuffer<magSample> *_mag_buffer {nullptr};
+	TimestampedRingBuffer<magSample> *_mag_buffer {nullptr};
 	uint64_t _time_last_mag_buffer_push{0};
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-	RingBuffer<airspeedSample> *_airspeed_buffer {nullptr};
+	TimestampedRingBuffer<airspeedSample> *_airspeed_buffer {nullptr};
 	bool _synthetic_airspeed{false};
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	RingBuffer<extVisionSample> *_ext_vision_buffer {nullptr};
+	TimestampedRingBuffer<extVisionSample> *_ext_vision_buffer {nullptr};
 	uint64_t _time_last_ext_vision_buffer_push{0};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 #if defined(CONFIG_EKF2_AUXVEL)
-	RingBuffer<auxVelSample> *_auxvel_buffer {nullptr};
+	TimestampedRingBuffer<auxVelSample> *_auxvel_buffer {nullptr};
 #endif // CONFIG_EKF2_AUXVEL
-	RingBuffer<systemFlagUpdate> *_system_flag_buffer {nullptr};
+	TimestampedRingBuffer<systemFlagUpdate> *_system_flag_buffer {nullptr};
 
 #if defined(CONFIG_EKF2_BAROMETER)
-	RingBuffer<baroSample> *_baro_buffer {nullptr};
+	TimestampedRingBuffer<baroSample> *_baro_buffer {nullptr};
 	uint64_t _time_last_baro_buffer_push{0};
 #endif // CONFIG_EKF2_BAROMETER
 

src/modules/ekf2/EKF/output_predictor/output_predictor.h

@@ -36,7 +36,7 @@
 
 #include <matrix/math.hpp>
 
-#include "../RingBuffer.h"
+#include <lib/ringbuffer/TimestampedRingBuffer.hpp>
 
 #include <lib/geo/geo.h>
 #include <lib/lat_lon_alt/lat_lon_alt.hpp>
@@ -165,8 +165,8 @@ class OutputPredictor
 
 	LatLonAlt _global_ref{0.0, 0.0, 0.f};
 
-	RingBuffer<outputSample> _output_buffer{12};
-	RingBuffer<outputVert> _output_vert_buffer{12};
+	TimestampedRingBuffer<outputSample> _output_buffer{12};
+	TimestampedRingBuffer<outputVert> _output_vert_buffer{12};
 
 	matrix::Vector3f _accel_bias{};
 	matrix::Vector3f _gyro_bias{};

src/modules/ekf2/test/CMakeLists.txt

@@ -54,7 +54,6 @@ px4_add_unit_gtest(SRC test_EKF_initialization.cpp LINKLIBS ecl_EKF ecl_sensor_s
 px4_add_unit_gtest(SRC test_EKF_mag.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
-px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_terrain.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)