124 lines
2.9 KiB
C++
124 lines
2.9 KiB
C++
#ifndef FIXEDFREQUENCYINTERPOLATOR_H
|
|
#define FIXEDFREQUENCYINTERPOLATOR_H
|
|
|
|
#include "Interpolator.h"
|
|
#include "../data/Timestamp.h"
|
|
#include <functional>
|
|
#include "../Assertions.h"
|
|
|
|
/**
|
|
* 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.
|
|
* supports both: up and downscaling
|
|
*/
|
|
template <typename Entry> class FixedFrequencyInterpolator {
|
|
|
|
private:
|
|
|
|
bool first = true;
|
|
|
|
/** how often to provide output data */
|
|
Timestamp outputInterval;
|
|
|
|
/** track the timestamps when to output the next value */
|
|
Timestamp nextOutput;
|
|
|
|
/** combine value-at-timestamp */
|
|
struct Timed {
|
|
Timestamp ts;
|
|
Entry entry;
|
|
Timed(const Timestamp ts, const Entry entry) : ts(ts), entry(entry) {;}
|
|
Timed() : ts(), entry() {;}
|
|
};
|
|
|
|
Timed last;
|
|
|
|
|
|
|
|
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 (first) {
|
|
first = false;
|
|
last = Timed(ts, entry);
|
|
nextOutput = last.ts;// + outputInterval;
|
|
return;
|
|
}
|
|
|
|
// new value
|
|
Timed cur(ts, entry);
|
|
|
|
// no time-change? -> ignore
|
|
if (last.ts == cur.ts) {return;}
|
|
|
|
// output needed?
|
|
//if (nextOutput > last.ts) {
|
|
|
|
// available timeslice ybetween last and current entry
|
|
const Timestamp diff = cur.ts - last.ts;
|
|
|
|
// create outputs
|
|
while(nextOutput < cur.ts) {
|
|
|
|
// interpolation rate
|
|
const float percent = (nextOutput.finest() - last.ts.finest()) / (float) (diff.finest());
|
|
|
|
// sanity checks
|
|
Assert::isNotNaN(percent, "detected NaN for interpolation");
|
|
Assert::isTrue(percent <= 1, "detected an invalid interpolation value");
|
|
|
|
const Entry res = last.entry + (cur.entry - last.entry) * percent;
|
|
callback(nextOutput, res);
|
|
|
|
// increment
|
|
nextOutput += outputInterval;
|
|
|
|
}
|
|
|
|
//}
|
|
|
|
// next step
|
|
last = cur;
|
|
|
|
|
|
|
|
// // 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());
|
|
|
|
// // sanity checks
|
|
// Assert::isNotNaN(percent, "detected NaN for interpolation");
|
|
// Assert::isTrue(percent <= 1, "detected an invalid interpolation value");
|
|
|
|
// const Entry res = lastEntry + (entry - lastEntry) * percent;
|
|
|
|
// callback(Timestamp::fromMS(t), res);
|
|
|
|
// }
|
|
|
|
// lastEntry = entry;
|
|
// lastTS = ts;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // FIXEDFREQUENCYINTERPOLATOR_H
|