[wpimath] LinearFilter lastValue(): Fix runtime exception (#6713)

wpilibsuite · Jun 8, 2024 · 0606da6 · 0606da6
1 parent 1c884b2
commit 0606da6
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 deletions.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -56,6 +56,7 @@ public class LinearFilter {
   private final DoubleCircularBuffer m_outputs;
   private final double[] m_inputGains;
   private final double[] m_outputGains;
+  private double m_lastOutput;
 
   private static int instances;
 
@@ -310,6 +311,7 @@ public double calculate(double input) {
       m_outputs.addFirst(retVal);
     }
 
+    m_lastOutput = retVal;
     return retVal;
   }
 
@@ -319,7 +321,7 @@ public double calculate(double input) {
    * @return The last value.
    */
   public double lastValue() {
-    return m_outputs.getFirst();
+    return m_lastOutput;
   }
 
   /**

diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -367,6 +367,7 @@ class LinearFilter {
       m_outputs.push_front(retVal);
     }
 
+    m_lastOutput = retVal;
     return retVal;
   }
 
@@ -375,13 +376,14 @@ class LinearFilter {
    *
    * @return The last value.
    */
-  T LastValue() const { return m_outputs.front(); }
+  T LastValue() const { return m_lastOutput; }
 
  private:
   wpi::circular_buffer<T> m_inputs;
   wpi::circular_buffer<T> m_outputs;
   std::vector<double> m_inputGains;
   std::vector<double> m_outputGains;
+  T m_lastOutput{0.0};
 
   /**
    * Factorial of n.