Filter: added DerivativeFilter implementation

this adds a DerivativeFilter implementation of up to 10 points

libraries/Filter/DerivativeFilter.cpp

@@ -0,0 +1,89 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+// This is free software; you can redistribute it and/or modify it under
+// the terms of the GNU Lesser General Public License as published by the
+// Free Software Foundation; either version 2.1 of the License, or (at
+// your option) any later version.
+//
+
+/// @file	Derivative.cpp
+/// @brief	A class to implement a derivative (slope) filter
+/// See http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
+
+#include <FastSerial.h>
+#include <inttypes.h>
+#include <Filter.h>
+#include <DerivativeFilter.h>
+
+template <class T, class U, uint8_t FILTER_SIZE>
+float DerivativeFilter<T,U,FILTER_SIZE>::apply(T sample)
+{
+	float result = 0;
+    uint32_t tnow = millis();
+    float deltat = (tnow - _last_time) * 1.0e-3;
+    _last_time = tnow;
+	// call parent's apply function to get the sample into the array
+	FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
+
+	// increment the number of samples so far
+	_num_samples++;
+	if( _num_samples > FILTER_SIZE || _num_samples == 0 )
+		_num_samples = FILTER_SIZE;
+
+    // use f() to make the code match the maths a bit better. Note
+    // that unlike an average filter, we care about the order of the elements
+    #define f(i) FilterWithBuffer<T,FILTER_SIZE>::samples[(((FilterWithBuffer<T,FILTER_SIZE>::sample_index-1)-i)+FILTER_SIZE) % FILTER_SIZE]
+
+    // N in the paper is _num_samples-1
+    switch (FILTER_SIZE-1) {
+    case 1:
+        result = f(0) - f(1);
+        break;
+    case 2:
+        result = f(0) - f(2);
+        break;
+    case 3:
+        result = f(0) + f(1) - f(2) - f(3);
+        break;
+    case 4:
+        result = f(0) + 2*f(1) - 2*f(3) - f(4);
+        break;
+    case 5:
+        result = f(0) + 3*f(1) + 2*f(2) - 2*f(3) - 3*f(4) - f(5);
+        break;
+    case 6:
+        result = f(0) + 4*f(1) + 5*f(2) - 5*f(4) - 4*f(5) - f(6);
+        break;
+    case 7:
+        result = f(0) + 5*f(1) + 9*f(2) + 5*f(3) - 5*f(4) - 9*f(5) - 5*f(6) - f(7);
+        break;
+    case 8:
+        result = f(0) + 6*f(1) + 14*f(2) + 14*f(3) - 14*f(5) - 14*f(6) - 6*f(7) - f(8);
+        break;
+    case 9:
+        result = f(0) + 7*f(1) + 20*f(2) + 28*f(3) + 14*f(4) - 14*f(5) - 28*f(6) - 20*f(7) - 7*f(8) - f(9);
+        break;
+    default:
+        result = 0;
+        break;
+    }
+
+    result /= deltat * (1<<(uint16_t)(FILTER_SIZE-2));
+
+    return result;
+}
+
+// reset - clear all samples
+template <class T, class U, uint8_t FILTER_SIZE>
+void DerivativeFilter<T,U,FILTER_SIZE>::reset(void)
+{
+	// call parent's apply function to get the sample into the array
+	FilterWithBuffer<T,FILTER_SIZE>::reset();
+
+	// clear our variable
+	_num_samples = 0;
+}
+
+// add new instances as needed here
+template float DerivativeFilter<float,float,9>::apply(float sample);
+template void DerivativeFilter<float,float,9>::reset(void);

libraries/Filter/DerivativeFilter.h

@@ -0,0 +1,45 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+// This is free software; you can redistribute it and/or modify it under
+// the terms of the GNU Lesser General Public License as published by the
+// Free Software Foundation; either version 2.1 of the License, or (at
+// your option) any later version.
+//
+
+/// @file	Derivative.h
+/// @brief	A class to implement a derivative (slope) filter
+/// See http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
+
+#ifndef Derivative_h
+#define Derivative_h
+
+#include <inttypes.h>
+#include <Filter.h>
+#include <FilterWithBuffer.h>
+
+// 1st parameter <T> is the type of data being filtered.  
+// 2nd parameter <U> is a larger data type used during summation to prevent overflows
+// 3rd parameter <FILTER_SIZE> is the number of elements in the filter
+template <class T, class U, uint8_t FILTER_SIZE>
+class DerivativeFilter : public FilterWithBuffer<T,FILTER_SIZE>
+{
+  public:
+	// constructor
+	DerivativeFilter() : FilterWithBuffer<T,FILTER_SIZE>(), _num_samples(0) {};
+
+	// apply - Add a new raw value to the filter, retrieve the filtered result
+	virtual float apply(T sample);
+
+	// reset - clear the filter
+	virtual void reset();
+
+  private:
+    uint32_t _last_time;
+	uint8_t	_num_samples;	// the number of samples in the filter, maxes out at size of the filter
+};
+
+typedef DerivativeFilter<float,float,9> DerivativeFilterFloat_Size9;
+
+
+#endif // Derivative_h
+