- added data validation in calibrate compass

- activatre auto load calibration data
- give possibility to force update in navigation getCourseCorrection
- added drive mode for compass calibration
- removed compass calibration manualControl

lib/Navigation/navigation.cpp

@@ -64,9 +64,9 @@ void Navigation::init(Route* route) {
     Wire.write(0x01);
     Wire.endTransmission();
     this->compass->setMode(0x01,0x0C,0x10,0X00);
-    // CalibrateCompass caliCompass(this->compass);
-    // caliCompass.loadData();
-    // caliCompass.useData();
+    CalibrateCompass caliCompass(this->compass);
+    caliCompass.loadData();
+    caliCompass.useData();
     // std::cout << "Navigation::init compass correction data: " << caliCompass << std::endl;
 }
 
@@ -124,15 +124,18 @@ bool Navigation::startNavigation() {
     return this->navigationStarted;
 }
 
-Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction) {
+Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
     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))
+    if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
+        && !forceUpdate) {
+        correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
         return Status::Unchanged;
+    }
 
     double distance = this->currentPosition.distanceTo(this->targetPoint);
 
@@ -146,7 +149,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction)
         distance = this->currentPosition.distanceTo(this->targetPoint);
     }
 
-    correction.correction = this->calculateCourseCorrection();
+    correction.correction = this->calculateCourseCorrection(this->currentPosition);
     correction.distance = distance;
     this->lastPointCalcCorrection = this->currentPosition;
     return Status::Updated;
@@ -186,8 +189,8 @@ void Navigation::updateCurrentLocation() {
     this->currentPosition = Point(coords, this->ubxData->hAcc);
 }
 
-int16_t Navigation::calculateCourseCorrection() {
-    int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
+int16_t Navigation::calculateCourseCorrection(Point& point) {
+    int16_t targetCourse = point.courseTo(this->targetPoint);
     // correction = targetCourse - currentCourse
     int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
     int16_t correctionCourse = targetCourse - signedAzimuth;

lib/Navigation/navigation.h

@@ -131,7 +131,7 @@ class Navigation {
          * @return true if new correction data provided
          * @return false if route is finished 
          */
-        Status getCourseCorrection(CourseCorrection& correction);
+        Status getCourseCorrection(CourseCorrection& correction, bool forceUpdate = false);
 
         /**
          * @brief Tries to add the current Position to the route
@@ -197,7 +197,7 @@ class Navigation {
 
     private:
         void updateCurrentLocation();
-        int16_t calculateCourseCorrection();
+        int16_t calculateCourseCorrection(Point& point);
 
         /**
          * @brief Set the next point as target