refactor all autopilot stuff

2023-05-11 19:37:57 +02:00
parent 8945e5eb21
commit 6874aed6ed
5 changed files with 217 additions and 116 deletions
@@ -12,52 +12,131 @@
 #include "menuAutopilot.h"

 void MenuAutopilot::printPage() const {
-    char buf1[17];
-    char buf2[17];
-    char unit[3];
-
-    bool navigationStarted = this->autopilot->getNavigationStarted();
-    bool navigationEnded = this->autopilot->getNavigationEnded();
-    bool selfDriving = this->autopilot->getSelfDriving();
-    bool selfDrivingAvailable = this->autopilot->getSelfDrivingAvailable();
-
-    // 999m -360°
    CourseCorrection correction = this->autopilot->getCourseCorrection();
-    uint16_t distance = 0;
+    UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
+    bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted;

-    if (correction.distance < 1) {
-        distance = (uint16_t) (correction.distance * 100);
-        sprintf(unit, "cm");
-    } else if (correction.distance < 1000) {
-        distance = (uint16_t) correction.distance;
-        sprintf(unit, "m");
-    } else {
-        distance = (uint16_t) (correction.distance / 1000);
-        sprintf(unit, "km");
+    String distanceString = "";
+    if (navigationStarted) {
+        uint16_t distance = 0;
+        if (correction.distance < 1) {
+            distance = (uint16_t) (correction.distance * 100);
+            distanceString.concat(distance);
+            distanceString.concat("cm");
+        } else if (correction.distance < 1000) {
+            distance = (uint16_t) correction.distance;
+            distanceString.concat(distance);
+            distanceString.concat("m");
+        } else {
+            distance = (uint16_t) (correction.distance / 1000);
+            distanceString.concat(distance);
+            distanceString.concat("km");
+        }
    }

-    if (navigationStarted) {
-        sprintf(buf1 ,"Points %u/%u",this->routeInfo.currentPoint ,this->routeInfo.totalPoints);
-        if (selfDriving) {
-            sprintf(buf2, "Auto: %u%s %d", distance, unit, correction.correction);
-        } else {
-            if (selfDrivingAvailable) {
-                sprintf(buf2, "Self -> Auto");
-            } else {
-                sprintf(buf2, "Self: %u%s %d", distance, unit, correction.correction);
-            }
-        }
-    } else {
-        if (navigationEnded) {
-            sprintf(buf1, "Am Ziel");
-            sprintf(buf2, "angekommen.");
-        } else {
-            sprintf(buf1, "Keine Route");
-            sprintf(buf2, "vorhanden.");
-        }
-    } 
+    String lineOne = "No information";
+    String lineTwo = "available";

-    this->print(buf1, buf2);
+    switch (this->getCurrentPage()) {
+        case 0: {
+            lineOne = "Status: ";
+            lineTwo = "";
+            switch (this->autopilot->getState()) {
+                case Autopilot::State::NoNtrip :
+                    lineTwo = "Err: No NTRIP";
+                    break;
+
+                case Autopilot::State::NoRoute :
+                    lineTwo = "Err: No route";
+                    break;
+
+                case Autopilot::State::NavigationStarted :
+                    lineTwo = "Nav started";
+                    break;
+
+                case Autopilot::State::GetToStartPoint :
+                    lineTwo = "Drive to start";
+                    break;
+
+                case Autopilot::State::SelfDrivingAvailable :
+                    lineTwo = "Autopilot ready";
+                    break;
+
+                case Autopilot::State::SelfDriving :
+                    lineTwo = "Autopilot active";
+                    break;
+
+                case Autopilot::State::TargetReached :
+                    lineTwo = "Target reached";
+                    break;
+                
+                default:
+                    lineTwo = "UNKOWN";
+                    break;
+            }
+            break;
+        }
+
+        case 1:
+            if (!navigationStarted)
+                break;
+            lineOne = "Distance: ";
+            lineOne.concat(distanceString);
+            lineTwo = "Turn: ";
+            lineTwo.concat(correction.correction);
+            break;
+
+        case 2:
+            lineOne = "Target waypoint";
+            lineTwo = "";
+            lineTwo.concat(this->autopilot->getRouteInfo().currentPoint);
+            lineTwo.concat(" from ");
+            lineTwo.concat(this->autopilot->getRouteInfo().totalPoints);
+            break;
+
+        case 3: {
+            NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
+            lineOne = "NTRIP Client is";
+
+            if (status ==  NTRIPClientStates::pushData)
+                lineTwo = "enabled";
+            else if (status == NTRIPClientStates::notAvailable)
+                lineTwo = "not available";
+            else
+                lineTwo = "disabled";
+            break;
+        }
+
+        case 4: {
+            lineOne = "Carrier Solution";
+            uint8_t carrSoln = gpsData->flags.bits.carrSoln;
+            if (carrSoln == 0)
+                lineTwo = "None";
+            else if (carrSoln == 1)
+                lineTwo = "Floating";
+            else if (carrSoln == 2)
+                lineTwo = "Fixed";
+            else
+                lineTwo = "UNKNOWN";
+            break;
+        }
+
+        case 5: 
+            lineOne = "hAccuracy: ";
+            lineTwo = "Azimuth: ";
+            lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
+            if (gpsData->fixType)
+                lineOne.concat(gpsData->hAcc);
+            else
+                lineOne.concat("0");
+            break;
+
+        default:
+            this->printDefault();
+            return;
+    }
+
+    this->print(lineOne, lineTwo);
 }

 void MenuAutopilot::update() {
@@ -69,7 +148,7 @@ void MenuAutopilot::update() {
 }

 void MenuAutopilot::init() {
-    this->setCountPages(4);
+    this->setCountPages(6);
    this->driveManager->changeModus(Modi::Autopilot);
    this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
    this->routeInfo = this->autopilot->getRouteInfo();