- 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

lib/calibrateCompass/calibrateCompass.cpp

@@ -63,7 +63,7 @@ void CalibrateCompass::loop() {
 
     if (millis() - this->lastChange > this->maxTimeWithoutChange) {
         this->state = State::Finished;
-        this->newData = true;
+        this->checkDataValidity();
     }
 }
 
@@ -78,6 +78,11 @@ void CalibrateCompass::start() {
 }
 
 void CalibrateCompass::useData() {
+    if (!this->dataValid) {
+        std::cout << "CalibrateCompass::useData - Data not valid" << std::endl;
+        return;
+    }
+
     this->compass->setCalibration(  this->data.data[0][0],
                                     this->data.data[0][1],
                                     this->data.data[1][0],
@@ -89,8 +94,7 @@ void CalibrateCompass::useData() {
     std::cout << "CalibrateCompass::useData " << *this << std::endl;                                    
 }
 
-void CalibrateCompass::resetData() {
-    this->reset();
+void CalibrateCompass::removeCalibration() {
     this->compass->removeCalibration();
 }
 
@@ -100,7 +104,7 @@ void CalibrateCompass::reset() {
 }
 
 void CalibrateCompass::saveData() {
-    if (!this->newData)
+    if (!this->dataValid)
         return;
 
     Preferences preferences;
@@ -128,6 +132,7 @@ void CalibrateCompass::loadData() {
     this->data.data[2][1] = preferences.getInt("zHigh", 0);
 
     preferences.end();
+    this->checkDataValidity();
 }
 
 void CalibrateCompass::clearData() {
@@ -135,6 +140,22 @@ void CalibrateCompass::clearData() {
         this->data.data[i][0] = 0;
         this->data.data[i][1] = 0;
     }
+    this->dataValid = false;
+}
+
+void CalibrateCompass::checkDataValidity() {
+    int sum = 0;
+    for (uint8_t i = 0; i < 3; i++) {
+        if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN
+            || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN) 
+            {
+            this->dataValid = false;
+            return;
+        }
+        sum += this->data.data[i][0];
+        sum += this->data.data[i][1];
+    }
+    this->dataValid = sum;
 }
 
 std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp) {

lib/calibrateCompass/calibrateCompass.h

@@ -32,7 +32,7 @@ class CalibrateCompass {
         void loop();
         void start();
         void useData();
-        void resetData();
+        void removeCalibration();
         void reset();
         void saveData();
         void loadData();
@@ -43,13 +43,15 @@ class CalibrateCompass {
         friend std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp);
 
     private:
+        void checkDataValidity();
+
         QMC5883LCompass* compass;
         State state;
         CallibrationData data;
 
         void clearData();
 
-        bool newData = false;
+        bool dataValid = false;
         const uint16_t maxTimeWithoutChange = 5000;
         uint32_t lastChange = 0;
 };