[wpiutil] Improve wpi::circular_buffer iterators (#3410)

The implementation of wpi::circular_buffer has been effectively replaced
with a dynamically sized copy of wpi::static_circular_buffer with a
resize() member function.

wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java

@@ -150,7 +150,9 @@ public class LinearFilter {
     double retVal = 0.0;
 
     // Rotate the inputs
-    m_inputs.addFirst(input);
+    if (m_inputGains.length > 0) {
+      m_inputs.addFirst(input);
+    }
 
     // Calculate the new value
     for (int i = 0; i < m_inputGains.length; i++) {
@@ -161,7 +163,9 @@ public class LinearFilter {
     }
 
     // Rotate the outputs
-    m_outputs.addFirst(retVal);
+    if (m_outputGains.length > 0) {
+      m_outputs.addFirst(retVal);
+    }
 
     return retVal;
   }

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

@@ -4,9 +4,10 @@
 
 #pragma once
 
-#include <cassert>
+#include <algorithm>
 #include <cmath>
 #include <initializer_list>
+#include <stdexcept>
 #include <vector>
 
 #include <wpi/ArrayRef.h>
@@ -81,6 +82,13 @@ class LinearFilter {
         m_outputs(fbGains.size()),
         m_inputGains(ffGains),
         m_outputGains(fbGains) {
+    for (size_t i = 0; i < ffGains.size(); ++i) {
+      m_inputs.emplace_front(0.0);
+    }
+    for (size_t i = 0; i < fbGains.size(); ++i) {
+      m_outputs.emplace_front(0.0);
+    }
+
     static int instances = 0;
     instances++;
     wpi::math::MathSharedStore::ReportUsage(
@@ -148,7 +156,9 @@ class LinearFilter {
    *             slower
    */
   static LinearFilter<T> MovingAverage(int taps) {
-    assert(taps > 0);
+    if (taps <= 0) {
+      throw std::runtime_error("Number of taps must be greater than zero.");
+    }
 
     std::vector<double> gains(taps, 1.0 / taps);
     return LinearFilter(gains, {});
@@ -158,8 +168,8 @@ class LinearFilter {
    * Reset the filter state.
    */
   void Reset() {
-    m_inputs.reset();
-    m_outputs.reset();
+    std::fill(m_inputs.begin(), m_inputs.end(), T{0.0});
+    std::fill(m_outputs.begin(), m_outputs.end(), T{0.0});
   }
 
   /**
@@ -170,21 +180,25 @@ class LinearFilter {
    * @return The filtered value at this step
    */
   T Calculate(T input) {
-    T retVal = T(0.0);
+    T retVal{0.0};
 
     // Rotate the inputs
-    m_inputs.push_front(input);
+    if (m_inputGains.size() > 0) {
+      m_inputs.push_front(input);
+    }
 
     // Calculate the new value
-    for (size_t i = 0; i < m_inputGains.size(); i++) {
+    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++) {
+    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);
+    if (m_outputGains.size() > 0) {
+      m_outputs.push_front(retVal);
+    }
 
     return retVal;
   }

wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp

@@ -5,7 +5,6 @@
 #include "frc/LinearFilter.h"  // NOLINT(build/include_order)
 
 #include <cmath>
-#include <memory>
 #include <random>
 
 #include <wpi/numbers>
@@ -14,26 +13,12 @@
 #include "units/time.h"
 
 // Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr auto kFilterStep = 5_ms;
+static constexpr auto kFilterTime = 2_s;
 static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
 static constexpr int32_t kMovAvgTaps = 6;
 
-enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
-
-std::ostream& operator<<(std::ostream& os,
-                         const LinearFilterNoiseTestType& type) {
-  switch (type) {
-    case TEST_SINGLE_POLE_IIR:
-      os << "LinearFilter SinglePoleIIR";
-      break;
-    case TEST_MOVAVG:
-      os << "LinearFilter MovingAverage";
-      break;
-  }
-
-  return os;
-}
+enum LinearFilterNoiseTestType { kTestSinglePoleIIR, kTestMovAvg };
 
 static double GetData(double t) {
   return 100.0 * std::sin(2.0 * wpi::numbers::pi * t);
@@ -42,24 +27,17 @@ static double GetData(double t) {
 class LinearFilterNoiseTest
     : public testing::TestWithParam<LinearFilterNoiseTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter<double>> m_filter;
-
-  void SetUp() override {
+  frc::LinearFilter<double> m_filter = [=] {
     switch (GetParam()) {
-      case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                                     kFilterStep));
+      case kTestSinglePoleIIR:
+        return frc::LinearFilter<double>::SinglePoleIIR(
+            kSinglePoleIIRTimeConstant, kFilterStep);
         break;
-      }
-
-      case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+      default:
+        return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
         break;
-      }
     }
-  }
+  }();
 };
 
 /**
@@ -76,7 +54,7 @@ TEST_P(LinearFilterNoiseTest, NoiseReduce) {
   for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
     double theory = GetData(t.to<double>());
     double noise = distr(gen);
-    filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
+    filterError += std::abs(m_filter.Calculate(theory + noise) - theory);
     noiseGenError += std::abs(noise - theory);
   }
 
@@ -88,4 +66,4 @@ TEST_P(LinearFilterNoiseTest, NoiseReduce) {
 }
 
 INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
-                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));
+                         testing::Values(kTestSinglePoleIIR, kTestMovAvg));

wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp

@@ -15,8 +15,8 @@
 #include "units/time.h"
 
 // Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr auto kFilterStep = 5_ms;
+static constexpr auto kFilterTime = 2_s;
 static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
 static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
 static constexpr double kHighPassTimeConstant = 0.006631;
@@ -25,32 +25,12 @@ static constexpr int32_t kMovAvgTaps = 6;
 static constexpr double kMovAvgExpectedOutput = -10.191644;
 
 enum LinearFilterOutputTestType {
-  TEST_SINGLE_POLE_IIR,
-  TEST_HIGH_PASS,
-  TEST_MOVAVG,
-  TEST_PULSE
+  kTestSinglePoleIIR,
+  kTestHighPass,
+  kTestMovAvg,
+  kTestPulse
 };
 
-std::ostream& operator<<(std::ostream& os,
-                         const LinearFilterOutputTestType& type) {
-  switch (type) {
-    case TEST_SINGLE_POLE_IIR:
-      os << "LinearFilter SinglePoleIIR";
-      break;
-    case TEST_HIGH_PASS:
-      os << "LinearFilter HighPass";
-      break;
-    case TEST_MOVAVG:
-      os << "LinearFilter MovingAverage";
-      break;
-    case TEST_PULSE:
-      os << "LinearFilter Pulse";
-      break;
-  }
-
-  return os;
-}
-
 static double GetData(double t) {
   return 100.0 * std::sin(2.0 * wpi::numbers::pi * t) +
          20.0 * std::cos(50.0 * wpi::numbers::pi * t);
@@ -70,41 +50,48 @@ static double GetPulseData(double t) {
 class LinearFilterOutputTest
     : public testing::TestWithParam<LinearFilterOutputTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter<double>> m_filter;
+  frc::LinearFilter<double> m_filter = [=] {
+    switch (GetParam()) {
+      case kTestSinglePoleIIR:
+        return frc::LinearFilter<double>::SinglePoleIIR(
+            kSinglePoleIIRTimeConstant, kFilterStep);
+        break;
+      case kTestHighPass:
+        return frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+                                                   kFilterStep);
+        break;
+      case kTestMovAvg:
+        return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
+        break;
+      default:
+        return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
+        break;
+    }
+  }();
   std::function<double(double)> m_data;
   double m_expectedOutput = 0.0;
 
-  void SetUp() override {
+  LinearFilterOutputTest() {
     switch (GetParam()) {
-      case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                                     kFilterStep));
+      case kTestSinglePoleIIR: {
         m_data = GetData;
         m_expectedOutput = kSinglePoleIIRExpectedOutput;
         break;
       }
 
-      case TEST_HIGH_PASS: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
-                                                kFilterStep));
+      case kTestHighPass: {
         m_data = GetData;
         m_expectedOutput = kHighPassExpectedOutput;
         break;
       }
 
-      case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+      case kTestMovAvg: {
         m_data = GetData;
         m_expectedOutput = kMovAvgExpectedOutput;
         break;
       }
 
-      case TEST_PULSE: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
+      case kTestPulse: {
         m_data = GetPulseData;
         m_expectedOutput = 0.0;
         break;
@@ -119,7 +106,7 @@ class LinearFilterOutputTest
 TEST_P(LinearFilterOutputTest, Output) {
   double filterOutput = 0.0;
   for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
-    filterOutput = m_filter->Calculate(m_data(t.to<double>()));
+    filterOutput = m_filter.Calculate(m_data(t.to<double>()));
   }
 
   RecordProperty("LinearFilterOutput", filterOutput);
@@ -129,5 +116,5 @@ TEST_P(LinearFilterOutputTest, Output) {
 }
 
 INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
-                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
-                                         TEST_MOVAVG, TEST_PULSE));
+                         testing::Values(kTestSinglePoleIIR, kTestHighPass,
+                                         kTestMovAvg, kTestPulse));