diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index e5a5c00..f298ca4 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -25,17 +25,89 @@ void Navigation::loop() {
         gps->encode(Serial1.read());
 }
 
-Point Navigation::addCurrentPosToRoute() {
+bool Navigation::startNavigation() {
+    Point newTargetPoint = this->route->startRoute();
+    this->navigationStarted = this->setTargetPoint(newTargetPoint);
+    if (this->navigationStarted)
+        this->navigationFinished = false;
+    return this->navigationStarted;
+}
+
+CourseCorrection Navigation::getCourseCorrection() {
+    CourseCorrection courseCorrection = {0, 0};
+
+    Point currentPos = this->currentLocation();
+    double targetCourse = Navigation::courseTo(currentPos, this->targetPoint);
+    double distance = Navigation::distanceBetwenn(currentPos, this->targetPoint);
+
+    if (targetCourse < 0 || distance < 0 || this->navigationFinished)
+        return courseCorrection;
+
+    // Check if I need a new Point
+    if (distance < MIN_DISTANCE_BETWEEN_POINTS) {
+        bool goOn = this->nextPoint();
+        if (!goOn) {
+            this->navigationFinished = true;
+            this->navigationStarted = false;
+            return courseCorrection; // End of navigation
+        }
+    }
+
+    double currentCourse = gps->course.deg();
+    int16_t correction = currentCourse - targetCourse;
+    if (correction > 180)
+        correction -= 360;
+    else if (correction < -180)
+        correction += 360;
+
+    courseCorrection.correction = correction;
+    courseCorrection.distance = distance;
+    return courseCorrection;
+}
+
+bool Navigation::addCurrentPosToRoute() {
+    Point p = currentLocation();
+    if (!p.isValid()) { return false; }
+    if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetwenn(p, this->lastPoint)) {
+        this->route->addPointToRoute(p);
+        this->lastPoint = p;
+        return true;
+    }
+    return false;
+}
+
+Point Navigation::currentLocation() {
     Point p;
-
-
-
-    if (this->gps->location.isUpdated() && this->gps->location.isValid()) {
+    if (this->gps->location.isValid()) {
         p.lat = this->gps->location.lat();
         p.lon = this->gps->location.lng();
-        if (MIN_DISTANCE_BETWEEN_POINTS <= 
-            TinyGPSPlus::distanceBetween(p.lat, p.lon, this->lastPoint.lat, this->lastPoint.lon))
-            this->route->addPointToRoute(p);
     }
     return p;
 }
+
+bool Navigation::nextPoint() {
+    if (!this->navigationStarted)
+        return false;
+    Point newTargetPoint = this->route->getNextPoint();
+    return this->setTargetPoint(newTargetPoint);
+}
+
+bool Navigation::setTargetPoint(Point target) {
+    if (target.isValid()) {
+        this->targetPoint = target;
+        return true;
+    }
+    return false;
+}
+
+double Navigation::distanceBetwenn(Point p1, Point p2) {
+    if (p1.isValid() && p2.isValid())
+        return TinyGPSPlus::distanceBetween(p1.lat, p1.lon, p2.lat, p2.lon);
+    return -1;
+}
+
+double Navigation::courseTo(Point p1, Point p2) {
+    if (p1.isValid() && p2.isValid())
+        return TinyGPSPlus::courseTo(p1.lat, p1.lon, p2.lat, p2.lon);
+    return -1;
+}
\ No newline at end of file
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index 9c59047..8c504cf 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -19,21 +19,41 @@
 #include "route.h"
 
 #define MIN_DISTANCE_BETWEEN_POINTS 1
+
+struct CourseCorrection {
+    int16_t correction;
+    double distance;
+};
 class Navigation {
     public:
         Navigation(uint8_t rx, uint8_t tx);
         ~Navigation();
         void loop();
 
-        Point addCurrentPosToRoute();
+        void newRoute();
+
+        bool startNavigation();
+        CourseCorrection getCourseCorrection();
+
+        bool addCurrentPosToRoute();
 
         TinyGPSPlus* getGPS() { return this->gps; }
 
     private:
+        Point currentLocation();
+        bool nextPoint();
+        bool setTargetPoint(Point target);
+        static double distanceBetwenn(Point p1, Point p2);
+        static double courseTo(Point p1, Point p2);
+
         TinyGPSPlus* gps;
         Route* route;
 
         Point lastPoint;
+        Point targetPoint;
+
+        bool navigationStarted = false;
+        bool navigationFinished = false;
 };
 
 #endif // NAVIGATION_H
\ No newline at end of file
diff --git a/lib/Navigation/route.cpp b/lib/Navigation/route.cpp
index a2d9135..346075c 100644
--- a/lib/Navigation/route.cpp
+++ b/lib/Navigation/route.cpp
@@ -20,13 +20,17 @@ void Route::addPointToRoute(Point point) {
 }
 
 Point Route::startRoute() {
+    if (this->points.size() < 1)
+        return Point(0, 0);
     *this->it = this->points.begin();
     return **this->it;
 }
 
 Point Route::getNextPoint() {
-    if (*this->it != this->points.end())
+    if (*this->it != this->points.end()) {
+        this->it++;
         return **this->it;
+    }
     else
         return Point(0, 0);
 }
\ No newline at end of file
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index 55746fd..5bd9589 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -1,9 +1,18 @@
+/**
+ * @file autopilot.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-02
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
 #include "driveModi/Modi/Autopilot/autopilot.h"
 
 Autopilot::Autopilot() {
-}
 
-void Autopilot::init() {
 }
 
 void Autopilot::runAutopilot() {
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index f1f0b3c..8200c93 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -1,25 +1,33 @@
+/**
+ * @file autopilot.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-02
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
 #ifndef AUTOPILOT_H
 #define AUTOPILOT_H
 
-#include <TinyGPS++.h>
 
-#include "route.h"
+#include "navigation.h"
 #include "moveControl.h"
-#include "debugMqtt.h"
 #include "driveModi/driveModi.h"
 
 class Autopilot : DriveModi{
     public:
         Autopilot();
 
-        void init();
         void loop();
         void runAutopilot();
 
 
     private:
-        MoveControl *moveControl;
-        DebugMqtt *debug;
+        MoveControl* moveControl;
+        Navigation* navigation;
 
 };
 
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h
index 0086e18..1e58412 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.h
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h
@@ -14,9 +14,13 @@ class CaptureRoute : public ManualControl {
         void loop();
         void runCaptureRoute();
 
+        Navigation* getNavigation() const { return this->navigation; }
+
     private:
         Navigation* navigation;
 
+        uint16_t savedPoints = 0;
+
         uint32_t last_millis = 0;
         uint16_t delay = 200;
 };