added compass amd displayWrapper

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;