From 9a515c80f8486e67025f6bc98a1b0dcd9cfcaf33 Mon Sep 17 00:00:00 2001 From: Oblarg Date: Sun, 1 Dec 2019 02:12:02 -0500 Subject: [PATCH] Template C++ LinearFilter to work with unit types (#2142) --- wpilibc/src/main/native/cpp/LinearFilter.cpp | 70 ------------------- wpilibc/src/main/native/cpp/PIDBase.cpp | 4 +- .../main/native/include/frc/LinearFilter.h | 62 +++++++++++++--- wpilibc/src/main/native/include/frc/PIDBase.h | 2 +- .../test/native/cpp/LinearFilterNoiseTest.cpp | 12 ++-- .../native/cpp/LinearFilterOutputTest.cpp | 21 +++--- .../src/main/native/cpp/MotorEncoderTest.cpp | 5 +- 7 files changed, 76 insertions(+), 100 deletions(-) delete mode 100644 wpilibc/src/main/native/cpp/LinearFilter.cpp diff --git a/wpilibc/src/main/native/cpp/LinearFilter.cpp b/wpilibc/src/main/native/cpp/LinearFilter.cpp deleted file mode 100644 index 948ad5c1c1..0000000000 --- a/wpilibc/src/main/native/cpp/LinearFilter.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#include "frc/LinearFilter.h" - -#include -#include - -#include - -using namespace frc; - -LinearFilter::LinearFilter(wpi::ArrayRef ffGains, - wpi::ArrayRef fbGains) - : m_inputs(ffGains.size()), - m_outputs(fbGains.size()), - m_inputGains(ffGains), - m_outputGains(fbGains) { - static int instances = 0; - instances++; - HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances); -} - -LinearFilter LinearFilter::SinglePoleIIR(double timeConstant, - units::second_t period) { - double gain = std::exp(-period.to() / timeConstant); - return LinearFilter(1.0 - gain, -gain); -} - -LinearFilter LinearFilter::HighPass(double timeConstant, - units::second_t period) { - double gain = std::exp(-period.to() / timeConstant); - return LinearFilter({gain, -gain}, {-gain}); -} - -LinearFilter LinearFilter::MovingAverage(int taps) { - assert(taps > 0); - - std::vector gains(taps, 1.0 / taps); - return LinearFilter(gains, {}); -} - -void LinearFilter::Reset() { - m_inputs.reset(); - m_outputs.reset(); -} - -double LinearFilter::Calculate(double input) { - double retVal = 0.0; - - // Rotate the inputs - m_inputs.push_front(input); - - // Calculate the new value - for (size_t i = 0; i < m_inputGains.size(); i++) { - retVal += m_inputs[i] * m_inputGains[i]; - } - for (size_t i = 0; i < m_outputGains.size(); i++) { - retVal -= m_outputs[i] * m_outputGains[i]; - } - - // Rotate the outputs - m_outputs.push_front(retVal); - - return retVal; -} diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp index c328e4a042..c8f6fed3d5 100644 --- a/wpilibc/src/main/native/cpp/PIDBase.cpp +++ b/wpilibc/src/main/native/cpp/PIDBase.cpp @@ -35,7 +35,7 @@ PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source, m_F = Kf; m_pidInput = &source; - m_filter = LinearFilter::MovingAverage(1); + m_filter = LinearFilter::MovingAverage(1); m_pidOutput = &output; @@ -196,7 +196,7 @@ void PIDBase::SetPercentTolerance(double percent) { void PIDBase::SetToleranceBuffer(int bufLength) { std::scoped_lock lock(m_thisMutex); - m_filter = LinearFilter::MovingAverage(bufLength); + m_filter = LinearFilter::MovingAverage(bufLength); } bool PIDBase::OnTarget() const { diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h index 88e5222576..2b83a32d4e 100644 --- a/wpilibc/src/main/native/include/frc/LinearFilter.h +++ b/wpilibc/src/main/native/include/frc/LinearFilter.h @@ -7,9 +7,12 @@ #pragma once +#include +#include #include #include +#include #include #include #include @@ -66,6 +69,7 @@ namespace frc { * Combining this with Note 1 - the impetus is on YOU as a developer to make * sure Calculate() gets called at the desired, constant frequency! */ +template class LinearFilter { public: /** @@ -74,7 +78,15 @@ class LinearFilter { * @param ffGains The "feed forward" or FIR gains. * @param fbGains The "feed back" or IIR gains. */ - LinearFilter(wpi::ArrayRef ffGains, wpi::ArrayRef fbGains); + LinearFilter(wpi::ArrayRef ffGains, wpi::ArrayRef fbGains) + : m_inputs(ffGains.size()), + m_outputs(fbGains.size()), + m_inputGains(ffGains), + m_outputGains(fbGains) { + static int instances = 0; + instances++; + HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances); + } /** * Create a linear FIR or IIR filter. @@ -99,8 +111,11 @@ class LinearFilter { * @param period The period in seconds between samples taken by the * user. */ - static LinearFilter SinglePoleIIR(double timeConstant, - units::second_t period); + static LinearFilter SinglePoleIIR(double timeConstant, + units::second_t period) { + double gain = std::exp(-period.to() / timeConstant); + return LinearFilter(1.0 - gain, -gain); + } /** * Creates a first-order high-pass filter of the form:
@@ -113,7 +128,10 @@ class LinearFilter { * @param period The period in seconds between samples taken by the * user. */ - static LinearFilter HighPass(double timeConstant, units::second_t period); + static LinearFilter HighPass(double timeConstant, units::second_t period) { + double gain = std::exp(-period.to() / timeConstant); + return LinearFilter({gain, -gain}, {-gain}); + } /** * Creates a K-tap FIR moving average filter of the form:
@@ -124,12 +142,20 @@ class LinearFilter { * @param taps The number of samples to average over. Higher = smoother but * slower */ - static LinearFilter MovingAverage(int taps); + static LinearFilter MovingAverage(int taps) { + assert(taps > 0); + + std::vector gains(taps, 1.0 / taps); + return LinearFilter(gains, {}); + } /** * Reset the filter state. */ - void Reset(); + void Reset() { + m_inputs.reset(); + m_outputs.reset(); + } /** * Calculates the next value of the filter. @@ -138,11 +164,29 @@ class LinearFilter { * * @return The filtered value at this step */ - double Calculate(double input); + T Calculate(T input) { + T retVal = T(0.0); + + // Rotate the inputs + m_inputs.push_front(input); + + // Calculate the new value + for (size_t i = 0; i < m_inputGains.size(); i++) { + retVal += m_inputs[i] * m_inputGains[i]; + } + for (size_t i = 0; i < m_outputGains.size(); i++) { + retVal -= m_outputs[i] * m_outputGains[i]; + } + + // Rotate the outputs + m_outputs.push_front(retVal); + + return retVal; + } private: - wpi::circular_buffer m_inputs; - wpi::circular_buffer m_outputs; + wpi::circular_buffer m_inputs; + wpi::circular_buffer m_outputs; std::vector m_inputGains; std::vector m_outputGains; }; diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h index 098718f349..513d46b0c0 100644 --- a/wpilibc/src/main/native/include/frc/PIDBase.h +++ b/wpilibc/src/main/native/include/frc/PIDBase.h @@ -402,7 +402,7 @@ class PIDBase : public PIDInterface, double m_error = 0; double m_result = 0; - LinearFilter m_filter{{}, {}}; + LinearFilter m_filter{{}, {}}; }; } // namespace frc diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp index 888d183b69..0bb10021c7 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp +++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp @@ -45,20 +45,20 @@ static double GetData(double t) { class LinearFilterNoiseTest : public testing::TestWithParam { protected: - std::unique_ptr m_filter; + std::unique_ptr> m_filter; void SetUp() override { switch (GetParam()) { case TEST_SINGLE_POLE_IIR: { - m_filter = std::make_unique( - frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant, - kFilterStep)); + m_filter = std::make_unique>( + frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant, + kFilterStep)); break; } case TEST_MOVAVG: { - m_filter = std::make_unique( - frc::LinearFilter::MovingAverage(kMovAvgTaps)); + m_filter = std::make_unique>( + frc::LinearFilter::MovingAverage(kMovAvgTaps)); break; } } diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp index 1f1476c5cb..8db91b3937 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp @@ -73,40 +73,41 @@ static double GetPulseData(double t) { class LinearFilterOutputTest : public testing::TestWithParam { protected: - std::unique_ptr m_filter; + std::unique_ptr> m_filter; std::function m_data; double m_expectedOutput = 0.0; void SetUp() override { switch (GetParam()) { case TEST_SINGLE_POLE_IIR: { - m_filter = std::make_unique( - frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant, - kFilterStep)); + m_filter = std::make_unique>( + frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant, + kFilterStep)); m_data = GetData; m_expectedOutput = kSinglePoleIIRExpectedOutput; break; } case TEST_HIGH_PASS: { - m_filter = std::make_unique( - frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep)); + m_filter = std::make_unique>( + frc::LinearFilter::HighPass(kHighPassTimeConstant, + kFilterStep)); m_data = GetData; m_expectedOutput = kHighPassExpectedOutput; break; } case TEST_MOVAVG: { - m_filter = std::make_unique( - frc::LinearFilter::MovingAverage(kMovAvgTaps)); + m_filter = std::make_unique>( + frc::LinearFilter::MovingAverage(kMovAvgTaps)); m_data = GetData; m_expectedOutput = kMovAvgExpectedOutput; break; } case TEST_PULSE: { - m_filter = std::make_unique( - frc::LinearFilter::MovingAverage(kMovAvgTaps)); + m_filter = std::make_unique>( + frc::LinearFilter::MovingAverage(kMovAvgTaps)); m_data = GetPulseData; m_expectedOutput = 0.0; break; diff --git a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp index ca6653cd94..e368fc2e4c 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp @@ -48,7 +48,7 @@ class MotorEncoderTest : public testing::TestWithParam { protected: SpeedController* m_speedController; Encoder* m_encoder; - LinearFilter* m_filter; + LinearFilter* m_filter; void SetUp() override { switch (GetParam()) { @@ -70,7 +70,8 @@ class MotorEncoderTest : public testing::TestWithParam { TestBench::kTalonEncoderChannelB); break; } - m_filter = new LinearFilter(LinearFilter::MovingAverage(50)); + m_filter = + new LinearFilter(LinearFilter::MovingAverage(50)); } void TearDown() override {