fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 04:33:35 +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-16 11:51:37 +01:00
committed by Mathieu Bresciani
parent e90f8b500f
commit edc2536bdd
9 changed files with 52 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/RingBuffer.h
-195
View File
@@ -1,195 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015-2022 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file RingBuffer.h
* @author Roman Bapst <bapstroman@gmail.com>
* Template RingBuffer.
*/
#ifndef EKF_RINGBUFFER_H
#define EKF_RINGBUFFER_H
#include <inttypes.h>
#include <cstdio>
#include <cstring>
template <typename data_type>
class RingBuffer
{
public:
explicit RingBuffer(size_t size) { allocate(size); }
RingBuffer() = delete;
~RingBuffer() { delete[] _buffer; }
// no copy, assignment, move, move assignment
RingBuffer(const RingBuffer &) = delete;
RingBuffer &operator=(const RingBuffer &) = delete;
RingBuffer(RingBuffer &&) = delete;
RingBuffer &operator=(RingBuffer &&) = delete;
bool allocate(uint8_t size)
{
if (valid() && (size == _size)) {
// no change
return true;
}
if (size == 0) {
return false;
}
if (_buffer != nullptr) {
delete[] _buffer;
}
_buffer = new data_type[size] {};
if (_buffer == nullptr) {
return false;
}
_size = size;
reset();
return true;
}
bool valid() const { return (_buffer != nullptr) && (_size > 0); }
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;
// move tail if we overwrite it
if (_head == _tail && !_first_write) {
_tail = (_tail + 1) % _size;
} else {
_first_write = false;
}
}
uint8_t get_length() const { return _size; }
data_type &operator[](const uint8_t index) { return _buffer[index]; }
const data_type &get_newest() const { return _buffer[_head]; }
const data_type &get_oldest() const { return _buffer[_tail]; }
uint8_t get_oldest_index() const { return _tail; }
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];
// Now we can set the tail to the item which
// comes after the one we removed since we don't
// want to have any older data in the buffer
if (index == _head) {
_tail = _head;
_first_write = true;
} else {
_tail = (index + 1) % _size;
}
_buffer[index].time_us = 0;
return true;
}
if (index == _tail) {
// we have reached the tail and haven't got a
// match
return false;
}
}
return false;
}
int get_used_size() const { return sizeof(*this) + sizeof(data_type) * entries(); }
int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
int entries() const
{
int count = 0;
for (uint8_t i = 0; i < _size; i++) {
if (_buffer[i].time_us != 0) {
count++;
}
}
return count;
}
void reset()
{
if (_buffer) {
for (uint8_t i = 0; i < _size; i++) {
_buffer[i] = {};
}
_head = 0;
_tail = 0;
_first_write = true;
}
}
private:
data_type *_buffer{nullptr};
uint8_t _head{0};
uint8_t _tail{0};
uint8_t _size{0};
bool _first_write{true};
};
#endif // !EKF_RINGBUFFER_H
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp
+2 -2
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 {
src/modules/ekf2/EKF/ekf.cpp
+1 -1
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",
src/modules/ekf2/EKF/estimator_interface.cpp
+10 -10
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;
src/modules/ekf2/EKF/estimator_interface.h
+12 -12
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
src/modules/ekf2/EKF/output_predictor/output_predictor.h
+3 -3
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{};
src/modules/ekf2/test/CMakeLists.txt
-1
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)
src/modules/ekf2/test/test_EKF_ringbuffer.cpp
-160
View File
@@ -1,160 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-2023 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <math.h>
#include "EKF/ekf.h"
struct sample {
uint64_t time_us;
float data[3];
};
class EkfRingBufferTest : public ::testing::Test
{
public:
sample _x, _y, _z;
RingBuffer<sample> *_buffer{nullptr};
void SetUp() override
{
_buffer = new RingBuffer<sample>(3);
_x.time_us = 1000000;
_x.data[0] = _x.data[1] = _x.data[2] = 1.5f;
_y.time_us = 2000000;
_y.data[0] = _y.data[1] = _y.data[2] = 3.0f;
_z.time_us = 3000000;
_z.data[0] = _z.data[1] = _z.data[2] = 4.0f;
}
void TearDown() override
{
delete _buffer;
}
};
TEST_F(EkfRingBufferTest, goodInitialisation)
{
// WHEN: buffer was allocated
// THEN: allocation should have succeed
ASSERT_EQ(true, _buffer->allocate(3));
}
TEST_F(EkfRingBufferTest, badInitialisation)
{
// WHEN: buffer allocation input is bad
// THEN: allocation should fail
// TODO: Change buffer implementation to pass this test
// ASSERT_EQ(false, _buffer->allocate(-1));
// ASSERT_EQ(false, _buffer->allocate(0));
}
TEST_F(EkfRingBufferTest, orderOfSamples)
{
ASSERT_EQ(true, _buffer->allocate(3));
// GIVEN: allocated buffer
// WHEN: adding multiple samples
// THEN: they should be added in order
_buffer->push(_x);
EXPECT_EQ(_x.time_us, _buffer->get_newest().time_us);
EXPECT_EQ(_x.time_us, _buffer->get_oldest().time_us);
_buffer->push(_z);
_buffer->push(_y);
EXPECT_EQ(_x.time_us, _buffer->get_oldest().time_us);
EXPECT_EQ(_y.time_us, _buffer->get_newest().time_us);
}
TEST_F(EkfRingBufferTest, popSample)
{
ASSERT_EQ(true, _buffer->allocate(3));
_buffer->push(_x);
_buffer->push(_y);
_buffer->push(_z);
// GIVEN: allocated and filled buffer
sample pop = {};
// WHEN: we want to retrieve a sample that is older than any in the buffer
// THEN: we should not get any sample
EXPECT_EQ(false, _buffer->pop_first_older_than(0, &pop));
// WHEN: when calling "pop_first_older_than"
// THEN: we should get the first sample from the head that is older
EXPECT_EQ(true, _buffer->pop_first_older_than(_x.time_us + 1, &pop));
EXPECT_EQ(_x.time_us, pop.time_us);
EXPECT_EQ(true, _buffer->pop_first_older_than(_y.time_us + 10, &pop));
EXPECT_EQ(_y.time_us, pop.time_us);
EXPECT_EQ(true, _buffer->pop_first_older_than(_z.time_us + 100, &pop));
EXPECT_EQ(_z.time_us, pop.time_us);
// TODO: When changing the order of popping sample it does not behave as expected, fix this
}
TEST_F(EkfRingBufferTest, askingForTooNewSample)
{
ASSERT_EQ(true, _buffer->allocate(3));
_buffer->push(_x);
_buffer->push(_y);
_buffer->push(_z);
sample pop = {};
// WHEN: all buffered samples are older by 0.1s than your query timestamp
// THEN: should get no sample returned
EXPECT_EQ(true, _buffer->pop_first_older_than(_z.time_us + 99000, &pop));
EXPECT_EQ(false, _buffer->pop_first_older_than(_y.time_us + 100000, &pop));
}
TEST_F(EkfRingBufferTest, reallocateBuffer)
{
ASSERT_EQ(true, _buffer->allocate(5));
_buffer->push(_x);
_buffer->push(_y);
_buffer->push(_y);
_buffer->push(_y);
_buffer->push(_z);
// GIVEN: allocated and filled buffer
// WHEN: do another allocate call
_buffer->allocate(3);
// THEN: its length should update
EXPECT_EQ(3, _buffer->get_length());
}