a/math: C wrapper for the float low-pass filter too.

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

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 <ryan.pavlik@collabora.com>
+ * @ingroup aux_math
+ */
+
+#include "m_lowpass_float.h"
+#include "m_lowpass_float.hpp"
+
+#include "util/u_logging.h"
+
+#include <memory>
+
+using xrt::auxiliary::math::LowPassIIRFilter;
+
+struct m_lowpass_float
+{
+	m_lowpass_float(float cutoff_hz) : filter(cutoff_hz) {}
+
+	LowPassIIRFilter<float> 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<m_lowpass_float>(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()
+}

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 <ryan.pavlik@collabora.com>
+ * @ingroup aux_math
+ */
+
+#pragma once
+
+#include "util/u_time.h"
+
+#include <stdint.h>
+
+#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

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',

tests/tests_lowpass_float.cpp

@@ -7,6 +7,7 @@
  */
 
 #include <math/m_lowpass_float.hpp>
+#include <math/m_lowpass_float.h>
 
 #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);
+		}
+	}
+}