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 index 7a568c8613..c26270fa49 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java @@ -246,6 +246,43 @@ public class LinearFilter { m_outputs.clear(); } + /** + * Resets the filter state, initializing internal buffers to the provided values. + * + *

These are the expected lengths of the buffers, depending on what type of linear filter used: + * + * + * + * + * + * + * + * + * + * + *
TypeInput Buffer LengthOutput Buffer Length
Unspecifiedlength of {@code ffGains}length of {@code fbGains}
Single Pole IIR11
High-Pass21
Moving Average{@code taps}0
Finite Differencelength of {@code stencil}0
Backward Finite Difference{@code samples}0
+ * + * @param inputBuffer Values to initialize input buffer. + * @param outputBuffer Values to initialize output buffer. + * @throws IllegalArgumentException if length of inputBuffer or outputBuffer does not match the + * length of ffGains and fbGains provided in the constructor. + */ + public void reset(double[] inputBuffer, double[] outputBuffer) { + // Clear buffers + reset(); + + if (inputBuffer.length != m_inputGains.length || outputBuffer.length != m_outputGains.length) { + throw new IllegalArgumentException("Incorrect length of inputBuffer or outputBuffer"); + } + + for (double input : inputBuffer) { + m_inputs.addFirst(input); + } + for (double output : outputBuffer) { + m_outputs.addFirst(output); + } + } + /** * Calculates the next value of the filter. * diff --git a/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/wpimath/src/main/native/include/frc/filter/LinearFilter.h index fc558dfd58..051a6ddd26 100644 --- a/wpimath/src/main/native/include/frc/filter/LinearFilter.h +++ b/wpimath/src/main/native/include/frc/filter/LinearFilter.h @@ -271,6 +271,75 @@ class LinearFilter { std::fill(m_outputs.begin(), m_outputs.end(), T{0.0}); } + /** + * Resets the filter state, initializing internal buffers to the provided + * values. + * + * These are the expected lengths of the buffers, depending on what type of + * linear filter used: + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
TypeInput Buffer SizeOutput Buffer Size
Unspecifiedsize of {@code ffGains}size of {@code fbGains}
Single Pole IIR11
High-Pass21
Moving Average{@code taps}0
Finite Differencesize of {@code stencil}0
Backward Finite Difference{@code Samples}0
+ * + * @param inputBuffer Values to initialize input buffer. + * @param outputBuffer Values to initialize output buffer. + * @throws std::runtime_error if size of inputBuffer or outputBuffer does not + * match the size of ffGains and fbGains provided in the constructor. + */ + void Reset(std::span inputBuffer, + std::span outputBuffer) { + // Clear buffers + Reset(); + + if (inputBuffer.size() != m_inputGains.size() || + outputBuffer.size() != m_outputGains.size()) { + throw std::runtime_error( + "Incorrect length of inputBuffer or outputBuffer"); + } + + for (size_t i = 0; i < inputBuffer.size(); ++i) { + m_inputs.push_front(inputBuffer[i]); + } + for (size_t i = 0; i < outputBuffer.size(); ++i) { + m_outputs.push_front(outputBuffer[i]); + } + } + /** * Calculates the next value of the filter. *