From 8796da97b552cc2d61c2571b613d8b900f332dda Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Fri, 2 Jun 2023 22:30:21 +0200
Subject: [PATCH 01/27] - added wifi channel on boot screen - added wifi to
 systeminformation - added accuracy settings to capturee route and autopilot -
 fix data loss in calibrate compass - fix autopilot dont switch to ready for
 selfdriving

---
 include/network.h                             |  2 +
 lib/MotorControl/motorControl.h               |  2 +-
 lib/Navigation/navigation.h                   |  3 ++
 .../menuSysteminformatio.cpp                  |  8 +++-
 .../Systeminformation/menuSysteminformation.h |  1 +
 .../driveModi/Autopilot/menuAutopilot.cpp     | 17 ++++++-
 .../CalibrateCompass/menuCalibrateCompass.cpp | 18 ++++++--
 .../CaptureRoute/menuCaptureRoute.cpp         | 25 +++++++++-
 .../driveModi/CaptureRoute/menuCaptureRoute.h |  2 +
 src/driveModi/Modi/Autopilot/autopilot.cpp    | 14 +++---
 src/main.cpp                                  | 46 ++++++-------------
 src/network.cpp                               | 13 ++++++
 12 files changed, 104 insertions(+), 47 deletions(-)

diff --git a/include/network.h b/include/network.h
index 2a85d24..cf2cd9b 100644
--- a/include/network.h
+++ b/include/network.h
@@ -16,6 +16,7 @@
 #include <WiFi.h>
 #include <PubSubClient.h>
 #include <esp_now.h>
+#include <esp_wifi.h>
 
 #include "networkConfig.h"
 
@@ -44,6 +45,7 @@ class Network {
          */
         static bool connectWifi();
         static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send);
+        static uint8_t getCurrentChannel();
 
         /**
          * @brief Set all general settings to connect to a broker.
diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h
index 4880a42..9632e99 100644
--- a/lib/MotorControl/motorControl.h
+++ b/lib/MotorControl/motorControl.h
@@ -21,7 +21,7 @@
 #define PWMRES 8
 #define POWERSTEPS 2       //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
 #define PWMMIN 55
-#define PWMMAX 80         // Max 98% of 2^PWM_RES
+#define PWMMAX 90         // Max 98% of 2^PWM_RES
 
 /**
  * @brief A class which use PWM to control the power of DC Motor
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index 56f4366..f9665b3 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -185,6 +185,9 @@ class Navigation {
         uint16_t getAzimuth() const { return this->azimuth; }
         QMC5883LCompass* getCompass() const { return this->compass; }
 
+        Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
+        void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
+
         /**
          * @brief Set the output status for PVTdata.
          * 
diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
index 44793a2..9773c38 100644
--- a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
+++ b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
@@ -12,7 +12,7 @@
 #include "menuSysteminformation.h"
 
 MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) 
-            : MenuInformationSites(5) {
+            : MenuInformationSites(6) {
     this->mainBattery = mainBattery;
 }
 
@@ -44,6 +44,12 @@ void MenuSysteminformation::printPage() const {
             break;
 
         case 4:
+            lineOne = "Current WiFi";
+            lineTwo = "channel: ";
+            lineTwo.concat(Network::getCurrentChannel());
+            break;
+
+        case 5:
             lineOne = "Kleiax Rover by";
             lineTwo = "Alexander Klein";
             break;
diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformation.h b/src/SpecialMenus/Systeminformation/menuSysteminformation.h
index 2a046ab..cc51d39 100644
--- a/src/SpecialMenus/Systeminformation/menuSysteminformation.h
+++ b/src/SpecialMenus/Systeminformation/menuSysteminformation.h
@@ -17,6 +17,7 @@
 #include "controlPadInput.h"
 #include "menuInformationSites.h"
 #include "battery.h"
+#include "network.h"
 
 /**
  * @brief Prints information about the current system status.
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 4f7ac06..1c6bb46 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -158,6 +158,14 @@ void MenuAutopilot::printPage() const {
             lineTwo.concat(" - Decrease");
             break;
 
+        case 10:
+            lineOne = "Current minimal";
+            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+                lineTwo = "accuracy is high";
+            else 
+                lineTwo = "accuracy is low";
+            break;
+
         default:
             this->printDefault();
             return;
@@ -189,6 +197,13 @@ void MenuAutopilot::runCommand() const {
         case 9:
             this->minDistance = this->driveManager->getNavigation()->decreaseMinDistanceToReachPoint();
             break;
+
+        case 10:
+            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+                this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
+            else 
+                this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
+            break;
         
         default:
             break;
@@ -204,7 +219,7 @@ void MenuAutopilot::update() {
 }
 
 void MenuAutopilot::init() {
-    this->setCountPages(10);
+    this->setCountPages(11);
     this->driveManager->changeModus(Modi::Autopilot);
     this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
     this->routeInfo = this->autopilot->getRouteInfo();
diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp
index 82c18f1..013a5f5 100644
--- a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp
+++ b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp
@@ -77,20 +77,25 @@ void MenuCalibrateCompass::printPage() const {
             break;
 
         case 7:
+            lineOne = "Reset for new";
+            lineTwo = "calibration run";
+            break;
+
+        case 8:
             lineOne = "X min: ";
             lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]);
             lineTwo = "X max: ";
             lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]);
             break;
 
-        case 8:
+        case 9:
             lineOne = "Y min: ";
             lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]);
             lineTwo = "Y max: ";
             lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]);
             break;
 
-        case 9:
+        case 10:
             lineOne = "Z min: ";
             lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]);
             lineTwo = "Z max: ";
@@ -109,7 +114,7 @@ void MenuCalibrateCompass::init() {
     this->firstPrint = false;
     this->driveManager->changeModus(Modi::ManualControl);
     this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
-    this->setCountPages(10);
+    this->setCountPages(11);
     this->updateDelay = 500; 
 }
 
@@ -125,7 +130,6 @@ void MenuCalibrateCompass::runCommand() const {
                 case CalibrateCompass::State::Finished:
                     this->manualControl->setCalibrateCompass();
                     this->caliCompass->useData();
-                    this->caliCompass->reset();
                     break;
                 
                 default:
@@ -147,7 +151,11 @@ void MenuCalibrateCompass::runCommand() const {
 
         case 6:
             this->caliCompass->useData();
-            break;        
+            break;     
+
+        case 7:
+            this->caliCompass->reset();
+            break;   
         
         default:
             break;
diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
index 8d6846f..94da0a9 100644
--- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
+++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
@@ -91,6 +91,14 @@ void MenuCaptureRoute::printPage() const {
             else
                 lineOne.concat("0");
             break;
+
+        case 7:
+            lineOne = "Current minimal";
+            if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+                lineTwo = "accuracy is high";
+            else 
+                lineTwo = "accuracy is low";
+            break;
         
         default:
             this->printDefault();
@@ -106,8 +114,23 @@ void MenuCaptureRoute::update() {
     this->printMenu();
 }
 
+void MenuCaptureRoute::runCommand() const {
+    switch (this->getCurrentPage())
+    {
+    case 7:
+        if (this->driveManager->getNavigation()->getMinAccuracy() >= Point::Accuracy::twoDigOfCM)
+            this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::none);
+        else 
+            this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
+        break;
+    
+    default:
+        break;
+    }
+}
+
 void MenuCaptureRoute::init() {
-    this->setCountPages(7);
+    this->setCountPages(8);
     this->updateDelay = 500;
     this->driveManager->changeModus(Modi::CaptureRoute);
     this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();
diff --git a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h
index c29dfe0..3f321ea 100644
--- a/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h
+++ b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h
@@ -42,6 +42,8 @@ class MenuCaptureRoute : public MenuDriveMode {
          */
         void update() override;
 
+        void runCommand() const override;
+
     private:
         void init() override;
 
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index 46efabe..8fbc417 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -53,6 +53,7 @@ void Autopilot::runAutopilot() {
 
         case State::NavigationStarted:
             this->askNavigationForOrder();
+            this->checkButtonInput();
             break;
 
         case State::GetToStartPoint:
@@ -128,15 +129,14 @@ void Autopilot::checkButtonInput() {
     if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
         && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
         
-        if (this->state  == State::SelfDrivingAvailable) {
+        if (this->state  == State::SelfDrivingAvailable)
             this->state = State::SelfDriving;
-            this->updateDisplay = true;
-            this->lastAutopilotChangeMillis = millis();
-        } else if (this->state == State::SelfDriving) {
+        else if (this->state == State::SelfDriving)
             this->state = State::SelfDrivingAvailable;
-            this->updateDisplay = true;
-            this->lastAutopilotChangeMillis = millis();
-        }
+        else if (this->state == State::NavigationStarted)
+            this->state = State::GetToStartPoint;
+        this->updateDisplay = true;
+        this->lastAutopilotChangeMillis = millis();
     }
 }
 
diff --git a/src/main.cpp b/src/main.cpp
index a02a11b..163feed 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -118,7 +118,7 @@ void setup() {
     lcd->backlight();
     lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
     lcd->setCursor(0, 1);
-    lcd->print("Press PS-Button");
+    lcd->printf("WiFi channel %u", Network::getCurrentChannel());
     lcdWrapper = new LcdWrapper(lcd);
     lcdWrapper->setCallback(lcdWrapperCallback);
 
@@ -231,41 +231,25 @@ void makeMenu() {
     sys_m->setUpdateDelay(1500);
     rout_m->setLcd(lcdWrapper);
 
-
-
     // Entry for the main menu
-    MenuAction* mode_e = new MenuAction("Mode", mode_m);
-    MenuAction* gps_e = new MenuAction("GPS", gps_m);
-    MenuAction* pid_e = new MenuAction("PID", pid_m);
-    MenuAction* restart_e = new MenuAction("Restart", restart);
-    MenuAction* sys_e = new MenuAction("Systeminfo", sys_m);
-    MenuAction* rout_e = new MenuAction("Route", rout_m);
-    main_m->addEntry(mode_e);
-    main_m->addEntry(gps_e);
-    main_m->addEntry(rout_e);
-    main_m->addEntry(pid_e);
-    main_m->addEntry(sys_e);
-    main_m->addEntry(restart_e);
+    main_m->addEntry(new MenuAction("Mode", mode_m));
+    main_m->addEntry(new MenuAction("GPS", gps_m));
+    main_m->addEntry(new MenuAction("Route", rout_m));
+    main_m->addEntry(new MenuAction("PID", pid_m));
+    main_m->addEntry(new MenuAction("Systeminfo", sys_m));
+    main_m->addEntry(new MenuAction("Restart", restart));
 
     // Entry for the mode Menu
-    MenuAction* autopilot_e = new MenuAction("Autopilot", auto_m);
-    MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
-    MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
-    MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
-    MenuAction* testMode_e = new MenuAction("Test Mode", testM_m);
-    MenuAction* caliComp_e = new MenuAction("Gauge Compass", comp_m);
-    mode_m->addEntry(manualControl_e);
-    mode_m->addEntry(captureRoute_e);
-    mode_m->addEntry(autopilot_e);
-    mode_m->addEntry(caliComp_e);
-    mode_m->addEntry(testMode_e);
-    mode_m->addEntry(consolControl_e);
+    mode_m->addEntry(new MenuAction("Manual Control", man_m));
+    mode_m->addEntry(new MenuAction("Capture Route", cap_m));
+    mode_m->addEntry(new MenuAction("Autopilot", auto_m));
+    mode_m->addEntry(new MenuAction("Gauge Compass", comp_m));
+    mode_m->addEntry(new MenuAction("Test Mode", testM_m));
+    mode_m->addEntry(new MenuAction("Consol Control", dummy));
 
     // Entry for the PID Menu
-    MenuAction* pidl_e = new MenuAction("Left", pidl_m);
-    MenuAction* pidr_e = new MenuAction("Right", pidr_m);
-    pid_m->addEntry(pidl_e);
-    pid_m->addEntry(pidr_e);
+    pid_m->addEntry(new MenuAction("Left", pidl_m));
+    pid_m->addEntry(new MenuAction("Right", pidr_m));
 
     // Other config
     pidl_m->setMinMax(0, UINT8_MAX);
diff --git a/src/network.cpp b/src/network.cpp
index 8d1a1e0..baaa932 100644
--- a/src/network.cpp
+++ b/src/network.cpp
@@ -113,6 +113,19 @@ bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
     return true;
 }
 
+uint8_t Network::getCurrentChannel() {
+    uint8_t channel;
+    wifi_second_chan_t secondChannel;
+    if (esp_wifi_get_channel(&channel, &secondChannel) != ESP_OK) {
+        std::cout << "Network::getCurrentChannel - Error!" << std::endl;
+        return -1;
+    }
+    // std::cout << "Network::getCurrentChannel - Current WiFi channel: " 
+    //           << (int) channel << " second channel: " << (int) secondChannel 
+    //           << std::endl;
+    return channel;
+}
+
 bool Network::checkMQTT() {
     static uint64_t lastReconnectAttempt = 0;
     if (!mqtt_client->connected()) {

From b297a31b6bd9e39b3b0c33843e8a3271f66417b0 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Sat, 3 Jun 2023 10:40:05 +0200
Subject: [PATCH 02/27] - added delete function for the api

---
 src/SpecialMenus/Route/menuRoute.cpp | 70 ++++++++++++++++++++++------
 src/SpecialMenus/Route/menuRoute.h   |  2 +
 2 files changed, 58 insertions(+), 14 deletions(-)

diff --git a/src/SpecialMenus/Route/menuRoute.cpp b/src/SpecialMenus/Route/menuRoute.cpp
index c60196b..772c23a 100644
--- a/src/SpecialMenus/Route/menuRoute.cpp
+++ b/src/SpecialMenus/Route/menuRoute.cpp
@@ -83,18 +83,13 @@ void MenuRoute::init() {
 
     this->mainMenu = new Menu;
     MenuRoutePoints* pointsMenu = new MenuRoutePoints(this->route);
-    Menu* importMenu = new Menu;
-    Menu* exportMenu = new Menu;
-    Menu* clearMenu = new Menu;
 
     this->mainMenu->setLcd(this->lcd);
     pointsMenu->setLcd(this->lcd);
-    importMenu->setLcd(this->lcd);
-    exportMenu->setLcd(this->lcd);
-    clearMenu->setLcd(this->lcd);
 
     MenuIntInput* importWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::importRoute));
     MenuIntInput* exportWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::exportRoute));
+    MenuIntInput* deleteWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::deleteRoute));
     MenuActionRoute* clearWrapper = new MenuActionRoute(this, &MenuRoute::clearRoute);
 
     importWrapper->setLcd(this->lcd);
@@ -107,15 +102,16 @@ void MenuRoute::init() {
     exportWrapper->setPrintParentMenu(false);
     exportWrapper->setEntry(0, "Export Route");
 
-    MenuAction* pointsAction = new MenuAction("Points", pointsMenu);
-    MenuAction* importAction = new MenuAction("Import", importWrapper);
-    MenuAction* exportAction = new MenuAction("Export", exportWrapper);
-    MenuAction* clearAction = new MenuAction("Clear", clearWrapper);
+    deleteWrapper->setLcd(this->lcd);
+    deleteWrapper->setMinMax(0, 100);
+    deleteWrapper->setPrintParentMenu(false);
+    deleteWrapper->setEntry(0, "Delete Route");
 
-    this->mainMenu->addEntry(pointsAction);
-    this->mainMenu->addEntry(importAction);
-    this->mainMenu->addEntry(exportAction);
-    this->mainMenu->addEntry(clearAction);
+    this->mainMenu->addEntry(new MenuAction("Points", pointsMenu));
+    this->mainMenu->addEntry(new MenuAction("Clear", clearWrapper));
+    this->mainMenu->addEntry(new MenuAction("Import", importWrapper));
+    this->mainMenu->addEntry(new MenuAction("Export", exportWrapper));
+    this->mainMenu->addEntry(new MenuAction("Delete", deleteWrapper));
 }
 
 void MenuRoute::importRoute(uint8_t routeNumber) {
@@ -236,6 +232,52 @@ void MenuRoute::exportRoute(uint8_t routeNumber) {
     http.end();
 }
 
+void MenuRoute::deleteRoute(uint8_t routeNumber) {
+    this->blockInput = true;
+
+    String lineOne = "";
+    String lineTwo = "";
+
+    if (WiFi.status() != WL_CONNECTED) {
+        lineOne = "Not connected to";
+        lineTwo = "the WiFi.";
+        this->print(lineOne, lineTwo);
+        this->blockInput = false;
+        return;
+    }
+
+    if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE) {
+        lineOne = "Not enough mem";
+        lineTwo = "for Json obj";
+        this->print(lineOne, lineTwo);
+        this->blockInput = false;
+        return;
+    }
+
+    WiFiClient client;
+    HTTPClient http;
+    DynamicJsonDocument doc(JSON_DOCUMENT_SIZE_ROUTE);
+    
+    String host = "http://rover.kleiax.de/api/";
+    host.concat(routeNumber);
+    http.begin(client, host);
+    int httpResponseCode = http.sendRequest("DELETE");
+
+    lineOne = "Delete complete";
+    lineTwo = "Code: ";
+
+    if (httpResponseCode == 202)
+        deserializeJson(doc, http.getStream());
+    else
+        lineOne = "HTTP Error";
+
+    lineTwo.concat(httpResponseCode);
+    this->print(lineOne, lineTwo);
+
+    this->blockInput = false;
+    http.end();
+}
+
 void MenuRoute::clearRoute(uint8_t none) {
     this->route->clear();
     this->print("Currente route", "deleted...");
diff --git a/src/SpecialMenus/Route/menuRoute.h b/src/SpecialMenus/Route/menuRoute.h
index ebfc50e..b8cdae8 100644
--- a/src/SpecialMenus/Route/menuRoute.h
+++ b/src/SpecialMenus/Route/menuRoute.h
@@ -109,6 +109,8 @@ class MenuRoute : public MenuControl {
          */
         void exportRoute(uint8_t routeNumber);
 
+        void deleteRoute(uint8_t routeNumber);
+
         /**
          * @brief Delete the current Route.
          * 

From 3549af2470063962e88b61a86745a051fcf43b09 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 13:04:19 +0200
Subject: [PATCH 03/27] update compass lib

---
 lib/calibrateCompass/calibrateCompass.cpp | 4 ++--
 lib/calibrateCompass/calibrateCompass.h   | 2 +-
 platformio.ini                            | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/lib/calibrateCompass/calibrateCompass.cpp b/lib/calibrateCompass/calibrateCompass.cpp
index 4d7a1c2..3d17770 100644
--- a/lib/calibrateCompass/calibrateCompass.cpp
+++ b/lib/calibrateCompass/calibrateCompass.cpp
@@ -73,7 +73,7 @@ void CalibrateCompass::start() {
 
     this->clearData();
     this->state = State::Calibrating;
-    this->compass->removeCalibration();
+    this->compass->clearCalibration();
     this->lastChange = millis();
 }
 
@@ -95,7 +95,7 @@ void CalibrateCompass::useData() {
 }
 
 void CalibrateCompass::removeCalibration() {
-    this->compass->removeCalibration();
+    this->compass->clearCalibration();
 }
 
 void CalibrateCompass::reset() {
diff --git a/lib/calibrateCompass/calibrateCompass.h b/lib/calibrateCompass/calibrateCompass.h
index 8a8d786..3c5de15 100644
--- a/lib/calibrateCompass/calibrateCompass.h
+++ b/lib/calibrateCompass/calibrateCompass.h
@@ -52,6 +52,6 @@ class CalibrateCompass {
         void clearData();
 
         bool dataValid = false;
-        const uint16_t maxTimeWithoutChange = 5000;
+        const uint16_t maxTimeWithoutChange = 10000;
         uint32_t lastChange = 0;
 };
diff --git a/platformio.ini b/platformio.ini
index 3b37405..7266acd 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -22,7 +22,7 @@ lib_deps =
 	br3ttb/PID@^1.2.1
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
 	bblanchon/ArduinoJson@^6.20.0
-	mprograms/QMC5883LCompass@^1.1.1
+	mprograms/QMC5883LCompass@^1.2.0
 	https://git.kleiax.de/PlatformIO-Libs/Menu.git
 	nrf24/RF24@^1.4.5
 upload_port = COM3

From 8ed91c4b11b53a430e7334a2738eeb29918951d9 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 13:05:42 +0200
Subject: [PATCH 04/27] small fixes

---
 doc/Notes and TODOs/TODO allgemein.txt             |  4 ++++
 lib/MotorControl/motorControl.h                    |  2 +-
 lib/Navigation/navigation.cpp                      |  1 +
 .../driveModi/Autopilot/menuAutopilot.cpp          |  2 +-
 src/driveModi/Modi/Autopilot/autopilot.cpp         | 14 ++++++--------
 src/driveModi/Modi/Autopilot/autopilot.h           |  2 +-
 src/moveControl.cpp                                |  4 ++--
 7 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/doc/Notes and TODOs/TODO allgemein.txt b/doc/Notes and TODOs/TODO allgemein.txt
index 03df2c4..d987cd2 100644
--- a/doc/Notes and TODOs/TODO allgemein.txt	
+++ b/doc/Notes and TODOs/TODO allgemein.txt	
@@ -1,3 +1,7 @@
     -> Program underfloorLighting
     -> Add an beeper
     -> Program the beeper
+
+    Speedometer buffer ergibt kaum Sinn
+    Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
+    
diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h
index 9632e99..ed070aa 100644
--- a/lib/MotorControl/motorControl.h
+++ b/lib/MotorControl/motorControl.h
@@ -21,7 +21,7 @@
 #define PWMRES 8
 #define POWERSTEPS 2       //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
 #define PWMMIN 55
-#define PWMMAX 90         // Max 98% of 2^PWM_RES
+#define PWMMAX 94         // Max 98% of 2^PWM_RES
 
 /**
  * @brief A class which use PWM to control the power of DC Motor
diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index cdae8df..23a2bc9 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -134,6 +134,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
     if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
         && !forceUpdate) {
         correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
+        correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
         return Status::Unchanged;
     }
 
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 1c6bb46..94d3828 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -43,7 +43,7 @@ void MenuAutopilot::printPage() const {
             lineTwo = "";
             switch (this->autopilot->getState()) {
                 case Autopilot::State::InsufficientAccuarcy :
-                    lineTwo = "Err: No NTRIP";
+                    lineTwo = "Err: LowAccuracy";
                     break;
 
                 case Autopilot::State::NoRoute :
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index 8fbc417..fe5ddbd 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -119,10 +119,10 @@ void Autopilot::drive() {
 void Autopilot::rotate() {
     this->moveControl->setSpeed(0);
     if (this->courseCorrection.correction > 0)
-        this->moveControl->setRotationSpeed(this->rotationSpeed);
+        this->moveControl->setRotationSpeed(-this->rotationSpeed);
     else
 
-    this->moveControl->setRotationSpeed(-this->rotationSpeed);
+    this->moveControl->setRotationSpeed(this->rotationSpeed);
 }
 
 void Autopilot::checkButtonInput() {
@@ -172,10 +172,8 @@ void Autopilot::askNavigationForOrder() {
 }
 
 void Autopilot::selfDriving() {
-    if (this->lastOrderStatus == Navigation::Status::Updated) {
-        if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
-            this->rotate();
-        else
-            this->drive();
-    }
+    if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
+        this->rotate();
+    else
+        this->drive();
 }
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index 4cd124b..a64c87b 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -132,7 +132,7 @@ class Autopilot : public ManualControl {
         uint32_t lastAutopilotChangeMillis = 0;
 
         double drivingSpeed = 1;
-        double rotationSpeed = 3;
+        double rotationSpeed = 4.5;
         double minRemainingDistance = 0.25;
 };
 
diff --git a/src/moveControl.cpp b/src/moveControl.cpp
index 48090c6..a8bc80d 100644
--- a/src/moveControl.cpp
+++ b/src/moveControl.cpp
@@ -187,9 +187,9 @@ void MoveControl::calcTargetWheelSpeed() {
                \ 1 -b / \ T /   \ Xr /  */
 
     // (1 / r) * 1
-    const static double A = 15.82278481;
+    constexpr double A = 15.82278481;
     // (1 / r) * b
-    const static double B = 2.096518987;
+    constexpr double B = 2.096518987;
 
     this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
     this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);

From 8d437b35e8859241519c09aa8ac1d8e5e29d9885 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 13:29:45 +0200
Subject: [PATCH 05/27] clean up

---
 doc/Notes and TODOs/SPIpushRawData.txt       |  9 ------
 doc/Notes and TODOs/ps3 reconnect.txt        | 26 -----------------
 doc/PinBelegungen.txt                        | 30 --------------------
 doc/{Notes and TODOs => }/TODO allgemein.txt |  3 ++
 doc/controller.txt                           | 17 -----------
 5 files changed, 3 insertions(+), 82 deletions(-)
 delete mode 100644 doc/Notes and TODOs/SPIpushRawData.txt
 delete mode 100644 doc/Notes and TODOs/ps3 reconnect.txt
 delete mode 100644 doc/PinBelegungen.txt
 rename doc/{Notes and TODOs => }/TODO allgemein.txt (76%)
 delete mode 100644 doc/controller.txt

diff --git a/doc/Notes and TODOs/SPIpushRawData.txt b/doc/Notes and TODOs/SPIpushRawData.txt
deleted file mode 100644
index 3bd1e86..0000000
--- a/doc/Notes and TODOs/SPIpushRawData.txt	
+++ /dev/null
@@ -1,9 +0,0 @@
-Added this code in SparkFun_u-blox_GNSS_Arduino_Libary.cpp in the function pushRawData.
-
-_spiPort->beginTransaction(SPISettings(_spiSpeed, MSBFIRST, SPI_MODE0));
-digitalWrite(_csPin, LOW);
-for (uint16_t i = 0; i < numDataBytes; i++) {
-    spiTransfer(dataBytes[i]);
-}
-digitalWrite(_csPin, HIGH);
-_spiPort->endTransaction();
diff --git a/doc/Notes and TODOs/ps3 reconnect.txt b/doc/Notes and TODOs/ps3 reconnect.txt
deleted file mode 100644
index 6b97f40..0000000
--- a/doc/Notes and TODOs/ps3 reconnect.txt	
+++ /dev/null
@@ -1,26 +0,0 @@
-ps3.h
-void ps3ResetGlobals();
-
-ps3.c
-void ps3ResetGlobals() {
-    // Own Code
-    is_active = false;
-    ps3_connection_cb = NULL;
-    ps3_connection_object_cb = NULL;
-    ps3_connection_object = NULL;
-    ps3_event_cb = NULL;
-    ps3_event_object_cb = NULL;
-    ps3_event_object = NULL;
-}
-
-Ps3Controller.h
-private
-void resetGlobals();
-
-Ps3Controller.cpp
-void Ps3Controller::resetGlobals() {
-    ps3ResetGlobals();
-}
-
-in -> Ps3Controller::begin
-this->resetGlobals();
diff --git a/doc/PinBelegungen.txt b/doc/PinBelegungen.txt
deleted file mode 100644
index 9026913..0000000
--- a/doc/PinBelegungen.txt
+++ /dev/null
@@ -1,30 +0,0 @@
-ESP32 Rover Pinbelegung
-
-		3V3				GND
-		x				23		DirR1
-		x				22		PWMR
-		x				TX		PC
-		x				RX		PC				Connector I2C
-BATTERY	35				21		SDA					SDA
-INTL1	32				GND							GND
-INTL2	33				19		SCL					SCL
-INTR1	25				18							VCC
-INTR2	26				5		SPI_CSK
-DirL1	27				17		SPI_CS
-DirR2	14				16		SPI_COPI
-DirL2	12				4		SPI_CIPO
-		GND				x
-PWML	13				2		Probleme beim flashen
-		x				15		WS2812 LED ?
-		x				x
-		CMD				x
-		5V	   USB		x
-		
-
-Connector Encoder
-
--	2	1	+
-OB	4	3	-
-3V3	6  [5]	OA
-	8	7
-	10	9
diff --git a/doc/Notes and TODOs/TODO allgemein.txt b/doc/TODO allgemein.txt
similarity index 76%
rename from doc/Notes and TODOs/TODO allgemein.txt
rename to doc/TODO allgemein.txt
index d987cd2..4f6e1bb 100644
--- a/doc/Notes and TODOs/TODO allgemein.txt	
+++ b/doc/TODO allgemein.txt	
@@ -1,7 +1,10 @@
+Für irgendwann:
     -> Program underfloorLighting
     -> Add an beeper
     -> Program the beeper
+    -> Update GNSS Lib to v3
 
+Besser zügig:
     Speedometer buffer ergibt kaum Sinn
     Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
     
diff --git a/doc/controller.txt b/doc/controller.txt
deleted file mode 100644
index 33966b5..0000000
--- a/doc/controller.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-Controller
-
-PS3-Button:     Change Mode
-
-Mode: ManualControl
-    L3: Joystick Mode
-        Left Joystick:  Drive
-
-    R3: Shoulder Trigger Mode
-        L2:             Spin left side
-        R2:             Spin right side
-
-Mode: Autopilot
-
-Mode: CaptureRoute
-
-Mode: ConsolControl
\ No newline at end of file

From c4d7afcdb7c9c00051b005789807afde48e5bb56 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 20:38:44 +0200
Subject: [PATCH 06/27] added auto version increment

---
 .gitmodules                  | 3 +++
 .version_no_increment        | 0
 doc/TODO allgemein.txt       | 1 +
 include/Version.h            | 9 +++++++++
 platformio.ini               | 3 +++
 platformio_version_increment | 1 +
 src/main.cpp                 | 4 ++++
 version                      | 1 +
 8 files changed, 22 insertions(+)
 create mode 100644 .gitmodules
 create mode 100644 .version_no_increment
 create mode 100644 include/Version.h
 create mode 160000 platformio_version_increment
 create mode 100644 version

diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..60410d0
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "platformio_version_increment"]
+	path = platformio_version_increment
+	url = https://github.com/sblantipodi/platformio_version_increment.git
diff --git a/.version_no_increment b/.version_no_increment
new file mode 100644
index 0000000..e69de29
diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt
index 4f6e1bb..c33ae7c 100644
--- a/doc/TODO allgemein.txt	
+++ b/doc/TODO allgemein.txt	
@@ -7,4 +7,5 @@ Für irgendwann:
 Besser zügig:
     Speedometer buffer ergibt kaum Sinn
     Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
+    Doxygen Kommentare aktualisieren
     
diff --git a/include/Version.h b/include/Version.h
new file mode 100644
index 0000000..a96e295
--- /dev/null
+++ b/include/Version.h
@@ -0,0 +1,9 @@
+
+    // AUTO GENERATED FILE, DO NOT EDIT
+    #ifndef VERSION
+        #define VERSION "0.1.3"
+    #endif
+    #ifndef BUILD_TIMESTAMP
+        #define BUILD_TIMESTAMP "2023-08-07 20:34:58.828579"
+    #endif
+    
\ No newline at end of file
diff --git a/platformio.ini b/platformio.ini
index 7266acd..06a990a 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -26,6 +26,9 @@ lib_deps =
 	https://git.kleiax.de/PlatformIO-Libs/Menu.git
 	nrf24/RF24@^1.4.5
 upload_port = COM3
+extra_scripts = 
+   pre:platformio_version_increment/version_increment_pre.py
+   post:platformio_version_increment/version_increment_post.py
 
 [env:release]
 build_type = release
diff --git a/platformio_version_increment b/platformio_version_increment
new file mode 160000
index 0000000..fd51b62
--- /dev/null
+++ b/platformio_version_increment
@@ -0,0 +1 @@
+Subproject commit fd51b62f000d23d5649da611c2560af8e2327415
diff --git a/src/main.cpp b/src/main.cpp
index 163feed..77a5668 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,6 +22,8 @@
 #include <LiquidCrystal_I2C.h>
 #include <SPI.h>
 
+#include "Version.h"
+
 #include "driveModi/driveManager.h"
 #include "OutputBuf/outputBuf.h"
 #include "LcdWrapper.h"
@@ -109,6 +111,8 @@ void setup() {
     Network::connectEspNow(receiveCallback, sendCallback);
 
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
+    std::cout << "Project verion: " << VERSION << std::endl;
+    std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl;
     std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
 
     driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive);
diff --git a/version b/version
new file mode 100644
index 0000000..7693c96
--- /dev/null
+++ b/version
@@ -0,0 +1 @@
+0.1.3
\ No newline at end of file

From 47a6e770fe0007d62085a4c8814a6977138a084b Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 21:00:08 +0200
Subject: [PATCH 07/27] added version information to rover menu

---
 .vscode/settings.json                         | 42 ++++++++++++++++++-
 include/Version.h                             |  4 +-
 .../menuSysteminformatio.cpp                  | 12 +++++-
 .../Systeminformation/menuSysteminformation.h |  2 +
 version                                       |  2 +-
 5 files changed, 57 insertions(+), 5 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 1b79192..ba00c90 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -77,7 +77,47 @@
         "istream": "cpp",
         "streambuf": "cpp",
         "functional": "cpp",
-        "cmath": "cpp"
+        "cmath": "cpp",
+        "atomic": "cpp",
+        "cctype": "cpp",
+        "chrono": "cpp",
+        "clocale": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "limits": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "cinttypes": "cpp",
+        "typeinfo": "cpp"
     },
     "cSpell.words": [
         "Ahnung",
diff --git a/include/Version.h b/include/Version.h
index a96e295..d57b88b 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.1.3"
+        #define VERSION "0.1.5"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-07 20:34:58.828579"
+        #define BUILD_TIMESTAMP "2023-08-07 20:54:44.940398"
     #endif
     
\ No newline at end of file
diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
index 9773c38..fd2e5ab 100644
--- a/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
+++ b/src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp
@@ -12,7 +12,7 @@
 #include "menuSysteminformation.h"
 
 MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) 
-            : MenuInformationSites(6) {
+            : MenuInformationSites(8) {
     this->mainBattery = mainBattery;
 }
 
@@ -50,6 +50,16 @@ void MenuSysteminformation::printPage() const {
             break;
 
         case 5:
+            lineOne = "Software verion:";
+            lineTwo = VERSION;
+            break;
+
+        case 6:
+            lineOne = "Build timestamp:";
+            lineTwo = String(BUILD_TIMESTAMP).substring(0, 16);
+            break;
+
+        case 7:
             lineOne = "Kleiax Rover by";
             lineTwo = "Alexander Klein";
             break;
diff --git a/src/SpecialMenus/Systeminformation/menuSysteminformation.h b/src/SpecialMenus/Systeminformation/menuSysteminformation.h
index cc51d39..d0d885a 100644
--- a/src/SpecialMenus/Systeminformation/menuSysteminformation.h
+++ b/src/SpecialMenus/Systeminformation/menuSysteminformation.h
@@ -14,6 +14,8 @@
 
 #include <Arduino.h>
 
+#include "Version.h"
+
 #include "controlPadInput.h"
 #include "menuInformationSites.h"
 #include "battery.h"
diff --git a/version b/version
index 7693c96..def9a01 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.1.3
\ No newline at end of file
+0.1.5
\ No newline at end of file

From d6bc7e42392d23b060effa20ec91634cb6f1a489 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 21:41:03 +0200
Subject: [PATCH 08/27] change some things, because azimuth is now by default a
 signed value

---
 include/Version.h             | 4 ++--
 lib/Navigation/navigation.cpp | 4 +---
 lib/Navigation/navigation.h   | 5 +++--
 version                       | 2 +-
 4 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/include/Version.h b/include/Version.h
index d57b88b..9ce3961 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.1.5"
+        #define VERSION "0.1.7"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-07 20:54:44.940398"
+        #define BUILD_TIMESTAMP "2023-08-07 21:22:27.260906"
     #endif
     
\ No newline at end of file
diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index 23a2bc9..70e6e6c 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -192,9 +192,7 @@ void Navigation::updateCurrentLocation() {
 
 int16_t Navigation::calculateCourseCorrection(Point& point) {
     int16_t targetCourse = point.courseTo(this->targetPoint);
-    // correction = targetCourse - currentCourse
-    int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
-    int16_t correctionCourse = targetCourse - signedAzimuth;
+    int16_t correctionCourse = targetCourse - this->azimuth;
     if (correctionCourse > 180)
         correctionCourse -= 360;
     else if (correctionCourse < -180)
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index f9665b3..5388a66 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -182,7 +182,7 @@ class Navigation {
          * 
          * @return uint16_t degree
          */
-        uint16_t getAzimuth() const { return this->azimuth; }
+        int16_t getAzimuth() const { return this->azimuth; }
         QMC5883LCompass* getCompass() const { return this->compass; }
 
         Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
@@ -244,9 +244,10 @@ class Navigation {
         char* user;
         char* password;
 
+        int16_t azimuth = INT16_MAX;
+
         uint8_t timeToWait = 200;
         uint16_t port;
-        uint16_t azimuth = UINT16_MAX;
         uint32_t lastMillis = 0;
         uint32_t ubxUpdateTime = 0;
 
diff --git a/version b/version
index def9a01..a1e1395 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.1.5
\ No newline at end of file
+0.1.7
\ No newline at end of file

From ebe4d32523d458785d5f5da8a23cfb0b1cb310c2 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Mon, 7 Aug 2023 23:25:31 +0200
Subject: [PATCH 09/27] added magDec to manualDrive Menu

---
 include/Version.h                                |  4 ++--
 .../driveModi/ManualDrive/menuManualDrive.cpp    | 16 +++++++++++++++-
 version                                          |  2 +-
 3 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/include/Version.h b/include/Version.h
index 9ce3961..bca35fc 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.1.7"
+        #define VERSION "0.1.10"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-07 21:22:27.260906"
+        #define BUILD_TIMESTAMP "2023-08-07 22:02:51.554203"
     #endif
     
\ No newline at end of file
diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
index dffebd8..4b37fcb 100644
--- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
+++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
@@ -65,6 +65,20 @@ void MenuManualControl::printPage() const {
             lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
             break;
 
+        case 8: {
+            UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
+            lineOne = "magDec: ";
+            lineTwo = "magAcc: ";
+            if (gpsData->valid.bits.validMag) {
+                lineOne.concat(gpsData->magDec);
+                lineTwo.concat(gpsData->magAcc);
+            } else {
+                lineOne.concat("invalid");
+                lineTwo.concat("invalid");   
+            }
+            break;
+        }
+
         default:
             this->printDefault();
         return;
@@ -77,7 +91,7 @@ void MenuManualControl::init() {
     this->firstPrint = false;
     this->driveManager->changeModus(Modi::ManualControl);
     this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
-    this->setCountPages(8);
+    this->setCountPages(9);
     this->updateDelay = 500; 
 }
 
diff --git a/version b/version
index a1e1395..345f8cc 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.1.7
\ No newline at end of file
+0.1.10
\ No newline at end of file

From bbad2108403352949fbc6aa065b589bc275a2f22 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Tue, 8 Aug 2023 12:05:42 +0200
Subject: [PATCH 10/27] change submodul name for version increment

---
 .gitmodules                  | 2 +-
 platformio_version_increment | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)
 delete mode 160000 platformio_version_increment

diff --git a/.gitmodules b/.gitmodules
index 60410d0..9375b66 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,3 @@
 [submodule "platformio_version_increment"]
-	path = platformio_version_increment
+	path = autoVersionIncrement
 	url = https://github.com/sblantipodi/platformio_version_increment.git
diff --git a/platformio_version_increment b/platformio_version_increment
deleted file mode 160000
index fd51b62..0000000
--- a/platformio_version_increment
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit fd51b62f000d23d5649da611c2560af8e2327415

From 4540966f87b967902caabe768c902fbd32f37966 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Tue, 8 Aug 2023 12:49:17 +0200
Subject: [PATCH 11/27] fix auto increment script path

---
 platformio.ini | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 06a990a..39ceb01 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -27,8 +27,8 @@ lib_deps =
 	nrf24/RF24@^1.4.5
 upload_port = COM3
 extra_scripts = 
-   pre:platformio_version_increment/version_increment_pre.py
-   post:platformio_version_increment/version_increment_post.py
+   pre:autoVersionIncrement/version_increment_pre.py
+   post:autoVersionIncrement/version_increment_post.py
 
 [env:release]
 build_type = release

From 115da0f8a84d80866cbe26cf0f5a1fa24d90c710 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Tue, 8 Aug 2023 14:48:15 +0200
Subject: [PATCH 12/27] added digital control to manualControl

---
 include/Version.h                             |  4 +-
 .../driveModi/ManualDrive/menuManualDrive.cpp | 43 +++++++++++-------
 .../Modi/ManualControl/manualControl.cpp      | 44 +++++++++++++++++--
 .../Modi/ManualControl/manualControl.h        | 32 +++++++++++---
 version                                       |  2 +-
 5 files changed, 96 insertions(+), 29 deletions(-)

diff --git a/include/Version.h b/include/Version.h
index bca35fc..6672895 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.1.10"
+        #define VERSION "0.8.20"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-07 22:02:51.554203"
+        #define BUILD_TIMESTAMP "2023-08-08 14:33:17.266785"
     #endif
     
\ No newline at end of file
diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
index 4b37fcb..e122de6 100644
--- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
+++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
@@ -27,45 +27,53 @@ void MenuManualControl::printPage() const {
             break;
 
         case 1:
-            lineOne = "Speed: ";
-            lineOne.concat(this->manualControl->getMaxSpeed());
-            lineTwo = "Increase by 0.1";
+            lineOne = "Input mode:";
+            if (this->manualControl->getInputMode() == ManualControl::InputMode::Analog)
+                lineTwo = "Analog";
+            else
+                lineTwo = "Digital";
             break;
 
         case 2:
             lineOne = "Speed: ";
             lineOne.concat(this->manualControl->getMaxSpeed());
-            lineTwo = "Decrease by 0.1";
+            lineTwo = "Increase by 0.1";
             break;
 
         case 3:
-            lineOne = "RotSpeed: ";
-            lineOne.concat(this->manualControl->getMaxRotation());
-            lineTwo = "Increase by 0.1";
+            lineOne = "Speed: ";
+            lineOne.concat(this->manualControl->getMaxSpeed());
+            lineTwo = "Decrease by 0.1";
             break;
 
         case 4:
             lineOne = "RotSpeed: ";
             lineOne.concat(this->manualControl->getMaxRotation());
-            lineTwo = "Decrease by 0.1";
+            lineTwo = "Increase by 0.1";
             break;
 
         case 5:
+            lineOne = "RotSpeed: ";
+            lineOne.concat(this->manualControl->getMaxRotation());
+            lineTwo = "Decrease by 0.1";
+            break;
+
+        case 6:
             lineOne = "Dutycycle Left:";
             lineTwo.concat(this->manualControl->getDutycycleLeft());
             break;
 
-        case 6:
+        case 7:
             lineOne = "Dutycycle Right:";
             lineTwo.concat(this->manualControl->getDutycycleRight());
             break;
 
-        case 7:
+        case 8:
             lineOne = "Azimuth:";
             lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
             break;
 
-        case 8: {
+        case 9: {
             UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
             lineOne = "magDec: ";
             lineTwo = "magAcc: ";
@@ -91,28 +99,31 @@ void MenuManualControl::init() {
     this->firstPrint = false;
     this->driveManager->changeModus(Modi::ManualControl);
     this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
-    this->setCountPages(9);
+    this->setCountPages(10);
     this->updateDelay = 500; 
 }
 
 void MenuManualControl::runCommand() const {
     switch (this->getCurrentPage()) {
-
         case 1:
-            this->manualControl->increaseMaxSpeed();
+            this->manualControl->switchInputMode();
             break;
 
         case 2:
-            this->manualControl->decreaseMaxSpeed();
+            this->manualControl->increaseMaxSpeed();
             break;
 
         case 3:
-            this->manualControl->increaseMaxRotation();
+            this->manualControl->decreaseMaxSpeed();
             break;
 
         case 4:
             this->manualControl->increaseMaxRotation();
             break;
+
+        case 5:
+            this->manualControl->increaseMaxRotation();
+            break;
         
         default:
             break;
diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp
index 385f06b..a8f3643 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.cpp
+++ b/src/driveModi/Modi/ManualControl/manualControl.cpp
@@ -26,14 +26,30 @@ void ManualControl::loop() {
     if (this->caliCompass)
         this->caliCompass->loop();
 
-    if (millis() - this->lastMillis < delay) {
+    if (millis() - this->lastMillis < delay)
         return;
+    
+    switch (this->inputMode) {
+        case InputMode::Analog :
+            this->analogControl();
+            break;
+
+        case InputMode::Digital :
+            this->digitalControl();
+            break;
+        
+        default:
+            break;
     }
-    this->runManualControl();
+
     this->lastMillis = millis();
 }
 
-void ManualControl::runManualControl() {
+void ManualControl::switchInputMode() {
+    this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
+}
+
+void ManualControl::analogControl() {
     // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
     int16_t y = this->input->x - 127;
     int16_t x = this->input->y - 127;
@@ -49,3 +65,25 @@ void ManualControl::runManualControl() {
     value_per_step = this->max_rotation * 2 / 256;
     this->moveControl->setRotationSpeed(x * value_per_step);
 }
+
+void ManualControl::digitalControl() {
+    int16_t y = this->input->x - 127;
+    int16_t x = this->input->y - 127;
+
+    if (y > 120)
+        this->moveControl->setSpeed(-this->max_speed);
+    else if (y < -120)
+        this->moveControl->setSpeed(this->max_speed);
+    else
+        this->moveControl->setSpeed(0);
+
+    if (x > 120)
+        this->moveControl->setRotationSpeed(this->max_rotation);
+    else if (x < -120)
+        this->moveControl->setRotationSpeed(-this->max_rotation);
+    else
+        this->moveControl->setRotationSpeed(0);
+
+    if (this->directionChangeWrapper && (x > 120 || x < -120))
+        this->directionChangeWrapper->action();
+}
diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h
index d64062b..ad70d2d 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.h
+++ b/src/driveModi/Modi/ManualControl/manualControl.h
@@ -16,6 +16,11 @@
 #include "controlPadInput.h"
 #include "calibrateCompass.h"
 
+class DirectionChangeWrapper {
+    public:
+        virtual void action() = 0;
+};
+
 /**
  * @brief Drive the Rover with a Joystick
  * 
@@ -26,6 +31,11 @@
  */
 class ManualControl : public DriveModi {
     public:
+        enum class InputMode : uint8_t {
+            Analog,
+            Digital
+        };
+
         ManualControl(MoveControl *moveControl, const ControlPadInput *input);
 
         /**
@@ -35,7 +45,7 @@ class ManualControl : public DriveModi {
         ~ManualControl();
 
         /**
-         * @brief Calls runManualControl() to update all values.
+         * @brief Calls analogControl() to update all values.
          * 
          * This function should be called every mainloop. If the delay is not reached, than the
          * functions returns immediately.
@@ -44,12 +54,6 @@ class ManualControl : public DriveModi {
          */
         void loop() override;
 
-        /**
-         * @brief Noramly called repeatedly by loop() to calcluate new values.
-         * Set new values for speed and rotation in moveControl
-         */
-        void runManualControl();
-
         /**
          * @brief Set the min delay between each loop
          * 
@@ -79,6 +83,11 @@ class ManualControl : public DriveModi {
 
         void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; }
 
+        void switchInputMode();
+        void setInputMode(InputMode mode) { this->inputMode = mode; }
+        InputMode getInputMode() const { return this->inputMode; }
+        void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; }
+
         uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); }
         uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }
 
@@ -87,11 +96,20 @@ class ManualControl : public DriveModi {
         const ControlPadInput* input;
 
     private:
+        /**
+         * @brief Noramly called repeatedly by loop() to calcluate new values.
+         * Set new values for speed and rotation in moveControl
+         */
+        void analogControl();
+        void digitalControl();
+
         uint32_t lastMillis = 0;
         uint8_t delay = 10;
         double max_speed = 1;
         double max_rotation = 7;
         CalibrateCompass* caliCompass = nullptr;
+        DirectionChangeWrapper* directionChangeWrapper = nullptr;
+        InputMode inputMode = InputMode::Analog;
 };
 
 #endif // MANUAL_CONTROL_H
diff --git a/version b/version
index 345f8cc..e12118d 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.1.10
\ No newline at end of file
+0.8.20
\ No newline at end of file

From 620812b5f83b012f13adb086b5c414f5f8b6a6eb Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Tue, 8 Aug 2023 18:56:36 +0200
Subject: [PATCH 13/27] added interface for calced azimuth

---
 lib/Navigation/navigation.cpp | 74 +++++++++++++++++++++++++++++++----
 lib/Navigation/navigation.h   | 44 ++++++++++-----------
 2 files changed, 88 insertions(+), 30 deletions(-)

diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index 70e6e6c..70ad78c 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -98,14 +98,11 @@ void Navigation::loop() {
         this->ntripClient->loop();
 
     if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
-    // if (millis() - this->lastMillis > 500) {
         this->compass->read();
-        this->azimuth = this->compass->getAzimuth();
-        // std::cout   << "Compass x: " << compass->getX() 
-        //             << " y: " << compass->getY() 
-        //             << " z: " << compass->getZ() 
-        //             << " Azi: " << compass->getAzimuth() 
-        //             << std::endl; 
+        this->realAzimuth = this->compass->getAzimuth();
+
+        this->updateMagneticDeclination();
+
         this->lastMillis = millis();
     }
 }
@@ -124,6 +121,14 @@ bool Navigation::startNavigation() {
     return this->navigationStarted;
 }
 
+void Navigation::drivingDirectionChange() {
+    Point tmp = this->currentPosition;
+    if (tmp.isInit() && tmp.isValid()) {
+        this->directionChangeMode = true;
+        this->lastPointDrivingDirectionChange = tmp;
+    }
+}
+
 Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
     if (this->navigationFinished)
         return Status::Complete;
@@ -190,9 +195,62 @@ void Navigation::updateCurrentLocation() {
     this->currentPosition = Point(coords, this->ubxData->hAcc);
 }
 
+void Navigation::updateMagneticDeclination() {
+    if (!this->directionChangeMode
+        || this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Invalid;
+        return;
+    }
+
+    this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition);
+
+    // Map point accuracy to CalcAzimuthState
+    if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Good;
+    } 
+    else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Ok;
+    } 
+    else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Bad;
+    } 
+    else 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Invalid;
+    }
+
+    // Upgrade quality if the range grows up
+    if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) {
+        switch (this->calcAzimuthState) {
+            case CalcAzimuthState::Bad :
+                this->calcAzimuthState = CalcAzimuthState::Ok;
+                break;
+            
+            case CalcAzimuthState::Ok :
+                this->calcAzimuthState = CalcAzimuthState::Good;
+                break;
+
+            case CalcAzimuthState::Good :
+                this->calcAzimuthState = CalcAzimuthState::Super;
+                break;
+
+            default:
+                break;
+        }
+    }
+
+}
+
 int16_t Navigation::calculateCourseCorrection(Point& point) {
     int16_t targetCourse = point.courseTo(this->targetPoint);
-    int16_t correctionCourse = targetCourse - this->azimuth;
+    int16_t correctionCourse = targetCourse - this->realAzimuth;
     if (correctionCourse > 180)
         correctionCourse -= 360;
     else if (correctionCourse < -180)
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index 5388a66..0cb4b86 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -62,6 +62,14 @@ class Navigation {
             Complete
         };
 
+        enum CalcAzimuthState {
+            Invalid,
+            Bad,
+            Ok,
+            Good,
+            Super
+        };
+
 
         /**
          * @brief Construct a new Navigation object and using I2C
@@ -118,6 +126,8 @@ class Navigation {
          */
         bool startNavigation();
         void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
+        void drivingDirectionChange();
+        void dissableCalcAzimuth() { this->directionChangeMode = false; }
         
         double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
         double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
@@ -175,14 +185,14 @@ class Navigation {
         Point getCurrentPosition() const { return this->currentPosition; }
 
         /**
-         * @brief Get azimuth
+         * @brief Get realAzimuth
          * 
          * This value represents the angle between north and 
          * the line of sight. Clockwise.
          * 
          * @return uint16_t degree
          */
-        int16_t getAzimuth() const { return this->azimuth; }
+        int16_t getAzimuth() const { return this->realAzimuth; }
         QMC5883LCompass* getCompass() const { return this->compass; }
 
         Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
@@ -200,26 +210,11 @@ class Navigation {
 
     private:
         void updateCurrentLocation();
-        int16_t calculateCourseCorrection(Point& point);
-
-        /**
-         * @brief Set the next point as target
-         * 
-         * @return true
-         * @return false 
-         */
-        bool nextPoint();
-
-        /**
-         * @brief Set the target point
-         * 
-         * @param target 
-         * @return true 
-         * @return false 
-         */
-        bool setTargetPoint(Point target);
-
+        void updateMagneticDeclination();
         void init(Route* route);
+        bool nextPoint();
+        bool setTargetPoint(Point target);
+        int16_t calculateCourseCorrection(Point& point);
 
         SFE_UBLOX_GNSS* gps;
         UBX_NAV_PVT_data_t* ubxData = nullptr;
@@ -229,22 +224,27 @@ class Navigation {
 
         Point lastPointRouteInsert;
         Point lastPointCalcCorrection;
+        Point lastPointDrivingDirectionChange;
         Point targetPoint;
         Point currentPosition;
         Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
+        CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid;
+
 
 
         bool navigationStarted = false;
         bool navigationFinished = false;
         bool isNtripInit = false;
         bool preventNextPoint = false;
+        bool directionChangeMode = false;
 
         char* host;
         char* mountPoint;
         char* user;
         char* password;
 
-        int16_t azimuth = INT16_MAX;
+        int16_t realAzimuth = INT16_MAX;
+        int16_t calcAzimuth = INT16_MAX;
 
         uint8_t timeToWait = 200;
         uint16_t port;

From f236d98be2a57fdae23ef8460af0a7d305cbce8a Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Tue, 8 Aug 2023 22:59:28 +0200
Subject: [PATCH 14/27] - added calc azimuth to autopilot menu - getters in
 navigation for calc azimuth - wrapper for manualControl for automatic
 orintaion change

---
 doc/TODO allgemein.txt                        |  1 +
 include/Version.h                             |  4 +-
 lib/Navigation/navigation.h                   |  2 +
 .../driveModi/Autopilot/menuAutopilot.cpp     | 41 ++++++++++++++++---
 src/driveModi/Modi/Autopilot/autopilot.cpp    | 22 +++++++++-
 src/driveModi/Modi/Autopilot/autopilot.h      | 11 +++++
 version                                       |  2 +-
 7 files changed, 73 insertions(+), 10 deletions(-)

diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt
index c33ae7c..37eaf10 100644
--- a/doc/TODO allgemein.txt	
+++ b/doc/TODO allgemein.txt	
@@ -8,4 +8,5 @@ Besser zügig:
     Speedometer buffer ergibt kaum Sinn
     Abweichung max 25 cm bevor zu ungenau damit nie mehr als 50 cm gesamt.
     Doxygen Kommentare aktualisieren
+    Motortreiber vielleicht bei wenigster Leistung ohne Kurve
     
diff --git a/include/Version.h b/include/Version.h
index 6672895..79e1506 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.8.20"
+        #define VERSION "0.8.21"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-08 14:33:17.266785"
+        #define BUILD_TIMESTAMP "2023-08-08 22:55:37.389313"
     #endif
     
\ No newline at end of file
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index 0cb4b86..b44abae 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -194,6 +194,8 @@ class Navigation {
          */
         int16_t getAzimuth() const { return this->realAzimuth; }
         QMC5883LCompass* getCompass() const { return this->compass; }
+        int16_t getCalcAzimuth() const { return this->calcAzimuth; }
+        CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; }
 
         Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
         void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 94d3828..b5dcd8b 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -166,10 +166,41 @@ void MenuAutopilot::printPage() const {
                 lineTwo = "accuracy is low";
             break;
 
-        default:
-            this->printDefault();
-            return;
-    }
+        case 11:
+            lineOne = "CalcAzi: ";
+            lineTwo = "State: ";
+            lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth());
+            switch (this->driveManager->getNavigation()->getCalcAzimuthState()) {
+                case Navigation::CalcAzimuthState::Bad :
+                    lineTwo.concat("Bad");
+                    break;
+                
+                case Navigation::CalcAzimuthState::Good :
+                    lineTwo.concat("Good");
+                    break;
+
+                case Navigation::CalcAzimuthState::Invalid :
+                    lineTwo.concat("Invalid");
+                    break;
+
+                case Navigation::CalcAzimuthState::Ok :
+                    lineTwo.concat("Ok");
+                    break;
+
+                case Navigation::CalcAzimuthState::Super :
+                    lineTwo.concat("Super");
+                    break;
+
+                default:
+                    lineTwo.concat("Unkown");
+                    break;
+            }
+            break;
+
+            default:
+                this->printDefault();
+                return;
+            }
 
     this->print(lineOne, lineTwo);
 }
@@ -219,7 +250,7 @@ void MenuAutopilot::update() {
 }
 
 void MenuAutopilot::init() {
-    this->setCountPages(11);
+    this->setCountPages(12);
     this->driveManager->changeModus(Modi::Autopilot);
     this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
     this->routeInfo = this->autopilot->getRouteInfo();
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index fe5ddbd..c3d413f 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -12,13 +12,31 @@
 #include "driveModi/Modi/Autopilot/autopilot.h"
 #include "autopilot.h"
 
+DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
+    this->navigation = navigation;
+    this->action();
+}
+
+DirectionChangeSignal::~DirectionChangeSignal() {
+    navigation->dissableCalcAzimuth();
+}
+
+void DirectionChangeSignal::action() {
+    this->navigation->drivingDirectionChange();
+}
+
+
 Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
     : ManualControl(moveControl, input) {
+        this->setInputMode(ManualControl::InputMode::Digital);
+        this->setDirectionChangeCallback(this->directionChangeSignal);
+        this->directionChangeSignal = new DirectionChangeSignal(navigation);
         this->navigation = navigation;
         this->init();
 }
 
 Autopilot::~Autopilot() {
+    delete this->directionChangeSignal;
     // this->navigation->getNTRIPClient()->setActivated(false);
 }
 
@@ -121,8 +139,8 @@ void Autopilot::rotate() {
     if (this->courseCorrection.correction > 0)
         this->moveControl->setRotationSpeed(-this->rotationSpeed);
     else
-
-    this->moveControl->setRotationSpeed(this->rotationSpeed);
+        this->moveControl->setRotationSpeed(this->rotationSpeed);
+    this->navigation->drivingDirectionChange();
 }
 
 void Autopilot::checkButtonInput() {
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index a64c87b..fa33412 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -17,6 +17,16 @@
 #include "moveControl.h"
 #include "driveModi/Modi/ManualControl/manualControl.h"
 
+class DirectionChangeSignal : public DirectionChangeWrapper {
+    public:
+        DirectionChangeSignal(Navigation* navigation);
+        ~DirectionChangeSignal();
+        void action() override;
+
+    private:
+        Navigation* navigation;
+};
+
 /**
  * @brief This class use the navigate class to drive automaticaly
  * 
@@ -120,6 +130,7 @@ class Autopilot : public ManualControl {
         State state = State::None;
         State lastState = State::None;
         Navigation::Status lastOrderStatus;
+        DirectionChangeSignal* directionChangeSignal;
 
         bool updateDisplay = false;
 
diff --git a/version b/version
index e12118d..49d791b 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.8.20
\ No newline at end of file
+0.8.21
\ No newline at end of file

From bc79b2a4a6e7a099811599c8ae34b7735262aa7b Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 10:11:07 +0200
Subject: [PATCH 15/27] increase buffersize and tiemout time for less
 connection aborts

---
 lib/NtripClient/ntripClient.cpp | 2 +-
 lib/NtripClient/ntripClient.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/NtripClient/ntripClient.cpp b/lib/NtripClient/ntripClient.cpp
index 93d7969..d5cefa1 100644
--- a/lib/NtripClient/ntripClient.cpp
+++ b/lib/NtripClient/ntripClient.cpp
@@ -221,7 +221,7 @@ void NTRIPClient::closeConnection() {
 
 bool NTRIPClient::processConnection() {
     if (this->ntripClient->connected()) {
-        uint8_t rtcmData[this->bufferSize * 4];
+        uint8_t rtcmData[this->bufferSize * 8];
         uint16_t rtcmCount = 0;
 
         while (this->ntripClient->available()) {
diff --git a/lib/NtripClient/ntripClient.h b/lib/NtripClient/ntripClient.h
index 900c6eb..7dc6809 100644
--- a/lib/NtripClient/ntripClient.h
+++ b/lib/NtripClient/ntripClient.h
@@ -129,7 +129,7 @@ class NTRIPClient {
         const uint8_t delayTime = 20;
         const uint8_t maxReconnectAttemps = 10;
         const uint16_t reconnectDelayTime = 1000;
-        const uint16_t timeOut = 5000;
+        const uint16_t timeOut = 10000;
         const uint16_t bufferSize = 512;
         const uint16_t pushGPGGATime = 10000;
 };

From 6b8b56fc1aecd66e3d463c61ba0d67af8fd58dd0 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 13:03:33 +0200
Subject: [PATCH 16/27] removed arduino dependency

---
 lib/Navigation/route.cpp | 22 +++++++++++-----------
 lib/Navigation/route.h   | 10 ++++------
 2 files changed, 15 insertions(+), 17 deletions(-)

diff --git a/lib/Navigation/route.cpp b/lib/Navigation/route.cpp
index 8d08a78..07c03a4 100644
--- a/lib/Navigation/route.cpp
+++ b/lib/Navigation/route.cpp
@@ -11,35 +11,35 @@
 
 #include "route.h"
 
-Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
+Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
     this->coordinates.lat = lat;
     this->coordinates.lon = lon; 
-    this->init(horizontalAccuracy);
+    this->init(horizontalAccuracy, creationTime);
 }
 
-Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) {
+Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
     this->coordinates.lat = lat / 10000000.0;
     this->coordinates.lon = lon / 10000000.0;
-    this->init(horizontalAccuracy);
+    this->init(horizontalAccuracy, creationTime);
 }
 
-Point::Point(Coordinates coords, uint32_t horizontalAccuracy) {
+Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
     this->coordinates = coords;
-    this->init(horizontalAccuracy);
+    this->init(horizontalAccuracy, creationTime);
 }
 
 Point::Point(Coordinates coords, bool imported) {
     this->coordinates = coords;
     if (imported)
-        this->init(UINT32_MAX);
+        this->init(UINT32_MAX, 0);
     else
-        this->init(0);
+        this->init(0, 0);
 }
 
 Point::Point() {
     this->coordinates.lat = 0;
     this->coordinates.lon = 0; 
-    this->init(0);
+    this->init(0, 0);
 }
 
 bool Point::operator==(const Point& rhs) const {
@@ -86,8 +86,8 @@ int16_t Point::courseTo(const Point &point) const {
     return this->courseTo(point.getCoordinates());
 }
 
-void Point::init(uint32_t horizontalAccuracy) {
-    this->creationTime = millis();
+void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
+    this->creationTime = creationTime;
 
     if (horizontalAccuracy == UINT32_MAX)
         this->accuracy = Accuracy::imported;
diff --git a/lib/Navigation/route.h b/lib/Navigation/route.h
index 62d6c19..e358bfb 100644
--- a/lib/Navigation/route.h
+++ b/lib/Navigation/route.h
@@ -12,8 +12,6 @@
 #ifndef ROUTE_H
 #define ROUTE_H
 
-#include <Arduino.h>
-
 #include <cstdint>
 #include <list> 
 #include <cmath>   
@@ -66,9 +64,9 @@ class Point{
          * @param coords Coordinates
          * @param imported if true than highest accuracy
          */
-        Point(double lat, double lon, uint32_t horizontalAccuracy = 0);
-        Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0);
-        Point(Coordinates coords, uint32_t horizontalAccuracy = 0);
+        Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
+        Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
+        Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
         Point(Coordinates coords, bool imported);
         Point();
 
@@ -133,7 +131,7 @@ class Point{
         Accuracy getAccuracy() { return this->accuracy; }
 
     private:
-        void init(uint32_t horizontalAccuracy);
+        void init(uint32_t horizontalAccuracy, uint32_t creationTime);
 
         Accuracy accuracy = Accuracy::none;
         Coordinates coordinates;

From a00ee4080c366d9a00c0239d842df22db1387157 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 14:33:51 +0200
Subject: [PATCH 17/27] moved class route in seperate folder

---
 lib/{Navigation => Route}/route.cpp | 0
 lib/{Navigation => Route}/route.h   | 0
 2 files changed, 0 insertions(+), 0 deletions(-)
 rename lib/{Navigation => Route}/route.cpp (100%)
 rename lib/{Navigation => Route}/route.h (100%)

diff --git a/lib/Navigation/route.cpp b/lib/Route/route.cpp
similarity index 100%
rename from lib/Navigation/route.cpp
rename to lib/Route/route.cpp
diff --git a/lib/Navigation/route.h b/lib/Route/route.h
similarity index 100%
rename from lib/Navigation/route.h
rename to lib/Route/route.h

From 984c1891e1227d962df700baae629b36dd1b65f3 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 14:37:22 +0200
Subject: [PATCH 18/27] run callback in digatalControl only after rotation
 finished

---
 src/driveModi/Modi/ManualControl/manualControl.cpp | 7 ++++++-
 src/driveModi/Modi/ManualControl/manualControl.h   | 3 ++-
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp
index a8f3643..a02c561 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.cpp
+++ b/src/driveModi/Modi/ManualControl/manualControl.cpp
@@ -85,5 +85,10 @@ void ManualControl::digitalControl() {
         this->moveControl->setRotationSpeed(0);
 
     if (this->directionChangeWrapper && (x > 120 || x < -120))
-        this->directionChangeWrapper->action();
+        this->lastLoopTurned = true;
+    else if (this->lastLoopTurned) {
+        if (this->directionChangeWrapper)
+            this->directionChangeWrapper->action();
+        this->lastLoopTurned = false;
+    }      
 }
diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h
index ad70d2d..35d242d 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.h
+++ b/src/driveModi/Modi/ManualControl/manualControl.h
@@ -103,8 +103,9 @@ class ManualControl : public DriveModi {
         void analogControl();
         void digitalControl();
 
-        uint32_t lastMillis = 0;
+        bool lastLoopTurned = false;
         uint8_t delay = 10;
+        uint32_t lastMillis = 0;
         double max_speed = 1;
         double max_rotation = 7;
         CalibrateCompass* caliCompass = nullptr;

From a9f09960c4eea591d212822dfa2ce95ccb12a832 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 14:37:56 +0200
Subject: [PATCH 19/27] init test framework

---
 platformio.ini                          | 12 ++++++------
 test/test_desktop/bootstrap.cpp         | 22 ++++++++++++++++++++++
 test/test_desktop/courseCalculation.hpp |  9 +++++++++
 3 files changed, 37 insertions(+), 6 deletions(-)
 create mode 100644 test/test_desktop/bootstrap.cpp
 create mode 100644 test/test_desktop/courseCalculation.hpp

diff --git a/platformio.ini b/platformio.ini
index 39ceb01..03e3aa1 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -8,7 +8,7 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env]
+[env:embedded]
 platform = espressif32
 board = esp32doit-devkit-v1
 board_build.partitions = no_ota.csv
@@ -29,14 +29,14 @@ upload_port = COM3
 extra_scripts = 
    pre:autoVersionIncrement/version_increment_pre.py
    post:autoVersionIncrement/version_increment_post.py
-
-[env:release]
-build_type = release
-
-[env:debug]
+test_ignore = test_desktop
 build_type = debug
 monitor_filters = esp32_exception_decoder
 check_tool = clangtidy
 
+[env:native]
+platform = native
+test_ignore = test_embedded
+
 [platformio]
 description = A Rover who should be drive a route by gps.
diff --git a/test/test_desktop/bootstrap.cpp b/test/test_desktop/bootstrap.cpp
new file mode 100644
index 0000000..1bbb8aa
--- /dev/null
+++ b/test/test_desktop/bootstrap.cpp
@@ -0,0 +1,22 @@
+#include "courseCalculation.hpp"
+#include <unity.h>
+
+void setUp() {
+
+}
+
+void tearDown() {
+
+}
+
+void uselessTest() {
+  TEST_ASSERT_EQUAL(12, 12);
+}
+
+int main(int argc, char **argv) {
+  UNITY_BEGIN();
+  RUN_TEST(testCourseCalculation);
+  UNITY_END();
+
+  return 0;
+}
diff --git a/test/test_desktop/courseCalculation.hpp b/test/test_desktop/courseCalculation.hpp
new file mode 100644
index 0000000..969ba19
--- /dev/null
+++ b/test/test_desktop/courseCalculation.hpp
@@ -0,0 +1,9 @@
+#include "route.h"
+#include <unity.h>
+#include <iostream>
+
+void testCourseCalculation(void) {
+    Point point;
+    std::cout << "Hello world from Test" << std::endl;
+    TEST_ASSERT_EQUAL_INT(20, 20);
+}

From 57553b3ccef5b876eca5486262e4a0a399eb5514 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 15:14:49 +0200
Subject: [PATCH 20/27] fix cast error

---
 lib/Route/route.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/Route/route.cpp b/lib/Route/route.cpp
index 07c03a4..5f5b395 100644
--- a/lib/Route/route.cpp
+++ b/lib/Route/route.cpp
@@ -74,7 +74,7 @@ int16_t Point::courseTo(const Coordinates& point) const {
     double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
     double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
 
-    int16_t res = (int16_t) atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
+    int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
 
     // if (res < 0)
     //     res += 360; 

From cf111426bdd3c521c2ff935de7a864d2dc59928f Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 15:32:55 +0200
Subject: [PATCH 21/27] courseCalculationTest

---
 test/test_desktop/courseCalculation.hpp | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/test/test_desktop/courseCalculation.hpp b/test/test_desktop/courseCalculation.hpp
index 969ba19..4950cc0 100644
--- a/test/test_desktop/courseCalculation.hpp
+++ b/test/test_desktop/courseCalculation.hpp
@@ -3,7 +3,13 @@
 #include <iostream>
 
 void testCourseCalculation(void) {
-    Point point;
-    std::cout << "Hello world from Test" << std::endl;
-    TEST_ASSERT_EQUAL_INT(20, 20);
+    // Point pointA(53.106725, 7.248069); // 45
+    // Point pointA(53.112188, 7.264404); // 90
+    Point pointA(53.112072, 7.266619); // -90
+    Point pointB(53.112123, 7.265354); //Home
+    int res = pointA.courseTo(pointB);
+    std::cout << "" << std::endl;
+    std::cout << "" << std::endl;
+    std::cout << "Return Value: " << res << std::endl;
+    TEST_ASSERT_LESS_OR_EQUAL(10, res);
 }

From 98f3cb93f0d212e73a521661337896d648b76071 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 15:34:03 +0200
Subject: [PATCH 22/27] calc Azimuth finished

---
 .version_no_increment                      |  0
 include/Version.h                          |  4 ++--
 lib/Navigation/navigation.cpp              |  2 ++
 src/driveModi/Modi/Autopilot/autopilot.cpp |  2 +-
 src/driveModi/Modi/Autopilot/autopilot.h   | 10 +---------
 version                                    |  2 +-
 6 files changed, 7 insertions(+), 13 deletions(-)
 delete mode 100644 .version_no_increment

diff --git a/.version_no_increment b/.version_no_increment
deleted file mode 100644
index e69de29..0000000
diff --git a/include/Version.h b/include/Version.h
index 79e1506..534eccd 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.8.21"
+        #define VERSION "0.8.27"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-08 22:55:37.389313"
+        #define BUILD_TIMESTAMP "2023-08-09 10:33:06.900861"
     #endif
     
\ No newline at end of file
diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index 70ad78c..4a5ec70 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -126,6 +126,7 @@ void Navigation::drivingDirectionChange() {
     if (tmp.isInit() && tmp.isValid()) {
         this->directionChangeMode = true;
         this->lastPointDrivingDirectionChange = tmp;
+        this->calcAzimuthState = CalcAzimuthState::Invalid;
     }
 }
 
@@ -200,6 +201,7 @@ void Navigation::updateMagneticDeclination() {
         || this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0) 
     {
         this->calcAzimuthState = CalcAzimuthState::Invalid;
+        this->calcAzimuth = 999;
         return;
     }
 
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index c3d413f..e6a9e94 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -29,8 +29,8 @@ void DirectionChangeSignal::action() {
 Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
     : ManualControl(moveControl, input) {
         this->setInputMode(ManualControl::InputMode::Digital);
-        this->setDirectionChangeCallback(this->directionChangeSignal);
         this->directionChangeSignal = new DirectionChangeSignal(navigation);
+        this->setDirectionChangeCallback(this->directionChangeSignal);
         this->navigation = navigation;
         this->init();
 }
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index fa33412..dfbf21c 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -76,15 +76,6 @@ class Autopilot : public ManualControl {
          */
         void loop();
 
-        /**
-         * @brief Manges the autopilot
-         * 
-         * If the first Point is near to current location you can turn
-         * the autopilot on.
-         * Gets the course correction and decide what to do.
-         */
-        void runAutopilot();
-
         void restart();
 
         /**
@@ -120,6 +111,7 @@ class Autopilot : public ManualControl {
         void init();
         void drive();
         void rotate();
+        void runAutopilot();
         void checkButtonInput();
         void askNavigationForOrder();
         void selfDriving();
diff --git a/version b/version
index 49d791b..bdf3118 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.8.21
\ No newline at end of file
+0.8.27
\ No newline at end of file

From 2e0b7f7f8d3cb4fc308ca964afe51b3cb56a6675 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 9 Aug 2023 22:41:48 +0200
Subject: [PATCH 23/27] no time for comment

---
 autoVersionIncrement                          |  1 +
 include/Version.h                             |  4 +-
 lib/Navigation/navigation.cpp                 | 13 +++++-
 lib/Navigation/navigation.h                   |  2 +
 .../driveModi/Autopilot/menuAutopilot.cpp     | 46 +++++++++++++++++--
 src/driveModi/Modi/Autopilot/autopilot.cpp    | 35 +++++++++++---
 src/driveModi/Modi/Autopilot/autopilot.h      |  4 ++
 version                                       |  2 +-
 8 files changed, 91 insertions(+), 16 deletions(-)
 create mode 160000 autoVersionIncrement

diff --git a/autoVersionIncrement b/autoVersionIncrement
new file mode 160000
index 0000000..fd51b62
--- /dev/null
+++ b/autoVersionIncrement
@@ -0,0 +1 @@
+Subproject commit fd51b62f000d23d5649da611c2560af8e2327415
diff --git a/include/Version.h b/include/Version.h
index 534eccd..5ed91a8 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.8.27"
+        #define VERSION "0.8.28"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-09 10:33:06.900861"
+        #define BUILD_TIMESTAMP "2023-08-09 19:28:27.390516"
     #endif
     
\ No newline at end of file
diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index 4a5ec70..7c962fc 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -252,7 +252,18 @@ void Navigation::updateMagneticDeclination() {
 
 int16_t Navigation::calculateCourseCorrection(Point& point) {
     int16_t targetCourse = point.courseTo(this->targetPoint);
-    int16_t correctionCourse = targetCourse - this->realAzimuth;
+    int16_t correctionCourse;
+
+    if (this->calcAzimuthState == CalcAzimuthState::Good
+        || this->calcAzimuthState == CalcAzimuthState::Super)
+    {
+        correctionCourse = targetCourse - this->calcAzimuth;
+        this->lastUsedCalcAzimuth = true;
+    } else {
+        correctionCourse = targetCourse - this->realAzimuth;
+        this->lastUsedCalcAzimuth = false;
+    }
+
     if (correctionCourse > 180)
         correctionCourse -= 360;
     else if (correctionCourse < -180)
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index b44abae..b302bf7 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -196,6 +196,7 @@ class Navigation {
         QMC5883LCompass* getCompass() const { return this->compass; }
         int16_t getCalcAzimuth() const { return this->calcAzimuth; }
         CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; }
+        bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; }
 
         Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
         void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
@@ -239,6 +240,7 @@ class Navigation {
         bool isNtripInit = false;
         bool preventNextPoint = false;
         bool directionChangeMode = false;
+        bool lastUsedCalcAzimuth = false;
 
         char* host;
         char* mountPoint;
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index b5dcd8b..269b195 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -197,10 +197,30 @@ void MenuAutopilot::printPage() const {
             }
             break;
 
-            default:
-                this->printDefault();
-                return;
-            }
+        case 12:
+            lineOne = "Test rotate";
+            lineTwo = "180 degree";
+            break;
+
+        case 13:
+            lineOne = "Test rotate";
+            lineTwo = "45 degree";
+            break;
+
+        case 14:
+            lineOne = "Test rotate";
+            lineTwo = "15 degree";
+            break;
+
+        case 15:
+            lineOne = "Test rotate";
+            lineTwo = "5 degree";
+            break;
+
+        default:
+            this->printDefault();
+            return;
+        }
 
     this->print(lineOne, lineTwo);
 }
@@ -235,6 +255,22 @@ void MenuAutopilot::runCommand() const {
             else 
                 this->driveManager->getNavigation()->setMinAccuracy(Point::Accuracy::twoDigOfCM);
             break;
+
+        case 12:
+            this->autopilot->rotate(180);
+            break;
+
+        case 13:
+            this->autopilot->rotate(45);
+            break;
+
+        case 14:
+            this->autopilot->rotate(15);
+            break;
+
+        case 15:
+            this->autopilot->rotate(5);
+            break;
         
         default:
             break;
@@ -250,7 +286,7 @@ void MenuAutopilot::update() {
 }
 
 void MenuAutopilot::init() {
-    this->setCountPages(12);
+    this->setCountPages(16);
     this->driveManager->changeModus(Modi::Autopilot);
     this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
     this->routeInfo = this->autopilot->getRouteInfo();
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index e6a9e94..c36f362 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -91,6 +91,11 @@ void Autopilot::runAutopilot() {
             this->selfDriving();
             break;
 
+        case SelfDrivingRotate:
+            this->checkButtonInput();
+            this->rotate();
+            break;
+
         case State::TargetReached:
             break;
         
@@ -111,6 +116,11 @@ bool Autopilot::shouldUpdate() {
     return false;
 }
 
+void Autopilot::rotate(int16_t degree) {
+    this->courseCorrection.correction = degree;
+    this->rotate();
+}
+
 void Autopilot::init() {
     if (this->navigation->startNavigation())
             this->state = State::NavigationStarted;
@@ -135,12 +145,23 @@ void Autopilot::drive() {
 }
 
 void Autopilot::rotate() {
-    this->moveControl->setSpeed(0);
-    if (this->courseCorrection.correction > 0)
-        this->moveControl->setRotationSpeed(-this->rotationSpeed);
-    else
-        this->moveControl->setRotationSpeed(this->rotationSpeed);
-    this->navigation->drivingDirectionChange();
+    if (this->state != State::SelfDrivingRotate) {
+        this->lastState = this->state;
+        this->state = State::SelfDrivingRotate;
+        this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction;
+
+        this->moveControl->setSpeed(0);
+        if (this->courseCorrection.correction > 0)
+            this->moveControl->setRotationSpeed(-this->rotationSpeed);
+        else
+            this->moveControl->setRotationSpeed(this->rotationSpeed);
+    } else {
+        if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 3) {
+            this->state = this->lastState;
+            this->navigation->drivingDirectionChange();
+            this->moveControl->setRotationSpeed(0);
+        }
+    }
 }
 
 void Autopilot::checkButtonInput() {
@@ -149,7 +170,7 @@ void Autopilot::checkButtonInput() {
         
         if (this->state  == State::SelfDrivingAvailable)
             this->state = State::SelfDriving;
-        else if (this->state == State::SelfDriving)
+        else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate)
             this->state = State::SelfDrivingAvailable;
         else if (this->state == State::NavigationStarted)
             this->state = State::GetToStartPoint;
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index dfbf21c..d7bc259 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -46,6 +46,7 @@ class Autopilot : public ManualControl {
             GetToStartPoint,
             SelfDrivingAvailable,
             SelfDriving,
+            SelfDrivingRotate,
             TargetReached
         };
 
@@ -106,6 +107,7 @@ class Autopilot : public ManualControl {
          * @return false 
          */
         bool shouldUpdate();
+        void rotate(int16_t degree);
 
     private:
         void init();
@@ -134,6 +136,8 @@ class Autopilot : public ManualControl {
         uint32_t loopLastMillis = 0;
         uint32_t lastAutopilotChangeMillis = 0;
 
+        int16_t rotationAimAzimuth;
+
         double drivingSpeed = 1;
         double rotationSpeed = 4.5;
         double minRemainingDistance = 0.25;
diff --git a/version b/version
index bdf3118..09318a8 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.8.27
\ No newline at end of file
+0.8.28
\ No newline at end of file

From 1e15cf5d5d149fa575f7ecba78e128ef05b99734 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Sun, 13 Aug 2023 11:04:19 +0200
Subject: [PATCH 24/27] removed submodule structure

---
 .gitmodules | 3 ---
 1 file changed, 3 deletions(-)
 delete mode 100644 .gitmodules

diff --git a/.gitmodules b/.gitmodules
deleted file mode 100644
index 9375b66..0000000
--- a/.gitmodules
+++ /dev/null
@@ -1,3 +0,0 @@
-[submodule "platformio_version_increment"]
-	path = autoVersionIncrement
-	url = https://github.com/sblantipodi/platformio_version_increment.git

From cd319f43e47064aad43901c035fe6abe9c79c9fe Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Sun, 13 Aug 2023 13:42:22 +0200
Subject: [PATCH 25/27] refactore motorControl

---
 lib/MotorControl/motorControl.cpp | 62 +++++++++++++++----------------
 lib/MotorControl/motorControl.h   | 34 ++++++++---------
 2 files changed, 48 insertions(+), 48 deletions(-)

diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp
index f94111f..96375af 100644
--- a/lib/MotorControl/motorControl.cpp
+++ b/lib/MotorControl/motorControl.cpp
@@ -17,9 +17,9 @@ MotorControl::MotorControl() {
     this->setMaxPwm(PWMMAX);
 }
 
-void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2) {
-    this->pwm_pin = pwm_pin;
-    this->pwm_channel = pwm_channel;
+void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) {
+    this->pwmPin = pwmPin;
+    this->pwmChannel = pwmChannel;
     this->dir_1 = dir_1;
     this->dir_2 = dir_2;
 
@@ -29,17 +29,17 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uin
     digitalWrite(this->dir_1, LOW);
     digitalWrite(this->dir_2, LOW);
 
-    ledcSetup(this->pwm_channel, PWMFREQ, this->pwm_res);
-    ledcAttachPin(this->pwm_pin, this->pwm_channel);
-    ledcWrite(this->pwm_channel, 0);
+    ledcSetup(this->pwmChannel, PWMFREQ, this->pwmRes);
+    ledcAttachPin(this->pwmPin, this->pwmChannel);
+    ledcWrite(this->pwmChannel, 0);
 }
 
 uint16_t MotorControl::loop() {
     uint32_t time = millis();
     uint16_t elapsed_time = time - this->lastMillis;
 
-    //Cancel if delay is not reached
-    if (elapsed_time < delay)
+    //Cancel if delayLoop is not reached
+    if (elapsed_time < delayLoop)
         return elapsed_time;
 
     runMotorControl();
@@ -48,14 +48,14 @@ uint16_t MotorControl::loop() {
 }
 
 void MotorControl::runMotorControl() {
-    // Absolute difference between target_power and power
-    uint8_t abs_difference = abs(this->target_power - this->power);
+    // Absolute difference between targetPower and power
+    uint8_t abs_difference = abs(this->targetPower - this->power);
 
-    // Difference between target_power and power
-    int16_t difference = this->target_power - this->power;
+    // Difference between targetPower and power
+    int16_t difference = this->targetPower - this->power;
 
     // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps
-    if (abs(this->target_power) <  powersteps && abs_difference < powersteps) {
+    if (abs(this->targetPower) <  powersteps && abs_difference < powersteps) {
         this->setRealPower(0);
         return;
     }
@@ -66,7 +66,7 @@ void MotorControl::runMotorControl() {
     }
 
     // Positive or negative tagret speed
-    if (this->target_power >= 0) {
+    if (this->targetPower >= 0) {
         // Positive or negative speed
         if (this->power >= 0) {
             if (difference > 0) {
@@ -95,36 +95,36 @@ void MotorControl::runMotorControl() {
 void MotorControl::setMinPwm(uint8_t min) {
     if (min > 80) min = 80;
     //transform percentage to real pwm value
-    min = (uint8_t) (((1 << pwm_res) - 1) * (min / 100.0));
-    this->dutycycle_min = min;
+    min = (uint8_t) (((1 << pwmRes) - 1) * (min / 100.0));
+    this->dutycycleMin = min;
 }
 
 void MotorControl::setMaxPwm(uint8_t max) {
     if (max > 100) max = 100;
     //transform percentage to real pwm value
-    max = (uint8_t) (((1 << pwm_res) - 1) * (max / 100.0));
-    this->dutycycle_max = max;
+    max = (uint8_t) (((1 << pwmRes) - 1) * (max / 100.0));
+    this->dutycycleMax = max;
 }
 
 uint16_t MotorControl::setPowerSteps(uint8_t increment) {
     this->powersteps = increment;
-    return (uint16_t) (delay * ( 100 / powersteps ));
+    return (uint16_t) (delayLoop * ( 100 / powersteps ));
 }
 
 void MotorControl::setTargetPower(int8_t power) {
     if (power <= 100 && power >= -100)
-        this->target_power = power;
+        this->targetPower = power;
     else
         std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
 }
 
-uint16_t MotorControl::setDelay(uint8_t delay) {
-    this->delay = delay;
-    return (uint16_t) (delay * ( 100 / powersteps ));
+uint16_t MotorControl::setDelay(uint8_t delayLoop) {
+    this->delayLoop = delayLoop;
+    return (uint16_t) (delayLoop * ( 100 / powersteps ));
 }
 
 void MotorControl::stop() {
-    this->target_power = 0;
+    this->targetPower = 0;
 }
 
 void MotorControl::emergencyStop() {
@@ -136,23 +136,23 @@ int8_t MotorControl::getPower() {
 }
 
 int8_t MotorControl::getTargetPower() {
-    return this->target_power;
+    return this->targetPower;
 }
 
 bool MotorControl::isTargetPowerReached() {
-    if (this->target_power == this->power)
+    if (this->targetPower == this->power)
         return true;
     return false;
 }
 
 bool MotorControl::isAccelerationPositive() {
-    if (power < target_power)
+    if (power < targetPower)
         return true;
     return false;
 }
 
 bool MotorControl::isAccelerationNegative() {
-    if (power > target_power)
+    if (power > targetPower)
         return true;
     return false;
 }
@@ -169,12 +169,12 @@ void MotorControl::setRealPower(int8_t power) {
         this->direction = 0;
         digitalWrite(this->dir_1, LOW);
         digitalWrite(this->dir_2, LOW);
-        ledcWrite(this->pwm_channel, 0);
+        ledcWrite(this->pwmChannel, 0);
         this->dutycycle = 0;
         return;
     }
 
-    uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycle_min, this->dutycycle_max);
+    uint8_t pwm_val = map(abs(power), 0, 100, this->dutycycleMin, this->dutycycleMax);
 
     if ((this->direction == 1 || this->direction == 0) && power < 0){  // new direction backward
         this->direction = 2;
@@ -186,7 +186,7 @@ void MotorControl::setRealPower(int8_t power) {
         digitalWrite(this->dir_2, LOW);
     }
 
-    ledcWrite(this->pwm_channel, pwm_val);
+    ledcWrite(this->pwmChannel, pwm_val);
     this->dutycycle = pwm_val;
 }
 
diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h
index ed070aa..e5819ed 100644
--- a/lib/MotorControl/motorControl.h
+++ b/lib/MotorControl/motorControl.h
@@ -35,17 +35,17 @@ class MotorControl {
         /**
          * @brief Initialize the motorController
          * 
-         * @param pwm_pin The output pin for the signal on the esp.
-         * @param pwm_channel One of the pwm channels from the esp.
+         * @param pwmPin The output pin for the signal on the esp.
+         * @param pwmChannel One of the pwm channels from the esp.
          * @param dir_1 First direction pin for the H-Bridge.
          * @param dir_2 Second direction pin for the H-Bridge.
          */
-        void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uint8_t dir_2);
+        void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2);
 
         /**
          * @brief Calls runMotorControl() to update the pwm signal
          * 
-         * This function should be called every mainloop. If the delay is not reached, than the
+         * This function should be called every mainloop. If the delayLoop is not reached, than the
          * functions returns immediately.
          * @see runMotorControl()
          * @see DELAY
@@ -81,10 +81,10 @@ class MotorControl {
          * 
          * Set the increment of the steps with which the dutycycle is
          * increased or decreased. Note the dependency between the increment
-         * and delay().
+         * and delayLoop().
          * 
          * The formula for the time between 0% and 100% power is:
-         *      time[ms] = delay * ( 100 / increment )
+         *      time[ms] = delayLoop * ( 100 / increment )
          *      500 ms are recommended
          * 
          * @see setDelay()
@@ -105,17 +105,17 @@ class MotorControl {
         void setTargetPower(int8_t power);
 
         /**
-         * @brief Set the min delay between each loop
+         * @brief Set the min delayLoop between each loop
          * 
-         * Note the dependency between delay and
+         * Note the dependency between delayLoop and
          * setPowerSteps().
          * 
          * @see setPowerSteps()
          * 
-         * @param delay time in Milliseconds
+         * @param delayLoop time in Milliseconds
          * @return time from 0% power to 100% power in Milliseconds
          */
-        uint16_t setDelay(uint8_t delay);
+        uint16_t setDelay(uint8_t delayLoop);
 
         /**
          * @brief Stops the motor like setTargetPower() to 0
@@ -152,19 +152,19 @@ class MotorControl {
         void setRealPower(int8_t power);
         void increasePower(int8_t power);
 
-        int8_t target_power = 0;
+        int8_t targetPower = 0;
         int8_t power = 0;
         uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
 
-        uint8_t pwm_pin;
-        uint8_t pwm_channel;
-        uint8_t pwm_res = PWMRES;
+        uint8_t pwmPin;
+        uint8_t pwmChannel;
+        uint8_t pwmRes = PWMRES;
         uint16_t dutycycle = 0;
-        uint8_t dutycycle_min;
-        uint8_t dutycycle_max;
+        uint8_t dutycycleMin;
+        uint8_t dutycycleMax;
         uint8_t dir_1;
         uint8_t dir_2;
-        uint8_t delay = DELAY;
+        uint8_t delayLoop = DELAY;
         uint8_t powersteps = POWERSTEPS;
 
         uint32_t lastMillis = 0;

From 30e2b75ea664449858c2c4531b674d5af5a19554 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Sun, 13 Aug 2023 17:03:19 +0200
Subject: [PATCH 26/27] added extra function for azimuth overflow

---
 lib/Navigation/navigation.cpp | 23 ++++++++++++-----------
 lib/Navigation/navigation.h   |  2 ++
 2 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index 7c962fc..e09e260 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -263,17 +263,8 @@ int16_t Navigation::calculateCourseCorrection(Point& point) {
         correctionCourse = targetCourse - this->realAzimuth;
         this->lastUsedCalcAzimuth = false;
     }
-
-    if (correctionCourse > 180)
-        correctionCourse -= 360;
-    else if (correctionCourse < -180)
-        correctionCourse += 360;
-
-    //Rotate result by 180° to corrigate Azimuth
-    correctionCourse += 180;
-    correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
-
-    return correctionCourse;
+        
+    return Navigation::fixDegree(correctionCourse);
 }
 
 bool Navigation::nextPoint() { 
@@ -294,6 +285,16 @@ void Navigation::setOutputStatusPrintPVTdata(bool status) {
     Navigation::outputStatusPrintPVTdata = status;
 }
 
+int16_t Navigation::fixDegree(int16_t degree) {
+    while (degree < -180 || degree > 180) {
+        if (degree > 180)
+            degree -= 360;
+        else if (degree < -180)
+            degree += 360;
+    }
+    return degree;
+}
+
 void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
     if (!Navigation::outputStatusPrintPVTdata)
         return;
diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h
index b302bf7..ea1d65b 100644
--- a/lib/Navigation/navigation.h
+++ b/lib/Navigation/navigation.h
@@ -210,6 +210,8 @@ class Navigation {
          * @param status 
          */
         static void setOutputStatusPrintPVTdata(bool status);
+        // map input in range from -180 to 180 degree
+        static int16_t fixDegree(int16_t degree);
 
     private:
         void updateCurrentLocation();

From bad84b1ef3c8f0c64378f7548a980bcf3b1501b4 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Sun, 13 Aug 2023 17:04:53 +0200
Subject: [PATCH 27/27] refactore and fixes first route successfully absolved

---
 include/Version.h                             |  4 +-
 .../driveModi/Autopilot/menuAutopilot.cpp     | 18 ++++---
 src/driveModi/Modi/Autopilot/autopilot.cpp    | 50 +++++++++++++++----
 src/driveModi/Modi/Autopilot/autopilot.h      |  4 +-
 version                                       |  2 +-
 5 files changed, 57 insertions(+), 21 deletions(-)

diff --git a/include/Version.h b/include/Version.h
index 5ed91a8..109f133 100644
--- a/include/Version.h
+++ b/include/Version.h
@@ -1,9 +1,9 @@
 
     // AUTO GENERATED FILE, DO NOT EDIT
     #ifndef VERSION
-        #define VERSION "0.8.28"
+        #define VERSION "0.8.36"
     #endif
     #ifndef BUILD_TIMESTAMP
-        #define BUILD_TIMESTAMP "2023-08-09 19:28:27.390516"
+        #define BUILD_TIMESTAMP "2023-08-13 15:13:34.491991"
     #endif
     
\ No newline at end of file
diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
index 269b195..c7f4f1f 100644
--- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
@@ -66,6 +66,10 @@ void MenuAutopilot::printPage() const {
                     lineTwo = "Autopilot active";
                     break;
 
+                case Autopilot::State::SelfDrivingRotate :
+                    lineTwo = "Rotating";
+                    break;
+
                 case Autopilot::State::TargetReached :
                     lineTwo = "Target reached";
                     break;
@@ -204,17 +208,17 @@ void MenuAutopilot::printPage() const {
 
         case 13:
             lineOne = "Test rotate";
-            lineTwo = "45 degree";
+            lineTwo = "270 degree";
             break;
 
         case 14:
             lineOne = "Test rotate";
-            lineTwo = "15 degree";
+            lineTwo = "45 degree";
             break;
 
         case 15:
             lineOne = "Test rotate";
-            lineTwo = "5 degree";
+            lineTwo = "20 degree";
             break;
 
         default:
@@ -257,19 +261,19 @@ void MenuAutopilot::runCommand() const {
             break;
 
         case 12:
-            this->autopilot->rotate(180);
+            this->autopilot->testRotate(180);
             break;
 
         case 13:
-            this->autopilot->rotate(45);
+            this->autopilot->testRotate(270);
             break;
 
         case 14:
-            this->autopilot->rotate(15);
+            this->autopilot->testRotate(45);
             break;
 
         case 15:
-            this->autopilot->rotate(5);
+            this->autopilot->testRotate(20);
             break;
         
         default:
diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp
index c36f362..004138f 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.cpp
+++ b/src/driveModi/Modi/Autopilot/autopilot.cpp
@@ -116,9 +116,12 @@ bool Autopilot::shouldUpdate() {
     return false;
 }
 
-void Autopilot::rotate(int16_t degree) {
+void Autopilot::testRotate(int16_t degree) {
+    if (!degree)
+        return;
+
     this->courseCorrection.correction = degree;
-    this->rotate();
+    this->beginRotate();
 }
 
 void Autopilot::init() {
@@ -144,26 +147,50 @@ void Autopilot::drive() {
         this,moveControl->setSpeed(0);
 }
 
-void Autopilot::rotate() {
+void Autopilot::beginRotate() {
     if (this->state != State::SelfDrivingRotate) {
         this->lastState = this->state;
         this->state = State::SelfDrivingRotate;
         this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction;
+        this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);
 
         this->moveControl->setSpeed(0);
         if (this->courseCorrection.correction > 0)
             this->moveControl->setRotationSpeed(-this->rotationSpeed);
         else
             this->moveControl->setRotationSpeed(this->rotationSpeed);
-    } else {
-        if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 3) {
-            this->state = this->lastState;
-            this->navigation->drivingDirectionChange();
-            this->moveControl->setRotationSpeed(0);
-        }
     }
 }
 
+void Autopilot::rotate() {
+    if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) {
+        this->endRotate();
+        return;
+    }
+
+    if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15)
+        &&
+        ((this->courseCorrection.correction > 0 
+        && this->rotationAimAzimuth < this->navigation->getAzimuth())
+        || (this->courseCorrection.correction < 0
+        && this->rotationAimAzimuth > this->navigation->getAzimuth())))
+    {
+        this->endRotate();
+        std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
+    }    
+}
+
+void Autopilot::endRotate() {
+    if (this->state != State::SelfDrivingRotate)
+        return;
+
+    this->state = this->lastState;
+    this->navigation->drivingDirectionChange();
+    this->moveControl->setRotationSpeed(0);
+    this->moveControl->emergencyStop();
+    this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
+}
+
 void Autopilot::checkButtonInput() {
     if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
         && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
@@ -211,8 +238,11 @@ void Autopilot::askNavigationForOrder() {
 }
 
 void Autopilot::selfDriving() {
+    if (this->state != State::SelfDriving)
+        return;
+
     if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
-        this->rotate();
+        this->beginRotate();
     else
         this->drive();
 }
diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h
index d7bc259..2fd53c5 100644
--- a/src/driveModi/Modi/Autopilot/autopilot.h
+++ b/src/driveModi/Modi/Autopilot/autopilot.h
@@ -107,11 +107,13 @@ class Autopilot : public ManualControl {
          * @return false 
          */
         bool shouldUpdate();
-        void rotate(int16_t degree);
+        void testRotate(int16_t degree);
+        void endRotate();
 
     private:
         void init();
         void drive();
+        void beginRotate();
         void rotate();
         void runAutopilot();
         void checkButtonInput();
diff --git a/version b/version
index 09318a8..98bb028 100644
--- a/version
+++ b/version
@@ -1 +1 @@
-0.8.28
\ No newline at end of file
+0.8.36
\ No newline at end of file