From 1faab632d546efc6dbb446355891f0b5f181c034 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Thu, 25 May 2023 19:58:57 +0200
Subject: [PATCH] =?UTF-8?q?rotate=20correctionCourse=20in=20getCourseCorre?=
 =?UTF-8?q?ction=20by=20180=C2=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 lib/Navigation/navigation.cpp | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index 7206493..92546f8 100644
--- a/lib/Navigation/navigation.cpp
+++ b/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;