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;

lib/Navigation/navigation.h

@@ -13,6 +13,7 @@
 #define NAVIGATION_H
 
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
+#include <QMC5883LCompass.h>
 #include <Arduino.h>
 #include <iostream>
 #include <SPI.h>
@@ -28,6 +29,7 @@
  * 
  */
 #define MIN_DISTANCE_BETWEEN_POINTS 0.1
+#define AZIMUTH_UPDATE_DELAY 200
 
 /**
  * @brief This struct inherits the result of the navigation
@@ -125,6 +127,8 @@ class Navigation {
          */
         bool addCurrentPosToRoute();
 
+        bool setPointBeforeTurn();
+
         // /**
         //  * @brief Returns the GPS object
         //  * 
@@ -158,6 +162,7 @@ class Navigation {
          */
         RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
         Route* getRoute() const { return this->route; }
+        uint16_t getAzimuth() const { return this->azimuth; }
 
         static void setOutputStatusPrintPVTdata(bool status);
 
@@ -187,10 +192,12 @@ class Navigation {
         UBX_NAV_PVT_data_t* ubxData = nullptr;
         Route* route = nullptr;
         NTRIPClient* ntripClient = nullptr;
+        QMC5883LCompass* compass;
 
         Point lastPoint;
         Point targetPoint;
         Point currentPosition;
+        Point beforeTurnPoint;
 
 
         bool navigationStarted = false;
@@ -204,6 +211,7 @@ class Navigation {
 
         uint8_t timeToWait = 200;
         uint16_t port;
+        uint16_t azimuth = UINT16_MAX;
         uint32_t lastMillis = 0;
         uint32_t ubxUpdateTime = 0;