diff --git a/src/modm/math/filter/moving_average.hpp b/src/modm/math/filter/moving_average.hpp index 78a80746d4..4e5b576daa 100644 --- a/src/modm/math/filter/moving_average.hpp +++ b/src/modm/math/filter/moving_average.hpp @@ -7,6 +7,7 @@ * Copyright (c) 2015, Thorsten Lajewski * Copyright (c) 2017, Daniel Krebs * Copyright (c) 2018, Christopher Durand + * Copyright (c) 2022, Thomas Sommer * * This file is part of the modm project. * @@ -15,111 +16,94 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ // ---------------------------------------------------------------------------- - -#ifndef MODM_MOVING_AVERAGE_HPP -#define MODM_MOVING_AVERAGE_HPP +#pragma once #include #include -#include - -namespace modm -{ - namespace filter{ - /** - * \brief Moving average filter - * - * Calculates the average of N newest values, i.a. the sum of the last N - * values have been passed to update(...), divided by N. If less than N - * values have been passed to the filter, the division factor is still N, - * so missing values are assumed to be zero. - * - * For integer types this implementation stores the current sum of all values in the buffer - * and updates this value with every call of update() by subtracting - * the overwritten buffer index and adding the new one. - * - * Due to numerical instabillity for floating value types, inside the update function - * the sum has to be recalculated making it less efficient. - * - * The internal sum is always up to date and the getValue() - * method consists of only one division. - * - * \warning Input range is limited by the following equation - * \code N * input::maxValue < T::maxValue \endcode - * The sum off the last N input values must not be greater than - * the maximum value of T, otherwise an overflow will occur. - * - * \tparam T Input type - * \tparam N Number of samples (maximum is 65356 or 2**16) - * - * \ingroup modm_math_filter - */ - template - class MovingAverage - { - private: - using Index = std::conditional_t< - (N >= 256), - uint_fast16_t, - uint_fast8_t - >; - - public: - MovingAverage(const T& initialValue = 0); - /// Append new value - void - update(const T& input); +#include +#include +#include +#include - /// Get filtered value - const T - getValue() const; +#include - private: - Index index; - T buffer[N]; - T sum; - }; - } -} - -// ---------------------------------------------------------------------------- -template -modm::filter::MovingAverage::MovingAverage(const T& initialValue) : - index(0), sum(N * initialValue) +namespace modm::filter { - for (Index i = 0; i < N; ++i) { - buffer[i] = initialValue; - } -} -// ---------------------------------------------------------------------------- -// TODO implementierung für float anpassen +/** + * \brief Moving average filter + * + * Calculates the average of N newest values, i.a. the sum of the last N + * values have been passed to update(...), divided by N. If less than N + * values have been passed to the filter, the division factor is still N, + * so missing values are assumed to be zero. + * + * For integer types this implementation stores the current sum of all values in the buffer + * and updates this value with every call of update() by subtracting + * the overwritten buffer index and adding the new one. + * + * Due to numerical instabillity for floating value types, inside the update function + * the sum has to be recalculated making it less efficient. + * + * The internal sum is always up to date and the getValue() + * method consists of only one division. + * + * \warning Input range is limited by the following equation + * \code N * input::maxValue < T::maxValue \endcode + * The sum off the last N input values must not be greater than + * the maximum value of T, otherwise an overflow will occur. + * + * \tparam T Input type + * \tparam N Number of samples + * + * \ingroup modm_math_filter + */ template -void -modm::filter::MovingAverage::update(const T& input) +class MovingAverage { - sum -= buffer[index]; - sum += input; - - buffer[index] = input; - - index++; - if (index >= N) { - index = 0; +public: + constexpr MovingAverage(T initialValue = 0) + { + reset(initialValue); } -} -// ----------------------------------------------------------------------------- + /// Reset whole buffer to 'input' + /// Next call of getValue() returns 'input' + constexpr void reset(T input) + { + std::fill(std::begin(buffer), std::end(buffer), input); + sum = N * input; + } + /// Append new value + constexpr void + update(T input) + { + if constexpr(std::floating_point) { + buffer[index] = input; + sum = std::accumulate(std::begin(buffer), std::end(buffer), 0); + } else { + sum -= buffer[index]; + sum += input; + buffer[index] = input; + } + + if (++index == N) + index = 0; + } -template -const T -modm::filter::MovingAverage::getValue() const -{ - return (sum / static_cast(N)); -} + /// Get filtered value + T + constexpr getValue() const + { + return (sum / static_cast(N)); + } +private: + least_uint index{0}; + T buffer[N]; + T sum; +}; -#include "moving_average_float_impl.hpp" -#endif // MODM_MOVING_AVERAGE_HPP +} // namespace modm::filter diff --git a/src/modm/math/filter/moving_average_float_impl.hpp b/src/modm/math/filter/moving_average_float_impl.hpp deleted file mode 100644 index d56ae6f7de..0000000000 --- a/src/modm/math/filter/moving_average_float_impl.hpp +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright (c) 2009, Martin Rosekeit - * Copyright (c) 2009-2010, Fabian Greif - * Copyright (c) 2012, Niklas Hauser - * Copyright (c) 2015, Thorsten Lajewski - * Copyright (c) 2018, Christopher Durand - * - * This file is part of the modm project. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this - * file, You can obtain one at http://mozilla.org/MPL/2.0/. - */ -// ---------------------------------------------------------------------------- - -#ifndef MODM_MOVING_AVERAGE_HPP - #error "Don't include this file directly, use 'moving_average.hpp' instead!" -#endif - -namespace modm -{ - namespace filter{ - template - class MovingAverage - { - private: - using Index = std::conditional_t< - (N >= 256), - uint_fast16_t, - uint_fast8_t - >; - - public: - - MovingAverage(const double& initialValue = 0); - - /// Append new value - void - update(const double& input); - - /// Get filtered value - double - getValue() const; - - private: - Index index; - double buffer[N]; - double sum; - }; - } -} - -//--------------------------------------------------------------------------------- - -template -modm::filter::MovingAverage::MovingAverage(const double& initialValue) : - index(0), sum(N * initialValue) -{ - for (Index i = 0; i < N; ++i) { - buffer[i] = initialValue; - } -} - - -//--------------------------------------------------------------------------------- -template -void -modm::filter::MovingAverage::update(const double& input){ - buffer[index] = input; - - index++; - if (index >= N) { - index = 0; - } - - sum = 0; - for (Index i = 0; i < N; ++i) { - sum+= buffer[i]; - } - -} - -// ------------------------------------------------------------------------------ -template -double -modm::filter::MovingAverage::getValue() const -{ - return (sum / N); -} diff --git a/test/modm/math/filter/moving_average_test.cpp b/test/modm/math/filter/moving_average_test.cpp index 10eb6360e3..332d3b5af1 100644 --- a/test/modm/math/filter/moving_average_test.cpp +++ b/test/modm/math/filter/moving_average_test.cpp @@ -107,6 +107,20 @@ MovingAverageTest::testAverage() } } +void +MovingAverageTest::testReset() +{ + modm::filter::MovingAverage filter; + + for (uint_fast8_t i = 0; i < 4; ++i) + { + filter.update(data[i].input); + } + + filter.reset(42); + TEST_ASSERT_EQUALS(filter.getValue(), 42); +} + void MovingAverageTest::testFloatAverage() { @@ -117,3 +131,17 @@ MovingAverageTest::testFloatAverage() TEST_ASSERT_EQUALS_DELTA(filter.getValue(), dataF[i].output, double(1e-4)); } } + +void +MovingAverageTest::testFloatReset() +{ + modm::filter::MovingAverage filter; + + for (uint_fast8_t i = 0; i < 4; ++i) + { + filter.update(dataF[i].input); + } + + filter.reset(42.23); + TEST_ASSERT_EQUALS_FLOAT(filter.getValue(), 42.23); +} diff --git a/test/modm/math/filter/moving_average_test.hpp b/test/modm/math/filter/moving_average_test.hpp index 89e9faa78e..187f526af0 100644 --- a/test/modm/math/filter/moving_average_test.hpp +++ b/test/modm/math/filter/moving_average_test.hpp @@ -27,6 +27,12 @@ class MovingAverageTest : public unittest::TestSuite void testAverage(); + void + testReset(); + void testFloatAverage(); + + void + testFloatReset(); };