diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 7a9b22a..9f20e18 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -86,13 +86,21 @@ void MenuAutopilot::printPage() const {
             lineTwo.concat(correction.correction);
             break;
 
-        case 2:
-            lineOne = "Target waypoint";
-            lineTwo = "";
-            lineTwo.concat(this->autopilot->getRouteInfo().currentPoint);
-            lineTwo.concat(" from ");
-            lineTwo.concat(this->autopilot->getRouteInfo().totalPoints);
+        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);
+            }
             break;
+        }
 
         case 3: {
             NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
@@ -131,6 +139,12 @@ void MenuAutopilot::printPage() const {
                 lineOne.concat("0");
             break;
 
+        case 6:
+            lineOne = "Restart the";
+            lineTwo = "Autopilot";
+            this->autopilot->restart();
+            break;
+
         default:
             this->printDefault();
             return;
@@ -139,6 +153,17 @@ void MenuAutopilot::printPage() const {
     this->print(lineOne, lineTwo);
 }
 
+void MenuAutopilot::runCommand() const {
+    switch (this->getCountPages()) {
+        case 6:
+            this->autopilot->restart();
+            break;
+        
+        default:
+            break;
+    }
+}
+
 void MenuAutopilot::update() {
     if (!this->autopilot->shouldUpdate())
         return;
@@ -148,7 +173,7 @@ void MenuAutopilot::update() {
 }
 
 void MenuAutopilot::init() {
-    this->setCountPages(6);
+    this->setCountPages(7);
     this->driveManager->changeModus(Modi::Autopilot);
     this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
     this->routeInfo = this->autopilot->getRouteInfo();
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h
index 7781e06..0a3fee9 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h
@@ -37,6 +37,7 @@ class MenuAutopilot : public MenuDriveMode {
          * and save the Autopilot object.
          */
         void printPage() const override;
+        void runCommand() const override;
 
         /**
          * @brief can be called to update shown data
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index 37d50c0..7fbbc25 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -14,17 +14,7 @@
 Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
     : ManualControl(moveControl, input) {
         this->navigation = navigation;
-        if (this->navigation->startNavigation())
-            this->state = State::NavigationStarted;
-        else
-            this->state = State::NoRoute;
-        this->routeInfo = this->navigation->getRouteInfo();
-
-        this->lastState = this->navigation->getNTRIPClient()->getClientState();
-
-        this->courseCorrection.correction = 0;
-        this->courseCorrection.distance = 0;
-        this->updateDisplay = true;
+        this->init();
 }
 
 Autopilot::~Autopilot() {
@@ -51,6 +41,7 @@ void Autopilot::runAutopilot() {
         this->moveControl->setSpeed(0);
         this->moveControl->setRotationSpeed(0);
         this->updateDisplay = true;
+        return;
     }
 
     this->routeInfo = this->navigation->getRouteInfo();
@@ -72,12 +63,22 @@ void Autopilot::runAutopilot() {
         this->updateDisplay = true;
     }
 
+    if (this->state == State::SelfDriving
+        && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
+            this->state = State::SelfDrivingAvailable;
+            this->updateDisplay = true;
+    }
+
     if (this->state == State::SelfDriving) {
         this->setSpeedInRelToDistance(this->courseCorrection.distance);
         this->setRotInRelToDistance(this->courseCorrection.correction);
     }   
 }
 
+void Autopilot::restart() {
+    this->init();
+}
+
 bool Autopilot::shouldUpdate() {
     if (this->updateDisplay) {
         this->updateDisplay = false;
@@ -86,6 +87,20 @@ bool Autopilot::shouldUpdate() {
     return false;
 }
 
+void Autopilot::init() {
+    if (this->navigation->startNavigation())
+            this->state = State::NavigationStarted;
+        else
+            this->state = State::NoRoute;
+        this->routeInfo = this->navigation->getRouteInfo();
+
+        this->lastState = this->navigation->getNTRIPClient()->getClientState();
+
+        this->courseCorrection.correction = 0;
+        this->courseCorrection.distance = 0;
+        this->updateDisplay = true;
+}
+
 void Autopilot::checkNtrip() {
     NTRIPClient* client = this->navigation->getNTRIPClient();
     NTRIPClientStates state = client->getClientState();
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index 43b23e6..2af0554 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -76,6 +76,8 @@ class Autopilot : public ManualControl {
          */
         void runAutopilot();
 
+        void restart();
+
         /**
          * @brief Get the Route Info object
          * 
@@ -106,6 +108,7 @@ class Autopilot : public ManualControl {
         bool shouldUpdate();
 
     private:
+        void init();
         void checkNtrip();
         void setSpeedInRelToDistance(double distance);
         void setRotInRelToDistance(int16_t course);