fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

move EKF2 ring buffer to the lib folder

Browse Source
This commit is contained in:
jonas 2026-01-12 20:23:32 +01:00 committed by Mathieu Bresciani
parent e90f8b500f
commit edc2536bdd
9 changed files with 52 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/ringbuffer/CMakeLists.txt
View File

@ -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)

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

@ -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 @@ private:
bool _first_write{true};
};
#endif // !EKF_RINGBUFFER_H

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

@ -33,7 +33,7 @@
#include <gtest/gtest.h>
#include <math.h>
#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<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,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);

4
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp
View File

@ -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 @@ private:
};
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 {

2
src/modules/ekf2/EKF/ekf.cpp
View File

@ -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",

20
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -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;

24
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -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,7 +375,7 @@ protected:
#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{};
@ -383,7 +383,7 @@ protected:
#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 @@ protected:
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 @@ protected:
#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 @@ protected:
// 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

6
src/modules/ekf2/EKF/output_predictor/output_predictor.h
View File

@ -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 @@ private:
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{};

1
src/modules/ekf2/test/CMakeLists.txt
View File

@ -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)