From af6cb9889834e479ed2e35160a9d146ac1485e0d Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Tue, 13 Sep 2022 20:31:52 +0200 Subject: [PATCH] ntrip, testMode, zed9 gnss modul --- doc/Notes and TODOs/TODO allgemein.txt | 2 + doc/PinBelegungen.txt | 8 +- include/moveControl.h | 2 +- include/network.h | 6 +- include/networkConfig.h | 37 ++++- lib/MQTT/debugMqtt.h | 1 + lib/Menu/menu.cpp | 4 + lib/Menu/menu.h | 1 + lib/Menu/menuControl.h | 17 +- lib/Menu/menuIntInput.cpp | 97 +++++++++++ lib/Menu/menuIntInput.h | 83 +++++++++ lib/MotorControl/motorControl.cpp | 6 +- lib/MotorControl/motorControl.h | 2 +- lib/Navigation/navigation.cpp | 66 +++++++- lib/Navigation/navigation.h | 23 +++ lib/Ntrip/NTRIPClient.cpp | 69 ++++---- lib/Ntrip/NTRIPClient.h | 6 +- lib/Rtcm/rtcmInsert.cpp | 50 ------ lib/Rtcm/rtcmInsert.h | 31 ---- src/SpecialMenus/Akku/menuAkku.cpp | 4 +- src/SpecialMenus/Akku/menuAkku.h | 2 +- src/SpecialMenus/GPS/menuGPS.cpp | 138 ++++++++------- src/SpecialMenus/GPS/menuGPS.h | 2 +- .../driveModi/Autopilot/menuAutopilot.cpp | 4 +- .../driveModi/Autopilot/menuAutopilot.h | 2 +- .../driveModi/TestMode/menuTestMode.cpp | 157 ++++++++++++++++++ .../driveModi/TestMode/menuTestMode.h | 56 +++++++ src/driveModi/Modi/Autopilot/autopilot.cpp | 10 +- src/driveModi/Modi/Autopilot/autopilot.h | 2 +- .../Modi/Autopilot/autopilotConfig.h | 2 +- .../Modi/CaptureRoute/captureRoute.cpp | 4 +- .../Modi/CaptureRoute/captureRoute.h | 2 +- .../Modi/ManualControl/manualControl.cpp | 10 +- .../Modi/ManualControl/manualControl.h | 6 +- src/driveModi/Modi/Racing/racing.cpp | 0 src/driveModi/Modi/Racing/racing.h | 0 src/driveModi/Modi/TestMode/testMode.cpp | 124 ++++++++++++++ src/driveModi/Modi/TestMode/testMode.h | 54 ++++++ src/driveModi/driveManager.cpp | 13 +- src/driveModi/driveManager.h | 3 +- src/main.cpp | 37 +++-- src/moveControl.cpp | 8 +- src/network.cpp | 18 +- 43 files changed, 907 insertions(+), 262 deletions(-) create mode 100644 lib/Menu/menuIntInput.cpp create mode 100644 lib/Menu/menuIntInput.h delete mode 100644 lib/Rtcm/rtcmInsert.cpp delete mode 100644 lib/Rtcm/rtcmInsert.h create mode 100644 src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp create mode 100644 src/SpecialMenus/driveModi/TestMode/menuTestMode.h create mode 100644 src/driveModi/Modi/Racing/racing.cpp create mode 100644 src/driveModi/Modi/Racing/racing.h diff --git a/doc/Notes and TODOs/TODO allgemein.txt b/doc/Notes and TODOs/TODO allgemein.txt index b9552c1..b4899a5 100644 --- a/doc/Notes and TODOs/TODO allgemein.txt +++ b/doc/Notes and TODOs/TODO allgemein.txt @@ -1,4 +1,6 @@ -> Document the battery pack lib -> Program underfloorLighting -> Program battery pack tracking + -> Add an beeper + -> Program the beeper diff --git a/doc/PinBelegungen.txt b/doc/PinBelegungen.txt index 628d56e..d25e178 100644 --- a/doc/PinBelegungen.txt +++ b/doc/PinBelegungen.txt @@ -10,12 +10,12 @@ INTL1 32 GND GND INTL2 33 19 SCL SCL INTB1 25 18 VCC INTB2 26 5 -DirL1 27 17 GPS RX -DirR2 14 16 GPS TX +DirL1 27 17 GPS RX deprecated +DirR2 14 16 GPS TX deprecated DirL2 12 4 SD-Card GND x PWML 13 2 SD-Card - x 15 + x 15 WS2812 LED ? x x CMD x 5V USB x @@ -27,4 +27,4 @@ Connector Encoder OB 4 3 - 3V3 6 [5] OA 8 7 - 10 9 \ No newline at end of file + 10 9 diff --git a/include/moveControl.h b/include/moveControl.h index fde1163..b526542 100644 --- a/include/moveControl.h +++ b/include/moveControl.h @@ -94,7 +94,7 @@ class MoveControl { * target rotationspeed is set to zero. * @param speed rad/s */ - void setRotationspeed(double speed); + void setRotationSpeed(double speed); /** * @brief Set the pid tunings diff --git a/include/network.h b/include/network.h index df4e873..0a8f0b8 100644 --- a/include/network.h +++ b/include/network.h @@ -51,8 +51,11 @@ class Network { * If mqtt is not connect, this function try a reconnect * with the function connectMQTT. This function should be * called every mainloop. + * + * @return true if connected + * @return false if not connected */ - static void checkMQTT(); + static bool checkMQTT(); /** * @brief Checks if WiFi is still connected @@ -82,6 +85,7 @@ class Network { static IPAddress gateway; static IPAddress subnet; static IPAddress mqtt_server; + static IPAddress dnsServer; static WiFiClient wifi_client; static PubSubClient* mqtt_client; diff --git a/include/networkConfig.h b/include/networkConfig.h index 2fa1603..b815a0f 100644 --- a/include/networkConfig.h +++ b/include/networkConfig.h @@ -29,13 +29,28 @@ * The time in milliseconds until the next connection attempt. */ #define WLAN_CONNECT_LOOP_TIME 500 -#define ESP32BROKER +#define WLAN_DNS_SERVER "8.8.8.8" +#define DORO +#define NTRIP_NS + +// NTRIP config NRW +#ifdef NTRIP_NRW #define NTRIP_HOST "80.158.61.104" #define NTRIP_PORT 2101 #define NTRIP_MOUNT_POINT "EPS_NW" #define NTRIP_USER "nw-916771" #define NTRIP_PASSWORD "Schalke#246" +#endif // NTRIP_NRW + +// NTRIP config NS +#ifdef NTRIP_NS +#define NTRIP_HOST "openservice-sapos.niedersachsen.de" +#define NTRIP_PORT 2101 +#define NTRIP_MOUNT_POINT "VRS_3_4G_NI" +#define NTRIP_USER "ni_FHDo01" +#define NTRIP_PASSWORD "ALdx-1-3e49" +#endif // NTRIP_NS //Network config ESP32 #ifdef ESP32BROKER @@ -48,13 +63,14 @@ #define MQTT_PORT 1883 #endif //ESP32BROKER -//Network config RHEDE +//Network config Hotspot #ifdef HOTSPOT #define WLAN_SSID "Kleiax Handy" #define WLAN_PASSWORD "12345677" #define WLAN_IP "192.168.11.4" #define WLAN_SUBNETMASK "255.255.128.0" #define WLAN_GATEWAY "192.168.0.1" +#define MQTT #define MQTT_SERVER "192.168.1.7" #define MQTT_PORT 1883 #define MQTT_AUTH @@ -62,12 +78,14 @@ #define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#" #endif //HOTSPOT +//Network config Rhede #ifdef RHEDE #define WLAN_SSID "LebennigHuus" #define WLAN_PASSWORD "Punica-699" #define WLAN_IP "192.168.11.4" #define WLAN_SUBNETMASK "255.255.128.0" #define WLAN_GATEWAY "192.168.0.1" +#define MQTT #define MQTT_SERVER "192.168.1.7" #define MQTT_PORT 1883 #define MQTT_AUTH @@ -75,6 +93,18 @@ #define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#" #endif //RHEDE +//Network config Doro +#ifdef DORO +#define WLAN_SSID "Tuedelkram" +#define WLAN_PASSWORD "3Kaesehoch!" +#define WLAN_IP "192.168.178.4" +#define WLAN_SUBNETMASK "255.255.255.0" +#define WLAN_GATEWAY "192.168.178.1" + // Only for defines reasons + #define MQTT_SERVER "172.22.64.216" + #define MQTT_PORT 1883 +#endif //DORO + //Network config HW1 #ifdef HW1 #define WLAN_SSID "hw1_gast" @@ -82,6 +112,7 @@ #define WLAN_IP "192.168.0.67" #define WLAN_SUBNETMASK "255.255.255.0" #define WLAN_GATEWAY "192.168.0.1" +#define MQTT #define MQTT_SERVER "172.22.64.216" #define MQTT_PORT 1883 #endif //HW1 @@ -92,6 +123,7 @@ #define WLAN_PASSWORD "2W2ZLPJ9NVQ3" #define WLAN_IP 0x0F01a8c0 //192.168.1.15 #define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0 +#define MQTT #define WLAN_GATEWAY 0xfe01a8c0 //192.168.1.254 #define MQTT_SERVER 0x0201a8c0 //192.168.1.2 #define MQTT_PORT 1883 @@ -104,6 +136,7 @@ #define WLAN_IP 0x0F01a8c0 //192.168.1.15 #define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0 #define WLAN_GATEWAY 0x0101a8c0 //192.168.1.1 +#define MQTT #define MQTT_SERVER 0x0201a8c0 //192.168.1.2 #define MQTT_PORT 1883 #endif //TPMOBIL diff --git a/lib/MQTT/debugMqtt.h b/lib/MQTT/debugMqtt.h index 672e9a6..a93f783 100644 --- a/lib/MQTT/debugMqtt.h +++ b/lib/MQTT/debugMqtt.h @@ -14,6 +14,7 @@ #include #include + /** * @brief * diff --git a/lib/Menu/menu.cpp b/lib/Menu/menu.cpp index c52b01f..d786b48 100644 --- a/lib/Menu/menu.cpp +++ b/lib/Menu/menu.cpp @@ -21,6 +21,10 @@ void Menu::addEntry(MenuAction* entry) { this->selectedEntry = this->entrys.begin(); } +bool Menu::isInSubmenu() { + return this->inSubmenu; +} + void Menu::printMenu() { if (this->lcd) { lcd->clear(); diff --git a/lib/Menu/menu.h b/lib/Menu/menu.h index e72825b..d9b6259 100644 --- a/lib/Menu/menu.h +++ b/lib/Menu/menu.h @@ -31,6 +31,7 @@ class Menu : public MenuControl { * @param entry */ void addEntry(MenuAction* entry); + bool isInSubmenu(); void printMenu() override; void update() override; diff --git a/lib/Menu/menuControl.h b/lib/Menu/menuControl.h index e57f325..91cce6f 100644 --- a/lib/Menu/menuControl.h +++ b/lib/Menu/menuControl.h @@ -32,20 +32,20 @@ class MenuControl { * class. */ ///@{ - virtual void down(); - virtual void up(); - virtual void right(); - virtual void left(); - virtual void yes(); - virtual void no(); + virtual void down() = 0; + virtual void up() = 0; + virtual void right() = 0; + virtual void left() = 0; + virtual void yes() = 0; + virtual void no() = 0; ///@} /** * @brief can be called to update shown data */ - virtual void update(); + virtual void update() {} - virtual void printMenu(); + virtual void printMenu() = 0; /** * @brief Set the Parent Menu @@ -56,6 +56,7 @@ class MenuControl { * @param menu */ void setParentMenu(MenuControl* menu) { this->parentMenu = menu; } + // MenuControl* getParentMenu() { return this->parentMenu; } /** * @brief Set the Lcd object diff --git a/lib/Menu/menuIntInput.cpp b/lib/Menu/menuIntInput.cpp new file mode 100644 index 0000000..2d0d96b --- /dev/null +++ b/lib/Menu/menuIntInput.cpp @@ -0,0 +1,97 @@ +/** + * @file menuIntInput.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-09-12 + * + * @copyright Copyright (c) 2022 + * + */ + +#include "menuIntInput.h" + +MenuIntInput::MenuIntInput(int16_t values[], char *names, uint8_t length, MenuIntInputWrapper* wrapper) { + this->values = values; + this->names = names; + this->length = length; + this->wrapper = wrapper; +} + +void MenuIntInput::down() { + if (values[currentPosition] > this->min) + values[currentPosition]--; + + this->printMenu(); +} + +void MenuIntInput::up() { + if (values[currentPosition] < this->max) + values[currentPosition]++; + + this->printMenu(); +} + +void MenuIntInput::right() { + if (currentPosition < this->length - 1) + currentPosition++; + else + currentPosition = 0; + + this->printMenu(); +} + +void MenuIntInput::left() { + if (currentPosition > 0) + currentPosition--; + else + currentPosition = this->length - 1; + + this->printMenu(); +} + +void MenuIntInput::no() { + if (parentMenu) + this->parentMenu->printMenu(); +} + +void MenuIntInput::yes() { + this->wrapper->action(this->values, this->length); + if (parentMenu) + this->parentMenu->printMenu(); +} + +void MenuIntInput::printMenu() { + // TODO: Name max 12 chars + + char bufferName[17]; + if (this->currentPosition == 0 && this->length == 1) { + // No arrow + sprintf(bufferName, " %s ", this->names[this->currentPosition]); + } else if (this->currentPosition > 0 && this->length - 1 == this->currentPosition) { + // Left arrow only + sprintf(bufferName, "< %s ", this->names[this->currentPosition]); + } else if (this->currentPosition == 0 && this->length > 1) { + // Right arrow only + sprintf(bufferName, " %s >", this->names[this->currentPosition]); + } else { + // Both arrow + sprintf(bufferName, "< %s >", this->names[this->currentPosition]); + } + + char bufferValue[17]; + sprintf(bufferValue, " -> %5.d", this->values[this->currentPosition]); + + + if (this->lcd) { + lcd->clear(); + lcd->setCursor(0, 0); + lcd->print(bufferName); + lcd->setCursor(0, 1); + lcd->print(bufferValue); + } + std::cout << bufferName << " : " << bufferValue << std::endl; +} + + + diff --git a/lib/Menu/menuIntInput.h b/lib/Menu/menuIntInput.h new file mode 100644 index 0000000..10e3ae7 --- /dev/null +++ b/lib/Menu/menuIntInput.h @@ -0,0 +1,83 @@ +/** + * @file menuIntInput.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-09-12 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef MENU_INT_INPUT_H +#define MENU_INT_INPUT_H + +#include "menuControl.h" +#include + +#define MAX_NAME_LENGTH 12 + +class MenuIntInputWrapper { + public: + virtual void action(int16_t* values, uint8_t length) = 0; +}; + +class MenuIntInput : public MenuControl { + public: + MenuIntInput(int16_t values[], char *names, uint8_t length, MenuIntInputWrapper* wrapper); + + void setMin(int16_t min = 0) { this->min = min; } + void setMax(int16_t max = 100) { this->max = max; } + + /** + * @brief Decrement selected value + */ + void down() override; + + /** + * @brief Increment selected value + */ + void up() override; + + /** + * @brief Select next value + */ + void right() override; + + /** + * @brief Select previous value + */ + void left() override; + + /** + * @brief Leave menu without saving + */ + void no() override; + + /** + * @brief Leave menu with saving + */ + void yes() override; + + /** + * @brief Prints the Information to display and console + * + * The informations are only printed to the display if it + * set. + */ + void printMenu() override; + + void update() override {}; + + private: + MenuIntInputWrapper* wrapper; + + uint8_t currentPosition = 0; + uint8_t length; + int16_t *values; + int16_t min = INT16_MIN; + int16_t max = INT16_MAX; + char *names; + +}; + +#endif // MENU_INT_INPUT_H diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp index 4fb9fd7..36be868 100644 --- a/lib/MotorControl/motorControl.cpp +++ b/lib/MotorControl/motorControl.cpp @@ -36,14 +36,14 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uin uint16_t MotorControl::loop() { uint32_t time = millis(); - uint16_t elapsed_time = time - this->last_millis; + uint16_t elapsed_time = time - this->lastMillis; //Cancel if delay is not reached if (elapsed_time < delay) return elapsed_time; runMotorControl(); - this->last_millis = time; + this->lastMillis = time; return elapsed_time; } @@ -199,4 +199,4 @@ void MotorControl::increasePower(int8_t power) { } this->setRealPower(this->power + power); -} \ No newline at end of file +} diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h index 7337b3f..3959af5 100644 --- a/lib/MotorControl/motorControl.h +++ b/lib/MotorControl/motorControl.h @@ -163,7 +163,7 @@ class MotorControl { uint8_t delay = DELAY; uint8_t powersteps = POWERSTEPS; - uint32_t last_millis = 0; + uint32_t lastMillis = 0; }; diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 8261678..a8664c0 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -20,11 +20,14 @@ Navigation::Navigation(Route* route) { uint8_t versionHigh = this->gps->getProtocolVersionHigh(); uint8_t versionLow = this->gps->getProtocolVersionLow(); - std::cout << "Neo-M8Q protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl; + std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl; + this->gps->setI2COutput(COM_TYPE_UBX); this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA); - + this->gps->setPortInput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3); + + this->gps->getNavigationFrequency(1); if (route) this->route = route; @@ -34,13 +37,70 @@ Navigation::Navigation(Route* route) { Navigation::~Navigation() { delete this->gps; + + if (this->isNtripInit) { + delete this->ntripClient; + + delete this->host; + delete this->mountPoint; + delete this->user; + delete this->password; + } + if (this->route) delete this->route; } +void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) { + this->host = new char[host.length() + 1]; + this->mountPoint = new char[mountPoint.length() + 1]; + this->user = new char[user.length() + 1]; + this->password = new char[password.length() + 1]; + + strcpy(this->host, host.c_str()); + strcpy(this->mountPoint, mountPoint.c_str()); + strcpy(this->user, user.c_str()); + strcpy(this->password, password.c_str()); + this->port = port; + + this->ntripClient = new NTRIPClient; + this->isNtripInit = true; + + std::cout << "Requesting SourceTable." << std::endl; + if(this->ntripClient->reqSrcTbl(this->host, (int) this->port)){ + char buffer[512]; + delay(5); + while(this->ntripClient->available()) { + this->ntripClient->readLine(buffer,sizeof(buffer)); + std::cout << buffer << std::endl; + } + } else { + std::cout << "SourceTable request error" << std::endl; + } + std::cout << "Requesting SourceTable is OK" << std::endl; + this->ntripClient->stop(); //Need to call "stop" function for next request. + + std::cout << "Requesting MountPoint's Raw data" << std::endl; + if(!this->ntripClient->reqRaw(this->host, this->port, this->mountPoint, + this->user, this->password)){ + delay(15000); + ESP.restart(); + } + std::cout << "Requesting MountPoint is OK" << std::endl; +} + void Navigation::loop() { //FIXME: Hier sollte funktion zum aktualisieren des GPS Moduls stehen - //this->gps->checkUbloxI2C(); + + if (millis() - this->lastMillis < this->timeToWait) + return; + + uint8_t rtcmData[512 * 4]; + uint16_t rtcmCount = this->ntripClient->available(); + if (rtcmCount) { + this->ntripClient->readBytes(rtcmData, rtcmCount); + this->gps->pushRawData(rtcmData, rtcmCount); + } } void Navigation::newRoute() { diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index d717a58..de78f26 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -19,6 +19,7 @@ #include "route.h" #include "navigationUtils.h" +#include "NTRIPClient.h" /** * @brief The minimal distance between Points @@ -64,6 +65,17 @@ class Navigation { */ ~Navigation(); + /** + * @brief + * + * @param host + * @param port + * @param mountPoint + * @param user + * @param password + */ + void initNtrip(String host, uint16_t port, String mountPoint, String user, String password); + /** * @brief Decodes new GPS information * @@ -169,12 +181,23 @@ class Navigation { SFE_UBLOX_GNSS* gps; Route* route = nullptr; + NTRIPClient* ntripClient = nullptr; Point lastPoint; Point targetPoint; bool navigationStarted = false; bool navigationFinished = false; + bool isNtripInit = false; + + char* host; + char* mountPoint; + char* user; + char* password; + + uint8_t timeToWait = 5; + uint16_t port; + uint32_t lastMillis = 0; }; #endif // NAVIGATION_H diff --git a/lib/Ntrip/NTRIPClient.cpp b/lib/Ntrip/NTRIPClient.cpp index 4342fd8..9ffd5da 100644 --- a/lib/Ntrip/NTRIPClient.cpp +++ b/lib/Ntrip/NTRIPClient.cpp @@ -1,37 +1,34 @@ #include"NTRIPClient.h" -bool NTRIPClient::reqSrcTbl(char* host,int &port) -{ - if(!connect(host,port)){ - Serial.print("Cannot connect to "); - Serial.println(host); - return false; - }/*p = String("GET ") + String("/") + String(" HTTP/1.0\r\n"); - p = p + String("User-Agent: NTRIP Enbeded\r\n");*/ - print( - "GET / HTTP/1.0\r\n" - "User-Agent: NTRIPClient for Arduino v1.0\r\n" - ); - unsigned long timeout = millis(); - while (available() == 0) { - if (millis() - timeout > 5000) { - Serial.println("Client Timeout !"); - stop(); - return false; - } - delay(10); - } - char buffer[50]; - readLine(buffer,sizeof(buffer)); - if(strncmp((char*)buffer,"SOURCETABLE 200 OK",17)) - { - Serial.print((char*)buffer); - return false; - } - return true; - +bool NTRIPClient::reqSrcTbl(char* host,uint16_t port){ + if(!connect(host,port)){ + Serial.print("Cannot connect to "); + Serial.println(host); + return false; + } /*p = String("GET ") + String("/") + String(" HTTP/1.0\r\n"); + p = p + String("User-Agent: NTRIP Enbeded\r\n");*/ + print( + "GET / HTTP/1.0\r\n" + "User-Agent: NTRIPClient for Arduino v1.0\r\n" + ); + unsigned long timeout = millis(); + while (available() == 0) { + if (millis() - timeout > 5000) { + Serial.println("Client Timeout !"); + stop(); + return false; + } + delay(10); + } + char buffer[50]; + readLine(buffer,sizeof(buffer)); + if(strncmp((char*)buffer,"SOURCETABLE 200 OK",17)) { + Serial.print((char*)buffer); + return false; + } + return true; } -bool NTRIPClient::reqRaw(char* host,int &port,char* mntpnt,char* user,char* psw) -{ + +bool NTRIPClient::reqRaw(char* host,uint16_t port,char* mntpnt,char* user,char* psw) { if(!connect(host,port))return false; String p="GET /"; String auth=""; @@ -79,12 +76,12 @@ bool NTRIPClient::reqRaw(char* host,int &port,char* mntpnt,char* user,char* psw) } return true; } -bool NTRIPClient::reqRaw(char* host,int &port,char* mntpnt) -{ + +bool NTRIPClient::reqRaw(char* host,uint16_t port,char* mntpnt) { return reqRaw(host,port,mntpnt,"",""); } -int NTRIPClient::readLine(char* _buffer,int size) -{ + +int NTRIPClient::readLine(char* _buffer,int size) { int len = 0; while(available()) { _buffer[len] = read(); diff --git a/lib/Ntrip/NTRIPClient.h b/lib/Ntrip/NTRIPClient.h index 99760dd..1ad92b4 100644 --- a/lib/Ntrip/NTRIPClient.h +++ b/lib/Ntrip/NTRIPClient.h @@ -7,9 +7,9 @@ class NTRIPClient : public WiFiClient{ public : - bool reqSrcTbl(char* host,int &port); //request MountPoints List serviced the NTRIP Caster - bool reqRaw(char* host,int &port,char* mntpnt,char* user,char* psw); //request RAW data from Caster - bool reqRaw(char* host,int &port,char* mntpnt); //non user + bool reqSrcTbl(char* host,uint16_t port); //request MountPoints List serviced the NTRIP Caster + bool reqRaw(char* host,uint16_t port,char* mntpnt,char* user,char* psw); //request RAW data from Caster + bool reqRaw(char* host,uint16_t port,char* mntpnt); //non user int readLine(char* buffer,int size); diff --git a/lib/Rtcm/rtcmInsert.cpp b/lib/Rtcm/rtcmInsert.cpp deleted file mode 100644 index 37a05bd..0000000 --- a/lib/Rtcm/rtcmInsert.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/** - * @file rtcmInsert.cpp - * @author * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2022-03-24 - * - * @copyright Copyright (c) 2022 - * - */ - -#include "rtcmInsert.h" - - -RtcmInsert::RtcmInsert(uint8_t address, char* ntripHost, uint16_t ntripPort, char* ntripMountPoint, char* ntripUser, char* ntripPassword) - : address() { - this->ntrip_c = new NTRIPClient(); - - std::cout << "Requesting SourceTable." << std::endl; - if(this->ntrip_c->reqSrcTbl(ntripHost, ntripPort)) { - char buffer[512]; - delay(5); - while(this->ntrip_c->available()) { - this->ntrip_c->readLine(buffer,sizeof(buffer)); - std::cout << buffer; - } - } - else - std::cout << "SourceTable request error" << std::endl; - - std::cout << "Requesting SourceTable is OK." << std::endl; - this->ntrip_c->stop(); //Need to call "stop" function for next request. - - std::cout << "Requesting MountPoint's Raw data" << std::endl; - if(!this->ntrip_c->reqRaw(ntripHost, ntripPort, ntripMountPoint, ntripUser, ntripPassword)) - std::cout << "Error Requesting MountPoint's Raw data" << std::endl; - else - std::cout << "Requesting MountPoint is OK" << std::endl; -} - -void RtcmInsert::loop() { - if (this->ntrip_c->available()) { - Wire.beginTransmission(this->address); - while(this->ntrip_c->available()) { - char ch = this->ntrip_c->read(); - Wire.write(ch); - } - Wire.endTransmission(); - } -} diff --git a/lib/Rtcm/rtcmInsert.h b/lib/Rtcm/rtcmInsert.h deleted file mode 100644 index c3072f1..0000000 --- a/lib/Rtcm/rtcmInsert.h +++ /dev/null @@ -1,31 +0,0 @@ -/** - * @file rtcmInsert.h - * @author Alexander Klein (alex@kleiax.de) - * @brief - * @version 0.1 - * @date 2022-03-24 - * - * @copyright Copyright (c) 2022 - * - */ - -#ifndef RTCM_INSERT_H -#define RTCM_INSERT_H - -#include - -#include "NTRIPClient.h" - -class RtcmInsert { - public: - RtcmInsert(uint8_t address, char* ntripHost, uint16_t ntripPort, char* ntripMountPoint, char* ntripUser, char* ntripPassword); - - void loop(); - - private: - uint8_t address; - - NTRIPClient* ntrip_c; -}; - -#endif // RTCM_INSERT_H diff --git a/src/SpecialMenus/Akku/menuAkku.cpp b/src/SpecialMenus/Akku/menuAkku.cpp index 35078ac..527669e 100644 --- a/src/SpecialMenus/Akku/menuAkku.cpp +++ b/src/SpecialMenus/Akku/menuAkku.cpp @@ -61,9 +61,9 @@ void MenuAkku::printMenu() { } void MenuAkku::update() { - if (millis() - this->last_millis < this->delay) { + if (millis() - this->lastMillis < this->delay) { return; } this->printMenu(); - this->last_millis = millis(); + this->lastMillis = millis(); } diff --git a/src/SpecialMenus/Akku/menuAkku.h b/src/SpecialMenus/Akku/menuAkku.h index 635a803..b1d8d68 100644 --- a/src/SpecialMenus/Akku/menuAkku.h +++ b/src/SpecialMenus/Akku/menuAkku.h @@ -55,7 +55,7 @@ class MenuAkku : public MenuControl { private: const uint16_t delay = 15000; - uint32_t last_millis = 0; + uint32_t lastMillis = 0; Battery* mainBattery; diff --git a/src/SpecialMenus/GPS/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp index 340e3ec..4071d5d 100644 --- a/src/SpecialMenus/GPS/menuGPS.cpp +++ b/src/SpecialMenus/GPS/menuGPS.cpp @@ -13,7 +13,7 @@ MenuGPS::MenuGPS(SFE_UBLOX_GNSS* gps) { this->gps = gps; - this->last_millis = millis(); + this->lastMillis = millis(); } void MenuGPS::up() { @@ -54,76 +54,86 @@ void MenuGPS::printMenu() { } void MenuGPS::update() { - if (millis() - this->last_millis < this->delay) { + if (millis() - this->lastMillis < this->delay) { return; } this->printMenu(); - this->last_millis = millis(); + this->lastMillis = millis(); } void MenuGPS::printPage(uint8_t page) const { - //FIXME: new gpslib - // switch (this->currentPage) { - // case 0: - // if (this->lcd) { - // lcd->clear(); - // lcd->setCursor(0, 0); - // if (gps->satellites.isValid() && gps->satellites.value() > 2) { - // lcd->printf("Sats: %3.d", gps->satellites.value()); - // lcd->setCursor(0, 1); - // lcd->printf("HDOP: %f", gps->hdop.hdop()); - // } else { - // lcd->printf("Data isn't valid"); - // lcd->setCursor(0, 1); - // lcd->printf("or no GPS signal"); - // } - // } - // if (gps->satellites.isValid() && gps->satellites.value() > 2) - // std::cout << "Sats: " << gps->satellites.value() - // << "HDOP: " << gps->hdop.hdop() << std::endl; - // else - // std::cout << "Data isn't valid or no GPS signal" << std::endl; - // break; + uint8_t fixType = gps->getFixType(); + + switch (this->currentPage) { + case 0: { + uint8_t siv = gps->getSIV(); + uint16_t hdop = gps->getHorizontalDOP(); + if (this->lcd) { + lcd->clear(); + lcd->setCursor(0, 0); + if (fixType) { + lcd->printf("Sats: %3.u", siv); + lcd->setCursor(0, 1); + lcd->printf("HDOP: %u", hdop); + } else { + lcd->printf("Data isn't valid"); + lcd->setCursor(0, 1); + lcd->printf("or no GPS signal"); + } + } + if (fixType) + std::cout << "Sats: " << (int) siv + << "HDOP: " << (int) hdop << std::endl; + else + std::cout << "Data isn't valid or no GPS signal" << std::endl; + } + break; - // case 1: - // if (this->lcd) { - // lcd->clear(); - // lcd->setCursor(0, 0); - // if (gps->location.isValid() && gps->satellites.value() > 2) { - // lcd->printf("Lat: %9.6f", gps->location.lat()); - // lcd->setCursor(0, 1); - // lcd->printf("Lng: %9.6f", gps->location.lng()); - // } else { - // lcd->printf("Data isn't valid"); - // lcd->setCursor(0, 1); - // lcd->printf("or NO GPS signal"); - // } - // } - // if (gps->satellites.isValid() && gps->satellites.value() > 2) - // std::cout << "Lat: " << gps->location.lat() - // << "Lng: " << gps->location.lng() << std::endl; - // else - // std::cout << "Data isn't valid or NO GPS signal" << std::endl; - // break; + case 1: { + int32_t lat = gps->getLatitude(); + int32_t lng = gps->getLongitude(); + if (this->lcd) { + lcd->clear(); + lcd->setCursor(0, 0); + if (fixType) { + lcd->printf("Lat: %d", lat); + lcd->setCursor(0, 1); + lcd->printf("Lng: %d", lng); + } else { + lcd->printf("Data isn't valid"); + lcd->setCursor(0, 1); + lcd->printf("or NO GPS signal"); + } + } + if (fixType) + std::cout << "Lat: " << lat + << "Lng: " << lng << std::endl; + else + std::cout << "Data isn't valid or NO GPS signal" << std::endl; + } + break; - // case 2: - // if (this->lcd) { - // lcd->clear(); - // lcd->setCursor(0, 0); - // lcd->printf("Chars processed"); - // lcd->setCursor(0, 1); - // lcd->printf("-> %u", gps->charsProcessed()); - // } - // std::cout << "Chars processed: " << gps->charsProcessed() << std::endl; - // break; + case 2: { + uint32_t unixEpoch = gps->getUnixEpoch(); + + if (this->lcd) { + lcd->clear(); + lcd->setCursor(0, 0); + lcd->printf("Unix Epoch"); + lcd->setCursor(0, 1); + lcd->printf("-> %u", unixEpoch); + } + std::cout << "Unix Epoch: " << (int) unixEpoch << std::endl; + } + break; - // default: - // if (this->lcd) { - // lcd->clear(); - // lcd->setCursor(0, 0); - // lcd->printf("Page: %u", page); - // } - // std::cout << "Page: " << (int) page << std::endl; - // break; - // } + default: + if (this->lcd) { + lcd->clear(); + lcd->setCursor(0, 0); + lcd->printf("Page: %u", page); + } + std::cout << "Page: " << (int) page << std::endl; + break; + } } diff --git a/src/SpecialMenus/GPS/menuGPS.h b/src/SpecialMenus/GPS/menuGPS.h index 8571835..fc510b9 100644 --- a/src/SpecialMenus/GPS/menuGPS.h +++ b/src/SpecialMenus/GPS/menuGPS.h @@ -82,7 +82,7 @@ class MenuGPS : public MenuControl { SFE_UBLOX_GNSS* gps; const uint16_t delay = 5000; - uint32_t last_millis = 0; + uint32_t lastMillis = 0; }; #endif // MENU_GPS_H diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index b97af6f..4893ecf 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -74,9 +74,9 @@ void MenuAutopilot::printMenu() { } void MenuAutopilot::update() { - if (millis() - this->last_millis < this->minDelay) + if (millis() - this->lastMillis < this->minDelay) return; - this->last_millis = millis(); + this->lastMillis = millis(); if (!this->autopilot->shouldUpdate()) return; diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h index 40a90fd..b06c788 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h @@ -49,7 +49,7 @@ class MenuAutopilot : public MenuDriveMode { Autopilot* autopilot; RouteInfo routeInfo; - uint32_t last_millis = 0; + uint32_t lastMillis = 0; uint16_t minDelay = 1500; }; diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp new file mode 100644 index 0000000..9f35fde --- /dev/null +++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp @@ -0,0 +1,157 @@ +/** + * @file menuTestMode.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-09-08 + * + * @copyright Copyright (c) 2022 + * + */ +#include "menuTestMode.h" + +MenuTestMode::MenuTestMode(DriveManager* driveManager) + : MenuDriveMode(driveManager) { + // this->testMode = testMode; + this->values[0] = 1; + this->values[1] = 2; + this->values[2] = 3; + + strcpy(this->names[0], "Alpha"); + strcpy(this->names[1], "Beta"); + strcpy(this->names[2], "Gamma"); + } + +MenuTestMode::~MenuTestMode() { + if (isInit) { + delete this->mainMenu; + } +} + +void MenuTestMode::printMenu() { + if (!this->isInit) { + this->isInit = true; + this->init(); + } + + this->mainMenu->printMenu(); +} + +void MenuTestMode::down() { + this->mainMenu->down(); +} + +void MenuTestMode::up() { + this->mainMenu->up(); +} + +void MenuTestMode::right() { + this->mainMenu->right(); +} + +void MenuTestMode::left() { + if (this->mainMenu->isInSubmenu()) + this->mainMenu->left(); + else + this->parentMenu->printMenu(); +} + +void MenuTestMode::yes() { + this->mainMenu->yes(); +} + +void MenuTestMode::no() { + if (this->mainMenu->isInSubmenu()) + this->mainMenu->no(); + else + this->left(); +} + +void MenuTestMode::init() { + this->testMode = (TestMode*) this->driveManager->getDriveModiPtr(); + + auto dummy = []() { + std::cout << "Dummy in Action" <mainMenu = new Menu; + Menu* engineMenu = new Menu; + Menu* engineLeftMenu = new Menu; + Menu* engineRightMenu = new Menu; + Menu* drivingMenu = new Menu; + Menu* drivingForwardMenu = new Menu; + Menu* drivingBackwardMenu = new Menu; + Menu* lightMenu = new Menu; + + MenuIntInput* testInputMenu = new MenuIntInput(values, (char *)names, length, new MenuTestModeWrapper(this->testMode)); + + // Set menus on LCD + this->mainMenu->setLcd(this->lcd); + engineMenu->setLcd(this->lcd); + engineLeftMenu->setLcd(this->lcd); + engineRightMenu->setLcd(this->lcd); + drivingMenu->setLcd(this->lcd); + drivingForwardMenu->setLcd(this->lcd); + drivingBackwardMenu->setLcd(this->lcd); + lightMenu->setLcd(this->lcd); + testInputMenu->setLcd(this->lcd); + + // Entrys for the menus + MenuAction* engineAction = new MenuAction("Engine", engineMenu); + MenuAction* engineLeftAction = new MenuAction("Left", engineLeftMenu); + MenuAction* engineRightAction = new MenuAction("Right", engineRightMenu); + + MenuAction* drivingAction = new MenuAction("Driving", drivingMenu); + MenuAction* drivingForwardAction = new MenuAction("Forward", drivingForwardMenu); + MenuAction* drivingForwardLeftAction = new MenuAction("Left", testInputMenu); + MenuAction* drivingForwardStraightAction = new MenuAction("Straight", dummy); + MenuAction* drivingForwardRightAction = new MenuAction("Right", dummy); + MenuAction* drivingBackwardAction = new MenuAction("Backward", drivingBackwardMenu); + MenuAction* drivingBackwardLeftAction = new MenuAction("Left", dummy); + MenuAction* drivingBackwardStraightAction = new MenuAction("Straight", dummy); + MenuAction* drivingBackwardRightAction = new MenuAction("Right", dummy); + + MenuAction* lightAction = new MenuAction("Light", lightMenu); + MenuAction* lightFlashAction = new MenuAction("Flash", dummy); + MenuAction* lightFadeAction = new MenuAction("Fade", dummy); + MenuAction* lightRedAction = new MenuAction("Red", dummy); + MenuAction* lightGreenAction = new MenuAction("Green", dummy); + MenuAction* lightBlueAction = new MenuAction("Blue", dummy); + MenuAction* lightOffAction = new MenuAction("Off", dummy); + + // Add entrys to the menus + this->mainMenu->addEntry(engineAction); + this->mainMenu->addEntry(drivingAction); + this->mainMenu->addEntry(lightAction); + + engineMenu->addEntry(engineLeftAction); + engineMenu->addEntry(engineRightAction); + + drivingMenu->addEntry(drivingForwardAction); + drivingMenu->addEntry(drivingBackwardAction); + drivingForwardMenu->addEntry(drivingForwardLeftAction); + drivingForwardMenu->addEntry(drivingForwardStraightAction); + drivingForwardMenu->addEntry(drivingForwardRightAction); + drivingBackwardMenu->addEntry(drivingBackwardLeftAction); + drivingBackwardMenu->addEntry(drivingBackwardStraightAction); + drivingBackwardMenu->addEntry(drivingBackwardRightAction); + + lightMenu->addEntry(lightFlashAction); + lightMenu->addEntry(lightFadeAction); + lightMenu->addEntry(lightRedAction); + lightMenu->addEntry(lightGreenAction); + lightMenu->addEntry(lightBlueAction); + lightMenu->addEntry(lightOffAction); +} + +MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode) { + this->testMode = testMode; +} + +void MenuTestModeWrapper::action(int16_t* values, uint8_t length) { + if (length > 2) + std::cout << "Dummy in Action" << values[0] << values[1] << values[2] << std::endl; + else + std::cout << "Dummy in Action kene dre values" << std::endl; +} diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h new file mode 100644 index 0000000..b0580b1 --- /dev/null +++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h @@ -0,0 +1,56 @@ +/** + * @file menuTestMode.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-09-08 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef MENU_TEST_MODE +#define MENU_TEST_MODE + +#include "SpecialMenus/driveModi/menuDriveMode.h" +#include "driveModi/Modi/TestMode/testMode.h" +#include "menu.h" +#include "menuAction.h" +#include "menuIntInput.h" + +class MenuTestMode : public MenuDriveMode { + public: + MenuTestMode(DriveManager *driveManager); + ~MenuTestMode(); + void printMenu(); + + void down() override; + void up() override; + void right() override; + void left() override; + void yes() override; + void no() override; + + private: + void init(); + + TestMode* testMode; + Menu* mainMenu; + + bool isInit = false; + + uint8_t length = 3; + int16_t values[3]; + char names[3][MAX_NAME_LENGTH]; +}; + +class MenuTestModeWrapper : public MenuIntInputWrapper { + public: + MenuTestModeWrapper(TestMode* testMode); + + void action(int16_t* values, uint8_t length) override; + + private: + TestMode* testMode; +}; + +#endif // MENU_TEST_MODE diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 2e9e1be..d66467c 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -25,11 +25,11 @@ void Autopilot::loop() { if (!this->selfDriving && this->navigationStarted) ManualControl::loop(); - if (millis() - this->last_millis < delay) + if (millis() - this->lastMillis < delay) return; this->runAutopilot(); - this->last_millis = millis(); + this->lastMillis = millis(); } void Autopilot::runAutopilot() { @@ -76,7 +76,7 @@ void Autopilot::setSelfDriving(bool val) { this->selfDriving = val; this->moveControl->setSpeed(0); - this->moveControl->setRotationspeed(0); + this->moveControl->setRotationSpeed(0); } void Autopilot::setSpeedInRelToDistance(double distance) { @@ -97,7 +97,7 @@ void Autopilot::setRotInRelToDistance(int16_t course) { steps = 1; if (course == 0) - this->moveControl->setRotationspeed(0); + this->moveControl->setRotationSpeed(0); else - this->moveControl->setRotationspeed(steps); + this->moveControl->setRotationSpeed(steps); } diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index e7e91fe..1593619 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -127,7 +127,7 @@ class Autopilot : public ManualControl { bool selfDrivingAvailable = false; bool updateDisplay = false; - uint16_t last_millis = 0; + uint16_t lastMillis = 0; uint8_t delay = 40; }; diff --git a/src/driveModi/Modi/Autopilot/autopilotConfig.h b/src/driveModi/Modi/Autopilot/autopilotConfig.h index 0bdd382..626f14a 100644 --- a/src/driveModi/Modi/Autopilot/autopilotConfig.h +++ b/src/driveModi/Modi/Autopilot/autopilotConfig.h @@ -47,7 +47,7 @@ /** * @brief Course correction factor * This value is used by the setRotInRelToDistance function and is used to calculate - * the value for setRotationspeed. + * the value for setRotationSpeed. */ #define COURSE_CORRECTION_FACTOR 30 diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp index 4fd990f..d3361b9 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp @@ -20,11 +20,11 @@ CaptureRoute::CaptureRoute(MoveControl* moveControl, Navigation* navigation) void CaptureRoute::loop() { ManualControl::loop(); - if (millis() - this->last_millis < delay) { + if (millis() - this->lastMillis < delay) { return; } this->runCaptureRoute(); - this->last_millis = millis(); + this->lastMillis = millis(); } void CaptureRoute::runCaptureRoute() { diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h index 0c37209..344b962 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.h +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h @@ -75,7 +75,7 @@ class CaptureRoute : public ManualControl { Navigation* navigation; RouteInfo routeInfo; - uint32_t last_millis = 0; + uint32_t lastMillis = 0; uint16_t delay = 200; bool updateDisplay = false; diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index 767d927..9ed622d 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -17,16 +17,16 @@ ManualControl::ManualControl(MoveControl *moveControl) { ManualControl::~ManualControl() { this->moveControl->setSpeed(0); - this->moveControl->setRotationspeed(0); + this->moveControl->setRotationSpeed(0); this->moveControl->setDrivingStatus(DrivingStatus::stop); } void ManualControl::loop() { - if (millis() - this->last_millis < delay) { + if (millis() - this->lastMillis < delay) { return; } this->runManualControl(); - this->last_millis = millis(); + this->lastMillis = millis(); } void ManualControl::runManualControl() { @@ -37,5 +37,5 @@ void ManualControl::runManualControl() { this->moveControl->setSpeed((y * -1) * value_per_step); value_per_step = this->max_rotation * 2 / 256; - this->moveControl->setRotationspeed(-x * value_per_step); -} \ No newline at end of file + this->moveControl->setRotationSpeed(-x * value_per_step); +} diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h index 95114a3..8448eb7 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.h +++ b/src/driveModi/Modi/ManualControl/manualControl.h @@ -22,7 +22,7 @@ * * @see MoveControl */ -class ManualControl : public DriveModi{ +class ManualControl : public DriveModi { public: ManualControl(MoveControl *moveControl); @@ -40,7 +40,7 @@ class ManualControl : public DriveModi{ * @see runSpeedometer() * @see setDelay() */ - void loop(); + void loop() override; /** * @brief Noramly called repeatedly by loop() to calcluate new values. @@ -73,7 +73,7 @@ class ManualControl : public DriveModi{ MoveControl *moveControl; private: - uint32_t last_millis = 0; + uint32_t lastMillis = 0; uint8_t delay = 10; double max_speed = 1; double max_rotation = 7; diff --git a/src/driveModi/Modi/Racing/racing.cpp b/src/driveModi/Modi/Racing/racing.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/driveModi/Modi/Racing/racing.h b/src/driveModi/Modi/Racing/racing.h new file mode 100644 index 0000000..e69de29 diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index e69de29..198c572 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/src/driveModi/Modi/TestMode/testMode.cpp @@ -0,0 +1,124 @@ +/** + * @file testMode.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-09-08 + * + * @copyright Copyright (c) 2022 + * + */ +#include "testMode.h" + +TestMode::TestMode(MoveControl *moveControl) { + this->moveControl = moveControl; + this->moveControl->setDrivingStatus(DrivingStatus::drive); +} + +TestMode::~TestMode() { + this->moveControl->setSpeed(0); + this->moveControl->setRotationSpeed(0); + this->moveControl->setDrivingStatus(DrivingStatus::stop); +} + +void TestMode::loop() { + if (millis() - this->lastMillis < this->delay) + return; + + if (millis() - this->actionStart > this->maneuverTime) { + this->busy = false; + this->moveControl->setSpeed(0); + this->moveControl->setRotationSpeed(0); + } +} + +void TestMode::setSpeed(double speed) { + this->speed = speed; +} + +void TestMode::setRotationSpeed(double speed) { + this->rotationSpeed = speed; +} + +bool TestMode::driveForward(uint16_t cm) { + if (this->busy) + return false; + + this->moveControl->setRotationSpeed(0); + this->moveControl->setSpeed(this->speed); + + this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000; + this->actionStart = millis(); + + this->busy = true; + return true; +} + +bool TestMode::driveBackward(uint16_t cm) { + if (this->busy) + return false; + + this->moveControl->setRotationSpeed(0); + this->moveControl->setSpeed(-this->speed); + + this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000; + this->actionStart = millis(); + + this->busy = true; + return true; +} + +bool TestMode::turnLeft(uint16_t degree) { + if (this->busy) + return false; + + this->moveControl->setSpeed(0); + this->moveControl->setRotationSpeed(this->rotationSpeed); + + //TODO: calc this shit + this->maneuverTime = 10; + this->actionStart = millis(); + + this->busy = true; + return true; +} + +bool TestMode::turnRight(uint16_t degree) { + if (this->busy) + return false; + + this->moveControl->setSpeed(0); + this->moveControl->setRotationSpeed(this->rotationSpeed); + + //TODO: calc this shit + this->maneuverTime = 10; + this->actionStart = millis(); + + this->busy = true; + return true; +} + +bool TestMode::driveForwardLeft(uint16_t cm, uint16_t degree) { + if (this->busy) + return false; + return false; +} + +bool TestMode::driveForwardRight(uint16_t cm, uint16_t degree) { + if (this->busy) + return false; + return false; +} + +bool TestMode::driveBackwardLeft(uint16_t cm, uint16_t degree) { + if (this->busy) + return false; + return false; +} + +bool TestMode::driveBackwardRight(uint16_t cm, uint16_t degree) { + if (this->busy) + return false; + return false; +} + diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h index e69de29..12d3d40 100644 --- a/src/driveModi/Modi/TestMode/testMode.h +++ b/src/driveModi/Modi/TestMode/testMode.h @@ -0,0 +1,54 @@ +/** + * @file testMode.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-09-08 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef TEST_MODE_H +#define TEST_MODE_H + +#include "moveControl.h" +#include "driveModi/driveModi.h" + +class TestMode : public DriveModi { + public: + TestMode(MoveControl *MoveControl); + ~TestMode(); + + void loop() override; + + void setSpeed(double speed); + void setRotationSpeed(double speed); + + bool driveForward(uint16_t cm); + bool driveBackward(uint16_t cm); + + bool turnLeft(uint16_t degree); + bool turnRight(uint16_t degree); + + bool driveForwardLeft(uint16_t cm, uint16_t degree); + bool driveForwardRight(uint16_t cm, uint16_t degree); + bool driveBackwardLeft(uint16_t cm, uint16_t degree); + bool driveBackwardRight(uint16_t cm, uint16_t degree); + + private: + MoveControl *moveControl; + + double speed = 0; + double rotationSpeed = 0; + + bool busy = false; + + uint8_t delay = 10; + + uint32_t maneuverTime = 0; + uint32_t lastMillis = 0; + uint32_t actionStart = 0; + +}; + +#endif // TEST_MODE_H diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index 259a8b2..afb78f4 100644 --- a/src/driveModi/driveManager.cpp +++ b/src/driveModi/driveManager.cpp @@ -1,7 +1,7 @@ /** * @file driveManager.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief Implemention of the class DriveManager + * @brief Implementation of the class DriveManager * @version 0.1 * @date 2021-12-14 * @@ -15,9 +15,11 @@ Modi& operator++(Modi& m, int) { return m = (m == Modi::TestMode) ? Modi::Off : static_cast(static_cast(m)+1); } -DriveManager::DriveManager(MoveControl *moveControl) { +DriveManager::DriveManager(MoveControl *moveControl, bool wifi) { this->moveControl = moveControl; this->navigation = new Navigation(); + if (wifi) + this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD); } DriveManager::~DriveManager() { @@ -42,7 +44,7 @@ void DriveManager::changeModus(Modi modus) { delete this->currentModusPtr; this->moveControl->setSpeed(0); - this->moveControl->setRotationspeed(0); + this->moveControl->setRotationSpeed(0); this->moveControl->setDrivingStatus(DrivingStatus::stop); switch (modus) { @@ -69,8 +71,9 @@ void DriveManager::changeModus(Modi modus) { this->currentModusPtr = nullptr; break; - case Modi::TestMode: - this->currentModusPtr = nullptr; + case Modi::TestMode: { + this->currentModusPtr = new TestMode(this->moveControl); + } break; default: diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h index dfe94f9..76a5228 100644 --- a/src/driveModi/driveManager.h +++ b/src/driveModi/driveManager.h @@ -22,6 +22,7 @@ #include "moveControl.h" #include "driveModi/driveModi.h" #include "navigation.h" +#include "networkConfig.h" // All Drive Modi #include "driveModi/Modi/ManualControl/manualControl.h" @@ -52,7 +53,7 @@ class DriveManager { * * @param moveControl */ - DriveManager(MoveControl *moveControl); + DriveManager(MoveControl *moveControl, bool wifi = false); /** * @brief Destroy the Drive Manager object diff --git a/src/main.cpp b/src/main.cpp index b62776c..e6ca5aa 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -34,12 +34,13 @@ #include "SpecialMenus/driveModi/ManualDrive/menuManualDrive.h" #include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h" #include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h" +#include "SpecialMenus/driveModi/TestMode/menuTestMode.h" #include "SpecialMenus/PID/menuPidSettings.h" #include "SpecialMenus/GPS/menuGPS.h" #include "SpecialMenus/Akku/menuAkku.h" MoveControl moveController; -DriveManager driveManager(&moveController); +DriveManager* driveManager; Menu* main_m; LiquidCrystal_I2C* lcd; OutputBuf* outputBuf; @@ -63,6 +64,7 @@ void setup() { mainBattery = new Battery(35, 692000, 218500); Wire.begin(21, 19); + Wire.setClock(400000); i2cScanner(); lcd = new LiquidCrystal_I2C(0x3F,16,2); lcd->init(); @@ -73,14 +75,19 @@ void setup() { Network::setupMQTT(); wifiIsActive = Network::connectWifi(); if (wifiIsActive) { - Network::checkMQTT(); - DebugMqtt::init(Network::getMqttClient(), Loglevel::debug); - std::cout << "Activate additional output via MQTT..." << std::endl; - debugMqtt = new DebugMqtt("Console"); - outputBuf = new OutputBuf(debugMqtt); + driveManager = new DriveManager(&moveController, true); + if (Network::checkMQTT()) { + DebugMqtt::init(Network::getMqttClient(), Loglevel::debug); + std::cout << "Activate additional output via MQTT..." << std::endl; + debugMqtt = new DebugMqtt("Console"); + outputBuf = new OutputBuf(debugMqtt); + } else + outputBuf = new OutputBuf(); wifiIndicator = '-'; - } else + } else { + driveManager = new DriveManager(&moveController); outputBuf = new OutputBuf(); + } std::cout.rdbuf(outputBuf); std::cout << "Welcome to Kleiax-Rover" << std::endl; @@ -114,7 +121,7 @@ void loop() { Network::checkMQTT(); } - driveManager.loop(); + driveManager->loop(); mainBattery->loop(); // static uint32_t lastMillis = 0; @@ -228,10 +235,11 @@ void makeMenu() { Menu* pid_m = new Menu(); 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); - MenuGPS* gps_m = new MenuGPS(driveManager.getNavigation()->getGPS()); + MenuManualControl* man_m = new MenuManualControl(driveManager); + MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager); + MenuAutopilot* auto_m = new MenuAutopilot(driveManager); + MenuTestMode* test_m = new MenuTestMode(driveManager); + MenuGPS* gps_m = new MenuGPS(driveManager->getNavigation()->getGPS()); MenuAkku* akku_m = new MenuAkku(mainBattery); // Set Menus on LCD @@ -243,6 +251,7 @@ void makeMenu() { man_m->setLcd(lcd); cap_m->setLcd(lcd); auto_m->setLcd(lcd); + test_m->setLcd(lcd); gps_m->setLcd(lcd); akku_m->setLcd(lcd); @@ -252,7 +261,7 @@ void makeMenu() { MenuAction* gps_e = new MenuAction("GPS", gps_m); MenuAction* pid_e = new MenuAction("PID", pid_m); MenuAction* restart_e = new MenuAction("Restart", restart); - MenuAction* akku_e = new MenuAction("Akku", akku_m); + MenuAction* akku_e = new MenuAction("Battery", akku_m); MenuAction* contr_e = new MenuAction("Remove PS3", disconnectController); main_m->addEntry(mode_e); main_m->addEntry(gps_e); @@ -266,7 +275,7 @@ void makeMenu() { MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m); MenuAction* consolControl_e = new MenuAction("Consol Control", dummy); MenuAction* manualControl_e = new MenuAction("Manual Control", man_m); - MenuAction* testMode_e = new MenuAction("Test Mode", dummy); + MenuAction* testMode_e = new MenuAction("Test Mode", test_m); mode_m->addEntry(autopilot_e); mode_m->addEntry(captureRoute_e); mode_m->addEntry(manualControl_e); diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 74c5dcb..356253c 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -62,12 +62,12 @@ void MoveControl::loop() { std::cout << buf; } - static uint64_t last_millis = 0; - if (millis() - last_millis < delay) + static uint64_t lastMillis = 0; + if (millis() - lastMillis < delay) return; this->runMoveControl(); - last_millis = millis(); + lastMillis = millis(); } void MoveControl::runMoveControl() { @@ -104,7 +104,7 @@ void MoveControl::setSpeed(double speed) { this->x_speed = speed; } -void MoveControl::setRotationspeed(double speed) { +void MoveControl::setRotationSpeed(double speed) { if (speed < 0.7 && speed > -0.7) this->rotation_speed = 0; else diff --git a/src/network.cpp b/src/network.cpp index c81cb25..7520df7 100644 --- a/src/network.cpp +++ b/src/network.cpp @@ -15,6 +15,7 @@ IPAddress Network::local_IP; IPAddress Network::gateway; IPAddress Network::subnet; IPAddress Network::mqtt_server; +IPAddress Network::dnsServer; WiFiClient Network::wifi_client; PubSubClient* Network::mqtt_client; @@ -24,17 +25,20 @@ void Network::setIps() { gateway.fromString(WLAN_GATEWAY); subnet.fromString(WLAN_SUBNETMASK); mqtt_server.fromString(MQTT_SERVER); + dnsServer.fromString(WLAN_DNS_SERVER); } void Network::setupMQTT() { Network::mqtt_client = new PubSubClient(wifi_client); - // mqtt_client->setServer(mqtt_server, MQTT_PORT); Network::mqtt_client->setServer(MQTT_SERVER, MQTT_PORT); Network::mqtt_client->setSocketTimeout(1); - // Network::connectMQTT(); } bool Network::connectMQTT() { + #ifndef MQTT + return false; + #endif + // Create a random client ID String clientId = "ESP32Rover-"; clientId += String(random(0xffff), HEX); @@ -54,7 +58,7 @@ bool Network::connectMQTT() { } bool Network::connectWifi() { // Configures static IP address - if (!WiFi.config(local_IP, gateway, subnet)) + if (!WiFi.config(local_IP, gateway, subnet, dnsServer)) std::cout << "STA Failed to configure" << std::endl; // Connect to Wi-Fi network with SSID and password @@ -80,18 +84,20 @@ bool Network::connectWifi() { return true; } -void Network::checkMQTT() { +bool Network::checkMQTT() { static uint64_t lastReconnectAttempt = 0; if (!mqtt_client->connected()) { long now = millis(); if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) { lastReconnectAttempt = now; // Attempt to reconnect - if (connectMQTT()) + if (connectMQTT()) lastReconnectAttempt = 0; } - } else + return false; + } mqtt_client->loop(); + return true; } void Network::checkWiFi() {