new ntrip client

include/moveControl.h

@@ -27,7 +27,8 @@
  * 
  */
 enum DrivingStatus {stop,
-                    drive};
+                    drive,
+                    raw};
 
 /**
  * @brief This class manages the motors and the encoders
@@ -96,6 +97,9 @@ class MoveControl {
          */
         void setRotationSpeed(double speed);
 
+        void setRawPowerLeft(int16_t power);
+        void setRawPowerRight(int16_t power);
+
         /**
          * @brief Set the pid tunings
          * 
@@ -164,5 +168,7 @@ class MoveControl {
         double right_pid_out;
 
         uint8_t delay = 30;
+        int8_t rawPowerLeft = 0;
+        int8_t rawPowerRight = 0;
 };
 #endif // MOVE_CONTROL_H

include/networkConfig.h

@@ -30,15 +30,15 @@
  */
 #define WLAN_CONNECT_LOOP_TIME 500
 #define WLAN_DNS_SERVER     "8.8.8.8"
-#define DORO
+#define HOTSPOT
 
-#define NTRIP_NS
+#define NTRIP_NRW
 
 // NTRIP config NRW
 #ifdef NTRIP_NRW
-#define NTRIP_HOST          "80.158.61.104"
+#define NTRIP_HOST          "www.sapos-nw-ntrip.de"
 #define NTRIP_PORT          2101
-#define NTRIP_MOUNT_POINT   "EPS_NW"
+#define NTRIP_MOUNT_POINT   "VRS_3_3G_NW"
 #define NTRIP_USER          "nw-916771"
 #define NTRIP_PASSWORD      "Schalke#246"
 #endif // NTRIP_NRW
@@ -67,15 +67,12 @@
 #ifdef HOTSPOT
 #define WLAN_SSID "Kleiax Handy"
 #define WLAN_PASSWORD "12345677"
-#define WLAN_IP "192.168.11.4"
+#define WLAN_IP "192.168.43.4"
 #define WLAN_SUBNETMASK "255.255.128.0"
-#define WLAN_GATEWAY "192.168.0.1"
+#define WLAN_GATEWAY "192.168.43.1"
-#define MQTT
+// Only for defines reasons
-#define MQTT_SERVER "192.168.1.7"
+    #define MQTT_SERVER "172.22.64.216"
-#define MQTT_PORT 1883
+    #define MQTT_PORT 1883
-#define MQTT_AUTH
-#define MQTT_USER "kleiax"
-#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
 #endif //HOTSPOT
 
 //Network config Rhede

lib/Menu/menuIntInput.cpp

@@ -16,18 +16,143 @@ MenuIntInput::MenuIntInput(int16_t values[], char *names, uint8_t length, MenuIn
     this->names = names;
     this->length = length;
     this->wrapper = wrapper;
+
+    this->min = new int16_t[this->length];
+    this->max = new int16_t[this->length];
+    this->originalValues = new int16_t[this->length];
+    this->stepsPerInput = new uint8_t[this->length];
+
+    for (uint8_t i = 0; i < this->length; i++) {
+        this->min[i] = INT16_MIN;
+        this->max[i] = INT16_MAX;
+        this->originalValues[i] = this->values[i];
+        this->stepsPerInput[i] = 1;
+    }
+}
+
+MenuIntInput::MenuIntInput(uint8_t length, MenuIntInputWrapper *wrapper) {
+    this->length = length;
+    this->wrapper = wrapper;
+
+    this->values = new int16_t[this->length];
+    this->names = (char*) new char[this->length][MAX_NAME_LENGTH];
+    this->min = new int16_t[this->length];
+    this->max = new int16_t[this->length];
+    this->originalValues = new int16_t[this->length];
+    this->stepsPerInput = new uint8_t[this->length];
+
+    for (uint8_t i = 0; i < this->length; i++) {
+        this->min[i] = INT16_MIN;
+        this->max[i] = INT16_MAX;
+        this->values[i] = 0;
+        this->originalValues[i] = 0;
+        this->stepsPerInput[i] = 1;
+        strcpy(this->names + MAX_NAME_LENGTH * i, "Default");
+    }
+}
+
+MenuIntInput::~MenuIntInput() {
+    delete this->min;
+    delete this->max;
+    delete this->originalValues;
+    delete this->stepsPerInput;
+    delete this->values;
+    delete this->names;
+}
+
+void MenuIntInput::setEntry(uint8_t valueNumber, const char* name, int16_t startValue) {
+    if (this->length < valueNumber)
+        return;
+
+    if (strlen(name) > MAX_NAME_LENGTH)
+        return;
+
+
+    strcpy(this->names + MAX_NAME_LENGTH * valueNumber, name);
+    this->values[valueNumber] = startValue;
+    this->originalValues[valueNumber] = startValue;
+}
+
+void MenuIntInput::setMin(uint8_t valueNumber, int16_t min) {
+    if (this->length < valueNumber)
+        return;
+
+    this->min[valueNumber] = min;
+}
+
+void MenuIntInput::setMax(uint8_t valueNumber, int16_t max) {
+    if (this->length < valueNumber)
+        return;
+
+    this->max[valueNumber] = max;
+}
+
+void MenuIntInput::setMinMax(uint8_t valueNumber, int16_t min, int16_t max) {
+    this->setMin(valueNumber, min);
+    this->setMax(valueNumber, max);
+}
+
+void MenuIntInput::setMin(int16_t min) {
+    for (uint8_t i = 0; i < this->length; i++)
+        this->min[i] = min;
+}
+
+void MenuIntInput::setMax(int16_t max) {
+    for (uint8_t i = 0; i < this->length; i++)
+        this->max[i] = max;
+}
+
+void MenuIntInput::setMinMax(int16_t min, int16_t max) {
+    this->setMin(min);
+    this->setMax(max);
+}
+
+void MenuIntInput::setMinMaxSteps(uint8_t valueNumber, int16_t min, int16_t max, uint8_t steps) {
+    this->setMin(valueNumber, min);
+    this->setMax(valueNumber, max);
+    this->setStepsPerInput(valueNumber, steps);
+}
+
+void MenuIntInput::setMinMaxSteps(int16_t min, int16_t max, uint8_t steps) {
+    this->setMin(min);
+    this->setMax(max);
+    this->setStepsPerInput(steps);
+}
+
+void MenuIntInput::setStepsPerInput(uint8_t valueNumber, uint8_t steps) {
+    if (this->length < valueNumber)
+        return;
+
+    this->stepsPerInput[valueNumber] = steps;
+}
+
+void MenuIntInput::setStepsPerInput(uint8_t steps) {
+    for (uint8_t i = 0; i < this->length; i++)
+        this->stepsPerInput[i] = steps;
 }
 
 void MenuIntInput::down() {
-    if (values[currentPosition] > this->min)
+    if (this->values[this->currentPosition]
-        values[currentPosition]--;
+        -  this->stepsPerInput[this->currentPosition] 
+        >= this->min[currentPosition])
+    {
+        this->values[this->currentPosition] -= this->stepsPerInput[this->currentPosition];
+    } else {
+        this->values[this->currentPosition] = this->max[this->currentPosition];
+    }
 
     this->printMenu();
 }
 
 void MenuIntInput::up() {
-    if (values[currentPosition] < this->max)
+    if (this->values[this->currentPosition] 
-        values[currentPosition]++;
+        + this->stepsPerInput[this->currentPosition] 
+        <= this->max[this->currentPosition]) 
+    {
+        this->values[this->currentPosition] += this->stepsPerInput[this->currentPosition];
+    } else {
+        this->values[this->currentPosition] = this->min[this->currentPosition];
+    }
 
     this->printMenu();
 }
@@ -51,37 +176,42 @@ void MenuIntInput::left() {
 }
 
 void MenuIntInput::no() {
-    if (parentMenu)
+    for (uint8_t i = 0; i < this->length; i++)
+        this->values[i] = this->originalValues[i];
+    this->currentPosition = 0;
+
+    if (this->parentMenu)
         this->parentMenu->printMenu();
 }
 
 void MenuIntInput::yes() {
     this->wrapper->action(this->values, this->length);
-    if (parentMenu)
+    if (this->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]);
+        sprintf(bufferName, "  %s  ", this->names + 12 * this->currentPosition);
     } else if (this->currentPosition > 0 && this->length - 1 == this->currentPosition) {
         // Left arrow only
-        sprintf(bufferName, "< %s  ", this->names[this->currentPosition]);
+        sprintf(bufferName, "< %s  ", this->names + 12 * this->currentPosition);
     } else if (this->currentPosition == 0 && this->length > 1) {
         // Right arrow only
-        sprintf(bufferName, "  %s >", this->names[this->currentPosition]);
+        sprintf(bufferName, "  %s >", this->names + 12 * this->currentPosition);
     } else {
         // Both arrow
-        sprintf(bufferName, "< %s >", this->names[this->currentPosition]);
+        sprintf(bufferName, "< %s >", this->names + 12 * this->currentPosition);
     }
 
     char bufferValue[17];
-    sprintf(bufferValue, "  -> %5.d", this->values[this->currentPosition]);
+    if (this->values[this->currentPosition])
-
+        sprintf(bufferValue, "  -> %5.d", this->values[this->currentPosition]);
+    else
+        sprintf(bufferValue, "  ->     0");
 
     if (this->lcd) {
         lcd->clear();
@@ -92,6 +222,3 @@ void MenuIntInput::printMenu() {
     }
     std::cout << bufferName << " : " << bufferValue << std::endl;
 }
-
-
-

lib/Menu/menuIntInput.h

@@ -24,9 +24,22 @@ class MenuIntInputWrapper {
 class MenuIntInput : public MenuControl {
     public:
         MenuIntInput(int16_t values[], char *names, uint8_t length, MenuIntInputWrapper* wrapper);
+        MenuIntInput(uint8_t length, MenuIntInputWrapper *wrapper);
+        ~MenuIntInput();
 
-        void setMin(int16_t min = 0) { this->min = min; }
+        void setEntry(uint8_t valueNumber, const char* name, int16_t startValue = 0);
-        void setMax(int16_t max = 100) { this->max = max; }
+
+        void setMin(uint8_t valueNumber, int16_t min);
+        void setMin(int16_t min);
+        void setMax(uint8_t valueNumber, int16_t max);
+        void setMax(int16_t max);
+        void setMinMax(uint8_t valueNumber, int16_t min, int16_t max);
+        void setMinMax(int16_t min, int16_t max);
+        void setMinMaxSteps(uint8_t valueNumber, int16_t min, int16_t max, uint8_t steps);
+        void setMinMaxSteps(int16_t min, int16_t max, uint8_t steps);
+
+        void setStepsPerInput(uint8_t valueNumber, uint8_t steps);
+        void setStepsPerInput(uint8_t steps);
 
         /**
          * @brief Decrement selected value 
@@ -73,9 +86,12 @@ class MenuIntInput : public MenuControl {
 
         uint8_t currentPosition = 0;
         uint8_t length;
+        uint8_t *stepsPerInput;
+        uint8_t countSameActions = 0;
         int16_t *values;
-        int16_t min = INT16_MIN;
+        int16_t *originalValues;
-        int16_t max = INT16_MAX;
+        int16_t *min;
+        int16_t *max;
         char *names;
 
 };

lib/Navigation/navigation.cpp

@@ -95,11 +95,13 @@ void Navigation::loop() {
     if (millis() - this->lastMillis < this->timeToWait)
         return;
 
-    uint8_t rtcmData[512 * 4];
+    if (this->isNtripInit) {
-    uint16_t rtcmCount = this->ntripClient->available();
+        uint8_t rtcmData[512 * 4];
-    if (rtcmCount) {
+        uint16_t rtcmCount = this->ntripClient->available();
-        this->ntripClient->readBytes(rtcmData, rtcmCount);
+        if (rtcmCount) {
-        this->gps->pushRawData(rtcmData, rtcmCount);
+            this->ntripClient->readBytes(rtcmData, rtcmCount);
+            this->gps->pushRawData(rtcmData, rtcmCount);
+        }
     }
 }
 

lib/Ntrip/NTRIPClient.cpp

@@ -1,94 +1,264 @@
-#include"NTRIPClient.h"
+/**
-bool NTRIPClient::reqSrcTbl(char* host,uint16_t port){
+ * @file NTRIPClient.cpp
-  	if(!connect(host,port)){
+ * @author Alexander Klein (alex@kleiax.de)
-    	Serial.print("Cannot connect to ");
+ * @brief 
-    	Serial.println(host);
+ * @version 0.1
-    	return false;
+ * @date 2022-09-18
-  	}	/*p = String("GET ") + String("/") + String(" HTTP/1.0\r\n");
+ * 
-  		p = p + String("User-Agent: NTRIP Enbeded\r\n");*/
+ * @copyright Copyright (c) 2022
-  	print(
+ * 
-      	"GET / HTTP/1.0\r\n"
+ */
-      	"User-Agent: NTRIPClient for Arduino v1.0\r\n"
+
-    );
+#include "NTRIPClient.h"
-  	unsigned long timeout = millis();
+
-  	while (available() == 0) {
+NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password) {
-     	if (millis() - timeout > 5000) {
+    this->gps = gps;
-        	Serial.println("Client Timeout !");
+    this->host = host;
-        	stop();
+    this->port = port;
-        	return false;
+    this->mountPoint = mountPoint;
-     	}
+    this->user = user;
-     	delay(10);
+    this->password = password;
-  	}
+
-  	char buffer[50];
+    this->ntripClient = new WiFiClient;
-  	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,uint16_t port,char* mntpnt,char* user,char* psw) {
+NTRIPClient::~NTRIPClient() {
-    if(!connect(host,port))return false;
+    delete this->ntripClient;
-    String p="GET /";
+}
-    String auth="";
-    Serial.println("Request NTRIP");
-    
-    p = p + mntpnt + String(" HTTP/1.0\r\n"
-        "User-Agent: NTRIPClient for Arduino v1.0\r\n"
-    );
-    
-    if (strlen(user)==0) {
-        p = p + String(
-            "Accept: */*\r\n"
-            "Connection: close\r\n"
-            );
-    }
-    else {
-        auth = base64::encode(String(user) + String(":") + psw);
-        #ifdef Debug
-        Serial.println(String(user) + String(":") + psw);
-        #endif
 
-        p = p + String("Authorization: Basic ");
+void NTRIPClient::loop() {
-        p = p + auth;
+    if (millis() - this->lastLoopTime < this->delayTime)
-        p = p + String("\r\n");
+        return;
+    this->lastLoopTime = millis();
+
+    this->gps->checkUblox();
+    this->gps->checkCallbacks();
+
+    switch (this->state) {
+        case NTRIPClientStates::openConnection:
+            if (millis() - this->lastNtripConnectTime > this->tryReconnectTime)
+                break;
+
+            std::cout << "Connecting to the NTRIP caster..." << std::endl;
+            if (this->beginClient()) {
+                std::cout << "Connected to the NTRIP caster!" << std::endl;
+                this->state = NTRIPClientStates::pushData;
+            } else {
+                uint8_t seconds = this->tryReconnectTime / 1000;
+                std::cout << "Could not connect to the caster. Trying again in " 
+                          << seconds << " seconds." << std::endl;
+                this->lastNtripConnectTime = millis();
+            }
+            break;
+
+        case pushData:
+            if (!processConnection() || !this->activated)
+                this->state = NTRIPClientStates::closeConnection;
+            break;
+
+        case NTRIPClientStates::closeConnection:
+            std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
+            this->closeConnection();
+            state = NTRIPClientStates::wait;
+            break;
+
+        case NTRIPClientStates::wait:
+            if (this->activated)
+                this->state = NTRIPClientStates::openConnection;
+            break;
     }
-    p = p + String("\r\n");
+}
-    print(p);
+
-    #ifdef Debug
+void NTRIPClient::gpsConfiguration() {
-    Serial.println(p);
+    this->gps->setI2COutput(COM_TYPE_UBX | COM_TYPE_NMEA);
-    #endif
+    this->gps->setPortInput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
-    unsigned long timeout = millis();
+    this->gps->(SFE_UBLOX_DGNSS_MODE_FIXED); // Set the differential mode - ambiguities are fixed whenever possible
-    while (available() == 0) {
+    this->gps->setNavigationFrequency(1);
-        if (millis() - timeout > 20000) {
+    this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
-            Serial.println("Client Timeout !");
+    this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_I2C, 10);
+
+    this->gps->setNMEAGPGGAcallbackPtr(&pushGPGGA); // Set up the callback for GPGGA
+    this->gps->setAutoPVTcallbackPtr(&(NTRIPClient::printPVTdata));
+}
+
+bool NTRIPClient::beginClient() {
+    std::cout << "Opening socket to " << this->host << std::endl;
+
+    char serverRequest[this->bufferSize];
+    char credentials[this->bufferSize];
+
+    if (!this->ntripClient->connect(this->host, this->port)) {
+        std::cout << "Connection to caster failed" << std::endl;
+        return false;
+    } else {
+        std::cout << "Connected to " << this->host << " : " << this->port << std::endl;
+        std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl;
+
+        // Generate the server request (GET)
+        snprintf(serverRequest,
+                 this->bufferSize,
+                 "GET /%s HTTP/1.0\r\nUser-Agent: NTRIP SparkFun u-blox Client v1.0\r\n",
+                 this->mountPoint);
+
+        // Credentials
+        uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 1;
+        char* userCredentials = new char[userCredentialsLength];
+        snprintf(userCredentials, userCredentialsLength, "%s:%s", this->user, this->password);
+
+        std::cout << "Sending credentials: " << userCredentials << std::endl;
+
+        //Encode
+        base64 b;
+        String strEncodedCredentials = b.encode(userCredentials);
+        delete userCredentials;
+        char encodedCredentials[strEncodedCredentials.length() + 1];
+        strEncodedCredentials.toCharArray(encodedCredentials, sizeof(encodedCredentials));
+
+        snprintf(credentials, sizeof(credentials), "Authorization: Basic %s\r\n", encodedCredentials);
+    }
+
+    // Add the encoded credentials to the server request
+    strncat(serverRequest, credentials, this->bufferSize);
+    strncat(serverRequest, "\r\n", this->bufferSize);
+
+    std::cout   << "serverRequest size: " 
+                << strlen(serverRequest) 
+                << " of " 
+                << this->bufferSize 
+                << " bytes available" 
+                << std::endl;
+
+    // Send the server request
+    std::cout << "Sending server request: " << serverRequest << std::endl;
+    this->ntripClient->write(serverRequest, strlen(serverRequest));
+
+    //Wait up to 5 seconds for response
+    uint32_t lastMillis = millis();
+    while (!ntripClient->available()) {
+        if (millis() - lastMillis > this->timeOut) {
+            std::cout << "Caster timed out!" << std::endl;
+            this->ntripClient->stop();
             return false;
         }
-        delay(10);
+        delay(10)
     }
-    char buffer[50];
+
-    readLine(buffer,sizeof(buffer));
+    //Check reply
-    if(strncmp((char*)buffer,"ICY 200 OK",10))
+    uint16_t httpStatusCode = 0;
-    {
+    char response[this->bufferSize];
-      Serial.print((char*)buffer);
+    uint16_t responseIndex = 0;
-      return false;
+    while (this->ntripClient->available()) {
+        if (responseIndex == sizeof(response))
+            break;
+
+        response[responseIndex++] = ntripClient->read();
+
+        if (httpStatusCode == 0) {
+            if (strstr(response, "200") != nullptr)
+                httpStatusCode = 200;
+            if (strstr(response, "400") != nullptr)
+                httpStatusCode = 401;
+        }
     }
+    response[responseIndex] = '\0';
+
+    // std::cout << "Caster response: " << response << std::endl;
+
+    if (httpStatusCode != 200) {
+        std::cout << "Failed to connect to " << this->host << std::endl;
+        if (httpStatusCode == 401)
+            std::cout << "Statuscode 401 - Unauthorized" << std::endl;
+        return false;
+    }
+
+    std::cout << "Connected to: " << this->host << std::endl;
+    this->lastReceivedRtcmTime = millis();
     return true;
 }
 
-bool NTRIPClient::reqRaw(char* host,uint16_t port,char* mntpnt) {
+void NTRIPClient::closeConnection() {
-    return reqRaw(host,port,mntpnt,"","");
+    if (this->ntripClient->connected())
+        this->ntripClient->stop();
+    this->activated = false;
+    std::cout << "NtripClient disconnected from: " << this->host << std::endl;
 }
 
-int NTRIPClient::readLine(char* _buffer,int size) {
+bool NTRIPClient::processConnection() {
-  int len = 0;
+    if (this->ntripClient->connected()) {
-  while(available()) {
+        uint8_t rtcmData[this->bufferSize * 4];
-    _buffer[len] = read();
+        uint16_t rtcmCount = 0;
-    len++;
-    if(_buffer[len-1] == '\n' || len >= size) break;
-  }
-  _buffer[len]='\0';
 
-  return len;
+        while (this->ntripClient->available()) {
+            rtcmData[rtcmCount++] = ntripClient->read();
+            if (rtcmCount == sizeof(rtcmData))
+                break;
+        }
+
+        if (rtcmCount > 0) {
+            this->lastReceivedRtcmTime = millis();
+            this->gps->pushRawData(rtcmData, rtcmCount);
+            std::cout << "Pushed " << rtcmCount << " RTCM bytes to ZED." << std::endl;
+        }
+    } else {
+        std::cout << "Connection to " << this->host << " dropped!" << std::endl;
+        return false;
+    }
+
+    if (millis() - this->lastReceivedRtcmTime > this->timeOut) {
+        std::cout << "RTCM timeout!" << std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+void NTRIPClient::pushGPGGA(NMEA_GGA_data_t *nmeaData) {
+    if (this->ntripClient->connected() && this->transmitLocation) {
+        std::cout << "Pushing GGA to server: " << (const char *)nmeaData->nmea) << std::endl;
+        this->ntripClient->print((const char *)nmeaData->nmea);
+    }
+}
+
+void NTRIPClient::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
+    double latitude = (double) ubxDataStruct->lat / 10000000.0;
+    double longitude = (double) ubxDataStruct->lon / 10000000.0;
+    double altitude = (double) ubxDataStruct->hMSL / 1000.0;
+
+    uint8_t fixType = ubxDataStruct->fixType;
+    char fixTypeString[32];
+    if (fixType == 0)
+        strcpy(fixTypeString, "None");
+    else if (fixType == 1)
+        strcpy(fixTypeString, "Dead Reckoning");
+    else if (fixType == 2)
+        strcpy(fixTypeString, "2D");
+    else if (fixType == 3)
+        strcpy(fixTypeString, "3D");
+    else if (fixType == 3)
+        strcpy(fixTypeString, "GNSS + Dead Reckoning");
+    else if (fixType == 5)
+        strcpy(fixTypeString, "Time Only");
+    else
+        strcpy(fixTypeString, "UNKNOWN");
+
+    uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
+    char carrSolnString[16];
+    if (carrSoln == 0)
+        strcpy(fixTypeString, "None");
+    else if (carrSoln == 1)
+        strcpy(fixTypeString, "Floating");
+    else if (carrSoln == 2)
+        strcpy(fixTypeString, "Fixed");
+    else
+        strcpy(fixTypeString, "UNKNOWN");
+
+    uint32_t hAcc = ubxDataStruct->hAcc;
+
+    std::cout << "Lat: " << latitude
+              << "Lng: " << longitude
+              << "Alt: " << altitude << std::endl;
+
+    std::cout << "Fix: " << fixTypeString
+              << "Carrier Solution: " << carrSolnString
+              << "Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
 }

lib/Ntrip/NTRIPClient.h

@@ -3,16 +3,55 @@
 
 #include <WiFiClient.h>
 #include <Arduino.h>
-#include<base64.h>
+#include <base64.h>
+#include <iostream>
+#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 
-class NTRIPClient : public WiFiClient{
+enum NTRIPClientStates {
-  public :
+        openConnection,
-  bool reqSrcTbl(char* host,uint16_t port);   //request MountPoints List serviced the NTRIP Caster 
+        pushData,
-  bool reqRaw(char* host,uint16_t port,char* mntpnt,char* user,char* psw);      //request RAW data from Caster 
+        closeConnection,
-  bool reqRaw(char* host,uint16_t port,char* mntpnt); //non user
+        wait
-  int readLine(char* buffer,int size);
+};
+class NTRIPClient {
+    public:
+        NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password);
+        ~NTRIPClient();
 
-  
+        void loop();
+
+        void gpsConfiguration();
+        void pushGPGGA(NMEA_GGA_data_t *nmeaData);
+
+        void setTransmitLocation(bool b) { this->transmitLocation = b; }
+        void setActivated(bool b) { this->activated = b; }
+        static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
+
+    private:
+        bool beginClient();
+        void closeConnection();
+        bool processConnection();
+
+        SFE_UBLOX_GNSS* gps;
+        WiFiClient* ntripClient;
+        NTRIPClientStates state;
+
+        bool transmitLocation = true;
+        bool activated = true;
+
+        uint16_t port;
+        uint32_t lastReceivedRtcmTime = 0;
+        uint32_t lastNtripConnectTime = 0;
+        uint32_t lastLoopTime = 0;
+
+        const char* host;
+        const char* mountPoint;
+        const char* user;
+        const char* password;
+        const uint8_t delayTime = 20;
+        const uint16_t timeOut = 5000;
+        const uint16_t tryReconnectTime = 5000;
+        const uint16_t bufferSize = 512;
 };
 
 #endif

platformio.ini

@@ -16,7 +16,6 @@ framework = arduino
 monitor_speed = 115200
 upload_speed = 921600
 monitor_port = COM3
-;build_flags = -DCORE_DEBUG_LEVEL=5
 lib_deps = 
 	madhephaestus/ESP32Encoder@^0.4.0
 	jvpernis/PS3 Controller Host@^1.1.0
@@ -24,6 +23,7 @@ lib_deps =
 	knolleary/PubSubClient@^2.8
 	br3ttb/PID@^1.2.1
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
+	marian-craciunescu/ESP32Ping@^1.7
 upload_port = COM3
 
 [platformio]

src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp

@@ -13,13 +13,11 @@
 MenuTestMode::MenuTestMode(DriveManager* driveManager)
     : MenuDriveMode(driveManager) {
         // this->testMode = testMode;
-        this->values[0] = 1;
+        // this->values[0] = 0;
-        this->values[1] = 2;
+        // this->values[1] = 0;
-        this->values[2] = 3;
 
-        strcpy(this->names[0], "Alpha");
+        // strcpy(this->names[0], "Centimeter");
-        strcpy(this->names[1], "Beta");
+        // strcpy(this->names[1], "Degree");
-        strcpy(this->names[2], "Gamma");
     }
 
 MenuTestMode::~MenuTestMode() {
@@ -68,6 +66,7 @@ void MenuTestMode::no() {
 }
 
 void MenuTestMode::init() {
+    this->driveManager->changeModus(Modi::TestMode);
     this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
 
     auto dummy = []() {
@@ -77,40 +76,49 @@ void MenuTestMode::init() {
     // 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));
+    MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::drive));
+    MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::leftEngine));
+    MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::rightEngine));
+    MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::bothEngine));
+
 
     // Set menus on LCD
     this->mainMenu->setLcd(this->lcd);
     engineMenu->setLcd(this->lcd);
     engineLeftMenu->setLcd(this->lcd);
     engineRightMenu->setLcd(this->lcd);
+    engineBothMenu->setLcd(this->lcd);
     drivingMenu->setLcd(this->lcd);
-    drivingForwardMenu->setLcd(this->lcd);
-    drivingBackwardMenu->setLcd(this->lcd);
     lightMenu->setLcd(this->lcd);
-    testInputMenu->setLcd(this->lcd);
+
+    // Other menu config
+    drivingMenu->setEntry(0, "Centimeter", 100);
+    drivingMenu->setMinMaxSteps(0, -1000, 1000, 25);
+    drivingMenu->setEntry(1, "Degree");
+    drivingMenu->setMinMaxSteps(1, -360, 360, 15);
+    engineLeftMenu->setEntry(0, "Percentage");
+    engineLeftMenu->setMinMaxSteps(0, -100, 100, 5);
+    engineLeftMenu->setEntry(1, "Seconds", 10);
+    engineLeftMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineRightMenu->setEntry(0, "Percentage");
+    engineRightMenu->setMinMaxSteps(0, -100, 100, 5);
+    engineRightMenu->setEntry(1, "Seconds", 10);
+    engineRightMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineBothMenu->setEntry(0, "Percentage");
+    engineBothMenu->setMinMaxSteps(0, -100, 100, 5);
+    engineBothMenu->setEntry(1, "Seconds", 10);
+    engineBothMenu->setMinMaxSteps(1, 0, 120, 1);
+
 
     // Entrys for the menus
     MenuAction* engineAction = new MenuAction("Engine", engineMenu);
     MenuAction* engineLeftAction = new MenuAction("Left", engineLeftMenu);
     MenuAction* engineRightAction = new MenuAction("Right", engineRightMenu);
+    MenuAction* engineBothAction = new MenuAction("Both", engineBothMenu);
 
     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);
@@ -127,15 +135,7 @@ void MenuTestMode::init() {
 
     engineMenu->addEntry(engineLeftAction);
     engineMenu->addEntry(engineRightAction);
-
+    engineMenu->addEntry(engineBothAction);
-    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);
@@ -145,13 +145,22 @@ void MenuTestMode::init() {
     lightMenu->addEntry(lightOffAction);    
 }
 
-MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode) {
+MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction) {
     this->testMode = testMode;
+    this->testModeFunctionSingle = testModeFunction;
+}
+
+MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction) {
+    this->testMode = testMode;
+    this->testModeFunctionDouble = testModeFunction;
 }
 
 void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
-    if (length > 2)
+    // This function calls a member function with a pointer
-        std::cout << "Dummy in Action" << values[0] << values[1] << values[2] << std::endl;
+    // Very helpful site:
-    else
+    // https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types
-        std::cout << "Dummy in Action kene dre values" << std::endl;
+    if (length == 2 && this->testModeFunctionDouble)
+        (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
+    else if (length == 1 && this->testModeFunctionSingle)
+        (this->testMode->*this->testModeFunctionSingle)(values[0]);
 }   

src/SpecialMenus/driveModi/TestMode/menuTestMode.h

@@ -37,20 +37,21 @@ class MenuTestMode : public MenuDriveMode {
         Menu* mainMenu;
 
         bool isInit = false;
-
-        uint8_t length = 3;
-        int16_t values[3];
-        char names[3][MAX_NAME_LENGTH];
 };
 
+typedef  bool (TestMode::*TestModeFunctionSingle)(int16_t);
+typedef  bool (TestMode::*TestModeFunctionDouble)(int16_t, int16_t);
 class MenuTestModeWrapper : public MenuIntInputWrapper {
     public:
-        MenuTestModeWrapper(TestMode* testMode);
+        MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction);
+        MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction);
 
         void action(int16_t* values, uint8_t length) override;
 
     private:
         TestMode* testMode; 
+        TestModeFunctionSingle testModeFunctionSingle = nullptr;
+        TestModeFunctionDouble testModeFunctionDouble = nullptr;
 };
 
 #endif // MENU_TEST_MODE

src/driveModi/Modi/TestMode/testMode.cpp

@@ -12,12 +12,9 @@
 
 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);
 }
 
@@ -27,98 +24,117 @@ void TestMode::loop() {
 
     if (millis() - this->actionStart > this->maneuverTime) {
         this->busy = false;
-        this->moveControl->setSpeed(0);
+        this->moveControl->setDrivingStatus(DrivingStatus::stop);
-        this->moveControl->setRotationSpeed(0);
     }
 }
 
-void TestMode::setSpeed(double speed) {
+void TestMode::setSpeed(int16_t speed) {
-    this->speed = speed;
+    this->speed = (double) speed / 100.0;
 }
 
-void TestMode::setRotationSpeed(double speed) {
+void TestMode::setRotationSpeed(int16_t speed) {
-    this->rotationSpeed = speed;
+    this->rotationSpeed = (double) speed / 100.0;
 }
 
-bool TestMode::driveForward(uint16_t cm) {
+bool TestMode::drive(int16_t cm, int16_t degree) {
     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;
+    if (true) {
+        std::cout << "It works... cm: " << cm << " degree: " << degree << std::endl;
+        return false;
+    }
+
     this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+    if (cm == 0) {
+        //Only left or right
+        this->moveControl->setSpeed(0);
 
-    this->busy = true;
+        if (degree < 0)
-    return true;
+            this->moveControl->setRotationSpeed(-this->rotationSpeed);
-}
+        else if (degree > 0)
+            this->moveControl->setRotationSpeed(this->rotationSpeed);
+        
+        //TODO: calc this shit
+        this->maneuverTime = 10;
+        this->busy = true;
+        return true;
 
-bool TestMode::driveBackward(uint16_t cm) {
+    } else if (degree == 0) {
-    if (this->busy)
+        //Only forward or backward
-        return false;
+        this->moveControl->setRotationSpeed(0);
 
-    this->moveControl->setRotationSpeed(0);
+        if (cm < 0)
-    this->moveControl->setSpeed(-this->speed);
+            this->moveControl->setSpeed(-this->speed);
+        else if (cm > 0)
+            this->moveControl->setSpeed(this->speed);
+        
+        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+        this->busy = true;
+        return true;
+    } else {
+        //forward or backward and left or right
+    }
 
-    this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+    this->moveControl->setDrivingStatus(DrivingStatus::stop);    
-    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) {
+bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-    return false;
+
+    if (powerPercentage >= 100 || powerPercentage <= -100)
+        return false;
+
+    if (true) {
+        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
+        return false;
+    }
+
+    this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::raw);
+    this->moveControl->setRawPowerLeft(powerPercentage);
+    this->maneuverTime = seconds * 1000;
+    this->busy = true;
 }
 
-bool TestMode::driveBackwardLeft(uint16_t cm, uint16_t degree) {
+bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-    return false;
+
+    if (powerPercentage >= 100 || powerPercentage <= -100)
+        return false;
+
+    if (true) {
+        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
+        return false;
+    }
+
+    this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::raw);
+    this->moveControl->setRawPowerRight(powerPercentage);
+    this->maneuverTime = seconds * 1000;
+    this->busy = true;
 }
 
-bool TestMode::driveBackwardRight(uint16_t cm, uint16_t degree) {
+bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-    return false;
-}
 
+    if (powerPercentage >= 100 || powerPercentage <= -100)
+        return false;
+
+    if (true) {
+        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
+        return false;
+    }
+
+    this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::raw);
+    this->moveControl->setRawPowerLeft(powerPercentage);
+    this->moveControl->setRawPowerRight(powerPercentage);
+    this->maneuverTime = seconds * 1000;
+    this->busy = true;
+}

src/driveModi/Modi/TestMode/testMode.h

@@ -21,19 +21,14 @@ class TestMode : public DriveModi {
 
         void loop() override;
 
-        void setSpeed(double speed);
+        void setSpeed(int16_t speed);
-        void setRotationSpeed(double speed);
+        void setRotationSpeed(int16_t speed);
 
-        bool driveForward(uint16_t cm);
+        bool drive(int16_t cm = 0, int16_t degree = 0);
-        bool driveBackward(uint16_t cm);
 
-        bool turnLeft(uint16_t degree);
+        bool leftEngine(int16_t powerPercentage, int16_t seconds);
-        bool turnRight(uint16_t degree);
+        bool rightEngine(int16_t powerPercentage, int16_t seconds);
-
+        bool bothEngine(int16_t powerPercentage, int16_t seconds);
-        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;

src/driveModi/driveManager.cpp

@@ -20,6 +20,8 @@ DriveManager::DriveManager(MoveControl *moveControl, bool wifi) {
     this->navigation = new Navigation();
     if (wifi)
         this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
+    // if (wifi)
+    //     std::cout << "Wifi is true" << std::endl;
 }
 
 DriveManager::~DriveManager() {

src/main.cpp

@@ -19,6 +19,7 @@
 #include <Wire.h>
 #include <LiquidCrystal_I2C.h>
 #include <BluetoothSerial.h>
+#include <ESP32Ping.h>
 
 #include "config.h"
 
@@ -74,8 +75,16 @@ void setup() {
     Network::setIps();
     Network::setupMQTT();
     wifiIsActive = Network::connectWifi();
+    driveManager = new DriveManager(&moveController, wifiIsActive);
     if (wifiIsActive) {
-        driveManager = new DriveManager(&moveController, true);
+        
+        const char* remoteHost = "www.google.com";
+        std::cout << "Pinging host: " << remoteHost << std::endl;
+        if (Ping.ping(remoteHost))
+            std::cout << "Ping successful..." << std::endl;
+        else
+            std::cout << "Ping failed. :/" << std::endl;
+
         if (Network::checkMQTT()) {
             DebugMqtt::init(Network::getMqttClient(), Loglevel::debug);
             std::cout << "Activate additional output via MQTT..." << std::endl;
@@ -85,7 +94,6 @@ void setup() {
             outputBuf = new OutputBuf();
         wifiIndicator = '-';
     } else {
-        driveManager = new DriveManager(&moveController);
         outputBuf = new OutputBuf();
     }
     std::cout.rdbuf(outputBuf);

src/moveControl.cpp

@@ -81,6 +81,10 @@ void MoveControl::runMoveControl() {
 }
 
 void MoveControl::setDrivingStatus(DrivingStatus status) {
+    this->setSpeed(0);
+    this->setRotationSpeed(0);
+    this->setRawPowerLeft(0);
+    this->setRawPowerRight(0);
     this->driving_status = status;
     // switch (this->driving_status) {
     //     case DrivingStatus::stop :
@@ -111,6 +115,16 @@ void MoveControl::setRotationSpeed(double speed) {
         this->rotation_speed = speed;
 }
 
+void MoveControl::setRawPowerLeft(int16_t power) {
+    if (power <= 100 && power >= 100)
+        this->rawPowerLeft = power;
+}
+
+void MoveControl::setRawPowerRight(int16_t power) {
+    if (power <= 100 && power >= 100)
+        this->rawPowerRight = power;
+}
+
 void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
     PID* selectedPID = nullptr;
     if (side == 0)
@@ -165,6 +179,11 @@ void MoveControl::regulateMotors() {
             this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
             break;
 
+        case DrivingStatus::raw :
+            this->left_motor->setTargetPower(this->rawPowerLeft);
+            this->right_motor->setTargetPower(this->rawPowerRight);
+            break;
+
         default:
             Serial.println("Wrong drivingState in MoveControl::regulateMotors");
             break;