From e01127e3e36054c9cccb0bac1a13d88e301dca55 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Mon, 24 Oct 2022 22:08:19 +0200 Subject: [PATCH] try to work ntrip client --- include/moveControl.h | 4 ++ include/networkConfig.h | 5 ++- lib/Navigation/navigation.cpp | 2 + lib/Ntrip/NTRIPClient.cpp | 66 ++++++++++++++++++++++---------- lib/Ntrip/NTRIPClient.h | 12 +++--- lib/Times/debugTimes.cpp | 16 ++++++-- lib/Times/debugTimes.h | 9 ++++- src/SpecialMenus/GPS/menuGPS.cpp | 4 +- src/driveModi/driveManager.cpp | 6 +++ src/driveModi/driveManager.h | 1 + src/main.cpp | 10 ++++- src/moveControl.cpp | 17 +++++--- 12 files changed, 108 insertions(+), 44 deletions(-) diff --git a/include/moveControl.h b/include/moveControl.h index 0523ac5..f15be77 100644 --- a/include/moveControl.h +++ b/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 diff --git a/include/networkConfig.h b/include/networkConfig.h index 0957754..3dd027b 100644 --- a/include/networkConfig.h +++ b/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" diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index ee42178..da8f05e 100644 --- a/lib/Navigation/navigation.cpp +++ b/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; } diff --git a/lib/Ntrip/NTRIPClient.cpp b/lib/Ntrip/NTRIPClient.cpp index 0c10431..67792d0 100644 --- a/lib/Ntrip/NTRIPClient.cpp +++ b/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; - this->user = user; - this->password = password; + strcpy(this->mountPoint, mountPoint); + strcpy(this->user, user); + 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; - if (httpStatusCode == 401) + std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int) httpStatusCode + << " 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,23 +274,23 @@ 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; std::cout << "Lat: " << latitude - << "Lng: " << longitude - << "Alt: " << altitude << std::endl; + << " Lng: " << longitude + << " Alt: " << altitude << std::endl; std::cout << "Fix: " << fixTypeString - << "Carrier Solution: " << carrSolnString - << "Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl; + << " Carrier Solution: " << carrSolnString + << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl; } bool NTRIPClient::isConnected() { diff --git a/lib/Ntrip/NTRIPClient.h b/lib/Ntrip/NTRIPClient.h index e9e3bd7..0f5deef 100644 --- a/lib/Ntrip/NTRIPClient.h +++ b/lib/Ntrip/NTRIPClient.h @@ -7,6 +7,8 @@ #include #include +#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; - const char* mountPoint; - const char* user; - const char* password; + char host[128]; + char mountPoint[128]; + char user[128]; + char password[128]; const uint8_t delayTime = 20; const uint16_t timeOut = 5000; const uint16_t tryReconnectTime = 5000; diff --git a/lib/Times/debugTimes.cpp b/lib/Times/debugTimes.cpp index c1f3518..a229ab6 100644 --- a/lib/Times/debugTimes.cpp +++ b/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) - Serial.printf("%s needs %llu ms\n", name, time); -} \ No newline at end of file + if (time > minTime && DebugTimes::print) + std::cout << name << " needs " << time << " ms" << std::endl; + return time; +} diff --git a/lib/Times/debugTimes.h b/lib/Times/debugTimes.h index 50e97da..1f7e8f1 100644 --- a/lib/Times/debugTimes.h +++ b/lib/Times/debugTimes.h @@ -14,6 +14,7 @@ #include #include +#include #include @@ -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 \ No newline at end of file +#endif //DEBUG_TIMES_H diff --git a/src/SpecialMenus/GPS/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp index 4071d5d..ff290b3 100644 --- a/src/SpecialMenus/GPS/menuGPS.cpp +++ b/src/SpecialMenus/GPS/menuGPS.cpp @@ -83,7 +83,7 @@ void MenuGPS::printPage(uint8_t page) const { } if (fixType) std::cout << "Sats: " << (int) siv - << "HDOP: " << (int) hdop << std::endl; + << " HDOP: " << (int) hdop << std::endl; else std::cout << "Data isn't valid or no GPS signal" << std::endl; } @@ -107,7 +107,7 @@ void MenuGPS::printPage(uint8_t page) const { } if (fixType) std::cout << "Lat: " << lat - << "Lng: " << lng << std::endl; + << " Lng: " << lng << std::endl; else std::cout << "Data isn't valid or NO GPS signal" << std::endl; } diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index 9a6a451..e4a7355 100644 --- a/src/driveModi/driveManager.cpp +++ b/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(); } diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h index 76a5228..093c530 100644 --- a/src/driveModi/driveManager.h +++ b/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" diff --git a/src/main.cpp b/src/main.cpp index 9e3f56a..61f556a 100644 --- a/src/main.cpp +++ b/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) diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 88f0caa..86f859d 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -57,17 +57,22 @@ void MoveControl::loop() { || right_motor_time > 50 || left_speed_time > 50 || right_speed_time > 50) { - 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); - std::cout << buf; + + 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); + std::cout << buf; + this->overTimeCounter = 0; + } } - static uint64_t lastMillis = 0; - if (millis() - lastMillis < delay) + if (millis() - this->lastMillis < this->delay) return; this->runMoveControl(); - lastMillis = millis(); + this->lastMillis = millis(); } void MoveControl::runMoveControl() {