Filter: automatically cope with duplicate timestamps

this makes it easier for callers to avoid excessive slope calculations

Thanks to Jon Challinger for the idea

libraries/Filter/DerivativeFilter.cpp

@@ -18,17 +18,35 @@
 template <class T,  uint8_t FILTER_SIZE>
 void DerivativeFilter<T,FILTER_SIZE>::update(T sample, uint32_t timestamp)
 {
+    uint8_t i = FilterWithBuffer<T,FILTER_SIZE>::sample_index;
+    uint8_t i1;
+    if (i == 0) {
+        i1 = FILTER_SIZE-1;
+    } else {
+        i1 = i-1;
+    }
+    if (_timestamps[i1] == timestamp) {
+        // this is not a new timestamp - ignore
+        return;
+    }
+    
     // add timestamp before we apply to FilterWithBuffer
-    _timestamps[FilterWithBuffer<T,FILTER_SIZE>::sample_index] = timestamp;
+    _timestamps[i] = timestamp;
 
 	// call parent's apply function to get the sample into the array
 	FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
+
+    _new_data = true;
 }
 
 
 template <class T,  uint8_t FILTER_SIZE>
 float DerivativeFilter<T,FILTER_SIZE>::slope(void)
 {
+    if (!_new_data) {
+        return _last_slope;
+    }
+
 	float result = 0;
 
     // use f() to make the code match the maths a bit better. Note
@@ -74,6 +92,9 @@ float DerivativeFilter<T,FILTER_SIZE>::slope(void)
         break;
     }
 
+    _new_data = false;
+    _last_slope = result;
+
     return result;
 }
 

libraries/Filter/DerivativeFilter.h

@@ -36,7 +36,8 @@ class DerivativeFilter : public FilterWithBuffer<T,FILTER_SIZE>
 	virtual void reset();
 
   private:
-    uint32_t _last_time;
+    bool _new_data;
+    float _last_slope;
 
     // microsecond timestamps for samples. This is needed
     // to cope with non-uniform time spacing of the data