From 7b3a2e7c1a5cb3a4cd27d8a571fd34acfb95f108 Mon Sep 17 00:00:00 2001 From: Ryan Pavlik Date: Tue, 22 Feb 2022 16:42:38 -0600 Subject: [PATCH] a/math: C wrapper for the float low-pass filter too. --- src/xrt/auxiliary/CMakeLists.txt | 2 + src/xrt/auxiliary/math/m_lowpass_float.cpp | 101 +++++++++++++++++++++ src/xrt/auxiliary/math/m_lowpass_float.h | 99 ++++++++++++++++++++ src/xrt/auxiliary/meson.build | 2 + tests/tests_lowpass_float.cpp | 67 ++++++++++++++ 5 files changed, 271 insertions(+) create mode 100644 src/xrt/auxiliary/math/m_lowpass_float.cpp create mode 100644 src/xrt/auxiliary/math/m_lowpass_float.h diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index 4d3056652..4e98e177e 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -44,6 +44,8 @@ add_library( math/m_imu_3dof.h math/m_imu_pre.c math/m_imu_pre.h + math/m_lowpass_float.cpp + math/m_lowpass_float.h math/m_lowpass_float.hpp math/m_lowpass_float_vector.hpp math/m_lowpass_integer.cpp diff --git a/src/xrt/auxiliary/math/m_lowpass_float.cpp b/src/xrt/auxiliary/math/m_lowpass_float.cpp new file mode 100644 index 000000000..8b1de0f3e --- /dev/null +++ b/src/xrt/auxiliary/math/m_lowpass_float.cpp @@ -0,0 +1,101 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Wrap float filters for C + * @author Ryan Pavlik + * @ingroup aux_math + */ + +#include "m_lowpass_float.h" +#include "m_lowpass_float.hpp" + +#include "util/u_logging.h" + +#include + +using xrt::auxiliary::math::LowPassIIRFilter; + +struct m_lowpass_float +{ + m_lowpass_float(float cutoff_hz) : filter(cutoff_hz) {} + + LowPassIIRFilter filter; +}; + +#define DEFAULT_CATCH(RETURNVAL) \ + catch (std::exception const &e) \ + { \ + U_LOG_E("Caught exception: %s", e.what()); \ + return RETURNVAL; \ + } \ + catch (...) \ + { \ + U_LOG_E("Caught exception"); \ + return RETURNVAL; \ + } + +struct m_lowpass_float * +m_lowpass_float_create(float cutoff_hz) +{ + try { + auto ret = std::make_unique(cutoff_hz); + return ret.release(); + } + DEFAULT_CATCH(nullptr) +} + +void +m_lowpass_float_add_sample(struct m_lowpass_float *mlf, float sample, timepoint_ns timestamp_ns) +{ + try { + mlf->filter.addSample(sample, timestamp_ns); + } + DEFAULT_CATCH() +} + +float +m_lowpass_float_get_state(const struct m_lowpass_float *mlf) +{ + + try { + return mlf->filter.getState(); + } + DEFAULT_CATCH(0) +} + +timepoint_ns +m_lowpass_float_get_timestamp_ns(const struct m_lowpass_float *mlf) +{ + try { + return mlf->filter.getTimestampNs(); + } + DEFAULT_CATCH(0) +} + +bool +m_lowpass_float_is_initialized(const struct m_lowpass_float *mlf) +{ + + try { + return mlf->filter.isInitialized(); + } + DEFAULT_CATCH(false) +} + +void +m_lowpass_float_destroy(struct m_lowpass_float **ptr_to_mlf) +{ + try { + if (ptr_to_mlf == nullptr) { + return; + } + struct m_lowpass_float *mlf = *ptr_to_mlf; + if (mlf == nullptr) { + return; + } + delete mlf; + *ptr_to_mlf = nullptr; + } + DEFAULT_CATCH() +} diff --git a/src/xrt/auxiliary/math/m_lowpass_float.h b/src/xrt/auxiliary/math/m_lowpass_float.h new file mode 100644 index 000000000..e43320a3f --- /dev/null +++ b/src/xrt/auxiliary/math/m_lowpass_float.h @@ -0,0 +1,99 @@ +// Copyright 2019, 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Low-pass IIR filter for floats - C wrapper + * @author Ryan Pavlik + * @ingroup aux_math + */ + +#pragma once + +#include "util/u_time.h" + +#include + +#ifdef __cplusplus +extern "C" { +#endif +/*! + * An IIR (low pass) filter for scalar float values. + * + * Wraps xrt::auxiliary::math::LowPassIIRFilter - see that if you need a different scalar type, or its related types if + * you want to filter a vector. + * + */ +struct m_lowpass_float; + +/*! + * Constructor + * + * @param cutoff_hz A cutoff frequency in Hertz: signal changes much + * lower in frequency will be passed through the filter, while signal + * changes much higher in frequency will be blocked. + * + * @public @memberof m_lowpass_float + */ +struct m_lowpass_float * +m_lowpass_float_create(float cutoff_hz); + + +/*! + * Filter a sample + * + * @param mlf self-pointer + * @param sample The value to filter + * @param timestamp_ns The time that this sample was measured. + * + * @public @memberof m_lowpass_float + */ +void +m_lowpass_float_add_sample(struct m_lowpass_float *mlf, float sample, timepoint_ns timestamp_ns); + +/*! + * Access the filtered value. + * + * Probably 0 or other meaningless value if it's not initialized: see @ref m_lowpass_float_is_initialized + * + * @param mlf self-pointer + * + * @public @memberof m_lowpass_float + */ +float +m_lowpass_float_get_state(const struct m_lowpass_float *mlf); + +/*! + * Access the time of last update + * + * @param mlf self-pointer + * + * @public @memberof m_lowpass_float + */ +timepoint_ns +m_lowpass_float_get_timestamp_ns(const struct m_lowpass_float *mlf); + +/*! + * Access whether we have initialized state. + * + * @param mlf self-pointer + * + * @public @memberof m_lowpass_float + */ +bool +m_lowpass_float_is_initialized(const struct m_lowpass_float *mlf); + +/*! + * Destroy a lowpass integer filter. + * + * Does null checks. + * + * @param ptr_to_mlf Address of your lowpass integer filter. Will be set to zero. + * + * @public @memberof m_lowpass_float + */ +void +m_lowpass_float_destroy(struct m_lowpass_float **ptr_to_mlf); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/auxiliary/meson.build b/src/xrt/auxiliary/meson.build index c9def80b1..5946d7acd 100644 --- a/src/xrt/auxiliary/meson.build +++ b/src/xrt/auxiliary/meson.build @@ -180,6 +180,8 @@ lib_aux_math = static_library( 'math/m_imu_3dof.h', 'math/m_imu_pre.c', 'math/m_imu_pre.h', + 'math/m_lowpass_float.cpp', + 'math/m_lowpass_float.h', 'math/m_lowpass_float.hpp', 'math/m_lowpass_float_vector.hpp', 'math/m_lowpass_integer.cpp', diff --git a/tests/tests_lowpass_float.cpp b/tests/tests_lowpass_float.cpp index 90c917544..534ffe55b 100644 --- a/tests/tests_lowpass_float.cpp +++ b/tests/tests_lowpass_float.cpp @@ -7,6 +7,7 @@ */ #include +#include #include "catch/catch.hpp" @@ -73,3 +74,69 @@ TEMPLATE_TEST_CASE("LowPassIIRFilter", "", float, double) } } } + +TEST_CASE("m_lowpass_float") +{ + + struct m_lowpass_float *filter = m_lowpass_float_create(100); + CHECK(filter); + + CHECK_FALSE(m_lowpass_float_is_initialized(filter)); + + timepoint_ns now = InitialTime; + + m_lowpass_float_add_sample(filter, InitialState, now); + REQUIRE(m_lowpass_float_is_initialized(filter)); + CHECK(m_lowpass_float_get_state(filter) == InitialState); + + m_lowpass_float_add_sample(filter, InitialState, now); + CHECK(m_lowpass_float_get_state(filter) == InitialState); + CHECK(m_lowpass_float_get_timestamp_ns(filter) == now); + CHECK(m_lowpass_float_is_initialized(filter)); + + auto prev = m_lowpass_float_get_state(filter); + SECTION("Increase") + { + constexpr auto newTarget = InitialState * 2; + for (int i = 0; i < 20; ++i) { + now += StepSize; + m_lowpass_float_add_sample(filter, newTarget, now); + REQUIRE(m_lowpass_float_is_initialized(filter)); + CHECK(m_lowpass_float_get_timestamp_ns(filter) == now); + // not going to exceed this + if (prev == newTarget) { + REQUIRE(m_lowpass_float_get_state(filter) == prev); + } else { + REQUIRE(m_lowpass_float_get_state(filter) > prev); + prev = m_lowpass_float_get_state(filter); + } + } + } + + SECTION("Decrease") + { + constexpr auto newTarget = InitialState / 2; + for (int i = 0; i < 20; ++i) { + now += StepSize; + m_lowpass_float_add_sample(filter, newTarget, now); + REQUIRE(m_lowpass_float_is_initialized(filter)); + CHECK(m_lowpass_float_get_timestamp_ns(filter) == now); + if (prev == newTarget) { + REQUIRE(m_lowpass_float_get_state(filter) == newTarget); + } else { + REQUIRE(m_lowpass_float_get_state(filter) < prev); + prev = m_lowpass_float_get_state(filter); + } + } + } + SECTION("Stay Same") + { + for (int i = 0; i < 20; ++i) { + now += StepSize; + m_lowpass_float_add_sample(filter, InitialState, now); + REQUIRE(m_lowpass_float_is_initialized(filter)); + CHECK(m_lowpass_float_get_timestamp_ns(filter) == now); + REQUIRE(m_lowpass_float_get_state(filter) == InitialState); + } + } +}