122 lines
3.9 KiB
C++
122 lines
3.9 KiB
C++
#ifndef INDOOR_IMU_TURNDETECTIONPLOT_H
|
|
#define INDOOR_IMU_TURNDETECTIONPLOT_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/GnuplotSplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
|
|
|
#include "GyroscopeData.h"
|
|
#include "../../data/Timestamp.h"
|
|
#include "../../math/Matrix3.h"
|
|
|
|
class TurnDetectionPlot {
|
|
|
|
Timestamp plotRef;
|
|
Timestamp lastPlot;
|
|
|
|
K::Gnuplot gp1;
|
|
|
|
K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);
|
|
|
|
K::GnuplotPlot plotGyroRaw;
|
|
K::GnuplotPlotElementLines lineGyroRawX;
|
|
K::GnuplotPlotElementLines lineGyroRawY;
|
|
K::GnuplotPlotElementLines lineGyroRawZ;
|
|
|
|
K::GnuplotPlot plotGyroFix;
|
|
K::GnuplotPlotElementLines lineGyroFixX;
|
|
K::GnuplotPlotElementLines lineGyroFixY;
|
|
K::GnuplotPlotElementLines lineGyroFixZ;
|
|
|
|
K::GnuplotSplot plotPose;
|
|
K::GnuplotSplotElementLines linePose;
|
|
|
|
float plotCurHead = 0;
|
|
|
|
public:
|
|
|
|
TurnDetectionPlot() {
|
|
|
|
gp1 << "set autoscale xfix\n";
|
|
gp1 << "set view equal xyz\n";
|
|
|
|
multiplot.add(&plotGyroRaw);
|
|
multiplot.add(&plotGyroFix);
|
|
|
|
plotGyroRaw.setTitle("Gyroscope (raw)");
|
|
plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX");
|
|
plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY");
|
|
plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ");
|
|
|
|
plotGyroFix.setTitle("Gyroscope (fixed)");
|
|
plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX");
|
|
plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY");
|
|
plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ");
|
|
|
|
plotPose.setTitle("Pose");
|
|
plotPose.getView().setEnabled(false);
|
|
plotPose.add(&linePose);
|
|
plotPose.getAxisX().setRange(-5,+5);
|
|
plotPose.getAxisY().setRange(-5,+5);
|
|
plotPose.getAxisZ().setRange(-5,+5);
|
|
|
|
}
|
|
|
|
void add(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {
|
|
|
|
plotCurHead += delta;
|
|
|
|
if (plotRef.isZero()) {plotRef = ts;}
|
|
const Timestamp tsPlot = (ts-plotRef);
|
|
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
|
|
|
|
// raw gyro
|
|
lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
|
|
lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
|
|
lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );
|
|
|
|
// adjusted gyro
|
|
lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.x) );
|
|
lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) );
|
|
lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) );
|
|
|
|
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
|
|
|
lastPlot = tsPlot;
|
|
|
|
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
|
lineGyroRawX.removeIf(remove);
|
|
lineGyroRawY.removeIf(remove);
|
|
lineGyroRawZ.removeIf(remove);
|
|
lineGyroFixX.removeIf(remove);
|
|
lineGyroFixY.removeIf(remove);
|
|
lineGyroFixZ.removeIf(remove);
|
|
|
|
const float ax = 0.85 + std::cos(plotCurHead)*0.1;
|
|
const float ay = 0.85 + std::sin(plotCurHead)*0.1;
|
|
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(plotGyroRaw); // raw only
|
|
gp1.draw(plotGyroFix); // fixed only
|
|
//gp1.draw(multiplot); // both
|
|
|
|
gp1.flush();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif
|
|
|
|
#endif // INDOOR_IMU_TURNDETECTIONPLOT_H
|