From f2b63f1ce6ce8d7e4927bc2b0a40afa6881a8b24 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Thu, 11 May 2023 10:54:41 +0200
Subject: [PATCH] optimized navigation

---
 lib/Navigation/navigation.cpp                  |  3 +++
 src/driveModi/Modi/Autopilot/autopilot.cpp     | 16 +++++++++++-----
 src/driveModi/Modi/Autopilot/autopilot.h       |  8 +++++---
 src/driveModi/Modi/Autopilot/autopilotConfig.h |  2 +-
 src/driveModi/driveManager.cpp                 |  4 +++-
 5 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index 5812ae7..eb8b0c5 100644
--- a/lib/Navigation/navigation.cpp
+++ b/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;
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index e7a1ded..0fdb4d7 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/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)
-        this->setSelfDriving(ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action));
+    if (!this->selfDriving 
+        && this->selfDrivingAvailable 
+        && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action))
+        this->setSelfDriving(true);
 
     if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
         this->setSelfDriving(false);
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index b1323db..b9d8998 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/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 delay = 40;
+        uint8_t loopDelayMillis = 40;
+        uint16_t displayUpdateDelayMillis = 1000;
+        uint32_t displayUpdateLastMillis = 0;
+        uint32_t loopLastMillis = 0;
 };
 
 #endif // AUTOPILOT_H
diff --git a/src/driveModi/Modi/Autopilot/autopilotConfig.h b/src/driveModi/Modi/Autopilot/autopilotConfig.h
index 4157233..a2d6b89 100644
--- a/src/driveModi/Modi/Autopilot/autopilotConfig.h
+++ b/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
diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp
index 17a8391..0a17085 100644
--- a/src/driveModi/driveManager.cpp
+++ b/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);