diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt
index 5a23374..7dcd6bb 100644
--- a/doc/TODO allgemein.txt	
+++ b/doc/TODO allgemein.txt	
@@ -5,12 +5,8 @@ Für irgendwann:
     -> Update GNSS Lib to v3
 
 Besser zügig:
-    Speedometer buffer ergibt kaum Sinn
-    Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
     Doxygen Kommentare aktualisieren
-    Motortreiber vielleicht bei wenigster Leistung ohne Kurve
-
-    Capture Route Punkte auch nicht zu weit auseinander
-    Autopilot Funktionen zum einstellen der Geschwindigkeiten
-    Speedometer zweiter Buffer mit auslesezeiten, weil sonst Sinnlos da kein zeitliche Refernez zu berechnen vorhanden
+    Motortreiber vielleicht bei stoppen ohne Kurve
+    
+    Autopilot Funktionen zum einstellen der Geschwindigkeiten
     
diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index e09e260..064c2c4 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -174,7 +174,9 @@ Navigation::Status Navigation::addCurrentPosToRoute() {
     }
 
     // Every Point after the first
-    if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPointRouteInsert)) {
+    double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
+    if (MIN_DISTANCE_BETWEEN_POINTS <= distance
+        && MAX_DISTANCE_BETWEEN_POINTS >= distance){
         this->route->addPointToRoute(this->currentPosition);
         this->lastPointRouteInsert = this->currentPosition;
         return Status::Updated;
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index 43c552d..17091a8 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -30,6 +30,7 @@
  * 
  */
 #define MIN_DISTANCE_BETWEEN_POINTS 0.3
+#define MAX_DISTANCE_BETWEEN_POINTS 10
 #define MIN_DISTANCE_TO_REACH_POINT 0.5
 #define AZIMUTH_UPDATE_DELAY 20
 
diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h
index 6a66c69..55f05eb 100644
--- a/lib/Speedometer/speedometer.h
+++ b/lib/Speedometer/speedometer.h
@@ -46,7 +46,7 @@ class Speedometer {
          * 
          * If the Direction is Forward, the internal counter counts up and a positiv speed will be returned.
          * If the Direction is Backward, the internal counter counts down and a negativ speed will be returned.
-         * If the Dorection is None, no measurement will be taken.
+         * If the Direction is None, no measurement will be taken.
          */
         enum Direction {
             None,