refactor all autopilot stuff

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)
-        this->selfDrivingAvailable = true;
+    if (this->routeInfo.currentPoint >= 2 && this->state == State::GetToStartPoint)
+        this->state = State::SelfDrivingAvailable;
 
-    if (!this->selfDriving 
-        && this->selfDrivingAvailable 
-        && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action))
-        this->setSelfDriving(true);
-
-    if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
-        this->setSelfDriving(false);
-        this->navigationEnded = true;
-        this->navigationStarted = false;
-        this->selfDrivingAvailable = false;
+    if (this->state == State::SelfDrivingAvailable 
+        && ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
+        this->state = State::SelfDriving;
+        this->updateDisplay = true;
     }
 
-    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) {
-    if (!this->navigationStarted)
-        return;
+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;
 
-    this->updateDisplay = true;
+        if (this->ntripReconnectAttemps > this->ntripReconnectMaxAttemps) {
+            this->state = State::NoNtrip;
+            return;
+        }
 
-    this->selfDriving = val;
-    this->moveControl->setSpeed(0);
-    this->moveControl->setRotationSpeed(0);
+        this->reconnectNtripLastMillis = millis();
+        client->setActivated(true);
+        this->ntripReconnectAttemps++;
+    }
 }
 
 void Autopilot::setSpeedInRelToDistance(double distance) {