rotate correctionCourse in getCourseCorrection by

180°

lib/Navigation/navigation.cpp

@@ -58,16 +58,16 @@ void Navigation::init(Route* route) {
         this->route = new Route();
 
     this->compass = new QMC5883LCompass();
-
     // Init Compass
     Wire.beginTransmission(0x0d);
     Wire.write(0x0b);
     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;
 }
 
 Navigation::~Navigation() {
@@ -98,8 +98,14 @@ 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();
+        // std::cout   << "Compass x: " << compass->getX() 
+        //             << " y: " << compass->getY() 
+        //             << " z: " << compass->getZ() 
+        //             << " Azi: " << compass->getAzimuth() 
+        //             << std::endl; 
         this->lastMillis = millis();
     }
 }
@@ -132,7 +138,6 @@ bool Navigation::getCourseCorrection(CourseCorrection& correction) {
     // Check if I need a new Point
     if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
         if (!this->nextPoint()) {
-            std::cout << "Navigation::getCourseCorrection finished" << std::endl;
             this->navigationFinished = true;
             this->navigationStarted = false;
             return false; // End of navigation
@@ -147,8 +152,9 @@ bool Navigation::getCourseCorrection(CourseCorrection& correction) {
     else if (correctionCourse < -180)
         correctionCourse += 360;
 
-    // //Rotate result by 180° to corrigate Azimuth
-    // correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
+    //Rotate result by 180° to corrigate Azimuth
+    correctionCourse += 180;
+    correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
 
     correction.correction = correctionCourse;
     correction.distance = distance;