diff --git a/src/lib/ringbuffer/CMakeLists.txt b/src/lib/ringbuffer/CMakeLists.txt index 78a26de166..538c1ae907 100644 --- a/src/lib/ringbuffer/CMakeLists.txt +++ b/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) diff --git a/src/modules/ekf2/EKF/RingBuffer.h b/src/lib/ringbuffer/TimestampedRingBuffer.hpp similarity index 85% rename from src/modules/ekf2/EKF/RingBuffer.h rename to src/lib/ringbuffer/TimestampedRingBuffer.hpp index 2e4ff02bb3..42e4b0beb5 100644 --- a/src/modules/ekf2/EKF/RingBuffer.h +++ b/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 - * 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 #include #include template -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 @@ private: bool _first_write{true}; }; - -#endif // !EKF_RINGBUFFER_H diff --git a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp b/src/lib/ringbuffer/TimestampedRingBufferTest.cpp similarity index 89% rename from src/modules/ekf2/test/test_EKF_ringbuffer.cpp rename to src/lib/ringbuffer/TimestampedRingBufferTest.cpp index 0338bea816..83f5fa7940 100644 --- a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp +++ b/src/lib/ringbuffer/TimestampedRingBufferTest.cpp @@ -33,7 +33,7 @@ #include #include -#include "EKF/ekf.h" +#include "TimestampedRingBuffer.hpp" struct sample { uint64_t time_us; @@ -41,16 +41,16 @@ struct sample { }; -class EkfRingBufferTest : public ::testing::Test +class TimestampedRingBufferTest : public ::testing::Test { public: sample _x, _y, _z; - RingBuffer *_buffer{nullptr}; + TimestampedRingBuffer *_buffer{nullptr}; void SetUp() override { - _buffer = new RingBuffer(3); + _buffer = new TimestampedRingBuffer(3); _x.time_us = 1000000; _x.data[0] = _x.data[1] = _x.data[2] = 1.5f; @@ -67,7 +67,7 @@ public: } }; -TEST_F(EkfRingBufferTest, goodInitialisation) +TEST_F(TimestampedRingBufferTest, goodInitialisation) { // WHEN: buffer was allocated // THEN: allocation should have succeed @@ -75,7 +75,7 @@ TEST_F(EkfRingBufferTest, goodInitialisation) } -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); diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index 8740c2851e..8777be409a 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/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 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) @@ -95,7 +95,7 @@ private: }; estimator_aid_source2d_s _aid_src_aux_global_position{}; - RingBuffer _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate + TimestampedRingBuffer _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 { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 3e609eb8e4..af63fc2b72 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -425,7 +425,7 @@ void Ekf::updateParameters() } template -static void printRingBuffer(const char *name, RingBuffer *rb) +static void printRingBuffer(const char *name, TimestampedRingBuffer *rb) { if (rb) { printf("%s: %d/%d entries (%d/%d Bytes) (%zu Bytes per entry)\n", diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 64ee74f1d4..b6f71444e9 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/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(_obs_buffer_length); + _mag_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _gps_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _baro_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _airspeed_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _range_buffer = new TimestampedRingBuffer(_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(_imu_buffer_length); + _flow_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _ext_vision_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _auxvel_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _system_flag_buffer = new TimestampedRingBuffer(_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(_obs_buffer_length); + _drag_buffer = new TimestampedRingBuffer(_obs_buffer_length); if (_drag_buffer == nullptr || !_drag_buffer->valid()) { delete _drag_buffer; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 0c3f9bf7be..b4bb3a0f9b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -62,7 +62,7 @@ #endif #include "common.h" -#include "RingBuffer.h" +#include #include "imu_down_sampler/imu_down_sampler.hpp" #include "output_predictor/output_predictor.h" @@ -375,7 +375,7 @@ protected: #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_RANGE_FINDER) - RingBuffer *_range_buffer {nullptr}; + TimestampedRingBuffer *_range_buffer {nullptr}; uint64_t _time_last_range_buffer_push{0}; sensor::SensorRangeFinder _range_sensor{}; @@ -383,7 +383,7 @@ protected: #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) - RingBuffer *_flow_buffer {nullptr}; + TimestampedRingBuffer *_flow_buffer {nullptr}; flowSample _flow_sample_delayed{}; @@ -403,7 +403,7 @@ protected: float _local_origin_alt{NAN}; #if defined(CONFIG_EKF2_GNSS) - RingBuffer *_gps_buffer {nullptr}; + TimestampedRingBuffer *_gps_buffer {nullptr}; uint64_t _time_last_gps_buffer_push{0}; gnssSample _gps_sample_delayed{}; @@ -429,7 +429,7 @@ protected: #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_DRAG_FUSION) - RingBuffer *_drag_buffer {nullptr}; + TimestampedRingBuffer *_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 @@ protected: // data buffer instances static constexpr uint8_t kBufferLengthDefault = 12; - RingBuffer _imu_buffer{kBufferLengthDefault}; + TimestampedRingBuffer _imu_buffer{kBufferLengthDefault}; #if defined(CONFIG_EKF2_MAGNETOMETER) - RingBuffer *_mag_buffer {nullptr}; + TimestampedRingBuffer *_mag_buffer {nullptr}; uint64_t _time_last_mag_buffer_push{0}; #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) - RingBuffer *_airspeed_buffer {nullptr}; + TimestampedRingBuffer *_airspeed_buffer {nullptr}; bool _synthetic_airspeed{false}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_EXTERNAL_VISION) - RingBuffer *_ext_vision_buffer {nullptr}; + TimestampedRingBuffer *_ext_vision_buffer {nullptr}; uint64_t _time_last_ext_vision_buffer_push{0}; #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) - RingBuffer *_auxvel_buffer {nullptr}; + TimestampedRingBuffer *_auxvel_buffer {nullptr}; #endif // CONFIG_EKF2_AUXVEL - RingBuffer *_system_flag_buffer {nullptr}; + TimestampedRingBuffer *_system_flag_buffer {nullptr}; #if defined(CONFIG_EKF2_BAROMETER) - RingBuffer *_baro_buffer {nullptr}; + TimestampedRingBuffer *_baro_buffer {nullptr}; uint64_t _time_last_baro_buffer_push{0}; #endif // CONFIG_EKF2_BAROMETER diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index f9643b3ec2..2309fc7954 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -36,7 +36,7 @@ #include -#include "../RingBuffer.h" +#include #include #include @@ -165,8 +165,8 @@ private: LatLonAlt _global_ref{0.0, 0.0, 0.f}; - RingBuffer _output_buffer{12}; - RingBuffer _output_vert_buffer{12}; + TimestampedRingBuffer _output_buffer{12}; + TimestampedRingBuffer _output_vert_buffer{12}; matrix::Vector3f _accel_bias{}; matrix::Vector3f _gyro_bias{}; diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 3761758282..43749e49ac 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/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)