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

src/driveModi/Modi/Autopilot/autopilot.h

@@ -24,6 +24,14 @@ class Autopilot : public ManualControl {
         void loop();
         void runAutopilot();
 
+        RouteInfo getRouteInfo() const { return this->routeInfo; }
+        CourseCorrection getCourseCorrection() const { return this->courseCorrection; }
+        bool getNavigationStarted() const { return this->navigationStarted; }
+        bool getNavigationEnded() const { return this->navigationEnded; }
+        bool getSelfDriving() const { return this->selfDriving; }
+        bool getSelfDrivingAvailable() const { return this->selfDrivingAvailable; }
+        
+        bool shouldUpdate();
 
     private:
         void setSelfDriving(bool val);
@@ -32,9 +40,13 @@ class Autopilot : public ManualControl {
 
         Navigation* navigation;
         CourseCorrection courseCorrection;
+        RouteInfo routeInfo;
 
         bool navigationStarted = false;
+        bool navigationEnded = false;
         bool selfDriving = false;
+        bool selfDrivingAvailable = false;
+        bool updateDisplay = false;
 
         uint16_t last_millis = 0;
         uint8_t delay = 40;