geometry changes/fixes/new features

new grid walkers + fixes new test-cases worked on step/and turn detection android offline-data-reader worked on vap-grouping
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
--- a/tests/grid/TestGridWalk2RelPressure.cpp
+++ b/tests/grid/TestGridWalk2RelPressure.cpp
@@ -0,0 +1,133 @@
+#ifdef WITH_TESTS
+
+#include <fstream>
+
+#include "../Tests.h"
+
+#include "Plot.h"
+
+#include "../../grid/factory/v2/GridFactory.h"
+#include "../../grid/factory/v2/GridNodeImportance.h"
+
+#include "../../grid/walk/v2/GridWalker.h"
+#include "../../grid/walk/v2/modules/WalkModuleHeading.h"
+#include "../../grid/walk/v2/modules/WalkModuleHeadingControl.h"
+#include "../../grid/walk/v2/modules/WalkModuleFollowDestination.h"
+#include "../../grid/walk/v2/modules/WalkModuleRelativePressureControl.h"
+
+#include "../../sensors/radio/WiFiGridNode.h"
+
+
+#include <KLib/misc/gnuplot/Gnuplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplot.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
+#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
+
+#include <KLib/misc/gnuplot/GnuplotPlot.h>
+#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
+
+
+struct MyNode124234 : public GridPoint, public GridNode {
+	MyNode124234() {;}
+	MyNode124234(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
+};
+
+struct MyState316123 : public WalkState, public WalkStateRelativePressure {
+	MyState316123(const GridPoint& pos, const float hpa) : WalkState(pos), WalkStateRelativePressure(hpa) {;}
+};
+
+
+
+
+TEST(GridWalk2RelPressure, LIVE_walkHeading) {
+
+	Grid<MyNode124234> grid(20);
+
+	int sxy = 300;
+	int sz = 600;
+
+	for (int x = 0; x < sxy; x += 40) {
+		for (int y = 0; y < sxy; y+= 40) {
+			for (int z = 0; z < sz; z += 20) {
+				grid.add(MyNode124234(x,y,z));
+			}
+		}
+	}
+
+	for (int x = 0; x < sxy; x += 40) {
+		for (int y = 0; y < sxy; y+= 40) {
+			for (int z = 0; z < sz; z += 20) {
+
+				// connect vertically
+				for (int dz = -20; dz <= +20; dz += 40) {
+					MyNode124234* n1 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z));
+					MyNode124234* n2 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z+dz));
+					if (n1&&n2) {
+						grid.connectUniDir(*n1, *n2);
+					}
+				}
+
+				// connect horizontally
+				for (int dx = -40; dx <= +40; dx += 40) {
+					for (int dy = -40; dy <= +40; dy += 40) {
+						if (dx == 0 && dy == 0) {continue;}
+						MyNode124234* n1 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x,y,z));
+						MyNode124234* n2 = (MyNode124234*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,z));
+						if (n1&&n2) {
+							grid.connectUniDir(*n1, *n2);
+						}
+					}
+				}
+
+			}
+		}
+	}
+
+
+	struct MyControl {
+		struct Barometer {
+				float hPaRelativeToT0 = 0.0;
+				float estimatedSigma = 0.05;
+		} barometer;
+	} ctrl;
+
+	GridPoint start(120,120,sz/2);
+	const MyNode124234* n = grid.getNodePtrFor(start);
+
+	MyState316123 ms(start, 0);
+	GridWalker<MyNode124234, MyState316123> walker;
+	WalkModuleRelativePressureControl<MyNode124234, MyState316123, MyControl> modPres(&ctrl, 0.1);
+	walker.addModule(&modPres);
+
+	Plot p;
+	p.addEdges(grid);
+	p.addNodes(grid);
+
+	for (int i = 0; i < 1000; ++i) {
+
+		p.clearParticles();
+
+		ctrl.barometer.hPaRelativeToT0 = (std::sin(i/25.0)) * 0.3;
+		ms = walker.getDestination(grid, ms, 0.3);
+		p.addParticle(ms.position.inCentimeter());
+		usleep(1000*50);
+
+		p.gp << "set label 97 at screen 0.05, 0.95 ' baro: " << ctrl.barometer.hPaRelativeToT0 << "'\n";
+		p.gp << "set label 98 at screen 0.05, 0.90 ' state:" << ms.pressureRelToT0 << "'\n";
+		p.gp << "set label 99 at screen 0.05, 0.85 ' diff: " << std::abs(ctrl.barometer.hPaRelativeToT0-ms.pressureRelToT0) << "'\n";
+
+		p.fire();
+
+	}
+
+	p.fire();
+
+
+
+	sleep(1000);
+
+
+
+}
+
+#endif