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

tests/math/TestFixedFrequencyInterpolator.cpp

@@ -0,0 +1,61 @@
+#ifdef WITH_TESTS
+
+#include <cmath>
+#include "../Tests.h"
+#include "../../math/FixedFrequencyInterpolator.h"
+#include <fstream>
+
+TEST(FixedFrequencyInterpolator, test) {
+
+	FixedFrequencyInterpolator<float> ffi(Timestamp::fromMS(10));
+
+	std::ofstream out("/tmp/test.dat");
+
+	std::vector<float> data;
+	auto callback = [&] (const Timestamp ts, const float val) {
+		out << ts.ms() << " " << val << "\n";
+		data.push_back(val);
+	};
+
+	float x = 10;
+
+	// output should be a straight line
+	ffi.add(Timestamp::fromMS(55), x*55, callback);
+	ffi.add(Timestamp::fromMS(93), x*93, callback);
+	ffi.add(Timestamp::fromMS(102), x*102, callback);
+	ffi.add(Timestamp::fromMS(134), x*134, callback);
+	ffi.add(Timestamp::fromMS(212), x*212, callback);
+	ffi.add(Timestamp::fromMS(214), x*214, callback);
+	ffi.add(Timestamp::fromMS(216), x*216, callback);
+	ffi.add(Timestamp::fromMS(219), x*219, callback);
+	ffi.add(Timestamp::fromMS(220), x*220, callback);
+	ffi.add(Timestamp::fromMS(240), x*240, callback);
+
+	out.close();
+
+	const float d = 0.001;
+	ASSERT_NEAR(data[0], x*60, d);
+	ASSERT_NEAR(data[1], x*70, d);
+	ASSERT_NEAR(data[2], x*80, d);
+	ASSERT_NEAR(data[3], x*90, d);
+	ASSERT_NEAR(data[4], x*100, d);
+	ASSERT_NEAR(data[5], x*110, d);
+	ASSERT_NEAR(data[6], x*120, d);
+	ASSERT_NEAR(data[7], x*130, d);
+	ASSERT_NEAR(data[8], x*140, d);
+	ASSERT_NEAR(data[9], x*150, d);
+	ASSERT_NEAR(data[10], x*160, d);
+	ASSERT_NEAR(data[11], x*170, d);
+	ASSERT_NEAR(data[12], x*180, d);
+	ASSERT_NEAR(data[13], x*190, d);
+	ASSERT_NEAR(data[14], x*200, d);
+	ASSERT_NEAR(data[15], x*210, d);
+	ASSERT_NEAR(data[16], x*220, d);
+	ASSERT_NEAR(data[17], x*230, d);
+	ASSERT_NEAR(data[18], x*240, d);
+
+}
+
+
+
+#endif