added a fixed-frequency interpolator

math/FixedFrequencyInterpolator.h

@@ -1,4 +1,65 @@
 #ifndef FIXEDFREQUENCYINTERPOLATOR_H
 #define FIXEDFREQUENCYINTERPOLATOR_H
 
+#include "Interpolator.h"
+#include "../data/Timestamp.h"
+#include <functional>
+
+/**
+ * performs interpolation on provided sensor data
+ * for sensors that do not send their data at a fixed frequency
+ * or to adjust the frequency of the data provided by a sensor
+ */
+template <typename Entry> class FixedFrequencyInterpolator {
+
+private:
+
+	/** how often to provide output data */
+	Timestamp outputInterval;
+
+	Timestamp lastTS;
+	Entry lastEntry;
+
+
+public:
+
+	/** ctor with the desired output inteval */
+	FixedFrequencyInterpolator(const Timestamp outputInterval) : outputInterval(outputInterval) {
+
+	}
+
+	/** add a new sensor entry. the callback is called for every interpolated output */
+	void add(const Timestamp ts, const Entry& entry, std::function<void(Timestamp, const Entry&)> callback) {
+
+		// first value?
+		if (lastTS.isZero()) {
+			lastTS = ts;
+			lastEntry = entry;
+			return;
+		}
+
+		// available timeslice between last and current entry
+		const Timestamp diff = ts - lastTS;
+
+		// the region to output
+		const uint64_t start = std::ceil(lastTS.ms() / (float)outputInterval.ms() + 0.00001f) * outputInterval.ms();
+		const uint64_t end = std::floor(ts.ms() / (float)outputInterval.ms()) * outputInterval.ms();
+
+		// perform output
+		for (uint64_t t = start; t <= end; t += outputInterval.ms()) {
+
+			const float percent = (t - lastTS.ms()) / (float) (diff.ms());
+			const Entry res = lastEntry + (entry - lastEntry) * percent;
+
+			callback(Timestamp::fromMS(t), res);
+
+		}
+
+		lastEntry = entry;
+		lastTS = ts;
+
+	}
+
+};
+
 #endif // FIXEDFREQUENCYINTERPOLATOR_H