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

added missing code changes

Browse Source 
started working on refactoring of sensors
new test-cases
This commit is contained in:
FrankE
2016-03-17 17:28:41 +01:00
parent 6346231a64
commit f8d7f768f1
18 changed files with 524 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -65,10 +65,11 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g3
-O0
-O2
-DWITH_TESTS
-DWITH_ASSERTIONS
-march=native
)

11
geo/BBox3.h
View File

@@ -60,6 +60,17 @@ public:
p2 -= p; // decrease maximum
}
/** grow the bbox by the amount given for each dimension */
void grow(const float v) {
grow(Point3(v,v,v));
}
/** grow the bbox by the amount given for each dimension */
void grow(const Point3& p) {
p1 -= p; // decrease minimum
p2 += p; // increase maximum
}
/** does the bbox contain the given point? */
bool contains(const Point3& p) const {
if (p.x < p1.x) {return false;}

4
geo/Point3.h
View File

@@ -26,6 +26,8 @@ struct Point3 {
Point3 operator - (const Point3& o) const {return Point3(x-o.x, y-o.y, z-o.z);}
Point3 operator * (const Point3& o) const {return Point3(x*o.x, y*o.y, z*o.z);}
Point3 operator * (const float v) const {return Point3(v*x, v*y, v*z);}
Point3 operator / (const float v) const {return Point3(x/v, y/v, z/v);}
@@ -40,6 +42,8 @@ struct Point3 {
Point3& operator -= (const Point3& o) {x-=o.x; y-=o.y; z-=o.z; return *this;}
bool operator < (const Point3& o) const {return x<o.x && y<o.y && z<o.z;}
bool operator == (const Point3& o) const {return x==o.x && y==o.y && z==o.z;}
Point2 xy() const {return Point2(x,y);}

3
grid/factory/GridFactory.h
View File

@@ -293,6 +293,9 @@ public:
// get the original grid's bbox
BBox3 bb = gIn.getBBox();
// ensure we have an outer boundary
bb.grow(gIn.getGridSize_cm() * 2);
const int gridSize_cm = grid.getGridSize_cm();
// build new grid-points

12
grid/factory/GridImportance.h
View File

@@ -95,7 +95,8 @@ public:
const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));
// importance for this node (based on the distance from the next door)
n1.imp += favorDoors.getProbability(dist_m) * 0.30;
//n1.imp += favorDoors.getProbability(dist_m) * 0.30;
n1.imp += favorDoors.getProbability(dist_m);
}
@@ -207,9 +208,12 @@ public:
if (dist_m > 2.0) {dist_m = 2.0;}
// overall importance
return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls
+ stickToWalls.getProbability(dist_m) * 0.15 // walk near walls
+ farAway.getProbability(dist_m) * 0.15 // walk in the middle
// return - avoidWalls.getProbability(dist_m) * 0.30 // avoid walls
// + stickToWalls.getProbability(dist_m) * 0.15 // walk near walls
// + farAway.getProbability(dist_m) * 0.15 // walk in the middle
return - avoidWalls.getProbability(dist_m) // avoid walls
//+ stickToWalls.getProbability(dist_m) // walk near walls
//+ farAway.getProbability(dist_m) // walk in the middle
;

17
grid/walk/GridWalkPathControl.h
View File

@@ -32,6 +32,10 @@ private:
DrawList<T&> drawer;
public:
float pOther = 0.10;
public:
@@ -65,9 +69,11 @@ public:
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
distance_m = distance_m*dWalk.draw()*1.4; // TODO: why *2?
headChange_rad = headChange_rad*dHead.draw();
static Distribution::Normal<float> sWalk(0, 0.15);
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
return walk(grid, start, distance_m, headChange_rad);
@@ -75,12 +81,12 @@ public:
private:
double getProbability(const T& start, const T& possible, const Heading head) const {
double getProbability(const T& start, const T& prev, const T& possible, const Heading head) const {
// TODO: WHY?! not only when going back to the start?
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
if (start.z_cm == possible.z_cm) {return 0;} // back to the start
//throw 1;
throw 1;
return 0.5;// stair start/end TODO: fix
}
@@ -95,7 +101,8 @@ private:
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance
const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025;
//const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025; // from start
const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther; // from previous node
// bring it together
return angleProb * nodeProb;
@@ -123,7 +130,7 @@ private:
drawer.reset();
for (T& neighbor : grid.neighbors(*cur.node)) {
const double prob = getProbability(*start.node, neighbor, reqHeading);
const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading);
drawer.add(neighbor, prob);
}

82
grid/walk/GridWalkShortestPathControl.h
View File

@@ -17,6 +17,8 @@
#include "GridWalkHelper.h"
#include "GridWalk.h"
#include <KLib/math/statistics/Statistics.h>
template <typename T> class GridWalkShortestPathControl : public GridWalk<T> {
@@ -82,14 +84,17 @@ protected:
Dijkstra<T> dijkstra;
const T& target;
Point3 centerOfMass = Point3(0,0,0);
//Point3 centerOfMass = Point3(0,0,0);
Wrapper wrapper;
DijkstraPath<T>* path = nullptr;
KNN<Wrapper,3>* knn = nullptr;
//KNN<Wrapper,3>* knn = nullptr;
DrawList<T&> drawer;
std::vector<Point3> points;
Point3 centerOfMass;
float stdDevDist;
public:
@@ -109,17 +114,51 @@ public:
int recalc = 0;
int times = 3;
float pOther = 0.10;
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
// update the center-of-mass
centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
if (path == nullptr) {rebuildPath(grid);}
if (++recalc > 100 * 1000) {
// update the center-of-mass
points.push_back( (Point3)*start.node );
//centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
//if (path == nullptr) {rebuildPath(grid);}
if (++recalc >= 7500) {
// center of mass
Point3 sum(0,0,0);
for (const Point3& p : points) {sum += p;}
centerOfMass = sum/points.size();
// deviation from the center of mass
float dSum = 0;
float dSum2 = 0;
for (const Point3& p : points) {
const float d = p.getDistance(centerOfMass);
dSum += d;
dSum2 += d*d;
}
dSum /= points.size();
dSum2 /= points.size();
stdDevDist = std::sqrt( (dSum2) - (dSum * dSum) ) * times;
// restart
points.clear();
recalc = 0;
rebuildPath(grid);
// update
rebuildPath(grid, centerOfMass);
}
// if (knn != nullptr) {
// const float dist = knn->getNearestDistance( {(float)start.node->x_cm, (float)start.node->y_cm, (float)start.node->z_cm} );
// if (dist > 10000) {
@@ -133,9 +172,11 @@ public:
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
distance_m = distance_m*dWalk.draw()*1.5; // TODO: why *2?
headChange_rad = headChange_rad*dHead.draw();
static Distribution::Normal<float> sWalk(0, 0.10);
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
return walk(grid, start, distance_m, headChange_rad);
@@ -143,12 +184,12 @@ public:
private:
double getProbability(const T& start, const T& possible, const Heading head) const {
double getProbability(Grid<T>& grid, const T& start, const T& prev, const T& possible, const Heading head) const {
// TODO: WHY?! not only when going back to the start?
if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
if (start.z_cm == possible.z_cm) {return 0;} // back to the start
//throw 1;
throw 1;
return 0.5;// stair start/end TODO: fix
}
@@ -165,11 +206,16 @@ private:
// nodes own importance
double nodeProb = 1;//(possible.distToTarget < start.distToTarget) ? 1 : 0.025;
if (knn != nullptr) {
const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
if (path != nullptr) {
//const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
int steps = stdDevDist / grid.getGridSize_cm();
const float pToTarget = possible.getDistanceInMeter(*path->getFromStart(steps).element);
const float sToTarget = prev.getDistanceInMeter(*path->getFromStart(steps).element);
nodeProb = (pToTarget < sToTarget) ? (1.0) : (pOther);
//const float sd = knn->getNearestDistance( {(float)start.x_cm, (float)start.y_cm, (float)start.z_cm} );
//nodeProb = (pd < sd) ? 1 : 0.0;
nodeProb = Distribution::Exponential<float>::getProbability(0.15, pd_m);
//nodeProb = Distribution::Exponential<float>::getProbability(0.9, pToTarget);
//nodeProb = Distribution::Exponential<float>::getProbability(1.0, tDist_m);
}
// bring it together
@@ -198,7 +244,7 @@ private:
drawer.reset();
for (T& neighbor : grid.neighbors(*cur.node)) {
const double prob = getProbability(*start.node, neighbor, reqHeading);
const double prob = getProbability(grid, *start.node, *cur.node, neighbor, reqHeading);
drawer.add(neighbor, prob);
}
@@ -230,14 +276,14 @@ private:
}
/** rebuild the path for the given center point */
void rebuildPath(Grid<T>& grid) {
void rebuildPath(Grid<T>& grid, const Point3 centerOfMass) {
// find the grid node nearest to the current center-of-mass
auto nearestGridNode = [&] (const T& n1, const T& n2) { return ((Point3)n1).getDistance(centerOfMass) < ((Point3)n2).getDistance(centerOfMass); };
const T& currentMass = *std::min_element(grid.begin(), grid.end(), nearestGridNode);
delete path; path = nullptr;
delete knn; knn = nullptr;
//delete knn; knn = nullptr;
DijkstraNode<T>* dnTarget = dijkstra.getNode(target);
DijkstraNode<T>* dnStart = dijkstra.getNode(currentMass);
@@ -252,9 +298,9 @@ private:
// create k-nn lookup
wrapper = Wrapper(path);
knn = new KNN<Wrapper, 3>(wrapper);
//knn = new KNN<Wrapper, 3>(wrapper);
} catch (...) {
knn = nullptr;
//knn = nullptr;
path = nullptr;
}

10
grid/walk/GridWalkSimpleControl.h
View File

@@ -47,11 +47,13 @@ public:
static Distribution::Normal<float> dHead(1, 0.01);
// proportional change of the to-be-walked distance
static Distribution::Normal<float> dWalk(1, 0.50);
static Distribution::Normal<float> dWalk(1, 0.10);
distance_m = distance_m*dWalk.draw()*2; // TODO: why *2?
distance_m = distance_m*dWalk.draw()*1.4; // TODO: why * X?
headChange_rad = headChange_rad*dHead.draw();
static Distribution::Normal<float> sWalk(0, 0.10);
if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
return walk(grid, start, distance_m, headChange_rad);
@@ -79,8 +81,8 @@ private:
// const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally
// nodes own importance
//const double nodeProb = Distribution::Logistic<float>::getCDF(possible.imp, 1, 0.5);
const double nodeProb = std::pow(possible.imp, 5);
const double nodeProb = Distribution::Logistic<float>::getCDF(possible.imp, 1, 0.9);
//const double nodeProb = std::pow(possible.imp, 2);
//const double nodeProb = 1.0;
// bring it together

2
main.cpp
View File

@@ -15,7 +15,7 @@ int main(int argc, char** argv) {
#ifdef WITH_TESTS
::testing::InitGoogleTest(&argc, argv);
//::testing::GTEST_FLAG(filter) = "*Importance*";
::testing::GTEST_FLAG(filter) = "*huge*";
//::testing::GTEST_FLAG(filter) = "*Walk*";
return RUN_ALL_TESTS();

97
nav/dijkstra/Dijkstra.h
View File

@@ -44,18 +44,15 @@ public:
// sorted list of all to-be-processed nodes
ToProcess toBeProcessedNodes;
// all already processed edges
std::unordered_set<decltype(getEdge(nullptr,nullptr))> usedEdges;
// run from start
const T* cur = &start;
// create a node for the start element
DijkstraNode<T>* dnStart = getNode(cur);
DijkstraNode<T>* dnStart = getOrCreateNode(cur);
dnStart->cumWeight = 0;
// add this node to the processing list
toBeProcessedNodes.add(dnStart);
// add this node to the processing list (and mark it as "enqueued")
toBeProcessedNodes.addOnce(dnStart, dnStart->cumWeight);
// until we are done
while(unlikely(!toBeProcessedNodes.empty())) {
@@ -66,28 +63,14 @@ public:
// stop when end was reached??
//if (dnSrc->element == &end) {break;}
// process each neighbor of the current element
// visit (and maybe update) each neighbor of the current element
for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
// get the neighbor itself
const T* dst = acc.getNeighbor(*dnSrc->element, i);
// get-or-create a node for the neighbor
DijkstraNode<T>* dnDst = getNode(dst);
// get-or-create the edge describing the connection
//const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
const auto edge = getEdge(dnSrc, dnDst);
// was this edge already processed? -> skip it
if (usedEdges.find(edge) != usedEdges.end()) {continue;}
// otherwise: remember it
usedEdges.insert(edge);
// and add the node for later processing
//toBeProcessedNodes.push_back(dnDst);
toBeProcessedNodes.add(dnDst);
// get-or-create a DijkstraNode for the neighbor
DijkstraNode<T>* dnDst = getOrCreateNode(dst);
// get the distance-weight to the neighbor
const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
@@ -96,8 +79,17 @@ public:
// update the weight to the destination?
const float potentialWeight = dnSrc->cumWeight + weight;
if (potentialWeight < dnDst->cumWeight) {
// re-sort (weight has changed) within the list of to-be-processed nodes
toBeProcessedNodes.addOrUpdate(dnDst, dnDst->cumWeight, potentialWeight);
dnDst->cumWeight = potentialWeight;
dnDst->previous = dnSrc;
} else {
// if this neighbor was encountered for the first time, add it (and mark it as "enqueued")
toBeProcessedNodes.addOnce(dnDst, dnDst->cumWeight);
}
}
@@ -114,30 +106,67 @@ private:
/** helper class to sort to-be-processed nodes by their distance from the start */
class ToProcess {
/** sort comparator */
struct setComp {
bool operator() (const DijkstraNode<T>* dn1, const DijkstraNode<T>* dn2) {
return dn1->cumWeight < dn2->cumWeight;
/** assign a weight to a node and provide the corresponding comparator */
struct WeightedNode {
DijkstraNode<T>* dn;
const float weight;
/** ctor */
WeightedNode(DijkstraNode<T>* dn, const float weight) : dn(dn), weight(weight) {;}
/** compare by weight. same weight? : compare by pointer */
bool operator < (const WeightedNode& wn) const {
return (weight != wn.weight) ? (weight < wn.weight) : (dn < wn.dn);
}
};
/** sorted list of to-be-processed nodes */
std::set<DijkstraNode<T>*, setComp> toBeProcessedNodes;
std::set<WeightedNode> nodes;
public:
/** add a new to-be-processed node */
void add(DijkstraNode<T>* node) {toBeProcessedNodes.insert(node);}
void addOnce(DijkstraNode<T>* node, const float weight) {
// skip nodes that were already enqueued
if (node->enqueued) {return;}
// add the combination (node+weight)
nodes.insert(WeightedNode(node, weight));
// mark the node as processed
node->enqueued = true;
}
/** add a new to-be-processed node or update its old weight to the new one */
void addOrUpdate(DijkstraNode<T>* node, const float oldWeight, const float newWeight) {
// find and remove the previous combination (node+weight) if any
const auto old = nodes.find(WeightedNode(node, oldWeight));
if (old != nodes.end()) {nodes.erase(old);}
// add the new combination (node+weight)
nodes.insert(WeightedNode(node, newWeight));
// mark the node as processed
node->enqueued = true;
}
/** get the next to-be-processed node (smallest distance) */
DijkstraNode<T>* pop() {
DijkstraNode<T>* next = *toBeProcessedNodes.begin();
toBeProcessedNodes.erase(toBeProcessedNodes.begin());
return next;
DijkstraNode<T>* dn = (*nodes.begin()).dn;
nodes.erase(nodes.begin());
return dn;
}
/** set empty? */
bool empty() const {return toBeProcessedNodes.empty();}
bool empty() const {
return nodes.empty();
}
};
@@ -145,7 +174,7 @@ private:
/** get (or create) a new node for the given user-node */
inline DijkstraNode<T>* getNode(const T* userNode) {
inline DijkstraNode<T>* getOrCreateNode(const T* userNode) {
auto it = nodes.find(userNode);
if (unlikely(it == nodes.end())) {
DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);

2
nav/dijkstra/DijkstraPath.h
View File

@@ -48,6 +48,8 @@ public:
const DijkstraNode<T>& operator [] (const int idx) const {return *(path[idx]);}
size_t size() const {return path.size();}
/** get the idx-th element from the starting-node. if this is beyond the end, the last node is returned */
const DijkstraNode<T>& getFromStart(const int idx) const {return (idx >= (int) path.size()) ? (*path.back()) : (*path[idx]);}
/** NANOFLANN: number of elements in the path */
inline int kdtree_get_point_count() const {

11
nav/dijkstra/DijkstraStructs.h
View File

@@ -19,15 +19,12 @@ template <typename T> struct DijkstraNode {
/** the weight from the start up to this element */
float cumWeight;
/** whether we already enqueued this node into the processing list (do NOT examing nodes twice) */
bool enqueued;
/** ctor */
DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;}
// /** equal? (bi-dir) */
// bool operator == (const DijkstraNode<T>& other) const {
// return element == other.element;
// }
DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF), enqueued(false) {;}
};

129
sensors/MACAddress.h Normal file
View File

@@ -0,0 +1,129 @@
#ifndef MACADDRESS_H
#define MACADDRESS_H
#include <cstdint>
#include <string>
#include "../Exception.h"
/**
* describes a MAC-Address as 64-bit integer.
* provides 8-bit access to all 6 values
*/
union MACAddress {
private:
/** fieldwise access */
struct {
uint8_t h5;
uint8_t h4;
uint8_t h3;
uint8_t h2;
uint8_t h1;
uint8_t h0;
} fields;
/** store as 64-bit integer */
uint64_t mac;
public:
/** empty ctor */
MACAddress() {
;
}
/** copy ctor */
MACAddress(const MACAddress& o) : mac(o.mac) {
;
}
/** ctor form long */
MACAddress(const uint64_t mac) : mac(mac) {
;
}
/** ctor form string (e.g. "xx:xx:xx:xx:xx:xx") */
MACAddress(const std::string& str) {
// sanity check
if (str.size() != 17) {throw Exception("invalid hex string length. must be 17");}
mac = 0; // all zeros
fields.h5 = hexWordToInt(str[ 0], str[ 1]);
fields.h4 = hexWordToInt(str[ 3], str[ 4]);
fields.h3 = hexWordToInt(str[ 6], str[ 7]);
fields.h2 = hexWordToInt(str[ 9], str[10]);
fields.h1 = hexWordToInt(str[12], str[13]);
fields.h0 = hexWordToInt(str[15], str[16]);
}
/** convert to hex-string ("xx:xx:xx:xx:xx:xx") */
std::string asString() const {
std::string str = ":::::::::::::::::";
intToHexStr(fields.h5, &str[ 0]);
intToHexStr(fields.h4, &str[ 3]);
intToHexStr(fields.h3, &str[ 6]);
intToHexStr(fields.h2, &str[ 9]);
intToHexStr(fields.h1, &str[12]);
intToHexStr(fields.h0, &str[15]);
return str;
}
/** get the mac address as a long-int value */
uint64_t asLong() const {
return mac;
}
/** equal? */
bool operator == (const MACAddress& o) const {
return o.asLong() == asLong();
}
private:
/** convert the given hex char [0-F] to an integer [0-15] */
static uint8_t hexCharToInt(const char _hex) {
// to upper case
const char hex = (_hex >= 'a') ? (_hex - ('a' - 'A')) : (_hex);
// convert
return (hex - '0' < 10) ? (hex - '0') : (hex - 'A' + 10);
}
/** convert the given hex-word to an integer */
static uint8_t hexWordToInt(const char hi, const char lo) {
return hexCharToInt(hi) << 4 | hexCharToInt(lo);
}
/** conver the given integer [0-15] to a hex char [0-F] */
static char intToHexChar(const uint8_t val) {
return (val < 10) ? ('0' + val) : ('A' - 10 + val);
}
/** insert two hex chars into the provided string buffer */
static void intToHexStr(const uint8_t val, char* dst) {
dst[0] = intToHexChar((val >> 4) & 0xF);
dst[1] = intToHexChar((val >> 0) & 0xF);
}
};
/** hash-method for MAC-Addresses */
namespace std {
template <> struct hash<MACAddress> {
std::size_t operator() (const MACAddress& mac) const {
return std::hash<uint64_t>()(mac.asLong());
}
};
}
#endif // MACADDRESS_H

35
sensors/radio/AccessPoint.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef ACCESSPOINT_H
#define ACCESSPOINT_H
#include "../MACAddress.h"
/**
* represents a Wi-Fi-AccessPoint
* an AP is represented by its MAC-Address and
* may provide a readably SSID
*/
class AccessPoint {
public:
/** the AP's MAC-Address */
const MACAddress mac;
/** the AP's readable SSID */
const std::string ssid;
public:
/** ctor */
AccessPoint(const MACAddress& mac, const std::string& ssid) : mac(mac), ssid(ssid) {
;
}
/** ctor */
AccessPoint(const std::string& mac, const std::string& ssid) : mac(mac), ssid(ssid) {
;
}
};
#endif // ACCESSPOINT_H

47
sensors/radio/LogDistanceModel.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef LOGDISTANCEMODEL_H
#define LOGDISTANCEMODEL_H
#include <cmath>
class LogDistanceModel {
public:
/** convert from RSSI to a distance (in meter) */
static float rssiToDistance(const float txPower, const float pathLoss, const float rssi) {
return pow(10, (txPower - rssi) / (10 * pathLoss));
}
/** convert from a distance (in meter) to the expected RSSI */
static float distanceToRssi(const float txPower, const float pathLoss, const float distance) {
if (distance <= 1) {return txPower;}
return (txPower - (10 * pathLoss * std::log10(distance)));
}
};
#include <vector>
class LogDistFastModel {
private:
std::vector<float> lut;
public:
LogDistFastModel() {
lut.resize(2000);
for (int i = 0; i < 2000; ++i) {
lut[i] = std::log10(i/10.0f);
}
}
float distanceToRssi(const float txPower, const float pathLoss, const float distance) const {
if (distance <= 1) {return txPower;}
return (txPower - (10 * pathLoss * lut[ (int) (distance*10) ]));
}
};
#endif // LOGDISTANCEMODEL_H

64
tests/nav/dijkstra/TestDijkstra.cpp
View File

@@ -48,4 +48,68 @@ TEST(Dijkstra, build) {
}
void build(Grid<GP>& grid) {
const int size = 10000;
const int gs = grid.getGridSize_cm();
for (int x = 0; x < size; x += gs) {
for (int y = 0; y < size; y += gs) {
grid.add(GP(x,y,0));
}
}
std::set<int> done;
for (int x = 0; x < size; x += gs) {
for (int y = 0; y < size; y += gs) {
const GridPoint gp1(x,y,0);
const int idx1 = grid.getNodeFor(gp1).getIdx();
for (int x1 = -gs; x1 <= +gs; x1 += gs) {
for (int y1 = -gs; y1 <= +gs; y1 += gs) {
const GridPoint gp2(x+x1, y+y1, 0);
if (grid.hasNodeFor(gp2)) {
int idx2 = grid.getNodeFor(gp2).getIdx();
if (done.find(idx2) != done.end()) {continue;}
grid.connectBiDir(idx1, idx2);
}
}
}
done.insert(idx1);
}
}
}
void dijkstra(Grid<GP>& grid) {
class TMP {
Grid<GP>& grid;
public:
TMP(Grid<GP>& grid) : grid(grid) {;}
int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
} tmp(grid);
Dijkstra<GP> d;
d.build(grid[0], grid[0], tmp);
}
TEST(Dijkstra, huge) {
Grid<GP> grid(10);
build(grid);
dijkstra(grid);
}
#endif

32
tests/sensors/TestMAC.cpp Normal file
View File

@@ -0,0 +1,32 @@
#ifdef WITH_TESTS
#include "../Tests.h"
#include "../../sensors/MACAddress.h"
TEST(MAC, ctorSize) {
ASSERT_THROW(MACAddress("12:34:56:78:9A:A"), std::exception);
MACAddress("12:34:56:78:9A:AB");
ASSERT_THROW(MACAddress("12:34:56:78:9A:ABC"), std::exception);
}
TEST(MAC, caseInsensitive) {
MACAddress mac1("12:34:56:78:9A:BC");
MACAddress mac2("12:34:56:78:9a:bc");
ASSERT_EQ(mac1, mac2);
}
TEST(MAC, convertLong) {
MACAddress mac1("12:34:56:78:9A:BC");
MACAddress mac2 = MACAddress( mac1.asLong() );
ASSERT_EQ(mac1, mac2);
}
#endif

37
tests/sensors/radio/TestLogDistanceModel.cpp Normal file
View File

@@ -0,0 +1,37 @@
#ifdef WITH_TESTS
#include "../../Tests.h"
#include "../../../sensors/radio/LogDistanceModel.h"
TEST(LogDistanceModel, calc) {
const float txp = -40;
const float exp = 1;
ASSERT_EQ(-40, LogDistanceModel::distanceToRssi(txp, exp, 1.0));
// only exponent changed. at 1.0 meter: no difference
ASSERT_EQ(LogDistanceModel::distanceToRssi(-40, 1.0, 1.0), LogDistanceModel::distanceToRssi(-40, 2.0, 1.0));
// distance increment
ASSERT_GT(LogDistanceModel::distanceToRssi(-40, 1.0, 1.0), LogDistanceModel::distanceToRssi(-40, 1.0, 2.0));
// exponent at more than 1.0m
ASSERT_GT(LogDistanceModel::distanceToRssi(-40, 1.0, 3.0), LogDistanceModel::distanceToRssi(-40, 1.1, 3.0));
// other txp
ASSERT_GT(LogDistanceModel::distanceToRssi(-40, 1.0, 1.0), LogDistanceModel::distanceToRssi(-45, 1.0, 1.0));
}
TEST(LogDistanceModel, forwardBackward) {
const float txp = -40;
const float exp = 1;
ASSERT_EQ(1.0, LogDistanceModel::rssiToDistance(txp, exp, LogDistanceModel::distanceToRssi(txp, exp, 1.0)));
ASSERT_EQ(5.0, LogDistanceModel::rssiToDistance(txp, exp, LogDistanceModel::distanceToRssi(txp, exp, 5.0)));
ASSERT_EQ(10.0, LogDistanceModel::rssiToDistance(txp, exp, LogDistanceModel::distanceToRssi(txp, exp, 10.0)));
}
#endif