From 65cc85f68d3e09c08ce0eb6efd0917018beb45d5 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 4 Dec 2017 20:12:06 -0800 Subject: [PATCH] Add reference constructors/factory methods to LinearDigitalFilter (#810) Composition is less verbose with references than with smart pointers. --- .../src/main/native/cpp/Filters/Filter.cpp | 8 ++- .../cpp/Filters/LinearDigitalFilter.cpp | 70 +++++++++++++++++++ .../src/main/native/include/Filters/Filter.h | 1 + .../include/Filters/LinearDigitalFilter.h | 7 ++ 4 files changed, 85 insertions(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/Filters/Filter.cpp b/wpilibc/src/main/native/cpp/Filters/Filter.cpp index 2c8d5cf2fa..a8bb08438d 100644 --- a/wpilibc/src/main/native/cpp/Filters/Filter.cpp +++ b/wpilibc/src/main/native/cpp/Filters/Filter.cpp @@ -7,9 +7,15 @@ #include "Filters/Filter.h" +#include "Base.h" + using namespace frc; -Filter::Filter(std::shared_ptr source) { m_source = source; } +Filter::Filter(PIDSource& source) + : m_source(std::shared_ptr(&source, NullDeleter())) {} + +Filter::Filter(std::shared_ptr source) + : m_source(std::move(source)) {} void Filter::SetPIDSourceType(PIDSourceType pidSource) { m_source->SetPIDSourceType(pidSource); diff --git a/wpilibc/src/main/native/cpp/Filters/LinearDigitalFilter.cpp b/wpilibc/src/main/native/cpp/Filters/LinearDigitalFilter.cpp index a2e4eee6c3..53104ec066 100644 --- a/wpilibc/src/main/native/cpp/Filters/LinearDigitalFilter.cpp +++ b/wpilibc/src/main/native/cpp/Filters/LinearDigitalFilter.cpp @@ -12,6 +12,22 @@ using namespace frc; +/** + * Create a linear FIR or IIR filter. + * + * @param source The PIDSource object that is used to get values + * @param ffGains The "feed forward" or FIR gains + * @param fbGains The "feed back" or IIR gains + */ +LinearDigitalFilter::LinearDigitalFilter(PIDSource& source, + llvm::ArrayRef ffGains, + llvm::ArrayRef fbGains) + : Filter(source), + m_inputs(ffGains.size()), + m_outputs(fbGains.size()), + m_inputGains(ffGains), + m_outputGains(fbGains) {} + /** * Create a linear FIR or IIR filter. * @@ -28,6 +44,60 @@ LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, m_inputGains(ffGains), m_outputGains(fbGains) {} +/** + * Creates a one-pole IIR low-pass filter of the form:
+ * y[n] = (1 - gain) * x[n] + gain * y[n-1]
+ * where gain = e-dt / T, T is the time constant in seconds + * + * This filter is stable for time constants greater than zero. + * + * @param source The PIDSource object that is used to get values + * @param timeConstant The discrete-time time constant in seconds + * @param period The period in seconds between samples taken by the user + */ +LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(PIDSource& source, + double timeConstant, + double period) { + double gain = std::exp(-period / timeConstant); + return LinearDigitalFilter(source, {1.0 - gain}, {-gain}); +} + +/** + * Creates a first-order high-pass filter of the form:
+ * y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]
+ * where gain = e-dt / T, T is the time constant in seconds + * + * This filter is stable for time constants greater than zero. + * + * @param source The PIDSource object that is used to get values + * @param timeConstant The discrete-time time constant in seconds + * @param period The period in seconds between samples taken by the user + */ +LinearDigitalFilter LinearDigitalFilter::HighPass(PIDSource& source, + double timeConstant, + double period) { + double gain = std::exp(-period / timeConstant); + return LinearDigitalFilter(source, {gain, -gain}, {-gain}); +} + +/** + * Creates a K-tap FIR moving average filter of the form:
+ * y[n] = 1/k * (x[k] + x[k-1] + … + x[0]) + * + * This filter is always stable. + * + * @param source The PIDSource object that is used to get values + * @param taps The number of samples to average over. Higher = smoother but + * slower + */ +LinearDigitalFilter LinearDigitalFilter::MovingAverage(PIDSource& source, + int taps) { + assert(taps > 0); + + std::vector gains(taps, 1.0 / taps); + return LinearDigitalFilter(source, gains, {}); +} + /** * Creates a one-pole IIR low-pass filter of the form:
* y[n] = (1 - gain) * x[n] + gain * y[n-1]
diff --git a/wpilibc/src/main/native/include/Filters/Filter.h b/wpilibc/src/main/native/include/Filters/Filter.h index f4c4d133d3..1fec461eab 100644 --- a/wpilibc/src/main/native/include/Filters/Filter.h +++ b/wpilibc/src/main/native/include/Filters/Filter.h @@ -18,6 +18,7 @@ namespace frc { */ class Filter : public PIDSource { public: + explicit Filter(PIDSource& source); explicit Filter(std::shared_ptr source); virtual ~Filter() = default; diff --git a/wpilibc/src/main/native/include/Filters/LinearDigitalFilter.h b/wpilibc/src/main/native/include/Filters/LinearDigitalFilter.h index b5b8bfbbe8..d294c159ee 100644 --- a/wpilibc/src/main/native/include/Filters/LinearDigitalFilter.h +++ b/wpilibc/src/main/native/include/Filters/LinearDigitalFilter.h @@ -69,11 +69,18 @@ namespace frc { */ class LinearDigitalFilter : public Filter { public: + LinearDigitalFilter(PIDSource& source, llvm::ArrayRef ffGains, + llvm::ArrayRef fbGains); LinearDigitalFilter(std::shared_ptr source, llvm::ArrayRef ffGains, llvm::ArrayRef fbGains); // Static methods to create commonly used filters + static LinearDigitalFilter SinglePoleIIR(PIDSource& source, + double timeConstant, double period); + static LinearDigitalFilter HighPass(PIDSource& source, double timeConstant, + double period); + static LinearDigitalFilter MovingAverage(PIDSource& source, int taps); static LinearDigitalFilter SinglePoleIIR(std::shared_ptr source, double timeConstant, double period); static LinearDigitalFilter HighPass(std::shared_ptr source,