175 lines
5.6 KiB
C++
175 lines
5.6 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_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 addRelative(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();
|
||
|
||
}
|
||
|
||
}
|
||
|
||
void addAbsolute(Timestamp ts, const float curHead, const GyroscopeData& gyro, const Vector3& gyroFixed) {
|
||
|
||
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(curHead)*0.1;
|
||
const float ay = 0.85 + std::sin(curHead)*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
|