- remove NtripReconnect from Autopilot and CaptureRoute

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

lib/Navigation/navigation.cpp

@@ -124,69 +124,54 @@ bool Navigation::startNavigation() {
     return this->navigationStarted;
 }
 
-bool Navigation::getCourseCorrection(CourseCorrection& correction) {
-    //TODO: Check accuracy befor make something
-    correction.correction = 0;
-    correction.distance = 0;
+Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction) {
+    if (this->navigationFinished)
+        return Status::Complete;
+
+    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
-    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.correction = this->calculateCourseCorrection();
     correction.distance = distance;
-    return true;
+    this->lastPointCalcCorrection = this->currentPosition;
+    return Status::Updated;
 }
 
-bool Navigation::addCurrentPosToRoute() {
-    if (!this->currentPosition.isValid()) { return false; }
+Navigation::Status Navigation::addCurrentPosToRoute() {
+    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;
-        return true;
+        this->lastPointRouteInsert = this->currentPosition;
+        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;
-        return true;
+        this->lastPointRouteInsert = this->currentPosition;
+        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;