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

fixed some issues

Browse Source 
added new pose/turn detections
new helper classes
define-flags for libEigen
This commit is contained in:
FrankE
2018-09-04 10:49:00 +02:00
parent f990485d44
commit 857d7a1553
51 changed files with 2149 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
sensors/imu/TurnDetectionPlot.h
View File

@@ -69,7 +69,7 @@
}
void add(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {
void addRelative(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {
plotCurHead += delta;
@@ -114,6 +114,49 @@
}
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