added compass amd displayWrapper

lib/DisplayWrapper/displayWrapper.cpp

@@ -0,0 +1,61 @@
+/**
+ * @file displayWrapper.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-01-08
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#include "displayWrapper.h"
+
+DisplayWrapper::DisplayWrapper(LiquidCrystal_I2C* lcd) {
+    this->lcd = lcd;
+    this->clear();
+    this->changed = false;
+}
+
+void DisplayWrapper::loop() {
+    if (this->changed) {
+        this->lcd->clear();
+        for (uint8_t i = 0; i < DISPLAY_WRAPPER_LINES; i++) {
+            this->lcd->setCursor(0, i);
+            this->lcd->print(this->data[i]);
+        }
+        this->changed = false;
+    }
+}
+
+void DisplayWrapper::clear() {
+    for (uint8_t i = 0; i < DISPLAY_WRAPPER_LINES; i++) {
+        for (uint8_t j = 0; j < DISPLAY_WRAPPER_ROWS; j++) {
+            data[i][j] = ' ';
+        }
+    }
+    this->changed = true;
+}
+
+void DisplayWrapper::setCursor(uint8_t row, uint8_t line) {
+    if (row > DISPLAY_WRAPPER_ROWS - 1)
+        row = DISPLAY_WRAPPER_ROWS - 1;
+
+    if (line > DISPLAY_WRAPPER_LINES - 1)
+        line = DISPLAY_WRAPPER_LINES - 1;
+
+    this->cursorRow = row;
+    this->cursorLine = line;
+}
+
+void DisplayWrapper::print(const char *str) {
+    uint8_t inputStringPosition = 0;
+    for (uint8_t i = this->cursorRow; i < DISPLAY_WRAPPER_ROWS; i++) {
+        if (str[inputStringPosition] == '\0')
+            break;
+        else
+            this->data[this->cursorLine][i] = str[inputStringPosition];
+        inputStringPosition++;
+    }
+    this->changed = true;
+}

lib/DisplayWrapper/displayWrapper.h

@@ -0,0 +1,42 @@
+/**
+ * @file displayWrapper.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-01-08
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#ifndef DISPLAY_WRAPPER_H
+#define DISPLAY_WRAPPER_H
+
+#include <iostream>
+#include <LiquidCrystal_I2C.h>
+
+#define DISPLAY_WRAPPER_ROWS 16
+#define DISPLAY_WRAPPER_LINES 2
+
+class DisplayWrapper {
+    public:
+        DisplayWrapper(LiquidCrystal_I2C* lcd);
+
+        void loop();
+
+        void clear();
+        void setCursor(uint8_t row, uint8_t line);
+        void print(const char *str);
+
+
+    private:
+        LiquidCrystal_I2C* lcd;
+        char data[DISPLAY_WRAPPER_LINES][DISPLAY_WRAPPER_ROWS];
+
+        uint8_t cursorRow = 0;
+        uint8_t cursorLine = 0;
+
+        bool changed = false;
+};
+
+#endif // DISPLAY_WRAPPER_H

lib/Menu/menuControl.cpp

@@ -15,10 +15,8 @@ void MenuControl::print(String lineOne, String lineTwo) const {
     if (this->lcd) {
         lcd->clear();
         lcd->setCursor(0, 0);
-        lcd->printf(lineOne.c_str());  
+        lcd->print(lineOne.c_str());  
         lcd->setCursor(0, 1);
-        lcd->printf(lineTwo.c_str());
+        lcd->print(lineTwo.c_str());
     }
-
-    std::cout << lineOne.c_str() << " " << lineTwo.c_str() << std::endl;
 }

lib/Menu/menuControl.h

@@ -13,7 +13,7 @@
 #define MENU_CONTROLL_H
 
 #include <iostream>
-#include <LiquidCrystal_I2C.h>
+#include "displayWrapper.h"
 
 /**
  * @brief Baseclass to build Menus
@@ -66,12 +66,12 @@ class MenuControl {
          * 
          * @param lcd 
          */
-        void setLcd(LiquidCrystal_I2C* lcd) { this->lcd = lcd; }
+        void setLcd(DisplayWrapper* lcd) { this->lcd = lcd; }
 
     protected:
         void print(String lineOne, String lineTwo) const;
 
         MenuControl* parentMenu = nullptr;
-        LiquidCrystal_I2C* lcd = nullptr;
+        DisplayWrapper* lcd = nullptr;
 };
 #endif // MENU_CONTROLL_H

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;