optimized navigation

lib/Navigation/navigation.cpp

@@ -144,6 +144,9 @@ CourseCorrection Navigation::getCourseCorrection() {
     else if (correction < -180)
         correction += 360;
 
+    // //Rotate result by 180° to corrigate Azimuth
+    // correction = correction > 180 ? correction -360 : correction;
+
     courseCorrection.correction = correction;
     courseCorrection.distance = distance;
     return courseCorrection;

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -25,11 +25,11 @@ void Autopilot::loop() {
     if (!this->selfDriving && this->navigationStarted)
         ManualControl::loop();
 
-    if (millis() - this->lastMillis < delay)
+    if (millis() - this->loopLastMillis < loopDelayMillis)
         return;
 
     this->runAutopilot();
-    this->lastMillis = millis();
+    this->loopLastMillis = millis();
 }
 
 void Autopilot::runAutopilot() {
@@ -38,14 +38,20 @@ void Autopilot::runAutopilot() {
 
     this->courseCorrection = this->navigation->getCourseCorrection();
     this->routeInfo = this->navigation->getRouteInfo();
-    this->updateDisplay = true;
+
+    if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) {
+        this->updateDisplay = true;
+        this->displayUpdateLastMillis = millis();
+    }
 
     // Check if start Point is near to current Location
     if (this->routeInfo.currentPoint >= 2)
         this->selfDrivingAvailable = true;
 
-    if (!this->selfDriving && this->selfDrivingAvailable)
+    if (!this->selfDriving 
-        this->setSelfDriving(ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action));
+        && this->selfDrivingAvailable 
+        && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action))
+        this->setSelfDriving(true);
 
     if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
         this->setSelfDriving(false);

src/driveModi/Modi/Autopilot/autopilot.h

@@ -42,7 +42,7 @@ class Autopilot : public ManualControl {
          * 
          * This function should be called every main loop. If self
          * driving is activated the function call run Autopilot. If
-         * the delay is not reached the function returns immediately.
+         * the loopDelayMillis is not reached the function returns immediately.
          * 
          * If self driving is not activated this functions calls the 
          * ManualControl loop additionally.
@@ -127,8 +127,10 @@ class Autopilot : public ManualControl {
         bool selfDrivingAvailable = false;
         bool updateDisplay = false;
 
-        uint16_t lastMillis = 0;
+        uint8_t loopDelayMillis = 40;
-        uint8_t delay = 40;
+        uint16_t displayUpdateDelayMillis = 1000;
+        uint32_t displayUpdateLastMillis = 0;
+        uint32_t loopLastMillis = 0;
 };
 
 #endif // AUTOPILOT_H

src/driveModi/Modi/Autopilot/autopilotConfig.h

@@ -49,6 +49,6 @@
  * This value is used by the setRotInRelToDistance function and is used to calculate
  * the value for setRotationSpeed.
  */
-#define COURSE_CORRECTION_FACTOR 30
+#define COURSE_CORRECTION_FACTOR 60
 
 #endif // AUTOPILOT_CONFIG_H

src/driveModi/driveManager.cpp

@@ -52,9 +52,11 @@ void DriveManager::nextModus() {
 void DriveManager::changeModus(Modi modus) {
     this->currentModus = modus;
 
-    //Set moveControl to a safe state
     delete this->currentModusPtr;
+    //Reset Route to first point
+    this->navigation->startNavigation();
 
+    //Set moveControl to a safe state
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationSpeed(0);
     this->moveControl->setDrivingStatus(DrivingStatus::stop);