66 lines
1.6 KiB
C++
66 lines
1.6 KiB
C++
#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
|