Add reference constructors/factory methods to LinearDigitalFilter (#810)

Composition is less verbose with references than with smart pointers.

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<PIDSource> source) { m_source = source; }
+Filter::Filter(PIDSource& source)
+    : m_source(std::shared_ptr<PIDSource>(&source, NullDeleter<PIDSource>())) {}
+
+Filter::Filter(std::shared_ptr<PIDSource> source)
+    : m_source(std::move(source)) {}
 
 void Filter::SetPIDSourceType(PIDSourceType pidSource) {
   m_source->SetPIDSourceType(pidSource);

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<double> ffGains,
+                                         llvm::ArrayRef<double> 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<PIDSource> source,
       m_inputGains(ffGains),
       m_outputGains(fbGains) {}
 
+/**
+ * Creates a one-pole IIR low-pass filter of the form:<br>
+ *   y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
+ * where gain = e<sup>-dt / T</sup>, 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:<br>
+ *   y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
+ * where gain = e<sup>-dt / T</sup>, 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:<br>
+ *   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<double> gains(taps, 1.0 / taps);
+  return LinearDigitalFilter(source, gains, {});
+}
+
 /**
  * Creates a one-pole IIR low-pass filter of the form:<br>
  *   y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>