add menuTest

lib/Navigation/navigation.cpp

@@ -66,16 +66,14 @@ void Navigation::init(Route* route) {
     Wire.write(0x01);
     Wire.endTransmission();
     this->compass->setMode(0x01,0x0C,0x10,0X00);
+    this->compass->setCalibration(-1903, 367, -893, 1468, -1110, 310);
 }
 
 Navigation::~Navigation() {
     delete this->gps;
-
-    if (this->isNtripInit)
-        delete this->ntripClient;
-
-    if (this->route)
-        delete this->route;
+    delete this->compass;
+    delete this->ntripClient;
+    delete this->route;
 }
 
 void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
@@ -130,7 +128,7 @@ CourseCorrection Navigation::getCourseCorrection() {
         return courseCorrection;
 
     // Check if I need a new Point
-    if (distance < MIN_DISTANCE_BETWEEN_POINTS) {
+    if (distance < MIN_DISTANCE_TO_REACH_POINT) {
         bool goOn = this->nextPoint();
         if (!goOn) {
             this->navigationFinished = true;

lib/Navigation/navigation.h

@@ -29,6 +29,7 @@
  * 
  */
 #define MIN_DISTANCE_BETWEEN_POINTS 0.1
+#define MIN_DISTANCE_TO_REACH_POINT 3 // TODO: Only for testing!
 #define AZIMUTH_UPDATE_DELAY 20
 
 /**