no time for comment

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
     

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)

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;

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -197,6 +197,26 @@ void MenuAutopilot::printPage() const {
             }
             break;
 
+        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;
@@ -236,6 +256,22 @@ void MenuAutopilot::runCommand() const {
                 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() {
+    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;

version

@@ -1 +1 @@
-0.8.27
+0.8.28