3f8d21e146
1) when barometer produces false measurements, we needed some kind of upper boundary 2) coding error static.. while initializing this object over multiple test iterations isnt the best idea
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
|