refactor, added akku menu, finished autopilot menu

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -15,33 +15,65 @@ Autopilot::Autopilot(MoveControl* moveControl, Navigation* navigation)
     : ManualControl(moveControl) {
         this->navigation = navigation;
         this->navigationStarted = this->navigation->startNavigation();
+        this->navigation->getRouteInfo();
         this->courseCorrection.correction = 0;
         this->courseCorrection.distance = 0;
+        this->updateDisplay = true;
 }
 
 void Autopilot::loop() {
-    if (!this->selfDriving)
+    if (!this->selfDriving && this->navigationStarted)
         ManualControl::loop();
 
-    if (millis() - this->last_millis < delay) {
+    if (millis() - this->last_millis < delay)
         return;
-    }
+
     this->runAutopilot();
     this->last_millis = millis();
 }
 
 void Autopilot::runAutopilot() {
+    if (!this->navigationStarted || this->navigationEnded)
+        return;
+
     this->courseCorrection = this->navigation->getCourseCorrection();
+    this->routeInfo = this->navigation->getRouteInfo();
+    this->updateDisplay = true;
+
+    // Check if start Point is near to current Location
+    if (this->routeInfo.currentPoint >= 2)
+        this->selfDrivingAvailable = true;
+
+    if (!this->selfDriving && this->selfDrivingAvailable)
+        this->setSelfDriving(Ps3.data.button.triangle);
+
+    if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
+        this->navigationEnded = true;
+        this->navigationStarted = false;
+        this->selfDrivingAvailable = false;
+        this->setSelfDriving(false);
+    }
+
     if (selfDriving) {
         this->setSpeedInRelToDistance(this->courseCorrection.distance);
         this->setRotInRelToDistance(this->courseCorrection.correction);
+    }   
+}
+
+bool Autopilot::shouldUpdate() {
+    if (this->updateDisplay) {
+        this->updateDisplay = false;
+        return true;
     }
+    return false;
 }
 
 void Autopilot::setSelfDriving(bool val) {
     if (!this->navigationStarted)
         return;
 
+    this->updateDisplay = true;
+
     this->selfDriving = val;
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationspeed(0);
@@ -68,4 +100,4 @@ void Autopilot::setRotInRelToDistance(int16_t course) {
         this->moveControl->setRotationspeed(0);
     else
         this->moveControl->setRotationspeed(steps);
-}
+}