Template C++ LinearFilter to work with unit types (#2142)

wpilibc/src/main/native/cpp/LinearFilter.cpp

@@ -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 <cassert>
-#include <cmath>
-
-#include <hal/FRCUsageReporting.h>
-
-using namespace frc;
-
-LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
-                           wpi::ArrayRef<double> 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<double>() / timeConstant);
-  return LinearFilter(1.0 - gain, -gain);
-}
-
-LinearFilter LinearFilter::HighPass(double timeConstant,
-                                    units::second_t period) {
-  double gain = std::exp(-period.to<double>() / timeConstant);
-  return LinearFilter({gain, -gain}, {-gain});
-}
-
-LinearFilter LinearFilter::MovingAverage(int taps) {
-  assert(taps > 0);
-
-  std::vector<double> 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;
-}

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<double>::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<double>::MovingAverage(bufLength);
 }
 
 bool PIDBase::OnTarget() const {

wpilibc/src/main/native/include/frc/LinearFilter.h

@@ -7,9 +7,12 @@
 
 #pragma once
 
+#include <cassert>
+#include <cmath>
 #include <initializer_list>
 #include <vector>
 
+#include <hal/FRCUsageReporting.h>
 #include <units/units.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/circular_buffer.h>
@@ -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 T>
 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<double> ffGains, wpi::ArrayRef<double> fbGains);
+  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> 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<T> SinglePoleIIR(double timeConstant,
+                                       units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter(1.0 - gain, -gain);
+  }
 
   /**
    * Creates a first-order high-pass filter of the form:<br>
@@ -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<T> HighPass(double timeConstant, units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter({gain, -gain}, {-gain});
+  }
 
   /**
    * Creates a K-tap FIR moving average filter of the form:<br>
@@ -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<T> MovingAverage(int taps) {
+    assert(taps > 0);
+
+    std::vector<double> 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<double> m_inputs;
-  wpi::circular_buffer<double> m_outputs;
+  wpi::circular_buffer<T> m_inputs;
+  wpi::circular_buffer<T> m_outputs;
   std::vector<double> m_inputGains;
   std::vector<double> m_outputGains;
 };

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<double> m_filter{{}, {}};
 };
 
 }  // namespace frc

wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp

@@ -45,20 +45,20 @@ static double GetData(double t) {
 class LinearFilterNoiseTest
     : public testing::TestWithParam<LinearFilterNoiseTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter> m_filter;
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
 
   void SetUp() override {
     switch (GetParam()) {
       case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                             kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
         break;
       }
 
       case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         break;
       }
     }

wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp

@@ -73,40 +73,41 @@ static double GetPulseData(double t) {
 class LinearFilterOutputTest
     : public testing::TestWithParam<LinearFilterOutputTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter> m_filter;
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
   std::function<double(double)> m_data;
   double m_expectedOutput = 0.0;
 
   void SetUp() override {
     switch (GetParam()) {
       case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                             kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
         m_data = GetData;
         m_expectedOutput = kSinglePoleIIRExpectedOutput;
         break;
       }
 
       case TEST_HIGH_PASS: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+                                                kFilterStep));
         m_data = GetData;
         m_expectedOutput = kHighPassExpectedOutput;
         break;
       }
 
       case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         m_data = GetData;
         m_expectedOutput = kMovAvgExpectedOutput;
         break;
       }
 
       case TEST_PULSE: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         m_data = GetPulseData;
         m_expectedOutput = 0.0;
         break;

wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp

@@ -48,7 +48,7 @@ class MotorEncoderTest : public testing::TestWithParam<MotorEncoderTestType> {
  protected:
   SpeedController* m_speedController;
   Encoder* m_encoder;
-  LinearFilter* m_filter;
+  LinearFilter<double>* m_filter;
 
   void SetUp() override {
     switch (GetParam()) {
@@ -70,7 +70,8 @@ class MotorEncoderTest : public testing::TestWithParam<MotorEncoderTestType> {
                                 TestBench::kTalonEncoderChannelB);
         break;
     }
-    m_filter = new LinearFilter(LinearFilter::MovingAverage(50));
+    m_filter =
+        new LinearFilter<double>(LinearFilter<double>::MovingAverage(50));
   }
 
   void TearDown() override {