From 7f1ffc874f9b34f00527ce1d27c277d76295fccf Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Wed, 28 Dec 2022 10:24:09 +0100
Subject: [PATCH] revised getCourseCorrection

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

diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index d119589..e682381 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -106,13 +106,13 @@ bool Navigation::startNavigation() {
 }
 
 CourseCorrection Navigation::getCourseCorrection() {
+    //TODO: Check accuracy befor make something
     CourseCorrection courseCorrection = {0, 0};
 
-    this->updateCurrentLocation();
-    double targetCourse = this->currentPosition.courseTo(this->targetPoint);
+    int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
     double distance = this->currentPosition.distanceTo(this->targetPoint);
 
-    if (targetCourse < 0 || distance < 0 || this->navigationFinished)
+    if (distance < 0 || this->navigationFinished)
         return courseCorrection;
 
     // Check if I need a new Point
@@ -125,11 +125,8 @@ CourseCorrection Navigation::getCourseCorrection() {
         }
     }
 
-    //FIXME: next line
-    double currentCourse = 0;
-    
-    //double currentCourse = this->gps->course.deg();
-    int16_t correction = currentCourse - targetCourse;
+    // correction = targetCourse - currentCourse
+    int16_t correction = targetCourse - this->ubxData->magDec;
     if (correction > 180)
         correction -= 360;
     else if (correction < -180)