134 lines
3.3 KiB
C++
134 lines
3.3 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#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
|