revised getCourseCorrection

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)