- remove NtripReconnect from Autopilot and CaptureRoute

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

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;