added compass amd displayWrapper

lib/Navigation/navigation.cpp

@@ -58,6 +58,14 @@ void Navigation::init(Route* route) {
         this->route = new Route();
 
     this->ntripClient = new NTRIPClient();
+    this->compass = new QMC5883LCompass();
+
+    // Init Compass
+    Wire.beginTransmission(0x0d);
+    Wire.write(0x0b);
+    Wire.write(0x01);
+    Wire.endTransmission();
+    this->compass->setMode(0x01,0x04,0x10,0X00);
 }
 
 Navigation::~Navigation() {
@@ -89,6 +97,12 @@ void Navigation::loop() {
 
     if (this->isNtripInit)
         this->ntripClient->loop();
+
+    if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
+        this->compass->read();
+        this->azimuth = this->compass->getAzimuth();
+        this->lastMillis = millis();
+    }
 }
 
 void Navigation::newRoute() {
@@ -126,7 +140,8 @@ CourseCorrection Navigation::getCourseCorrection() {
     }
 
     // correction = targetCourse - currentCourse
-    int16_t correction = targetCourse - this->ubxData->magDec;
+    int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
+    int16_t correction = targetCourse - signedAzimuth;
     if (correction > 180)
         correction -= 360;
     else if (correction < -180)
@@ -156,6 +171,13 @@ bool Navigation::addCurrentPosToRoute() {
     return false;
 }
 
+bool Navigation::setPointBeforeTurn() {
+    if (millis() - this->currentPosition.getCreationTime() > 1000)
+        return false;
+    this->beforeTurnPoint = this->currentPosition;
+    return true;
+}
+
 void Navigation::updateCurrentLocation() {
     if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
         return;