try to work ntrip client

include/moveControl.h

@@ -19,6 +19,7 @@
 #include "motorControl.h"
 #include "speedometer.h"
 #include "moveControlConfig.h"
+#include "debugTimes.h"
 
 /**
  * @brief used to set driving status
@@ -168,7 +169,10 @@ class MoveControl {
         double right_pid_out;
 
         uint8_t delay = 30;
+        uint8_t overTimeCounter = 0;
+        uint8_t overTimeMax = 10;
         int8_t rawPowerLeft = 0;
         int8_t rawPowerRight = 0;
+        uint32_t lastMillis = 0;
 };
 #endif // MOVE_CONTROL_H

include/networkConfig.h

@@ -30,9 +30,8 @@
  */
 #define WLAN_CONNECT_LOOP_TIME 500
 #define WLAN_DNS_SERVER     "8.8.8.8"
-#define HOTSPOT
 
-#define NTRIP_NRW
+#define NTRIP_NS
 
 // NTRIP config NRW
 #ifdef NTRIP_NRW
@@ -52,6 +51,8 @@
 #define NTRIP_PASSWORD      "ALdx-1-3e49"
 #endif // NTRIP_NS
 
+#define RHEDE
+
 //Network config ESP32
 #ifdef ESP32BROKER
 #define WLAN_SSID "ESP32-Broker"

lib/Navigation/navigation.cpp

@@ -46,6 +46,8 @@ Navigation::~Navigation() {
 void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
     this->ntripClient = new NTRIPClient(this->gps, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
     this->ntripClient->gpsConfiguration();
+    this->ntripClient->loop();
+    this->ntripClient->setActivated(true);
     this->isNtripInit = true;
 }
 

lib/Ntrip/NTRIPClient.cpp

@@ -13,11 +13,11 @@
 
 NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password) {
     this->gps = gps;
-    this->host = host;
+    strcpy(this->host, host);
     this->port = port;
-    this->mountPoint = mountPoint;
+    strcpy(this->mountPoint, mountPoint);
-    this->user = user;
+    strcpy(this->user, user);
-    this->password = password;
+    strcpy(this->password, password);
 
     this->ntripClient = new WiFiClient;
 }
@@ -35,17 +35,27 @@ void NTRIPClient::loop() {
     this->gps->checkCallbacks();
 
     if (millis() - this->lastGPGGAPushTime > this->pushGPGGATime) {
-        NMEA_GGA_data_t *data = nullptr;
+        this->lastGPGGAPushTime = millis();
+        // std::cout << "Try to push GPGGA data." << std::endl;
+        NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
+        DebugTimes requestGpsData;
         uint8_t res = this->gps->getLatestNMEAGPGGA(data);
+        requestGpsData.stopConsol("RequestGpsData", 5);
         if (res == 2)
             this->pushGPGGA(data);
+        delete data;
     }
 
     switch (this->state) {
         case NTRIPClientStates::openConnection:
-            if (millis() - this->lastNtripConnectTime > this->tryReconnectTime)
+            if (millis() - this->lastNtripConnectTime < this->tryReconnectTime)
                 break;
 
+            if (!this->activated) {
+                this->state = NTRIPClientStates::closeConnection;
+                break;
+            }
+
             std::cout << "Connecting to the NTRIP caster..." << std::endl;
             if (this->beginClient()) {
                 std::cout << "Connected to the NTRIP caster!" << std::endl;
@@ -53,12 +63,12 @@ void NTRIPClient::loop() {
             } else {
                 uint8_t seconds = this->tryReconnectTime / 1000;
                 std::cout << "Could not connect to the caster. Trying again in " 
-                          << seconds << " seconds." << std::endl;
+                          << (int) seconds << " seconds." << std::endl;
                 this->lastNtripConnectTime = millis();
             }
             break;
 
-        case pushData:
+        case NTRIPClientStates::pushData:
             if (!processConnection() || !this->activated)
                 this->state = NTRIPClientStates::closeConnection;
             break;
@@ -72,6 +82,14 @@ void NTRIPClient::loop() {
         case NTRIPClientStates::wait:
             if (this->activated)
                 this->state = NTRIPClientStates::openConnection;
+            if (this->activated) {
+                std::cout << "state is openConnection" << std::endl;
+            }
+            break;
+
+        default:
+            std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
+            this->state = NTRIPClientStates::closeConnection;
             break;
     }
 }
@@ -79,7 +97,8 @@ void NTRIPClient::loop() {
 void NTRIPClient::gpsConfiguration() {
     this->gps->setI2COutput(COM_TYPE_UBX | COM_TYPE_NMEA);
     this->gps->setPortInput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
-    this->gps->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED); // Set the differential mode - ambiguities are fixed whenever possible
+    // Set the differential mode - ambiguities are fixed whenever possible
+    // this->gps->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED); 
     this->gps->setNavigationFrequency(1);
     this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
     this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_I2C, 10);
@@ -107,7 +126,7 @@ bool NTRIPClient::beginClient() {
                  this->mountPoint);
 
         // Credentials
-        uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 1;
+        uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
         char* userCredentials = new char[userCredentialsLength];
         snprintf(userCredentials, userCredentialsLength, "%s:%s", this->user, this->password);
 
@@ -162,7 +181,7 @@ bool NTRIPClient::beginClient() {
         if (httpStatusCode == 0) {
             if (strstr(response, "200") != nullptr)
                 httpStatusCode = 200;
-            if (strstr(response, "400") != nullptr)
+            if (strstr(response, "401") != nullptr)
                 httpStatusCode = 401;
         }
     }
@@ -171,8 +190,12 @@ bool NTRIPClient::beginClient() {
     // std::cout << "Caster response: " << response << std::endl;
 
     if (httpStatusCode != 200) {
-        std::cout << "Failed to connect to " << this->host << std::endl;
+        std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int) httpStatusCode 
-        if (httpStatusCode == 401)
+                  << " Length of Response: " << responseIndex << std::endl;
+
+        if (httpStatusCode == 0)
+            std::cout << "Response: " << response << std::endl;
+        else if (httpStatusCode == 401)
             std::cout << "Statuscode 401 - Unauthorized" << std::endl;
         return false;
     }
@@ -243,6 +266,7 @@ void NTRIPClient::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
     else if (fixType == 3)
         strcpy(fixTypeString, "GNSS + Dead Reckoning");
     else if (fixType == 5)
+
         strcpy(fixTypeString, "Time Only");
     else
         strcpy(fixTypeString, "UNKNOWN");
@@ -250,13 +274,13 @@ void NTRIPClient::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
     uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
     char carrSolnString[16];
     if (carrSoln == 0)
-        strcpy(fixTypeString, "None");
+        strcpy(carrSolnString, "None");
     else if (carrSoln == 1)
-        strcpy(fixTypeString, "Floating");
+        strcpy(carrSolnString, "Floating");
     else if (carrSoln == 2)
-        strcpy(fixTypeString, "Fixed");
+        strcpy(carrSolnString, "Fixed");
     else
-        strcpy(fixTypeString, "UNKNOWN");
+        strcpy(carrSolnString, "UNKNOWN");
 
     uint32_t hAcc = ubxDataStruct->hAcc;
 

lib/Ntrip/NTRIPClient.h

@@ -7,6 +7,8 @@
 #include <iostream>
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 
+#include "debugTimes.h"
+
 enum NTRIPClientStates {
         openConnection,
         pushData,
@@ -38,7 +40,7 @@ class NTRIPClient {
 
         SFE_UBLOX_GNSS* gps;
         WiFiClient* ntripClient;
-        NTRIPClientStates state;
+        NTRIPClientStates state = NTRIPClientStates::closeConnection;
 
         bool transmitLocation = true;
         bool activated = true;
@@ -49,10 +51,10 @@ class NTRIPClient {
         uint32_t lastGPGGAPushTime = 0;
         uint32_t lastLoopTime = 0;
 
-        const char* host;
+        char host[128];
-        const char* mountPoint;
+        char mountPoint[128];
-        const char* user;
+        char user[128];
-        const char* password;
+        char password[128];
         const uint8_t delayTime = 20;
         const uint16_t timeOut = 5000;
         const uint16_t tryReconnectTime = 5000;

lib/Times/debugTimes.cpp

@@ -12,6 +12,13 @@
 
 DebugTimes::DebugTimes() {
     this->startTime = millis();
+
+    static bool firstInstance = true;
+    if (firstInstance) {
+        firstInstance = false;
+        if (!DebugTimes::print)
+            std::cout << std::endl << "Warning: DebugTimes is muuted, no times are be shown." << std::endl << std::endl;
+    }
 }
 
 void DebugTimes::restart() {
@@ -22,8 +29,9 @@ uint16_t DebugTimes::stop() {
     return millis() - this->startTime;
 }
 
-void DebugTimes::stopConsol(const char* name, uint16_t minTime) {
+uint16_t DebugTimes::stopConsol(const char* name, uint16_t minTime) {
     uint64_t time = millis() - this->startTime;
-    if (time > minTime)
+    if (time > minTime && DebugTimes::print)
-        Serial.printf("%s needs %llu ms\n", name, time);
+        std::cout << name << " needs " << time << " ms" << std::endl;
+    return time;
 }

lib/Times/debugTimes.h

@@ -14,6 +14,7 @@
 
 #include <list>
 #include <cstdint>
+#include <iostream>
 
 #include <Arduino.h>
 
@@ -53,11 +54,15 @@ class DebugTimes {
          * 
          * @param name Functionname to print
          * @param minTime A minimum time before printing
+         * 
+         * @return uint16_t elapsed milliseconds 
          */
-        void stopConsol(const char* name, uint16_t minTime = 0);
+        uint16_t stopConsol(const char* name, uint16_t minTime = 0);
 
     private:
         uint64_t startTime;
+
+        static const bool print = false;
 };
 
 #endif //DEBUG_TIMES_H

src/driveModi/driveManager.cpp

@@ -29,8 +29,14 @@ DriveManager::~DriveManager() {
 }
 
 void DriveManager::loop() {
+    DebugTimes moveControlTime;
     this->moveControl->loop();
+    moveControlTime.stopConsol("MoveControlTime", 5);
+
+    DebugTimes navigationTime;
     this->navigation->loop();
+    navigationTime.stopConsol("NavigationTime", 5);
+
     if (currentModusPtr)
         this->currentModusPtr->loop();
 }

src/driveModi/driveManager.h

@@ -23,6 +23,7 @@
 #include "driveModi/driveModi.h"
 #include "navigation.h"
 #include "networkConfig.h"
+#include "debugTimes.h"
 
 // All Drive Modi
 #include "driveModi/Modi/ManualControl/manualControl.h"

src/main.cpp

@@ -29,6 +29,7 @@
 #include "network.h"
 #include "debugMqtt.h"
 #include "battery.h"
+#include "debugTimes.h"
 
 #include "menu.h"
 #include "menuAction.h"
@@ -125,12 +126,17 @@ void setup() {
 
 void loop() {
     if (wifiIsActive) {
+        DebugTimes wifiTime;
         Network::checkWiFi();
         Network::checkMQTT();
+        wifiTime.stopConsol("WiFi-Time", 5);
     }
     
     driveManager->loop();
+
+    DebugTimes mainBatteryTime;
     mainBattery->loop();
+    mainBatteryTime.stopConsol("MainBatteryTime", 5);
 
     // static uint32_t lastMillis = 0;
     // if (millis() - lastMillis > 2000) {
@@ -204,7 +210,7 @@ void i2cScanner() {
             std::cout << "I2C device found at address 0x";
             if (address<16)
                 std::cout << "0";
-            std::cout << std::hex << (int) address << std::endl;
+            std::cout << std::hex << (int) address << std::dec << std::endl;
             // std::cout << (int) address << std::endl;
             nDevices++;
         }
@@ -212,7 +218,7 @@ void i2cScanner() {
             std::cout << "Unknow error at address 0x";
             if (address<16)
                 std::cout << "0";
-            std::cout << std::hex << (int) address << std::endl;
+            std::cout << std::hex << (int) address << std::dec << std::endl;
         }    
     }
     if (nDevices == 0)

src/moveControl.cpp

@@ -57,17 +57,22 @@ void MoveControl::loop() {
         ||  right_motor_time > 50
         ||  left_speed_time > 50
         ||  right_speed_time > 50) {
+
+        this->overTimeCounter++;
+        if (this->overTimeCounter >= this->overTimeMax) {
             char buf[128];
-        sprintf(buf, "left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n", left_motor_time, right_motor_time, left_speed_time, right_speed_time);
+            sprintf(buf, "left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n",
+                    left_motor_time, right_motor_time, left_speed_time, right_speed_time);
             std::cout << buf;
+            this->overTimeCounter = 0;
+        }
     }
 
-    static uint64_t lastMillis = 0;
+    if (millis() - this->lastMillis < this->delay) 
-    if (millis() - lastMillis < delay) 
         return;
 
     this->runMoveControl();
-    lastMillis = millis();
+    this->lastMillis = millis();
 }
 
 void MoveControl::runMoveControl() {