no time for comment

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;