FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

worked on synthetic sensors

Browse Source 
worked on grid-walker
minor changes/fixes/improvements
This commit is contained in:
k-a-z-u
2017-10-18 16:54:57 +02:00
parent 72f083d32a
commit 3e31f6da53
8 changed files with 522 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
sensors/imu/StepDetection.h
View File

@@ -8,12 +8,14 @@
#include <cmath>
#include <vector>
#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>
#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"
@@ -43,11 +45,12 @@ private:
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lineDet;
K::GnuplotPlotElementPoints pointDet;
K::GnuplotPlotElementLines lineX;
K::GnuplotPlotElementLines lineY;
K::GnuplotPlotElementLines lineZ;
Timestamp plotRef;
int plotCnt = 0;
Timestamp lastPlot;
#endif
@@ -57,10 +60,13 @@ public:
StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
#ifdef WITH_DEBUG_PLOT
plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000");
plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00");
plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff");
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
}
@@ -78,19 +84,25 @@ public:
bool step = false;
if (blockUntil > ts) {return false;}
if (blockUntil > ts) {
// wait for a rising edge
if (waitForUp && delta > upperThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = false;
}
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;
}
// wait for a falling edge
if (!waitForUp && delta < lowerThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = true;
step = true;
}
@@ -98,6 +110,7 @@ public:
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) );
@@ -105,10 +118,24 @@ public:
lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );
if (++plotCnt % 25 == 0) {
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(1000*25);
usleep(100);
}
#endif