From 26095369dd732e8fddf95e54864ca1d4befacb77 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Mon, 9 Jan 2023 09:40:06 +0100 Subject: [PATCH] added compass amd displayWrapper --- lib/DisplayWrapper/displayWrapper.cpp | 61 +++++++++++++++++++++++++++ lib/DisplayWrapper/displayWrapper.h | 42 ++++++++++++++++++ lib/Menu/menuControl.cpp | 6 +-- lib/Menu/menuControl.h | 6 +-- lib/Navigation/navigation.cpp | 24 ++++++++++- lib/Navigation/navigation.h | 8 ++++ platformio.ini | 1 + src/SpecialMenus/GPS/menuGPS.cpp | 6 +++ src/main.cpp | 55 +++++++++++------------- 9 files changed, 170 insertions(+), 39 deletions(-) create mode 100644 lib/DisplayWrapper/displayWrapper.cpp create mode 100644 lib/DisplayWrapper/displayWrapper.h diff --git a/lib/DisplayWrapper/displayWrapper.cpp b/lib/DisplayWrapper/displayWrapper.cpp new file mode 100644 index 0000000..3c07121 --- /dev/null +++ b/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; +} diff --git a/lib/DisplayWrapper/displayWrapper.h b/lib/DisplayWrapper/displayWrapper.h new file mode 100644 index 0000000..722b272 --- /dev/null +++ b/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 +#include + +#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 diff --git a/lib/Menu/menuControl.cpp b/lib/Menu/menuControl.cpp index 8a06de5..36facc1 100644 --- a/lib/Menu/menuControl.cpp +++ b/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; } diff --git a/lib/Menu/menuControl.h b/lib/Menu/menuControl.h index 95c8c45..c5a4dd0 100644 --- a/lib/Menu/menuControl.h +++ b/lib/Menu/menuControl.h @@ -13,7 +13,7 @@ #define MENU_CONTROLL_H #include -#include +#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 diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index e682381..de94a88 100644 --- a/lib/Navigation/navigation.cpp +++ b/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; diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 536f7e8..130f840 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -13,6 +13,7 @@ #define NAVIGATION_H #include +#include #include #include #include @@ -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; diff --git a/platformio.ini b/platformio.ini index 7933f72..4da7359 100644 --- a/platformio.ini +++ b/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] diff --git a/src/SpecialMenus/GPS/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp index 979273f..474739f 100644 --- a/src/SpecialMenus/GPS/menuGPS.cpp +++ b/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; diff --git a/src/main.cpp b/src/main.cpp index 4e5e41a..8550836 100644 --- a/src/main.cpp +++ b/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(); - lcd->setCursor(0, 0); - lcd->print("Rebooting ..."); + lcdWrapper->clear(); + lcdWrapper->setCursor(0, 0); + lcdWrapper->print("Rebooting ..."); ESP.restart(); }; auto disconnectController = [&]() { disconnectPs3 = true; - lcd->clear(); - lcd->setCursor(0, 0); - lcd->print("Controller is"); - lcd->setCursor(0, 1); - lcd->print("disconnected."); + lcdWrapper->clear(); + lcdWrapper->setCursor(0, 0); + lcdWrapper->print("Controller is"); + lcdWrapper->setCursor(0, 1); + 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); - mode_m->setLcd(lcd); - pid_m->setLcd(lcd); - pidl_m->setLcd(lcd); - pidr_m->setLcd(lcd); - man_m->setLcd(lcd); - cap_m->setLcd(lcd); - auto_m->setLcd(lcd); - test_m->setLcd(lcd); - gps_m->setLcd(lcd); - sys_m->setLcd(lcd); - rout_m->setLcd(lcd); + main_m->setLcd(lcdWrapper); + mode_m->setLcd(lcdWrapper); + pid_m->setLcd(lcdWrapper); + pidl_m->setLcd(lcdWrapper); + pidr_m->setLcd(lcdWrapper); + man_m->setLcd(lcdWrapper); + cap_m->setLcd(lcdWrapper); + auto_m->setLcd(lcdWrapper); + test_m->setLcd(lcdWrapper); + gps_m->setLcd(lcdWrapper); + sys_m->setLcd(lcdWrapper); + rout_m->setLcd(lcdWrapper); // Entry for the main menu