no time for comment

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();

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;

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;