diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index f7afabb..7206493 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -130,8 +130,9 @@ bool Navigation::getCourseCorrection(CourseCorrection& correction) {
         return false;
 
     // Check if I need a new Point
-    if (distance < MIN_DISTANCE_TO_REACH_POINT && this->preventNextPoint == false) {
+    if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
         if (!this->nextPoint()) {
+            std::cout << "Navigation::getCourseCorrection finished" << std::endl;
             this->navigationFinished = true;
             this->navigationStarted = false;
             return false; // End of navigation
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index 08fdddf..0c7d57f 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -110,6 +110,9 @@ class Navigation {
          */
         bool startNavigation();
         void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
+        
+        double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
+        double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
 
         /**
          * @brief Get the Course Correction object
@@ -234,6 +237,8 @@ class Navigation {
         uint32_t lastMillis = 0;
         uint32_t ubxUpdateTime = 0;
 
+        double minDistanceToReachPoint = 0.5;
+
         static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
         static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
 
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 31f48c7..9bf3a70 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -86,21 +86,13 @@ void MenuAutopilot::printPage() const {
             lineTwo.concat(correction.correction);
             break;
 
-        case 2: {
-            uint16_t currentPoint = this->autopilot->getRouteInfo().currentPoint;
-            uint16_t totalPoints = this->autopilot->getRouteInfo().totalPoints;
-            if (currentPoint > totalPoints) {
-                lineOne = "Target reached";
-                lineTwo = "";
-            } else {
-                lineOne = "Target waypoint";
-                lineTwo = "";
-                lineTwo.concat(currentPoint);
-                lineTwo.concat(" from ");
-                lineTwo.concat(totalPoints);
-            }
+        case 2:
+            lineOne = "Target waypoint";
+            lineTwo = "";
+            lineTwo.concat(this->autopilot->getRouteInfo().currentPoint);
+            lineTwo.concat(" from ");
+            lineTwo.concat(this->autopilot->getRouteInfo().totalPoints);
             break;
-        }
 
         case 3: {
             NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
@@ -142,7 +134,6 @@ void MenuAutopilot::printPage() const {
         case 6:
             lineOne = "Restart the";
             lineTwo = "Autopilot";
-            this->autopilot->restart();
             break;
 
         case 7: