ntrip, testMode, zed9 gnss modul

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
 

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
+DirL1	27				17		GPS RX deprecated
-DirR2	14				16		GPS TX
+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
+	10	9

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

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;

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 

lib/MQTT/debugMqtt.h

@@ -14,6 +14,7 @@
 #include <iostream>
 #include <PubSubClient.h>
 
+
 /**
  * @brief 
  * 

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();

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;

lib/Menu/menuControl.h

@@ -32,20 +32,20 @@ class MenuControl {
          * class.
          */
         ///@{
-        virtual void down();
+        virtual void down() = 0;
-        virtual void up();
+        virtual void up() = 0;
-        virtual void right();
+        virtual void right() = 0;
-        virtual void left();
+        virtual void left() = 0;
-        virtual void yes();
+        virtual void yes() = 0;
-        virtual void no();
+        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

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;
+}
+
+
+

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 <stdint.h>
+
+#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

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);
-}
+}

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;
         
 };
 

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() {

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

lib/Ntrip/NTRIPClient.cpp

@@ -1,37 +1,34 @@
 #include"NTRIPClient.h"
-bool NTRIPClient::reqSrcTbl(char* host,int &port)
+bool NTRIPClient::reqSrcTbl(char* host,uint16_t port){
-{
+  	if(!connect(host,port)){
-  if(!connect(host,port)){
+    	Serial.print("Cannot connect to ");
-      Serial.print("Cannot connect to ");
+    	Serial.println(host);
-      Serial.println(host);
+    	return false;
-      return false;
+  	}	/*p = String("GET ") + String("/") + String(" HTTP/1.0\r\n");
-  }/*p = String("GET ") + String("/") + String(" HTTP/1.0\r\n");
+  		p = p + String("User-Agent: NTRIP Enbeded\r\n");*/
-  p = p + String("User-Agent: NTRIP Enbeded\r\n");*/
+  	print(
-  print(
+      	"GET / HTTP/1.0\r\n"
-      "GET / HTTP/1.0\r\n"
+      	"User-Agent: NTRIPClient for Arduino v1.0\r\n"
-      "User-Agent: NTRIPClient for Arduino v1.0\r\n"
+    );
-      );
+  	unsigned long timeout = millis();
-  unsigned long timeout = millis();
+  	while (available() == 0) {
-  while (available() == 0) {
+     	if (millis() - timeout > 5000) {
-     if (millis() - timeout > 5000) {
+        	Serial.println("Client Timeout !");
-        Serial.println("Client Timeout !");
+        	stop();
-        stop();
+        	return false;
-        return false;
+     	}
-     }
+     	delay(10);
-     delay(10);
+  	}
-  }
+  	char buffer[50];
-  char buffer[50];
+  	readLine(buffer,sizeof(buffer));
-  readLine(buffer,sizeof(buffer));
+  	if(strncmp((char*)buffer,"SOURCETABLE 200 OK",17)) {
-  if(strncmp((char*)buffer,"SOURCETABLE 200 OK",17))
+    	Serial.print((char*)buffer);
-  {
+    	return false;
-    Serial.print((char*)buffer);
+  	}
-    return false;
+  	return true;
-  }
-  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();

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 reqSrcTbl(char* host,uint16_t 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,uint16_t port,char* mntpnt,char* user,char* psw);      //request RAW data from Caster 
-  bool reqRaw(char* host,int &port,char* mntpnt); //non user
+  bool reqRaw(char* host,uint16_t port,char* mntpnt); //non user
   int readLine(char* buffer,int size);
 
   

lib/Rtcm/rtcmInsert.cpp

@@ -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();
-    }
-}

lib/Rtcm/rtcmInsert.h

@@ -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 <stdint.h>
-
-#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

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();
 }

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;
 

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
+    uint8_t fixType = gps->getFixType();
-    // switch (this->currentPage) {
+
-    //     case 0:
+    switch (this->currentPage) {
-    //         if (this->lcd) {
+        case 0: {
-    //             lcd->clear();
+            uint8_t siv = gps->getSIV();
-    //             lcd->setCursor(0, 0);
+            uint16_t hdop = gps->getHorizontalDOP();
-    //             if (gps->satellites.isValid() && gps->satellites.value() > 2) {
+            if (this->lcd) {
-    //                 lcd->printf("Sats: %3.d", gps->satellites.value());
+                lcd->clear();
-    //                 lcd->setCursor(0, 1);
+                lcd->setCursor(0, 0);
-    //                 lcd->printf("HDOP: %f", gps->hdop.hdop());
+                if (fixType) {
-    //             } else {
+                    lcd->printf("Sats: %3.u", siv);
-    //                 lcd->printf("Data isn't valid");
+                    lcd->setCursor(0, 1);
-    //                 lcd->setCursor(0, 1);
+                    lcd->printf("HDOP: %u", hdop);
-    //                 lcd->printf("or no GPS signal");
+                } else {
-    //             }
+                    lcd->printf("Data isn't valid");
-    //         }
+                    lcd->setCursor(0, 1);
-    //             if (gps->satellites.isValid() && gps->satellites.value() > 2)
+                    lcd->printf("or no GPS signal");
-    //                 std::cout   << "Sats: " << gps->satellites.value()
+                }
-    //                             << "HDOP: " << gps->hdop.hdop() << std::endl; 
+            }
-    //             else
+                if (fixType)
-    //                 std::cout << "Data isn't valid or no GPS signal" << std::endl;
+                    std::cout   << "Sats: " << (int) siv
-    //         break;
+                                << "HDOP: " << (int) hdop << std::endl; 
+                else
+                    std::cout << "Data isn't valid or no GPS signal" << std::endl;
+        }
+            break;
         
-    //     case 1:
+        case 1: {
-    //         if (this->lcd) {
+            int32_t lat = gps->getLatitude();
-    //             lcd->clear();
+            int32_t lng = gps->getLongitude();
-    //             lcd->setCursor(0, 0);
+            if (this->lcd) {
-    //             if (gps->location.isValid() && gps->satellites.value() > 2) {
+                lcd->clear();
-    //                 lcd->printf("Lat: %9.6f", gps->location.lat());
+                lcd->setCursor(0, 0);
-    //                 lcd->setCursor(0, 1);
+                if (fixType) {
-    //                 lcd->printf("Lng: %9.6f", gps->location.lng());
+                    lcd->printf("Lat: %d", lat);
-    //             } else {
+                    lcd->setCursor(0, 1);
-    //                 lcd->printf("Data isn't valid");
+                    lcd->printf("Lng: %d", lng);
-    //                 lcd->setCursor(0, 1);
+                } else {
-    //                 lcd->printf("or NO GPS signal");
+                    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;
+            if (fixType)
-    //         else 
+                std::cout   << "Lat: " << lat
-    //             std::cout << "Data isn't valid or NO GPS signal" << std::endl;
+                            << "Lng: " << lng << std::endl;
-    //         break;
+            else 
+                std::cout << "Data isn't valid or NO GPS signal" << std::endl;
+        }
+            break;
 
-    //     case 2:
+        case 2: {
-    //         if (this->lcd) {
+            uint32_t unixEpoch = gps->getUnixEpoch();
-    //             lcd->clear();
+            
-    //             lcd->setCursor(0, 0);
+            if (this->lcd) {
-    //             lcd->printf("Chars processed");
+                lcd->clear();
-    //             lcd->setCursor(0, 1);
+                lcd->setCursor(0, 0);
-    //             lcd->printf("-> %u", gps->charsProcessed());
+                lcd->printf("Unix Epoch");
-    //         }
+                lcd->setCursor(0, 1);
-    //         std::cout << "Chars processed: " << gps->charsProcessed() << std::endl;
+                lcd->printf("-> %u", unixEpoch);
-    //         break;
+            }
+            std::cout << "Unix Epoch: " << (int) unixEpoch << std::endl;
+        }
+            break;
 
-    //     default:
+        default:
-    //         if (this->lcd) {
+            if (this->lcd) {
-    //             lcd->clear();
+                lcd->clear();
-    //             lcd->setCursor(0, 0);
+                lcd->setCursor(0, 0);
-    //             lcd->printf("Page: %u", page);  
+                lcd->printf("Page: %u", page);  
-    //         }
+            }
-    //         std::cout << "Page: " << (int) page << std::endl;
+            std::cout << "Page: " << (int) page << std::endl;
-    //         break;
+            break;
-    // }
+    }
 }

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

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;

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;
 };
 

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" <<std::endl;
+    };
+
+    // Create menu
+    this->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;
+}   

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

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);
 }

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;
 };
 

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
 

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() {

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;

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);
+    this->moveControl->setRotationSpeed(-x * value_per_step);
-}
+}

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;

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;
+}
+

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

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<Modi>(static_cast<int>(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:
+        case Modi::TestMode: {
-            this->currentModusPtr = nullptr;
+                this->currentModusPtr = new TestMode(this->moveControl);
+            }
             break;
         
         default:

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

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();
+        driveManager = new DriveManager(&moveController, true);
-        DebugMqtt::init(Network::getMqttClient(), Loglevel::debug);
+        if (Network::checkMQTT()) {
-        std::cout << "Activate additional output via MQTT..." << std::endl;
+            DebugMqtt::init(Network::getMqttClient(), Loglevel::debug);
-        debugMqtt = new DebugMqtt("Console");
+            std::cout << "Activate additional output via MQTT..." << std::endl;
-        outputBuf = new OutputBuf(debugMqtt);
+            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);
+    MenuManualControl* man_m = new MenuManualControl(driveManager);
-    MenuCaptureRoute* cap_m = new MenuCaptureRoute(&driveManager);
+    MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager);
-    MenuAutopilot* auto_m = new MenuAutopilot(&driveManager);
+    MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
-    MenuGPS* gps_m = new MenuGPS(driveManager.getNavigation()->getGPS());
+    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);

src/moveControl.cpp

@@ -62,12 +62,12 @@ void MoveControl::loop() {
         std::cout << buf;
     }
 
-    static uint64_t last_millis = 0;
+    static uint64_t lastMillis = 0;
-    if (millis() - last_millis < delay) 
+    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

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() {