added restart to autopilot menu and

disable autopilot by pressing the actionbutton

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();

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);