added interface for calced azimuth

lib/Navigation/navigation.cpp

@@ -98,14 +98,11 @@ void Navigation::loop() {
         this->ntripClient->loop();
 
     if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
-    // if (millis() - this->lastMillis > 500) {
         this->compass->read();
-        this->azimuth = this->compass->getAzimuth();
+        this->realAzimuth = this->compass->getAzimuth();
-        // std::cout   << "Compass x: " << compass->getX() 
+
-        //             << " y: " << compass->getY() 
+        this->updateMagneticDeclination();
-        //             << " z: " << compass->getZ() 
+
-        //             << " Azi: " << compass->getAzimuth() 
-        //             << std::endl; 
         this->lastMillis = millis();
     }
 }
@@ -124,6 +121,14 @@ bool Navigation::startNavigation() {
     return this->navigationStarted;
 }
 
+void Navigation::drivingDirectionChange() {
+    Point tmp = this->currentPosition;
+    if (tmp.isInit() && tmp.isValid()) {
+        this->directionChangeMode = true;
+        this->lastPointDrivingDirectionChange = tmp;
+    }
+}
+
 Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
     if (this->navigationFinished)
         return Status::Complete;
@@ -190,9 +195,62 @@ void Navigation::updateCurrentLocation() {
     this->currentPosition = Point(coords, this->ubxData->hAcc);
 }
 
+void Navigation::updateMagneticDeclination() {
+    if (!this->directionChangeMode
+        || this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Invalid;
+        return;
+    }
+
+    this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition);
+
+    // Map point accuracy to CalcAzimuthState
+    if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Good;
+    } 
+    else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Ok;
+    } 
+    else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Bad;
+    } 
+    else 
+    {
+        this->calcAzimuthState = CalcAzimuthState::Invalid;
+    }
+
+    // Upgrade quality if the range grows up
+    if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) {
+        switch (this->calcAzimuthState) {
+            case CalcAzimuthState::Bad :
+                this->calcAzimuthState = CalcAzimuthState::Ok;
+                break;
+            
+            case CalcAzimuthState::Ok :
+                this->calcAzimuthState = CalcAzimuthState::Good;
+                break;
+
+            case CalcAzimuthState::Good :
+                this->calcAzimuthState = CalcAzimuthState::Super;
+                break;
+
+            default:
+                break;
+        }
+    }
+
+}
+
 int16_t Navigation::calculateCourseCorrection(Point& point) {
     int16_t targetCourse = point.courseTo(this->targetPoint);
-    int16_t correctionCourse = targetCourse - this->azimuth;
+    int16_t correctionCourse = targetCourse - this->realAzimuth;
     if (correctionCourse > 180)
         correctionCourse -= 360;
     else if (correctionCourse < -180)

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);
+        void updateMagneticDeclination();
-
-        /**
-         * @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 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;