Merge branch 'main' of git.kleiax.de:kleiax/Projektarbeit-Rover into main

lib/Navigation/navigation.h

@@ -62,6 +62,14 @@ class Navigation {
             Complete
         };
 
+        enum CalcAzimuthState {
+            Invalid,
+            Bad,
+            Ok,
+            Good,
+            Super
+        };
+
 
         /**
          * @brief Construct a new Navigation object and using I2C
@@ -118,6 +126,8 @@ class Navigation {
          */
         bool startNavigation();
         void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
+        void drivingDirectionChange();
+        void dissableCalcAzimuth() { this->directionChangeMode = false; }
         
         double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
         double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
@@ -178,15 +188,21 @@ class Navigation {
         Point getCurrentPosition() const { return this->currentPosition; }
 
         /**
-         * @brief Get azimuth
+         * @brief Get realAzimuth
          * 
          * This value represents the angle between north and 
          * the line of sight. Clockwise.
          * 
          * @return uint16_t degree
          */
-        uint16_t getAzimuth() const { return this->azimuth; }
+        int16_t getAzimuth() const { return this->realAzimuth; }
         QMC5883LCompass* getCompass() const { return this->compass; }
+        int16_t getCalcAzimuth() const { return this->calcAzimuth; }
+        CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; }
+        bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; }
+
+        Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
+        void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
 
         /**
          * @brief Set the output status for PVTdata.
@@ -197,29 +213,16 @@ class Navigation {
          * @param status 
          */
         static void setOutputStatusPrintPVTdata(bool status);
+        // map input in range from -180 to 180 degree
+        static int16_t fixDegree(int16_t degree);
 
     private:
         void updateCurrentLocation();
-        int16_t calculateCourseCorrection(Point& point);
-
-        /**
-         * @brief Set the next point as target
-         * 
-         * @return true
-         * @return false 
-         */
-        bool nextPoint();
-
-        /**
-         * @brief Set the target point
-         * 
-         * @param target 
-         * @return true 
-         * @return false 
-         */
-        bool setTargetPoint(Point target);
-
+        void updateMagneticDeclination();
         void init(Route* route);
+        bool nextPoint();
+        bool setTargetPoint(Point target);
+        int16_t calculateCourseCorrection(Point& point);
 
         SFE_UBLOX_GNSS* gps;
         UBX_NAV_PVT_data_t* ubxData = nullptr;
@@ -229,24 +232,31 @@ class Navigation {
 
         Point lastPointRouteInsert;
         Point lastPointCalcCorrection;
+        Point lastPointDrivingDirectionChange;
         Point targetPoint;
         Point currentPosition;
         Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
+        CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid;
+
 
 
         bool navigationStarted = false;
         bool navigationFinished = false;
         bool isNtripInit = false;
         bool preventNextPoint = false;
+        bool directionChangeMode = false;
+        bool lastUsedCalcAzimuth = false;
 
         char* host;
         char* mountPoint;
         char* user;
         char* password;
 
+        int16_t realAzimuth = INT16_MAX;
+        int16_t calcAzimuth = INT16_MAX;
+
         uint8_t timeToWait = 200;
         uint16_t port;
-        uint16_t azimuth = UINT16_MAX;
         uint32_t lastMillis = 0;
         uint32_t ubxUpdateTime = 0;