spelling

circle mode for autopilot
menu entry for circle mode

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -18,7 +18,7 @@ DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
 }
 
 DirectionChangeSignal::~DirectionChangeSignal() {
-    navigation->dissableCalcAzimuth();
+    navigation->disableCalcAzimuth();
 }
 
 void DirectionChangeSignal::action() {
@@ -59,7 +59,7 @@ void Autopilot::loop() {
 
 void Autopilot::runAutopilot() {
     switch (this->state) {
-        case State::InsufficientAccuarcy:
+        case State::InsufficientAccuracy:
             this->askNavigationForOrder();
             return;
 
@@ -97,6 +97,8 @@ void Autopilot::runAutopilot() {
             break;
 
         case State::TargetReached:
+            if (this->loopMode)
+                this->restartLoop();
             break;
         
         default:
@@ -163,12 +165,12 @@ void Autopilot::beginRotate() {
 }
 
 void Autopilot::rotate() {
-    if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) {
+    if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
         this->endRotate();
         return;
     }
 
-    if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15)
+    if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
         &&
         ((this->courseCorrection.correction > 0 
         && this->rotationAimAzimuth < this->navigation->getAzimuth())
@@ -217,18 +219,18 @@ void Autopilot::askNavigationForOrder() {
         
         case Navigation::Status::InsufficientAccuracy:
             this->lastState = this->state;
-            this->state = State::InsufficientAccuarcy;
+            this->state = State::InsufficientAccuracy;
             this->moveControl->setSpeed(0);
             this->moveControl->setRotationSpeed(0);
             break;
 
         case Navigation::Status::Unchanged:
-            if (this->state == State::InsufficientAccuarcy)
+            if (this->state == State::InsufficientAccuracy)
                 this->state = this->lastState;
             break;
 
         case Navigation::Status::Updated:
-            if (this->state == State::InsufficientAccuarcy)
+            if (this->state == State::InsufficientAccuracy)
                 this->state = this->lastState;
             break;
         
@@ -241,8 +243,16 @@ void Autopilot::selfDriving() {
     if (this->state != State::SelfDriving)
         return;
 
-    if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
+    if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct)
         this->beginRotate();
     else
         this->drive();
 }
+
+void Autopilot::restartLoop() {
+    this->navigation->startNavigation();
+    this->state = State::SelfDriving;
+    this->routeInfo = this->navigation->getRouteInfo();
+    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
+    this->updateDisplay = true;
+}

src/driveModi/Modi/Autopilot/autopilot.h

@@ -39,7 +39,7 @@ class DirectionChangeSignal : public DirectionChangeWrapper {
 class Autopilot : public ManualControl {
     public:
         enum State {
-            InsufficientAccuarcy = -2,
+            InsufficientAccuracy = -2,
             NoRoute = -1,
             None = 0,
             NavigationStarted,
@@ -109,6 +109,8 @@ class Autopilot : public ManualControl {
         bool shouldUpdate();
         void testRotate(int16_t degree);
         void endRotate();
+        void switchLoopMode() { this->loopMode = !this->loopMode; }
+        bool getLoopMode() const { return this->loopMode; }
 
     private:
         void init();
@@ -119,6 +121,7 @@ class Autopilot : public ManualControl {
         void checkButtonInput();
         void askNavigationForOrder();
         void selfDriving();
+        void restartLoop();
 
         Navigation* navigation;
         CourseCorrection courseCorrection;
@@ -129,9 +132,10 @@ class Autopilot : public ManualControl {
         DirectionChangeSignal* directionChangeSignal;
 
         bool updateDisplay = false;
+        bool loopMode = false;
 
         uint8_t loopDelayMillis = 40;
-        uint8_t maxCourseDeviationBerforeAct = 5;
+        uint8_t maxCourseDeviationBeforeAct = 5;
         uint16_t displayUpdateDelayMillis = 1000;
         uint16_t autopilotChangeDelayMillis = 500;
         uint32_t displayUpdateLastMillis = 0;