124 lines
4.0 KiB
C++
124 lines
4.0 KiB
C++
#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
|