Points are now saveable in captureRoute

lib/Navigation/navigation.cpp

@@ -78,9 +78,22 @@ CourseCorrection Navigation::getCourseCorrection() {
 }
 
 bool Navigation::addCurrentPosToRoute() {
-    Point p = currentLocation();
+    std::cout << "Navigation::addCurrentPosToRoute 1" << std::endl;
+    Point p = this->currentLocation();
+    std::cout << "Navigation::addCurrentPosToRoute " << p.lat << " " << p.lon << std::endl;
     if (!p.isValid()) { return false; }
+
+    // First Point
+    if (this->route->getRouteInfo().totalPoints == 0) {
+        std::cout << "Navigation::addCurrentPosToRoute 2" << std::endl;
+        this->route->addPointToRoute(p);
+        this->lastPoint = p;
+        return true;
+    }
+
+    // Ever Point after the first
     if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetwenn(p, this->lastPoint)) {
+        std::cout << "Navigation::addCurrentPosToRoute 3" << std::endl;
         this->route->addPointToRoute(p);
         this->lastPoint = p;
         return true;

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -18,10 +18,11 @@ void CaptureRoute::loop() {
 
 void CaptureRoute::runCaptureRoute() {
     if (Ps3.data.button.triangle) {
-        this->navigation->addCurrentPosToRoute();
+        if (this->navigation->addCurrentPosToRoute()) {
-        this->routeInfo.totalPoints++;
+            this->routeInfo = navigation->getRouteInfo();
             this->updateDisplay = true;
         }
+    }
 }
 
 bool CaptureRoute::shouldUpdate() {