diff --git a/Exception.h b/Exception.h index 43bc3c1..1e72f78 100755 --- a/Exception.h +++ b/Exception.h @@ -4,6 +4,10 @@ #include #include +#ifdef ANDROID +#include +#endif + class Exception : public std::exception { private: @@ -14,7 +18,14 @@ private: public: /** ctor */ - Exception(const std::string& str) : str(str) {;} + Exception(const std::string& str) : str(str) { + + // TODO better solution? + #ifdef ANDROID + QMessageBox::question(nullptr, "Exception", str.c_str(), QMessageBox::Ok); + #endif + + } const char* what() const throw() {return str.c_str();} diff --git a/geo/Angle.h b/geo/Angle.h index eec89ba..5732897 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -5,6 +5,8 @@ #include "../Assertions.h" #include "Point2.h" +#define PI ((float) M_PI) + struct Angle { @@ -14,7 +16,7 @@ public: static float getRAD_2PI(const float x1, const float y1, const float x2, const float y2) { Assert::isFalse( (x1==x2)&&(y1==y2), "(x1,y1) must not equal (x2,y2)!!"); const float tmp = std::atan2(y2-y1, x2-x1); - return (tmp < 0) ? (tmp + 2*M_PI) : (tmp); + return (tmp < 0) ? (tmp + 2*PI) : (tmp); } /** get the radians from (0,0) to (p.x,p.y) between 0 (to-the-right) and <2_PI */ @@ -34,10 +36,10 @@ public: * - as a change-in-direction between [0:PI] */ static float getDiffRAD_2PI_PI(const float r1, const float r2) { - Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); - Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); + Assert::isBetween(r1, 0.0f, (2*PI), "r1 out of bounds"); + Assert::isBetween(r2, 0.0f, (2*PI), "r2 out of bounds"); float tmp = std::abs(r1-r2); - return (tmp <= M_PI) ? (tmp) : (2*M_PI-tmp); + return (tmp <= PI) ? (tmp) : (2*PI-tmp); } /** @@ -46,23 +48,23 @@ public: * - as a change-in-direction between [-PI:+PI] */ static float getSignedDiffRAD_2PI(const float r1, const float r2) { - Assert::isBetween(r1, 0.0f, (float)(2*M_PI), "r1 out of bounds"); // [0:360] deg - Assert::isBetween(r2, 0.0f, (float)(2*M_PI), "r2 out of bounds"); // [0:360] deg + Assert::isBetween(r1, 0.0f, (float)(2*PI), "r1 out of bounds"); // [0:360] deg + Assert::isBetween(r2, 0.0f, (float)(2*PI), "r2 out of bounds"); // [0:360] deg float diff = r1-r2; - if (diff > +M_PI) {diff = -(2*M_PI - diff);} - else if (diff < -M_PI) {diff = +(2*M_PI + diff);} - Assert::isBetween(diff, (float)-M_PI, (float)(+M_PI), "result out of bounds"); // [-180:+180] deg + if (diff > +PI) {diff = -(2*PI - diff);} + else if (diff < -PI) {diff = +(2*PI + diff);} + Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds"); // [-180:+180] deg return diff; } /** convert degrees to radians */ static constexpr inline float degToRad(const float deg) { - return deg / 180.0f * M_PI; + return deg / 180.0f * PI; } /** convert radians to degrees */ static constexpr inline float radToDeg(const float rad) { - return rad * 180.0f / M_PI; + return rad * 180.0f / PI; } /** get a pointer vector (length 1) pointing to the given angle (in radians) */ @@ -74,4 +76,6 @@ public: }; +#undef PI + #endif // ANGLE_H diff --git a/geo/Heading.h b/geo/Heading.h index 51da517..eb9720e 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -8,7 +8,7 @@ struct Heading { -#define _2PI (2*M_PI) +#define _2PI (2*(float)M_PI) private: @@ -105,10 +105,10 @@ public: }; namespace Headings { - static const Heading RIGHT = Heading(M_PI*0/2); - static const Heading UP = Heading(M_PI*1/2); - static const Heading LEFT = Heading(M_PI*2/2); - static const Heading DOWN = Heading(M_PI*3/2); + static const Heading RIGHT = Heading((float)M_PI*0.0f/2.0f); + static const Heading UP = Heading((float)M_PI*1.0f/2.0f); + static const Heading LEFT = Heading((float)M_PI*2.0f/2.0f); + static const Heading DOWN = Heading((float)M_PI*3.0f/2.0f); } #endif // HEADING_H diff --git a/grid/walk/v2/modules/WalkModuleFollowDestination.h b/grid/walk/v2/modules/WalkModuleFollowDestination.h index 556f9e4..65532b9 100644 --- a/grid/walk/v2/modules/WalkModuleFollowDestination.h +++ b/grid/walk/v2/modules/WalkModuleFollowDestination.h @@ -20,7 +20,7 @@ private: const Grid& grid; Dijkstra dijkstra; - const DijkstraNode* dnDest; + const DijkstraNode* dnDest = nullptr; struct DijkstraAccess { const Grid& grid; @@ -58,9 +58,14 @@ public: /** get the shortest path from the given start to the configured destination */ DijkstraPath getShortestPath(const Node& start) { + + // destination unknown? -> empty path + if (!dnDest) {return DijkstraPath();} + const DijkstraNode* dnStart = dijkstra.getNode(start); const DijkstraPath path(dnStart, dnDest); return path; + } virtual void updateBefore(WalkState& state, const Node& startNode) override { diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index 2b31312..01fe668 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/grid/walk/v2/modules/WalkModuleHeadingControl.h @@ -14,7 +14,7 @@ template class WalkModuleH private: /** CURRENTLY NOT USED van-Mises distribution */ - Distribution::LUT dist; + Distribution::LUT dist; /** random noise */ Distribution::Normal distNoise; @@ -25,7 +25,7 @@ public: /** ctor 3.0 should be OK! */ WalkModuleHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) : - dist(Distribution::VonMises(0.0f, 2.0).getLUT()), + dist(Distribution::VonMises(0.0f, 2.0f).getLUT()), distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)), ctrl(ctrl) { diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index 35d8c7c..e2cfce2 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -56,6 +56,12 @@ public: double prob = 1; int numMatchingAPs = 0; + // after some runtime, check whether the wifi timestamps make sense + // those must not be zero, otherwise something is wrong! + if (!obs.entries.empty()) { + Assert::isFalse(curTime.ms() > 10000 && obs.entries.front().ts.isZero(), "WiFiMeasurement timestamp is 0. this does not make sense..."); + } + // process each observed measurement for (const WiFiMeasurement& measurement : obs.entries) {