added interface for calced azimuth

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; }
@@ -175,14 +185,14 @@ 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
          */
-        int16_t getAzimuth() const { return this->azimuth; }
+        int16_t getAzimuth() const { return this->realAzimuth; }
         QMC5883LCompass* getCompass() const { return this->compass; }
 
         Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
@@ -200,26 +210,11 @@ class Navigation {
 
     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,22 +224,27 @@ 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;
 
         char* host;
         char* mountPoint;
         char* user;
         char* password;
 
-        int16_t azimuth = INT16_MAX;
+        int16_t realAzimuth = INT16_MAX;
+        int16_t calcAzimuth = INT16_MAX;
 
         uint8_t timeToWait = 200;
         uint16_t port;