163 lines
4.1 KiB
C++
163 lines
4.1 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 STEPDETECTION_H
|
||
#define STEPDETECTION_H
|
||
|
||
|
||
#include "AccelerometerData.h"
|
||
#include "../../data/Timestamp.h"
|
||
|
||
#include <cmath>
|
||
#include <vector>
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
||
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
|
||
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
|
||
#endif
|
||
|
||
#include "../../Assertions.h"
|
||
#include "../../math/MovingAverageTS.h"
|
||
//#include "../../math/dsp/fir/RComplex.h"
|
||
|
||
|
||
/**
|
||
* simple step detection based on accelerometer magnitude.
|
||
* magnitude > threshold? -> step!
|
||
* block for several msec until detecting the next one
|
||
*/
|
||
class StepDetection {
|
||
|
||
private:
|
||
|
||
MovingAverageTS<float> avgLong;
|
||
MovingAverageTS<float> avgShort;
|
||
|
||
Timestamp blockUntil;
|
||
bool waitForUp = false;
|
||
|
||
const Timestamp blockTime; // 150-250 looks good
|
||
const float upperThreshold = +0.4*0.8f; // + is usually smaller than down (look at graphs)
|
||
const float lowerThreshold = -1.5*0.8f; // the 0.8 is for testing!
|
||
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
K::Gnuplot gp;
|
||
K::GnuplotPlot plot;
|
||
K::GnuplotPlotElementLines lineDet;
|
||
K::GnuplotPlotElementPoints pointDet;
|
||
K::GnuplotPlotElementLines lineX;
|
||
K::GnuplotPlotElementLines lineY;
|
||
K::GnuplotPlotElementLines lineZ;
|
||
Timestamp plotRef;
|
||
Timestamp lastPlot;
|
||
#endif
|
||
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
StepDetection(const Timestamp blockTime = Timestamp::fromMS(200)) : blockTime(blockTime), avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
gp << "set autoscale xfix\n";
|
||
plot.setTitle("Step Detection");
|
||
plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); lineX.setTitle("accX");
|
||
plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); lineY.setTitle("accY");
|
||
plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); lineZ.setTitle("accZ");
|
||
plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000");
|
||
plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
|
||
#endif
|
||
|
||
}
|
||
|
||
/** does the given data indicate a step? */
|
||
bool add(const Timestamp ts, const AccelerometerData& acc) {
|
||
|
||
// update averages
|
||
avgLong.add(ts, acc.magnitude());
|
||
avgShort.add(ts, acc.magnitude());
|
||
|
||
// difference between long-term-average (gravity) and very-short-time average
|
||
const float delta = avgShort.get() - avgLong.get();
|
||
|
||
bool step = false;
|
||
|
||
|
||
if (blockUntil > ts) {
|
||
|
||
step = false;
|
||
|
||
} else {
|
||
|
||
// wait for a rising edge
|
||
if (waitForUp && delta > upperThreshold) {
|
||
blockUntil = ts + blockTime; // block some time
|
||
waitForUp = false;
|
||
}
|
||
|
||
// wait for a falling edge
|
||
if (!waitForUp && delta < lowerThreshold) {
|
||
blockUntil = ts + blockTime; // block some time
|
||
waitForUp = true;
|
||
step = true;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
|
||
if (plotRef.isZero()) {plotRef = ts;}
|
||
const Timestamp tsPlot = (ts-plotRef);
|
||
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
|
||
|
||
//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
|
||
lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) );
|
||
lineY.add( K::GnuplotPoint2(tsPlot.ms(), acc.y) );
|
||
lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
|
||
lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );
|
||
|
||
if (step) {
|
||
pointDet.add( K::GnuplotPoint2(tsPlot.ms(), 0) );
|
||
}
|
||
|
||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||
|
||
lastPlot = tsPlot;
|
||
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
||
lineX.removeIf(remove);
|
||
lineY.removeIf(remove);
|
||
lineZ.removeIf(remove);
|
||
lineDet.removeIf(remove);
|
||
pointDet.removeIf(remove);
|
||
|
||
gp.draw(plot);
|
||
gp.flush();
|
||
usleep(100);
|
||
|
||
}
|
||
|
||
#endif
|
||
|
||
return step;
|
||
|
||
|
||
}
|
||
|
||
};
|
||
|
||
|
||
#endif // STEPDETECTION_H
|