159 lines
3.4 KiB
C++
159 lines
3.4 KiB
C++
#ifndef ACTIVITYDETECTOR_H
|
|
#define ACTIVITYDETECTOR_H
|
|
|
|
|
|
#include "../imu/AccelerometerData.h"
|
|
#include "../pressure/BarometerData.h"
|
|
|
|
#include "../../data/Timestamp.h"
|
|
|
|
#include "../../Assertions.h"
|
|
#include "../../math/MovingAverageTS.h"
|
|
#include "../../math/MovingStdDevTS.h"
|
|
|
|
#include "../../data/HistoryTS.h"
|
|
|
|
#include "../activity/Activity.h"
|
|
|
|
//#define ACT_DET_DEBUG_PLOT
|
|
|
|
#ifdef ACT_DET_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>
|
|
#endif
|
|
|
|
|
|
/**
|
|
* simple step detection based on accelerometer magnitude.
|
|
* magnitude > threshold? -> step!
|
|
* block for several msec until detecting the next one
|
|
*/
|
|
class ActivityDetector {
|
|
|
|
private:
|
|
|
|
MovingAverageTS<float> avgLong;
|
|
MovingAverageTS<float> avgShort;
|
|
|
|
MovingStdDevTS<float> stdDev;
|
|
MovingStdDevTS<float> stdDev2;
|
|
|
|
//MovingAverageTS<float> baroAvgSlow;
|
|
//MovingAverageTS<float> baroAvgFast;
|
|
|
|
MovingAverageTS<float> baroAvg;
|
|
HistoryTS<float> baroHistory;
|
|
|
|
Activity current;
|
|
|
|
public:
|
|
|
|
#ifdef ACT_DET_DEBUG_PLOT
|
|
K::Gnuplot gp;
|
|
K::GnuplotPlot gplot;
|
|
K::GnuplotPlotElementLines l1;
|
|
K::GnuplotPlotElementLines l2;
|
|
#endif
|
|
|
|
/** ctor */
|
|
ActivityDetector() : avgLong(Timestamp::fromMS(1500), 0), avgShort(Timestamp::fromMS(500), 0),
|
|
stdDev(Timestamp::fromMS(150), 0), stdDev2(Timestamp::fromMS(2000), 0),
|
|
baroAvg(Timestamp::fromMS(500), 0), baroHistory(Timestamp::fromMS(4000)) {
|
|
;
|
|
|
|
#ifdef ACT_DET_DEBUG_PLOT
|
|
gplot.add(&l1);
|
|
gplot.add(&l2); l2.getStroke().getColor().setHexStr("#ff0000");
|
|
#endif
|
|
|
|
}
|
|
|
|
//int xx = 0;
|
|
|
|
/** add barometer data */
|
|
void add(const Timestamp ts, const BarometerData& baro) {
|
|
if (baro.isValid()) {
|
|
baroAvg.add(ts, baro.hPa);
|
|
const float avg = baroAvg.get();
|
|
baroHistory.add(ts, avg);
|
|
//l1.add(K::GnuplotPoint2(xx, avg));
|
|
update();
|
|
}
|
|
}
|
|
|
|
/** get the currently detected activity */
|
|
Activity get() const {
|
|
return current;
|
|
}
|
|
|
|
/** does the given data indicate a step? */
|
|
void add(const Timestamp ts, const AccelerometerData& acc) {
|
|
|
|
// update averages
|
|
avgLong.add(ts, acc.magnitude());
|
|
avgShort.add(ts, acc.magnitude());
|
|
stdDev.add(ts, acc.magnitude());
|
|
stdDev2.add(ts, acc.magnitude());
|
|
|
|
// const float delta = std::abs(avgLong.get() - avgShort.get());
|
|
|
|
// static int x = 0; ++x;
|
|
|
|
// if (delta < 0.3) {
|
|
// return Activity::STANDING;
|
|
// }
|
|
|
|
// if (avgLong.get() > 9.81+0.5) {
|
|
// return Activity::WALKING_UP;
|
|
// } else if (avgLong.get() < 9.81-0.5) {
|
|
// return Activity::WALKING_DOWN;
|
|
// }
|
|
|
|
// return Activity::WALKING;
|
|
|
|
}
|
|
|
|
|
|
private:
|
|
|
|
/** estimate the current activity based on the sensor data */
|
|
void update() {
|
|
|
|
// delta in acceleration
|
|
const float delta_acc = std::abs(avgLong.get() - avgShort.get());
|
|
|
|
if (delta_acc < 0.015) {
|
|
current = Activity::STANDING;
|
|
return;
|
|
}
|
|
|
|
// delta in pressure
|
|
const float delta_hPa = baroHistory.getMostRecent() - baroHistory.getOldest();
|
|
|
|
#ifdef ACT_DET_DEBUG_PLOT
|
|
l2.add(K::GnuplotPoint2(xx, delta_hPa));
|
|
gp.draw(gplot);
|
|
gp.flush();
|
|
++xx;
|
|
#endif
|
|
|
|
if (std::abs(delta_hPa) < 0.042) {
|
|
current = Activity::WALKING;
|
|
return;
|
|
} else if (delta_hPa > 0) {
|
|
current = Activity::WALKING_DOWN;
|
|
return;
|
|
} else {
|
|
current = Activity::WALKING_UP;
|
|
return;
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // ACTIVITYDETECTOR_H
|