- remove NtripReconnect from Autopilot and CaptureRoute

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

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,44 +12,89 @@
 
 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())
-    {
-    case 0:
-        lineOne = "Capture Route";
-        lineTwo = "You can drive";
-        break;
+    switch (this->getCurrentPage()) {
+        case 0:
+            lineOne = "Capture Route";
+            lineTwo = "You can drive";
+            break;
 
-    case 1:
-        lineOne = "Saved waypoints";
-        lineTwo.concat(routeInfo.totalPoints);
-        break;
+        case 1:
+            lineOne = "Saved waypoints";
+            lineTwo.concat(routeInfo.totalPoints);
+            break;
 
-    case 2:
-        lineOne = "Distance to last";
-        lineTwo = "point: ";
-        lineTwo.concat(this->captureRoute->getDistanceToLastPoint());
-        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 3: {
-        NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
-        lineOne = "NTRIP Client is";
+                case Navigation::Status::Unchanged:
+                    lineTwo = "Point too close";
+                    break;
 
-        if (status ==  NTRIPClientStates::pushData)
-            lineTwo = "enabled";
-        else if (status == NTRIPClientStates::notAvailable)
-            lineTwo = "not available";
-        else
-            lineTwo = "disabled";
-        break;
-    }
-    
-    default:
-        this->printDefault();
-        return;
+                default:
+                    lineTwo = "---";
+                    break;
+            }
+            break;
+
+        case 3:
+            lineOne = "Distance to last";
+            lineTwo = "point: ";
+            lineTwo.concat(this->captureRoute->getDistanceToLastPoint());
+            break;
+
+        case 4: {
+            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 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;
     }
 
     this->print(lineOne, lineTwo);
@@ -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,65 +19,65 @@ 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
-    if (this->routeInfo.currentPoint >= 2 && this->state == State::GetToStartPoint)
-        this->state = State::SelfDrivingAvailable;
+    this->routeInfo = this->navigation->getRouteInfo();
+    this->runAutopilot();
+    this->loopLastMillis = millis();
+}
 
-    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
-        && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
+void Autopilot::runAutopilot() {
+    switch (this->state) {
+        case State::InsufficientAccuarcy:
+            this->askNavigationForOrder();
+            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;
         
-        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();
-        }
+        default:
+            break;
     }
-
-    if (this->state == State::SelfDriving) {
-        if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
-            this->rotate();
-        else
-            this->drive();
-    }   
 }
 
 void Autopilot::restart() {
@@ -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);
+
+    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 :
-    //         Serial.println("New drivingState = stop in MoveControl::setDrivingStatus");
+    //     case Status::Stop :
+    //         Serial.println("New drivingState = Stop in MoveControl::setDrivingStatus");
     //         break;
 
-    //     case DrivingStatus::drive :
-    //         Serial.println("New drivingState = drive in MoveControl::setDrivingStatus");
+    //     case Status::Drive :
+    //         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);