- remove NtripReconnect from Autopilot and CaptureRoute

- shiftet moveControl States in the class namespace
- added auto reconeect to NtripClient
- a lot of refactor

include/moveControl.h

@@ -21,15 +21,6 @@
 #include "moveControlConfig.h"
 #include "debugTimes.h"
 
-/**
- * @brief used to set driving status
- * 
- * When set to stop all motors are set to halt
- * 
- */
-enum DrivingStatus {stop,
-                    drive,
-                    raw};
 
 /**
  * @brief This class manages the motors and the encoders
@@ -41,6 +32,17 @@ enum DrivingStatus {stop,
  */
 class MoveControl {
     public:
+        /**
+         * @brief used to set driving status
+         * 
+         * When set to stop all motors are set to halt
+         * 
+         */
+        enum Status {Stop,
+                    Drive,
+                    Raw};
+
+
         /**
          * @brief Construct a new Move Control object
          * 
@@ -73,12 +75,12 @@ class MoveControl {
         void runMoveControl();
 
         /**
-         * @brief Set the DrivingStatus
+         * @brief Set the Status
          * 
-         * @see DrivingStatus
+         * @see Status
          * @param status 
          */
-        void setDrivingStatus(DrivingStatus status);
+        void setDrivingStatus(Status status);
 
         /**
          * @brief Stops the engine immediately
@@ -107,7 +109,7 @@ class MoveControl {
         /**
          * @brief Set Raw Power Left
          * 
-         * This value has only an effect if DrivingStatus is raw.
+         * This value has only an effect if Status is raw.
          * 
          * @param power between -100 and 100
          */
@@ -116,7 +118,7 @@ class MoveControl {
         /**
          * @brief Set the Raw Power Right
          * 
-         * This value has only an effect if DrivingStatus is raw.
+         * This value has only an effect if Status is raw.
          * 
          * @param power between -100 and 100
          */
@@ -178,7 +180,7 @@ class MoveControl {
         /**
          * @brief Set the target power to motors
          * 
-         * Checks if the driving_status is set to drive.
+         * Checks if the driving_status is set to Drive.
          * If yes, then the motors get the pid_out values as targetpower.
          * If no, then the motors target power is set to zero.
          */
@@ -196,7 +198,7 @@ class MoveControl {
         PID *left_pid;
         PID *right_pid;
 
-        DrivingStatus driving_status = DrivingStatus::stop;
+        Status driving_status = Status::Stop;
 
         double x_speed = 0;
         double rotation_speed = 0;

lib/Navigation/navigation.cpp

@@ -124,69 +124,54 @@ bool Navigation::startNavigation() {
     return this->navigationStarted;
 }
 
-bool Navigation::getCourseCorrection(CourseCorrection& correction) {
+Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction) {
-    //TODO: Check accuracy befor make something
+    if (this->navigationFinished)
-    correction.correction = 0;
+        return Status::Complete;
-    correction.distance = 0;
+
+    if (this->currentPosition.getAccuracy() <= this->minAccuracy)
+        return Status::InsufficientAccuracy;
+
+    if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0))
+        return Status::Unchanged;
 
-    int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
     double distance = this->currentPosition.distanceTo(this->targetPoint);
 
-    if (distance < 0 || this->navigationFinished)
-        return false;
-
     // Check if I need a new Point
     if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
         if (!this->nextPoint()) {
             this->navigationFinished = true;
             this->navigationStarted = false;
-            return false; // End of navigation
+            return Status::Complete; // End of navigation
         }
+        distance = this->currentPosition.distanceTo(this->targetPoint);
     }
 
-    // correction = targetCourse - currentCourse
+    correction.correction = this->calculateCourseCorrection();
-    int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
-    int16_t correctionCourse = targetCourse - signedAzimuth;
-    if (correctionCourse > 180)
-        correctionCourse -= 360;
-    else if (correctionCourse < -180)
-        correctionCourse += 360;
-
-    //Rotate result by 180° to corrigate Azimuth
-    correctionCourse += 180;
-    correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
-
-    correction.correction = correctionCourse;
     correction.distance = distance;
-    return true;
+    this->lastPointCalcCorrection = this->currentPosition;
+    return Status::Updated;
 }
 
-bool Navigation::addCurrentPosToRoute() {
+Navigation::Status Navigation::addCurrentPosToRoute() {
-    if (!this->currentPosition.isValid()) { return false; }
+    if (this->currentPosition.getAccuracy() <= this->minAccuracy)
+        return Status::InsufficientAccuracy;
 
     // First Point
     if (this->route->getRouteInfo().totalPoints == 0) {
         this->route->addPointToRoute(this->currentPosition);
-        this->lastPoint = this->currentPosition;
+        this->lastPointRouteInsert = this->currentPosition;
-        return true;
+        return Status::Updated;
     }
 
     // Every Point after the first
-    if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPoint)) {
+    if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPointRouteInsert)) {
         this->route->addPointToRoute(this->currentPosition);
-        this->lastPoint = this->currentPosition;
+        this->lastPointRouteInsert = this->currentPosition;
-        return true;
+        return Status::Updated;
     }
-    return false;
+    return Status::Unchanged;
 }
 
-// bool Navigation::setPointBeforeTurn() {
-//     if (millis() - this->currentPosition.getCreationTime() > 1000)
-//         return false;
-//     this->beforeTurnPoint = this->currentPosition;
-//     return true;
-// }
-
 void Navigation::updateCurrentLocation() {
     if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
         return;
@@ -201,6 +186,23 @@ void Navigation::updateCurrentLocation() {
     this->currentPosition = Point(coords, this->ubxData->hAcc);
 }
 
+int16_t Navigation::calculateCourseCorrection() {
+    int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
+    // correction = targetCourse - currentCourse
+    int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
+    int16_t correctionCourse = targetCourse - signedAzimuth;
+    if (correctionCourse > 180)
+        correctionCourse -= 360;
+    else if (correctionCourse < -180)
+        correctionCourse += 360;
+
+    //Rotate result by 180° to corrigate Azimuth
+    correctionCourse += 180;
+    correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
+
+    return correctionCourse;
+}
+
 bool Navigation::nextPoint() { 
     if (!this->navigationStarted)
         return false;

lib/Navigation/navigation.h

@@ -29,7 +29,7 @@
  * around the half accuracy of the positioning system
  * 
  */
-#define MIN_DISTANCE_BETWEEN_POINTS 0.1
+#define MIN_DISTANCE_BETWEEN_POINTS 0.3
 #define MIN_DISTANCE_TO_REACH_POINT 0.5
 #define AZIMUTH_UPDATE_DELAY 20
 
@@ -55,6 +55,14 @@ struct CourseCorrection {
  */
 class Navigation {
     public:
+        enum Status {
+            InsufficientAccuracy,
+            Unchanged,
+            Updated,
+            Complete
+        };
+
+
         /**
          * @brief Construct a new Navigation object and using I2C
          * 
@@ -123,7 +131,7 @@ class Navigation {
          * @return true if new correction data provided
          * @return false if route is finished 
          */
-        bool getCourseCorrection(CourseCorrection& correction);
+        Status getCourseCorrection(CourseCorrection& correction);
 
         /**
          * @brief Tries to add the current Position to the route
@@ -133,7 +141,7 @@ class Navigation {
          * @return true successful added point
          * @return false no point added to route
          */
-        bool addCurrentPosToRoute();
+        Status addCurrentPosToRoute();
 
     	// TODO: can be deleted?
         // bool setPointBeforeTurn();
@@ -189,6 +197,7 @@ class Navigation {
 
     private:
         void updateCurrentLocation();
+        int16_t calculateCourseCorrection();
 
         /**
          * @brief Set the next point as target
@@ -215,10 +224,11 @@ class Navigation {
         NTRIPClient* ntripClient = nullptr;
         QMC5883LCompass* compass;
 
-        Point lastPoint;
+        Point lastPointRouteInsert;
+        Point lastPointCalcCorrection;
         Point targetPoint;
         Point currentPosition;
-        Point beforeTurnPoint;
+        Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
 
 
         bool navigationStarted = false;

lib/Navigation/route.cpp

@@ -90,17 +90,17 @@ void Point::init(uint32_t horizontalAccuracy) {
     this->creationTime = millis();
 
     if (horizontalAccuracy == UINT32_MAX)
-        this->accuracy = PointAccuracy::imported;
+        this->accuracy = Accuracy::imported;
     else if (horizontalAccuracy > 9999)
-        this->accuracy = PointAccuracy::fourDigOfCM;
+        this->accuracy = Accuracy::fourDigOfCM;
     else if (horizontalAccuracy > 999)
-        this->accuracy = PointAccuracy::threeDigOfCM;
+        this->accuracy = Accuracy::threeDigOfCM;
     else if (horizontalAccuracy > 99)
-        this->accuracy = PointAccuracy::twoDigOfCM;
+        this->accuracy = Accuracy::twoDigOfCM;
     else if (horizontalAccuracy > 1)
-        this->accuracy = PointAccuracy::oneDigOfCM;
+        this->accuracy = Accuracy::oneDigOfCM;
     else 
-        this->accuracy = PointAccuracy::none;
+        this->accuracy = Accuracy::none;
         
 }
 

lib/Navigation/route.h

@@ -47,7 +47,7 @@ class Point{
          * @brief The Accuracy is set by the constructor
          * 
          */
-        enum PointAccuracy {
+        enum Accuracy {
             none,
             fourDigOfCM,
             threeDigOfCM,
@@ -126,16 +126,16 @@ class Point{
          * @brief Get the Accuracy object
          * 
          * The higher the value, the greater the accuracy.
-         * You can check it by PointAccuracy.
+         * You can check it by Accuracy.
          * 
-         * @return PointAccuracy 
+         * @return Accuracy 
          */
-        PointAccuracy getAccuracy() { return this->accuracy; }
+        Accuracy getAccuracy() { return this->accuracy; }
 
     private:
         void init(uint32_t horizontalAccuracy);
 
-        PointAccuracy accuracy = PointAccuracy::none;
+        Accuracy accuracy = Accuracy::none;
         Coordinates coordinates;
 
         uint32_t creationTime = 0;

lib/NtripClient/ntripClient.cpp

@@ -29,27 +29,14 @@ NTRIPClient::~NTRIPClient() {
 }
 
 void NTRIPClient::loop() {
+    this->pushGPGGA();
+
     if (millis() - this->lastLoopTime < this->delayTime)
         return;
     this->lastLoopTime = millis();
 
-    if (millis() - this->lastGPGGAPushTime > this->pushGPGGATime && this->activated) {
-        this->lastGPGGAPushTime = millis();
-        // std::cout << "Try to push GPGGA data." << std::endl;
-        NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
-        DebugTimes requestGpsData;
-        uint8_t res = this->gps->getLatestNMEAGPGGA(data);
-        requestGpsData.stopConsol("RequestGpsData", 5);
-        if (res == 2)
-            this->pushGPGGA(data);
-        delete data;
-    }
-
     switch (this->state) {
         case NTRIPClientStates::openConnection:
-            if (millis() - this->lastNtripConnectTime < this->tryReconnectTime)
-                break;
-
             if (!this->activated) {
                 this->state = NTRIPClientStates::closeConnection;
                 break;
@@ -60,10 +47,9 @@ void NTRIPClient::loop() {
                 std::cout << "Connected to the NTRIP caster!" << std::endl;
                 this->state = NTRIPClientStates::pushData;
             } else {
-                uint8_t seconds = this->tryReconnectTime / 1000;
+                std::cout << "Failed!" << std::endl;
-                std::cout << "Could not connect to the caster. Trying again in " 
+                this->state = NTRIPClientStates::wait;
-                          << (int) seconds << " seconds." << std::endl;
+                this->activated = false;
-                this->lastNtripConnectTime = millis();
             }
             break;
 
@@ -81,9 +67,8 @@ void NTRIPClient::loop() {
         case NTRIPClientStates::wait:
             if (this->activated)
                 this->state = NTRIPClientStates::openConnection;
-            if (this->activated) {
+            else
-                // std::cout << "state is openConnection" << std::endl;
+                this->checkAutoReconnect();
-            }
             break;
 
         case NTRIPClientStates::notAvailable:
@@ -105,16 +90,27 @@ void NTRIPClient::gpsConfiguration() {
     this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
 }
 
-bool NTRIPClient::setActivated(bool b) {
+bool NTRIPClient::setActivated(bool state) {
     // std::cout << "NTRIPClient::setActivated: b - " << b << std::endl;
-    if (b && this->state != NTRIPClientStates::notAvailable)
+    if (state && this->state != NTRIPClientStates::notAvailable)
         this->activated = true;
-    else if (b)
+    else if (state)
         return false;
-    else
+    else {
         this->activated = false;
+        this->autoReconnect = false;
+    }
     return true;
+}
 
+void NTRIPClient::setAutoReconnect(bool state) {
+    if (state) {
+        this->reconnectAttemps = 0;
+        this->autoReconnect = true;
+        return;
+    }
+
+    this->autoReconnect = false;
 }
 
 bool NTRIPClient::beginClient() {
@@ -252,11 +248,39 @@ bool NTRIPClient::processConnection() {
     return true;
 }
 
-void NTRIPClient::pushGPGGA(NMEA_GGA_data_t *nmeaData) {
+void NTRIPClient::checkAutoReconnect() {
-    if (this->ntripClient->connected() && this->transmitLocation)
+    if (!this->autoReconnect)
-        this->ntripClient->print((const char *)nmeaData->nmea);
+        return;
-    else
+
-        std::cout << "Failed to pushing GGA to server: " << (const char *)nmeaData->nmea << std::endl;
+    if (millis() - this->lastReconnectTime < this->reconnectDelayTime)
+        return;
+    this->lastReconnectTime = millis();
+
+    if (this->reconnectAttemps >= this->maxReconnectAttemps) {
+        this->autoReconnect = false;
+        return;
+    }
+
+    this->activated = true;
+    this->reconnectAttemps++;
+}
+
+void NTRIPClient::pushGPGGA() {
+    if (!this->transmitLocation && !this->activated)
+        return;
+
+    if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime)
+        return;
+    this->lastGPGGAPushTime = millis();
+
+    if (!this->ntripClient->connected())
+        std::cout << "Failed to pushing GGA to server: " << std::endl;
+    
+    NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
+    uint8_t res = this->gps->getLatestNMEAGPGGA(data);
+    if (res == 2)
+        this->ntripClient->print((const char *)data);
+    delete data;
 }
 
 bool NTRIPClient::isConnected() {

lib/NtripClient/ntripClient.h

@@ -81,11 +81,12 @@ class NTRIPClient {
         /**
          * @brief Activate or deactivate the connection to the server.
          * 
-         * @param b 
+         * @param state 
          * @return true success
          * @return false failure
          */
-        bool setActivated(bool b);
+        bool setActivated(bool state);
+        void setAutoReconnect(bool state);
 
         bool isConnected();
 
@@ -99,10 +100,11 @@ class NTRIPClient {
         NTRIPClientStates getClientState() { return this->state; }
 
     private:
-        void pushGPGGA(NMEA_GGA_data_t *nmeaData);
+        void pushGPGGA();
         bool beginClient();
         void closeConnection();
         bool processConnection();
+        void checkAutoReconnect();
 
         SFE_UBLOX_GNSS* gps;
         WiFiClient* ntripClient;
@@ -110,20 +112,24 @@ class NTRIPClient {
 
         bool transmitLocation = true;
         bool activated = true;
+        bool autoReconnect = false;
 
+        uint8_t reconnectAttemps = 0;
         uint16_t port;
         uint32_t lastReceivedRtcmTime = 0;
-        uint32_t lastNtripConnectTime = 0;
+        // uint32_t lastNtripConnectTime = 0;  // can deleted?
         uint32_t lastGPGGAPushTime = 0;
         uint32_t lastLoopTime = 0;
+        uint32_t lastReconnectTime = 0;
 
         char host[128];
         char mountPoint[128];
         char user[128];
         char password[128];
         const uint8_t delayTime = 20;
+        const uint8_t maxReconnectAttemps = 10;
+        const uint16_t reconnectDelayTime = 1000;
         const uint16_t timeOut = 5000;
-        const uint16_t tryReconnectTime = 5000;
         const uint16_t bufferSize = 512;
         const uint16_t pushGPGGATime = 10000;
 };

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -42,7 +42,7 @@ void MenuAutopilot::printPage() const {
             lineOne = "Status: ";
             lineTwo = "";
             switch (this->autopilot->getState()) {
-                case Autopilot::State::NoNtrip :
+                case Autopilot::State::InsufficientAccuarcy :
                     lineTwo = "Err: No NTRIP";
                     break;
 

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp

@@ -12,12 +12,12 @@
 
 void MenuCaptureRoute::printPage() const {   
     RouteInfo routeInfo = this->captureRoute->getRouteInfo();
+    UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
 
     String lineOne = "";
     String lineTwo = "";
     
-    switch (this->getCurrentPage())
+    switch (this->getCurrentPage()) {
-    {
         case 0:
             lineOne = "Capture Route";
             lineTwo = "You can drive";
@@ -29,12 +29,33 @@ void MenuCaptureRoute::printPage() const {
             break;
 
         case 2:
+            lineOne = "Last status:";
+            switch (this->captureRoute->getLastStatus()) {
+                case Navigation::Status::InsufficientAccuracy:
+                    lineTwo = "Poor Accuracy";
+                    break;
+                
+                case Navigation::Status::Updated:
+                    lineTwo = "Point added";
+                    break;
+
+                case Navigation::Status::Unchanged:
+                    lineTwo = "Point too close";
+                    break;
+
+                default:
+                    lineTwo = "---";
+                    break;
+            }
+            break;
+
+        case 3:
             lineOne = "Distance to last";
             lineTwo = "point: ";
             lineTwo.concat(this->captureRoute->getDistanceToLastPoint());
             break;
 
-    case 3: {
+        case 4: {
             NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
             lineOne = "NTRIP Client is";
 
@@ -47,6 +68,30 @@ void MenuCaptureRoute::printPage() const {
             break;
         }
 
+        case 5: {
+                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 6: 
+            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;
@@ -62,7 +107,8 @@ void MenuCaptureRoute::update() {
 }
 
 void MenuCaptureRoute::init() {
-    this->setCountPages(4);
+    this->setCountPages(7);
+    this->updateDelay = 500;
     this->driveManager->changeModus(Modi::CaptureRoute);
     this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();
     

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -10,6 +10,7 @@
  */
 
 #include "driveModi/Modi/Autopilot/autopilot.h"
+#include "autopilot.h"
 
 Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
     : ManualControl(moveControl, input) {
@@ -18,64 +19,64 @@ Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Nav
 }
 
 Autopilot::~Autopilot() {
-    this->navigation->getNTRIPClient()->setActivated(false);
+    // this->navigation->getNTRIPClient()->setActivated(false);
 }
 
 void Autopilot::loop() {
     if (this->state < State::SelfDriving)
         ManualControl::loop();
 
-    if (millis() - this->loopLastMillis < loopDelayMillis)
-        return;
-
-    this->runAutopilot();
-    this->loopLastMillis = millis();
-}
-
-void Autopilot::runAutopilot() {
-    if (this->state < State::NavigationStarted)
-        return;
-
-    if (!this->navigation->getCourseCorrection(this->courseCorrection)) {
-        this->state = State::TargetReached;
-        this->moveControl->setSpeed(0);
-        this->moveControl->setRotationSpeed(0);
-        this->updateDisplay = true;
-        return;
-    }
-
-    this->routeInfo = this->navigation->getRouteInfo();
-
     if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) {
         this->updateDisplay = true;
         this->displayUpdateLastMillis = millis();
     }
 
-    this->checkNtrip();
+    if (millis() - this->loopLastMillis < loopDelayMillis)
+        return;
 
-    // Check if start Point is near to current Location
+    this->routeInfo = this->navigation->getRouteInfo();
-    if (this->routeInfo.currentPoint >= 2 && this->state == State::GetToStartPoint)
+    this->runAutopilot();
-        this->state = State::SelfDrivingAvailable;
+    this->loopLastMillis = millis();
-
-    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
-        && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
-        
-        if (this->state  == State::SelfDrivingAvailable) {
-            this->state = State::SelfDriving;
-            this->updateDisplay = true;
-            this->lastAutopilotChangeMillis = millis();
-        } else if (this->state == State::SelfDriving) {
-            this->state = State::SelfDrivingAvailable;
-            this->updateDisplay = true;
-            this->lastAutopilotChangeMillis = millis();
-        }
 }
 
-    if (this->state == State::SelfDriving) {
+void Autopilot::runAutopilot() {
-        if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
+    switch (this->state) {
-            this->rotate();
+        case State::InsufficientAccuarcy:
-        else
+            this->askNavigationForOrder();
-            this->drive();
+            return;
+
+        case State::NoRoute:
+            return;
+
+        case State::None:
+            return;
+
+        case State::NavigationStarted:
+            this->askNavigationForOrder();
+            break;
+
+        case State::GetToStartPoint:
+            this->askNavigationForOrder();
+            if (this->routeInfo.currentPoint >= 2)
+                this->state = State::SelfDrivingAvailable;
+            break;
+
+        case State::SelfDrivingAvailable:
+            this->askNavigationForOrder();
+            this->checkButtonInput();
+            break;
+
+        case State::SelfDriving:
+            this->askNavigationForOrder();
+            this->checkButtonInput();
+            this->selfDriving();
+            break;
+
+        case State::TargetReached:
+            break;
+        
+        default:
+            break;
     }
 }
 
@@ -97,38 +98,15 @@ void Autopilot::init() {
         else
             this->state = State::NoRoute;
         this->routeInfo = this->navigation->getRouteInfo();
+        this->navigation->getNTRIPClient()->setAutoReconnect(true);
 
-        this->lastState = this->navigation->getNTRIPClient()->getClientState();
+        this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
 
         this->courseCorrection.correction = 0;
         this->courseCorrection.distance = 0;
         this->updateDisplay = true;
 }
 
-void Autopilot::checkNtrip() {
-    NTRIPClient* client = this->navigation->getNTRIPClient();
-    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;
-
-        if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
-            this->state = State::NoNtrip;
-            return;
-        }
-
-        this->reconnectNtripLastMillis = millis();
-        client->setActivated(true);
-        this->ntripReconnectAttemps++;
-    }
-}
-
 void Autopilot::drive() {
     this->moveControl->setRotationSpeed(0);
     if (this->courseCorrection.distance >= this->minRemainingDistance) 
@@ -142,5 +120,62 @@ void Autopilot::rotate() {
     if (this->courseCorrection.correction > 0)
         this->moveControl->setRotationSpeed(this->rotationSpeed);
     else
+
     this->moveControl->setRotationSpeed(-this->rotationSpeed);
 }
+
+void Autopilot::checkButtonInput() {
+    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
+        && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
+        
+        if (this->state  == State::SelfDrivingAvailable) {
+            this->state = State::SelfDriving;
+            this->updateDisplay = true;
+            this->lastAutopilotChangeMillis = millis();
+        } else if (this->state == State::SelfDriving) {
+            this->state = State::SelfDrivingAvailable;
+            this->updateDisplay = true;
+            this->lastAutopilotChangeMillis = millis();
+        }
+    }
+}
+
+void Autopilot::askNavigationForOrder() {
+    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
+    switch (this->lastOrderStatus) {
+        case Navigation::Status::Complete:
+            this->state = State::TargetReached;
+            this->moveControl->setSpeed(0);
+            this->moveControl->setRotationSpeed(0);
+            break;
+        
+        case Navigation::Status::InsufficientAccuracy:
+            this->lastState = this->state;
+            this->state = State::InsufficientAccuarcy;
+            this->moveControl->setSpeed(0);
+            this->moveControl->setRotationSpeed(0);
+            break;
+
+        case Navigation::Status::Unchanged:
+            if (this->state == State::InsufficientAccuarcy)
+                this->state = this->lastState;
+            break;
+
+        case Navigation::Status::Updated:
+            if (this->state == State::InsufficientAccuarcy)
+                this->state = this->lastState;
+            break;
+        
+        default:
+            break;
+    }
+}
+
+void Autopilot::selfDriving() {
+    if (this->lastOrderStatus == Navigation::Status::Updated) {
+        if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
+            this->rotate();
+        else
+            this->drive();
+    }
+}

src/driveModi/Modi/Autopilot/autopilot.h

@@ -29,7 +29,7 @@
 class Autopilot : public ManualControl {
     public:
         enum State {
-            NoNtrip = -2,
+            InsufficientAccuarcy = -2,
             NoRoute = -1,
             None = 0,
             NavigationStarted,
@@ -108,32 +108,31 @@ class Autopilot : public ManualControl {
 
     private:
         void init();
-        void checkNtrip();
         void drive();
         void rotate();
+        void checkButtonInput();
+        void askNavigationForOrder();
+        void selfDriving();
 
         Navigation* navigation;
         CourseCorrection courseCorrection;
         RouteInfo routeInfo;
         State state = State::None;
-        NTRIPClientStates lastState;
+        State lastState = State::None;
+        Navigation::Status lastOrderStatus;
 
         bool updateDisplay = false;
 
         uint8_t loopDelayMillis = 40;
-        uint8_t ntripReconnectAttemps = 0;
-        uint8_t ntripReconnectMaxAttemps = 10;
         uint8_t maxCourseDeviationBerforeAct = 5;
         uint16_t displayUpdateDelayMillis = 1000;
-        uint16_t reconnectNtripDelayMillis = 1000;
         uint16_t autopilotChangeDelayMillis = 500;
         uint32_t displayUpdateLastMillis = 0;
         uint32_t loopLastMillis = 0;
-        uint32_t reconnectNtripLastMillis = 0;
         uint32_t lastAutopilotChangeMillis = 0;
 
         double drivingSpeed = 1;
-        double rotationSpeed = 7;
+        double rotationSpeed = 3;
         double minRemainingDistance = 0.25;
 };
 

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -15,31 +15,28 @@ CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *inpu
     : ManualControl(moveControl, input) {
         this->navigation = navigation;
         this->navigation->getRoute()->clear();
+        this->navigation->getNTRIPClient()->setAutoReconnect(true);
         this->routeInfo = navigation->getRouteInfo();
 }
 
 CaptureRoute::~CaptureRoute() {
-    this->navigation->getNTRIPClient()->setActivated(false);
+    // this->navigation->getNTRIPClient()->setActivated(false);
 }
 
 void CaptureRoute::loop() {
     ManualControl::loop();
 
-    if (millis() - this->lastMillis < this->delay) {
+    if (millis() - this->lastMillis < this->delay)
         return;
-    }
+    
     this->runCaptureRoute();
     this->lastMillis = millis();
 }
 
 void CaptureRoute::runCaptureRoute() {
-    this->checkNtrip();
-
-    if (this->state != NtripState::Enabled)
-        return;
-
     if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
-        if (this->navigation->addCurrentPosToRoute()) {
+        this->status = this->navigation->addCurrentPosToRoute();
+        if (this->status == Navigation::Status::Updated) {
             this->lastSavedPoint = this->navigation->getCurrentPosition();
             this->routeInfo = navigation->getRouteInfo();
             this->updateDisplay = true;
@@ -60,27 +57,3 @@ bool CaptureRoute::shouldUpdate() {
     }
     return false;
 }
-
-void CaptureRoute::checkNtrip() {
-    NTRIPClient* client = this->navigation->getNTRIPClient();
-    NTRIPClientStates state = client->getClientState();
-    if (state == NTRIPClientStates::notAvailable) {
-        this->state = NtripState::NoNtrip;
-    } else if (state == NTRIPClientStates::pushData) {
-        this->ntripReconnectAttemps = 0;
-        this->state = NtripState::Enabled;
-    } else if (state ==NTRIPClientStates::wait) {
-        this->state = NtripState::Waiting;
-        if (millis() - this->reconnectNtripLastMillis < this->reconnectNtripDelayMillis)
-            return;
-
-        if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
-            this->state = NtripState::NoNtrip;
-            return;
-        }
-
-        this->reconnectNtripLastMillis = millis();
-        client->setActivated(true);
-        this->ntripReconnectAttemps++;
-    }
-}

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -27,12 +27,6 @@
  */
 class CaptureRoute : public ManualControl {
     public:
-        enum NtripState {
-            NoNtrip,
-            Waiting,
-            Enabled
-        };
-
         /**
          * @brief Construct a new Capture Route object
          * 
@@ -76,6 +70,7 @@ class CaptureRoute : public ManualControl {
          * @return RouteInfo 
          */
         RouteInfo getRouteInfo() const { return this->routeInfo; }
+        Navigation::Status getLastStatus() const { return this->status; }
 
         /**
          * @brief Get the distance to the last saved oint
@@ -92,19 +87,13 @@ class CaptureRoute : public ManualControl {
          */
         bool shouldUpdate();
     private:
-        void checkNtrip();
-
         Navigation* navigation;
         RouteInfo routeInfo;
         Point lastSavedPoint;
-        NtripState state = NtripState::Waiting;
+        Navigation::Status status = Navigation::Status::Complete;
 
-        uint8_t ntripReconnectAttemps = 0;
-        uint8_t ntripReconnectMaxAttemps = 10;
-        uint16_t reconnectNtripDelayMillis = 1000;
         uint16_t delay = 200;
         uint32_t lastMillis = 0;
-        uint32_t reconnectNtripLastMillis = 0;
 
         bool updateDisplay = false;
 };

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -13,13 +13,13 @@
 ManualControl::ManualControl(MoveControl *moveControl, const ControlPadInput *input) { 
     this->moveControl = moveControl;
     this->input = input;
-    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+    this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
 }
 
 ManualControl::~ManualControl() {
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationSpeed(0);
-    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
 }
 
 void ManualControl::loop() {

src/driveModi/Modi/ManualControl/manualControl.h

@@ -30,7 +30,7 @@ class ManualControl : public DriveModi {
 
         /**
          * @brief Destroy the Manual Control object
-         * Set in moveControl speed and rotation to 0 and set DrivingStatus::stop
+         * Set in moveControl speed and rotation to 0 and set DrivingStatus::Stop
          */
         ~ManualControl();
 

src/driveModi/Modi/TestMode/testMode.cpp

@@ -16,7 +16,7 @@ TestMode::TestMode(MoveControl *moveControl,  Navigation* navigation) {
 }
 
 TestMode::~TestMode() {
-    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
 }
 
 void TestMode::loop() {
@@ -32,7 +32,7 @@ void TestMode::loop() {
     if (millis() - this->actionStart > this->maneuverTime || this->abort) {
         this->busy = false;
         this->abort = false;
-        this->moveControl->setDrivingStatus(DrivingStatus::stop);
+        this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
         this->maneuver = Maneuver::None;
     }
 
@@ -52,7 +52,7 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         return false;
 
     this->actionStart = millis();
-    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+    this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
 
     if (cm == 0) {
         //Only left or right
@@ -97,7 +97,7 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         return true;
     }
 
-    this->moveControl->setDrivingStatus(DrivingStatus::stop);    
+    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);    
     return false;
 }
 
@@ -148,7 +148,7 @@ bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
         return false;
 
     this->actionStart = millis();
-    this->moveControl->setDrivingStatus(DrivingStatus::raw);
+    this->moveControl->setDrivingStatus(MoveControl::Status::Raw);
     this->maneuverTime = seconds * 1000;
     this->busy = true;
     return true;

src/driveModi/driveManager.cpp

@@ -59,7 +59,7 @@ void DriveManager::changeModus(Modi modus) {
     //Set moveControl to a safe state
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationSpeed(0);
-    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+    this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
     
     switch (modus) {
         case Modi::Off:

src/moveControl.cpp

@@ -47,6 +47,9 @@ MoveControl::~MoveControl() {
     delete this->right_motor;
     delete this->left_speedometer;
     delete this->right_speedometer;
+    delete this->left_pid;
+    delete this->right_pid;
+
 }
 
 void MoveControl::loop() {
@@ -91,19 +94,19 @@ void MoveControl::runMoveControl() {
     this->regulateMotors();
 }
 
-void MoveControl::setDrivingStatus(DrivingStatus status) {
+void MoveControl::setDrivingStatus(Status status) {
     this->setSpeed(0);
     this->setRotationSpeed(0);
     this->setRawPowerLeft(0);
     this->setRawPowerRight(0);
     this->driving_status = status;
     // switch (this->driving_status) {
-    //     case DrivingStatus::stop :
+    //     case Status::Stop :
-    //         Serial.println("New drivingState = stop in MoveControl::setDrivingStatus");
+    //         Serial.println("New drivingState = Stop in MoveControl::setDrivingStatus");
     //         break;
 
-    //     case DrivingStatus::drive :
+    //     case Status::Drive :
-    //         Serial.println("New drivingState = drive in MoveControl::setDrivingStatus");
+    //         Serial.println("New drivingState = Drive in MoveControl::setDrivingStatus");
     //         break;
 
     //     default:
@@ -119,7 +122,7 @@ void MoveControl::emergencyStop() {
     this->setRotationSpeed(0);
     this->setRawPowerLeft(0);
     this->setRawPowerRight(0);
-    this->driving_status = DrivingStatus::stop;
+    this->driving_status = Status::Stop;
 }
 
 void MoveControl::setSpeed(double speed) {
@@ -194,21 +197,21 @@ void MoveControl::calcTargetWheelSpeed() {
 
 void MoveControl::regulateMotors() {
     switch (this->driving_status) {
-        case DrivingStatus::stop :
+        case Status::Stop :
             this->left_motor->setTargetPower(0);
             this->right_motor->setTargetPower(0);
             this->setSpeedometerDirection(this->left_speedometer, 0);
             this->setSpeedometerDirection(this->right_speedometer, 0);
             break;
 
-        case DrivingStatus::drive :
+        case Status::Drive :
             this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
             this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
             this->setSpeedometerDirection(this->left_speedometer, this->left_pid_out);
             this->setSpeedometerDirection(this->right_speedometer, this->right_pid_out);
             break;
 
-        case DrivingStatus::raw :
+        case Status::Raw :
             this->left_motor->setTargetPower(this->rawPowerLeft);
             this->right_motor->setTargetPower(this->rawPowerRight);
             this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);