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;
 

platformio.ini

@@ -25,6 +25,7 @@ lib_deps =
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
 	marian-craciunescu/ESP32Ping@^1.7
 	bblanchon/ArduinoJson@^6.20.0
+	mprograms/QMC5883LCompass@^1.1.1
 upload_port = COM3
 
 [platformio]

src/SpecialMenus/GPS/menuGPS.cpp

@@ -93,6 +93,12 @@ void MenuGPS::printPage() const {
             }
             break;
 
+        case 6:
+            lineOne = "Azimuth: ";
+            lineTwo = "";
+            lineTwo.concat(this->navigation->getAzimuth());
+            break;
+
         default:
             this->printDefault();
             return;

src/main.cpp

@@ -27,6 +27,7 @@
 
 #include "driveModi/driveManager.h"
 #include "OutputBuf/outputBuf.h"
+#include "displayWrapper.h"
 #include "moveControl.h"
 #include "network.h"
 #include "debugMqtt.h"
@@ -48,6 +49,7 @@ MoveControl moveController;
 DriveManager* driveManager;
 Menu* main_m;
 LiquidCrystal_I2C* lcd;
+DisplayWrapper* lcdWrapper;
 OutputBuf* outputBuf;
 DebugMqtt* debugMqtt = nullptr;
 Battery* mainBattery;
@@ -120,6 +122,7 @@ void setup() {
     lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
     lcd->setCursor(0, 1);
     lcd->print("Press PS-Button");
+    lcdWrapper = new DisplayWrapper(lcd);
 
     makeMenu();
 
@@ -134,6 +137,7 @@ void loop() {
         wifiTime.stopConsol("WiFi-Time", 10);
     }
     driveManager->loop();
+    lcdWrapper->loop();
 
     DebugTimes mainBatteryTime;
     mainBattery->loop();
@@ -198,18 +202,9 @@ void callbackControllerAction() {
 }
 
 void callbackControllerConnect() {
-    static bool firstConnect = true;
-
     std::cout << "Controller connected to ESP32" << std::endl;
     std::cout << "All actions from the Controller run on Core -> " << xPortGetCoreID() << std::endl;
 
-    // Display is not functional without this seconde init, first init is in setup(),
-    // because after the setup the other core control the display.
-    if (firstConnect) {
-        lcd->init();
-        firstConnect = false;
-    }
-
     main_m->printMenu();
 }
 
@@ -255,19 +250,19 @@ void makeMenu() {
     };
 
     auto restart = [&]() {
-        lcd->clear();
+        lcdWrapper->clear();
-        lcd->setCursor(0, 0);
+        lcdWrapper->setCursor(0, 0);
-        lcd->print("Rebooting ...");
+        lcdWrapper->print("Rebooting ...");
         ESP.restart();
     };
 
     auto disconnectController = [&]() {
         disconnectPs3 = true;
-        lcd->clear();
+        lcdWrapper->clear();
-        lcd->setCursor(0, 0);
+        lcdWrapper->setCursor(0, 0);
-        lcd->print("Controller is");
+        lcdWrapper->print("Controller is");
-        lcd->setCursor(0, 1);
+        lcdWrapper->setCursor(0, 1);
-        lcd->print("disconnected.");
+        lcdWrapper->print("disconnected.");
     };
 
     // Create Menu
@@ -276,8 +271,6 @@ void makeMenu() {
     Menu* pid_m = new Menu();
     MenuIntInput* pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
     MenuIntInput* pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
-    // MenuPidSettings* pidl_m = new MenuPidSettings(moveController.getPID(0));
-    // MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
     MenuManualControl* man_m = new MenuManualControl(driveManager);
     MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager);
     MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
@@ -287,18 +280,18 @@ void makeMenu() {
     MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());
 
     // Set Menus on LCD
-    main_m->setLcd(lcd);
+    main_m->setLcd(lcdWrapper);
-    mode_m->setLcd(lcd);
+    mode_m->setLcd(lcdWrapper);
-    pid_m->setLcd(lcd);
+    pid_m->setLcd(lcdWrapper);
-    pidl_m->setLcd(lcd);
+    pidl_m->setLcd(lcdWrapper);
-    pidr_m->setLcd(lcd);
+    pidr_m->setLcd(lcdWrapper);
-    man_m->setLcd(lcd);
+    man_m->setLcd(lcdWrapper);
-    cap_m->setLcd(lcd);
+    cap_m->setLcd(lcdWrapper);
-    auto_m->setLcd(lcd);
+    auto_m->setLcd(lcdWrapper);
-    test_m->setLcd(lcd);
+    test_m->setLcd(lcdWrapper);
-    gps_m->setLcd(lcd);
+    gps_m->setLcd(lcdWrapper);
-    sys_m->setLcd(lcd);
+    sys_m->setLcd(lcdWrapper);
-    rout_m->setLcd(lcd);
+    rout_m->setLcd(lcdWrapper);
 
 
     // Entry for the main menu