kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

added compass amd displayWrapper

Browse Source
This commit is contained in:
kleiax
2023-01-09 09:40:06 +01:00
parent 9762aabb00
commit 26095369dd
9 changed files with 170 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/DisplayWrapper/displayWrapper.cpp
+61
View File
@@ -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
+42
View File
@@ -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
+2 -4
View File
@@ -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
+3 -3
View File
@@ -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
+23 -1
View File
@@ -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
+8
View File
@@ -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
+1
View File
@@ -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
+6
View File
@@ -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
+24 -31
View File
@@ -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