refactor all autopilot stuff

lib/Navigation/navigation.cpp

@@ -116,40 +116,40 @@ bool Navigation::startNavigation() {
     return this->navigationStarted;
 }
 
-CourseCorrection Navigation::getCourseCorrection() {
+bool Navigation::getCourseCorrection(CourseCorrection& correction) {
     //TODO: Check accuracy befor make something
-    CourseCorrection courseCorrection = {0, 0};
+    correction.correction = 0;
+    correction.distance = 0;
 
     int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
     double distance = this->currentPosition.distanceTo(this->targetPoint);
 
     if (distance < 0 || this->navigationFinished)
-        return courseCorrection;
+        return false;
 
     // Check if I need a new Point
     if (distance < MIN_DISTANCE_TO_REACH_POINT) {
-        bool goOn = this->nextPoint();
+        if (!this->nextPoint()) {
-        if (!goOn) {
             this->navigationFinished = true;
             this->navigationStarted = false;
-            return courseCorrection; // End of navigation
+            return false; // End of navigation
         }
     }
 
     // correction = targetCourse - currentCourse
     int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
-    int16_t correction = targetCourse - signedAzimuth;
+    int16_t correctionCourse = targetCourse - signedAzimuth;
-    if (correction > 180)
+    if (correctionCourse > 180)
-        correction -= 360;
+        correctionCourse -= 360;
-    else if (correction < -180)
+    else if (correctionCourse < -180)
-        correction += 360;
+        correctionCourse += 360;
 
     // //Rotate result by 180° to corrigate Azimuth
-    // correction = correction > 180 ? correction -360 : correction;
+    // correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
 
-    courseCorrection.correction = correction;
+    correction.correction = correctionCourse;
-    courseCorrection.distance = distance;
+    correction.distance = distance;
-    return courseCorrection;
+    return true;
 }
 
 bool Navigation::addCurrentPosToRoute() {

lib/Navigation/navigation.h

@@ -114,9 +114,11 @@ class Navigation {
          * 
          * This should be called by the driver to get new instructions.
          * 
-         * @return CourseCorrection 
+         * @param correction passed as refernce to get the data
+         * @return true if new correction data provided
+         * @return false if route is finished 
          */
-        CourseCorrection getCourseCorrection();
+        bool getCourseCorrection(CourseCorrection& correction);
 
         /**
          * @brief Tries to add the current Position to the route

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -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) {
-        sprintf(buf1 ,"Points %u/%u",this->routeInfo.currentPoint ,this->routeInfo.totalPoints);
+        uint16_t distance = 0;
-        if (selfDriving) {
+        if (correction.distance < 1) {
-            sprintf(buf2, "Auto: %u%s %d", distance, unit, correction.correction);
+            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 {
-            if (selfDrivingAvailable) {
+            distance = (uint16_t) (correction.distance / 1000);
-                sprintf(buf2, "Self -> Auto");
+            distanceString.concat(distance);
-            } else {
+            distanceString.concat("km");
-                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.");
         }
     }
 
-    this->print(buf1, buf2);
+    String lineOne = "No information";
+    String lineTwo = "available";
+
+    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();

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -14,15 +14,25 @@
 Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
     : ManualControl(moveControl, input) {
         this->navigation = navigation;
-        this->navigationStarted = this->navigation->startNavigation();
+        if (this->navigation->startNavigation())
+            this->state = State::NavigationStarted;
+        else
+            this->state = State::NoRoute;
         this->routeInfo = this->navigation->getRouteInfo();
+
+        this->lastState = this->navigation->getNTRIPClient()->getClientState();
+
         this->courseCorrection.correction = 0;
         this->courseCorrection.distance = 0;
         this->updateDisplay = true;
 }
 
+Autopilot::~Autopilot() {
+    this->navigation->getNTRIPClient()->setActivated(false);
+}
+
 void Autopilot::loop() {
-    if (!this->selfDriving && this->navigationStarted)
+    if (this->state < State::SelfDriving)
         ManualControl::loop();
 
     if (millis() - this->loopLastMillis < loopDelayMillis)
@@ -33,10 +43,16 @@ void Autopilot::loop() {
 }
 
 void Autopilot::runAutopilot() {
-    if (!this->navigationStarted || this->navigationEnded)
+    if (this->state < State::NavigationStarted)
         return;
 
-    this->courseCorrection = this->navigation->getCourseCorrection();
+    if (!this->navigation->getCourseCorrection(this->courseCorrection)) {
+        this->state = State::TargetReached;
+        this->moveControl->setSpeed(0);
+        this->moveControl->setRotationSpeed(0);
+        this->updateDisplay = true;
+    }
+
     this->routeInfo = this->navigation->getRouteInfo();
 
     if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) {
@@ -44,23 +60,19 @@ void Autopilot::runAutopilot() {
         this->displayUpdateLastMillis = millis();
     }
 
+    this->checkNtrip();
+
     // Check if start Point is near to current Location
-    if (this->routeInfo.currentPoint >= 2)
+    if (this->routeInfo.currentPoint >= 2 && this->state == State::GetToStartPoint)
-        this->selfDrivingAvailable = true;
+        this->state = State::SelfDrivingAvailable;
 
-    if (!this->selfDriving 
+    if (this->state == State::SelfDrivingAvailable 
-        && this->selfDrivingAvailable 
+        && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
-        && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action))
+        this->state = State::SelfDriving;
-        this->setSelfDriving(true);
+        this->updateDisplay = true;
-
-    if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
-        this->setSelfDriving(false);
-        this->navigationEnded = true;
-        this->navigationStarted = false;
-        this->selfDrivingAvailable = false;
     }
 
-    if (selfDriving) {
+    if (this->state == State::SelfDriving) {
         this->setSpeedInRelToDistance(this->courseCorrection.distance);
         this->setRotInRelToDistance(this->courseCorrection.correction);
     }   
@@ -74,15 +86,28 @@ bool Autopilot::shouldUpdate() {
     return false;
 }
 
-void Autopilot::setSelfDriving(bool val) {
+void Autopilot::checkNtrip() {
-    if (!this->navigationStarted)
+    NTRIPClient* client = this->navigation->getNTRIPClient();
-        return;
+    NTRIPClientStates state = client->getClientState();
+    if (state == NTRIPClientStates::notAvailable) {
+        this->state = State::NoNtrip;
+    } else if (state == NTRIPClientStates::pushData) {
+        this->ntripReconnectAttemps = 0;
+        if (this->state < State::GetToStartPoint)
+            this->state = State::GetToStartPoint;
+    } else if (state ==NTRIPClientStates::wait) {
+        if (millis() - this->reconnectNtripLastMillis < this->reconnectNtripDelayMillis)
+            return;
 
-    this->updateDisplay = true;
+        if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
+            this->state = State::NoNtrip;
+            return;
+        }
 
-    this->selfDriving = val;
+        this->reconnectNtripLastMillis = millis();
-    this->moveControl->setSpeed(0);
+        client->setActivated(true);
-    this->moveControl->setRotationSpeed(0);
+        this->ntripReconnectAttemps++;
+    }
 }
 
 void Autopilot::setSpeedInRelToDistance(double distance) {

src/driveModi/Modi/Autopilot/autopilot.h

@@ -29,6 +29,17 @@
  */
 class Autopilot : public ManualControl {
     public:
+        enum State {
+            NoNtrip = -2,
+            NoRoute = -1,
+            None = 0,
+            NavigationStarted,
+            GetToStartPoint,
+            SelfDrivingAvailable,
+            SelfDriving,
+            TargetReached
+        };
+
         /**
          * @brief Construct a new Autopilot object
          * 
@@ -37,6 +48,13 @@ class Autopilot : public ManualControl {
          */
         Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
 
+        /**
+         * @brief Destroy the Autopilot object
+         * 
+         * Disconnect the NTRIP-Client
+         */
+        ~Autopilot();
+
         /**
          * @brief Calls runAutopilot or ManualControl loop
          * 
@@ -73,36 +91,11 @@ class Autopilot : public ManualControl {
         CourseCorrection getCourseCorrection() const { return this->courseCorrection; }
 
         /**
-         * @brief Get the Navigation Started status
+         * @brief Get the State object
          * 
-         * @return true 
+         * @return State 
-         * @return false 
          */
-        bool getNavigationStarted() const { return this->navigationStarted; }
+        State getState() const { return this->state; }
-
-        /**
-         * @brief Get the Navigation Ended status
-         * 
-         * @return true 
-         * @return false 
-         */
-        bool getNavigationEnded() const { return this->navigationEnded; }
-
-        /**
-         * @brief Get the Self Driving status
-         * 
-         * @return true 
-         * @return false 
-         */
-        bool getSelfDriving() const { return this->selfDriving; }
-
-        /**
-         * @brief Get the Self Driving Available status
-         * 
-         * @return true 
-         * @return false 
-         */
-        bool getSelfDrivingAvailable() const { return this->selfDrivingAvailable; }
         
         /**
          * @brief Tells if there are new informations to display
@@ -113,24 +106,26 @@ class Autopilot : public ManualControl {
         bool shouldUpdate();
 
     private:
-        void setSelfDriving(bool val);
+        void checkNtrip();
         void setSpeedInRelToDistance(double distance);
         void setRotInRelToDistance(int16_t course);
 
         Navigation* navigation;
         CourseCorrection courseCorrection;
         RouteInfo routeInfo;
+        State state = State::None;
+        NTRIPClientStates lastState;
 
-        bool navigationStarted = false;
-        bool navigationEnded = false;
-        bool selfDriving = false;
-        bool selfDrivingAvailable = false;
         bool updateDisplay = false;
 
         uint8_t loopDelayMillis = 40;
+        uint8_t ntripReconnectAttemps = 0;
+        uint8_t ntripReconnectMaxAttemps = 10;
         uint16_t displayUpdateDelayMillis = 1000;
+        uint16_t reconnectNtripDelayMillis = 1000;
         uint32_t displayUpdateLastMillis = 0;
         uint32_t loopLastMillis = 0;
+        uint32_t reconnectNtripLastMillis = 0;
 };
 
 #endif // AUTOPILOT_H