FrankE
a203305628
- changed the wifi-estimation api - adjusted test-cases - worked on grid-bulding and grid-importance - new walking modules - fixed some minor issues
134 lines
3.2 KiB
C++
134 lines
3.2 KiB
C++
#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 MyNode : public GridPoint, public GridNode {
|
|
MyNode() {;}
|
|
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
|
|
};
|
|
|
|
struct MyState : public WalkState, public WalkStateRelativePressure {
|
|
MyState(const GridPoint& pos, const float hpa) : WalkState(pos), WalkStateRelativePressure(hpa) {;}
|
|
};
|
|
|
|
|
|
|
|
|
|
TEST(GridWalk2RelPressure, LIVE_walkHeading) {
|
|
|
|
Grid<MyNode> 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(MyNode(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) {
|
|
MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z));
|
|
MyNode* n2 = (MyNode*)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;}
|
|
MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z));
|
|
MyNode* n2 = (MyNode*)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 MyNode* n = grid.getNodePtrFor(start);
|
|
|
|
MyState ms(start, 0);
|
|
GridWalker<MyNode, MyState> walker;
|
|
WalkModuleRelativePressureControl<MyNode, MyState, 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.startPos.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
|