spelling

circle mode for autopilot
menu entry for circle mode

.vscode/settings.json

@@ -125,6 +125,7 @@
         "ardui",
         "blox",
         "bluedroid",
+        "carr",
         "CIPO",
         "COPI",
         "gast",
@@ -144,6 +145,7 @@
         "RHEDE",
         "Rtcm",
         "Schalke",
+        "Soln",
         "TPMOBIL",
         "UBLOX",
         "ZLPJ"

lib/Navigation/navigation.h

@@ -127,7 +127,7 @@ class Navigation {
         bool startNavigation();
         void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
         void drivingDirectionChange();
-        void dissableCalcAzimuth() { this->directionChangeMode = false; }
+        void disableCalcAzimuth() { this->directionChangeMode = false; }
         
         double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
         double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -42,7 +42,7 @@ void MenuAutopilot::printPage() const {
             lineOne = "Status: ";
             lineTwo = "";
             switch (this->autopilot->getState()) {
-                case Autopilot::State::InsufficientAccuarcy :
+                case Autopilot::State::InsufficientAccuracy :
                     lineTwo = "Err: LowAccuracy";
                     break;
 
@@ -75,15 +75,20 @@ void MenuAutopilot::printPage() const {
                     break;
                 
                 default:
-                    lineTwo = "UNKOWN";
+                    lineTwo = "UNKOWN - ";
+                    lineTwo.concat(static_cast<int>(this->autopilot->getState()));
+
                     break;
             }
             break;
         }
 
         case 1:
-            if (!navigationStarted)
+            if (!navigationStarted) {
+                lineOne = "Navigation is";
+                lineTwo = "not started";
                 break;
+            }
             lineOne = "Distance: ";
             lineOne.concat(distanceString);
             lineTwo = "Turn: ";
@@ -100,39 +105,75 @@ void MenuAutopilot::printPage() const {
 
         case 3: {
             NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
-            lineOne = "NTRIP Client is";
+            uint8_t carrSoln = gpsData->flags.bits.carrSoln;
+            lineOne = "GNSS: ";
 
             if (status ==  NTRIPClientStates::pushData)
-                lineTwo = "enabled";
+                if (carrSoln == 0)
-            else if (status == NTRIPClientStates::notAvailable)
+                    lineOne.concat("None");
-                lineTwo = "not available";
+                else if (carrSoln == 1)
+                    lineOne.concat("Floating");
+                else if (carrSoln == 2)
+                    lineOne.concat("Fixed");
                 else
-                lineTwo = "disabled";
+                    lineOne.concat("UNKNOWN");
+            else if (status == NTRIPClientStates::notAvailable)
+                lineOne.concat("No WiFi");
+            else
+                lineOne.concat("Offline");
+
+            lineTwo = "hAcc: ";
+            if (gpsData->fixType)
+                lineTwo.concat(gpsData->hAcc);
+            else
+                lineTwo.concat("0");
             break;
         }
 
-        case 4: {
+        case 4:
-            lineOne = "Carrier Solution";
+            if (this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Good
-            uint8_t carrSoln = gpsData->flags.bits.carrSoln;
+                ||this->driveManager->getNavigation()->getCalcAzimuthState() == Navigation::CalcAzimuthState::Super)
-            if (carrSoln == 0)
+            {
-                lineTwo = "None";
+                lineOne = "Calc Azi: ";
-            else if (carrSoln == 1)
+                lineTwo = "State: ";
-                lineTwo = "Floating";
+                lineOne.concat(this->driveManager->getNavigation()->getCalcAzimuth());
-            else if (carrSoln == 2)
+                switch (this->driveManager->getNavigation()->getCalcAzimuthState()) {
-                lineTwo = "Fixed";
+                    case Navigation::CalcAzimuthState::Bad :
-            else
+                        lineTwo.concat("Bad");
-                lineTwo = "UNKNOWN";
+                        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;
                 }
+            } else {
+                lineOne = "Real Azi: ";
+                lineOne.concat(this->driveManager->getNavigation()->getAzimuth());
+            }
+            break;
 
         case 5: 
-            lineOne = "hAccuracy: ";
+            lineOne = "Loop mode is";
-            lineTwo = "Azimuth: ";
+            if (this->autopilot->getLoopMode())
-            lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
+                lineTwo = "enabled";
-            if (gpsData->fixType)
-                lineOne.concat(gpsData->hAcc);
             else
-                lineOne.concat("0");
+                lineTwo = "disabled";            
             break;
 
         case 6:
@@ -170,37 +211,6 @@ void MenuAutopilot::printPage() const {
                 lineTwo = "accuracy is low";
             break;
 
-        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;
-
         case 12:
             lineOne = "Test rotate";
             lineTwo = "180 degree";
@@ -231,6 +241,10 @@ void MenuAutopilot::printPage() const {
 
 void MenuAutopilot::runCommand() const {
     switch (this->getCurrentPage()) {
+        case 5:
+            this->autopilot->switchLoopMode();
+            break;
+
         case 6:
             this->autopilot->restart();
             break;

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;