134 lines
4.3 KiB
C++
134 lines
4.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 INDOOR_IMU_COMPASSDETECTIONPLOT_H
|
||
#define INDOOR_IMU_COMPASSDETECTIONPLOT_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/GnuplotMultiplot.h>
|
||
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>
|
||
|
||
#include "MagnetometerData.h"
|
||
#include "../../data/Timestamp.h"
|
||
#include "../../math/Matrix3.h"
|
||
|
||
class CompassDetectionPlot {
|
||
|
||
Timestamp plotRef;
|
||
Timestamp lastPlot;
|
||
|
||
K::Gnuplot gp1;
|
||
K::Gnuplot gp2;
|
||
//K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);
|
||
|
||
K::GnuplotPlot plotMagRaw;
|
||
K::GnuplotPlotElementLines lineMagRawX;
|
||
K::GnuplotPlotElementLines lineMagRawY;
|
||
K::GnuplotPlotElementLines lineMagRawZ;
|
||
K::GnuplotPlotElementLines lineMagRawMagnitude;
|
||
|
||
K::GnuplotPlot plotMagFix;
|
||
K::GnuplotPlotElementLines lineMagFixX;
|
||
K::GnuplotPlotElementLines lineMagFixY;
|
||
K::GnuplotPlotElementLines lineMagFixZ;
|
||
|
||
K::GnuplotPlot plotMagScatter;
|
||
K::GnuplotPlotElementPoints scatterPoints;
|
||
|
||
|
||
|
||
public:
|
||
|
||
CompassDetectionPlot() {
|
||
|
||
gp1 << "set autoscale xfix\n";
|
||
gp2 << "set size ratio -1\n";
|
||
|
||
//multiplot.add(&plotMagRaw);
|
||
//multiplot.add(&plotMagFix);
|
||
|
||
plotMagRaw.setTitle("Magnetometer (raw)");
|
||
plotMagRaw.add(&lineMagRawX); lineMagRawX.getStroke().getColor().setHexStr("#ff0000"); lineMagRawX.setTitle("x");
|
||
plotMagRaw.add(&lineMagRawY); lineMagRawY.getStroke().getColor().setHexStr("#00ff00"); lineMagRawY.setTitle("y");
|
||
plotMagRaw.add(&lineMagRawZ); lineMagRawZ.getStroke().getColor().setHexStr("#0000ff"); lineMagRawZ.setTitle("z");
|
||
plotMagRaw.add(&lineMagRawMagnitude); lineMagRawMagnitude.getStroke().getColor().setHexStr("#000000"); lineMagRawMagnitude.setTitle("magnitude");
|
||
|
||
plotMagFix.setTitle("Magnetometer (re-aligned)");
|
||
plotMagFix.add(&lineMagFixX); lineMagFixX.getStroke().getColor().setHexStr("#ff0000"); lineMagFixX.setTitle("x");
|
||
plotMagFix.add(&lineMagFixY); lineMagFixY.getStroke().getColor().setHexStr("#00ff00"); lineMagFixY.setTitle("y");
|
||
plotMagFix.add(&lineMagFixZ); lineMagFixZ.getStroke().getColor().setHexStr("#0000ff"); lineMagFixZ.setTitle("z");
|
||
|
||
plotMagScatter.add(&scatterPoints);
|
||
|
||
|
||
}
|
||
|
||
void add(Timestamp ts, float curHeading, bool stable, const MagnetometerData& mag, const MagnetometerData& curMag) {
|
||
|
||
if (plotRef.isZero()) {plotRef = ts;}
|
||
const Timestamp tsPlot = (ts-plotRef);
|
||
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
|
||
|
||
// raw gyro
|
||
lineMagRawX.add( K::GnuplotPoint2(tsPlot.ms(), mag.x) );
|
||
lineMagRawY.add( K::GnuplotPoint2(tsPlot.ms(), mag.y) );
|
||
lineMagRawZ.add( K::GnuplotPoint2(tsPlot.ms(), mag.z) );
|
||
lineMagRawMagnitude.add( K::GnuplotPoint2(tsPlot.ms(), mag.magnitude()) );
|
||
|
||
// adjusted mag
|
||
lineMagFixX.add( K::GnuplotPoint2(tsPlot.ms(), curMag.x) );
|
||
lineMagFixY.add( K::GnuplotPoint2(tsPlot.ms(), curMag.y) );
|
||
lineMagFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curMag.z) );
|
||
|
||
const float len = 1;//curMag.magnitude();// std::sqrt(curMag.x*curMag.x + curMag.y*curMag.y);
|
||
scatterPoints.add(K::GnuplotPoint2(curMag.x/len, curMag.y/len));
|
||
|
||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||
|
||
lastPlot = tsPlot;
|
||
|
||
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
||
lineMagRawX.removeIf(remove);
|
||
lineMagRawY.removeIf(remove);
|
||
lineMagRawZ.removeIf(remove);
|
||
lineMagRawMagnitude.removeIf(remove);
|
||
lineMagFixX.removeIf(remove);
|
||
lineMagFixY.removeIf(remove);
|
||
lineMagFixZ.removeIf(remove);
|
||
|
||
|
||
const float s = stable ? 0.1 : 0.03;
|
||
const float ax = 0.85 + std::cos(curHeading)*s;
|
||
const float ay = 0.85 + std::sin(curHeading)*s;
|
||
gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
|
||
gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";
|
||
|
||
gp1.draw(plotMagFix);
|
||
gp1.flush();
|
||
|
||
gp2.draw(plotMagScatter);
|
||
gp2.flush();
|
||
|
||
}
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif
|
||
|
||
#endif
|