refactore all captureRoute stuff

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -14,13 +14,18 @@
 CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) 
     : ManualControl(moveControl, input) {
         this->navigation = navigation;
+        this->navigation->getRoute()->clear();
         this->routeInfo = navigation->getRouteInfo();
 }
 
+CaptureRoute::~CaptureRoute() {
+    this->navigation->getNTRIPClient()->setActivated(false);
+}
+
 void CaptureRoute::loop() {
     ManualControl::loop();
 
-    if (millis() - this->lastMillis < delay) {
+    if (millis() - this->lastMillis < this->delay) {
         return;
     }
     this->runCaptureRoute();
@@ -28,14 +33,26 @@ void CaptureRoute::loop() {
 }
 
 void CaptureRoute::runCaptureRoute() {
+    this->checkNtrip();
+
+    if (this->state != NtripState::Enabled)
+        return;
+
     if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
         if (this->navigation->addCurrentPosToRoute()) {
+            this->lastSavedPoint = this->navigation->getCurrentPosition();
             this->routeInfo = navigation->getRouteInfo();
             this->updateDisplay = true;
         }
     }
 }
 
+double CaptureRoute::getDistanceToLastPoint() {
+    if (!this->lastSavedPoint.isInit())
+        return 0;
+    return this->lastSavedPoint.distanceTo(this->navigation->getCurrentPosition());
+}
+
 bool CaptureRoute::shouldUpdate() {
     if (this->updateDisplay) {
         this->updateDisplay = false;
@@ -43,3 +60,27 @@ 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,6 +27,12 @@
  */
 class CaptureRoute : public ManualControl {
     public:
+        enum NtripState {
+            NoNtrip,
+            Waiting,
+            Enabled
+        };
+
         /**
          * @brief Construct a new Capture Route object
          * 
@@ -35,6 +41,13 @@ class CaptureRoute : public ManualControl {
          */
         CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
 
+        /**
+         * @brief Destroy the Capture Route object
+         * 
+         * Disconnect the NTRIP-Client
+         */
+        ~CaptureRoute();
+
         /**
          * @brief Calls runCaptureRoute and ManualControl::loop
          * 
@@ -64,6 +77,13 @@ class CaptureRoute : public ManualControl {
          */
         RouteInfo getRouteInfo() const { return this->routeInfo; }
 
+        /**
+         * @brief Get the distance to the last saved oint
+         * 
+         * @return double in meters
+         */
+        double getDistanceToLastPoint();
+
         /**
          * @brief Tells if there are new informations to display
          * 
@@ -72,11 +92,19 @@ class CaptureRoute : public ManualControl {
          */
         bool shouldUpdate();
     private:
+        void checkNtrip();
+
         Navigation* navigation;
         RouteInfo routeInfo;
+        Point lastSavedPoint;
+        NtripState state = NtripState::Waiting;
 
-        uint32_t lastMillis = 0;
+        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;
 };