From a1a976be57baec1a46ff7f26807e88635766b1d5 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Wed, 21 Sep 2022 12:50:01 +0200 Subject: [PATCH] new ntrip client --- include/moveControl.h | 8 +- include/networkConfig.h | 21 +- lib/Menu/menuIntInput.cpp | 161 ++++++++- lib/Menu/menuIntInput.h | 24 +- lib/Navigation/navigation.cpp | 12 +- lib/Ntrip/NTRIPClient.cpp | 328 +++++++++++++----- lib/Ntrip/NTRIPClient.h | 55 ++- platformio.ini | 2 +- .../driveModi/TestMode/menuTestMode.cpp | 83 +++-- .../driveModi/TestMode/menuTestMode.h | 11 +- src/driveModi/Modi/TestMode/testMode.cpp | 154 ++++---- src/driveModi/Modi/TestMode/testMode.h | 17 +- src/driveModi/driveManager.cpp | 2 + src/main.cpp | 12 +- src/moveControl.cpp | 19 + 15 files changed, 658 insertions(+), 251 deletions(-) diff --git a/include/moveControl.h b/include/moveControl.h index b526542..0523ac5 100644 --- a/include/moveControl.h +++ b/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 diff --git a/include/networkConfig.h b/include/networkConfig.h index b815a0f..0957754 100644 --- a/include/networkConfig.h +++ b/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 MQTT -#define MQTT_SERVER "192.168.1.7" -#define MQTT_PORT 1883 -#define MQTT_AUTH -#define MQTT_USER "kleiax" -#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#" +#define WLAN_GATEWAY "192.168.43.1" +// Only for defines reasons + #define MQTT_SERVER "172.22.64.216" + #define MQTT_PORT 1883 #endif //HOTSPOT //Network config Rhede diff --git a/lib/Menu/menuIntInput.cpp b/lib/Menu/menuIntInput.cpp index 2d0d96b..fca3430 100644 --- a/lib/Menu/menuIntInput.cpp +++ b/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) - values[currentPosition]--; + if (this->values[this->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) - values[currentPosition]++; + if (this->values[this->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; } - - - diff --git a/lib/Menu/menuIntInput.h b/lib/Menu/menuIntInput.h index 10e3ae7..2337d84 100644 --- a/lib/Menu/menuIntInput.h +++ b/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 setMax(int16_t max = 100) { this->max = max; } + void setEntry(uint8_t valueNumber, const char* name, int16_t startValue = 0); + + 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 max = INT16_MAX; + int16_t *originalValues; + int16_t *min; + int16_t *max; char *names; }; diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index a8664c0..6e24ef3 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -95,11 +95,13 @@ void Navigation::loop() { 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); + if (this->isNtripInit) { + uint8_t rtcmData[512 * 4]; + uint16_t rtcmCount = this->ntripClient->available(); + if (rtcmCount) { + this->ntripClient->readBytes(rtcmData, rtcmCount); + this->gps->pushRawData(rtcmData, rtcmCount); + } } } diff --git a/lib/Ntrip/NTRIPClient.cpp b/lib/Ntrip/NTRIPClient.cpp index 9ffd5da..9097406 100644 --- a/lib/Ntrip/NTRIPClient.cpp +++ b/lib/Ntrip/NTRIPClient.cpp @@ -1,94 +1,264 @@ -#include"NTRIPClient.h" -bool NTRIPClient::reqSrcTbl(char* host,uint16_t port){ - if(!connect(host,port)){ - Serial.print("Cannot connect to "); - Serial.println(host); - return false; - } /*p = String("GET ") + String("/") + String(" HTTP/1.0\r\n"); - p = p + String("User-Agent: NTRIP Enbeded\r\n");*/ - print( - "GET / HTTP/1.0\r\n" - "User-Agent: NTRIPClient for Arduino v1.0\r\n" - ); - unsigned long timeout = millis(); - while (available() == 0) { - if (millis() - timeout > 5000) { - Serial.println("Client Timeout !"); - stop(); - return false; - } - delay(10); - } - char buffer[50]; - readLine(buffer,sizeof(buffer)); - if(strncmp((char*)buffer,"SOURCETABLE 200 OK",17)) { - Serial.print((char*)buffer); - return false; - } - return true; +/** + * @file NTRIPClient.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2022-09-18 + * + * @copyright Copyright (c) 2022 + * + */ + +#include "NTRIPClient.h" + +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; + this->port = port; + this->mountPoint = mountPoint; + this->user = user; + this->password = password; + + this->ntripClient = new WiFiClient; } -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=""; - 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 +NTRIPClient::~NTRIPClient() { + delete this->ntripClient; +} - p = p + String("Authorization: Basic "); - p = p + auth; - p = p + String("\r\n"); +void NTRIPClient::loop() { + if (millis() - this->lastLoopTime < this->delayTime) + 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 - Serial.println(p); - #endif - unsigned long timeout = millis(); - while (available() == 0) { - if (millis() - timeout > 20000) { - Serial.println("Client Timeout !"); +} + +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->(SFE_UBLOX_DGNSS_MODE_FIXED); // Set the differential mode - ambiguities are fixed whenever possible + this->gps->setNavigationFrequency(1); + this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP); + 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)); - if(strncmp((char*)buffer,"ICY 200 OK",10)) - { - Serial.print((char*)buffer); - return false; + + //Check reply + uint16_t httpStatusCode = 0; + char response[this->bufferSize]; + uint16_t responseIndex = 0; + 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) { - return reqRaw(host,port,mntpnt,"",""); +void NTRIPClient::closeConnection() { + 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) { - int len = 0; - while(available()) { - _buffer[len] = read(); - len++; - if(_buffer[len-1] == '\n' || len >= size) break; - } - _buffer[len]='\0'; +bool NTRIPClient::processConnection() { + if (this->ntripClient->connected()) { + uint8_t rtcmData[this->bufferSize * 4]; + uint16_t rtcmCount = 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; } diff --git a/lib/Ntrip/NTRIPClient.h b/lib/Ntrip/NTRIPClient.h index 1ad92b4..8e3d9f7 100644 --- a/lib/Ntrip/NTRIPClient.h +++ b/lib/Ntrip/NTRIPClient.h @@ -3,16 +3,55 @@ #include #include -#include +#include +#include +#include -class NTRIPClient : public WiFiClient{ - public : - bool reqSrcTbl(char* host,uint16_t port); //request MountPoints List serviced the NTRIP Caster - bool reqRaw(char* host,uint16_t port,char* mntpnt,char* user,char* psw); //request RAW data from Caster - bool reqRaw(char* host,uint16_t port,char* mntpnt); //non user - int readLine(char* buffer,int size); +enum NTRIPClientStates { + openConnection, + pushData, + closeConnection, + wait +}; +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 diff --git a/platformio.ini b/platformio.ini index 02db329..c5bc508 100644 --- a/platformio.ini +++ b/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] diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp index 9f35fde..4b726f8 100644 --- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp +++ b/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[1] = 2; - this->values[2] = 3; + // this->values[0] = 0; + // this->values[1] = 0; - strcpy(this->names[0], "Alpha"); - strcpy(this->names[1], "Beta"); - strcpy(this->names[2], "Gamma"); + // strcpy(this->names[0], "Centimeter"); + // strcpy(this->names[1], "Degree"); } 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); - - drivingMenu->addEntry(drivingForwardAction); - drivingMenu->addEntry(drivingBackwardAction); - drivingForwardMenu->addEntry(drivingForwardLeftAction); - drivingForwardMenu->addEntry(drivingForwardStraightAction); - drivingForwardMenu->addEntry(drivingForwardRightAction); - drivingBackwardMenu->addEntry(drivingBackwardLeftAction); - drivingBackwardMenu->addEntry(drivingBackwardStraightAction); - drivingBackwardMenu->addEntry(drivingBackwardRightAction); + engineMenu->addEntry(engineBothAction); 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) - std::cout << "Dummy in Action" << values[0] << values[1] << values[2] << std::endl; - else - std::cout << "Dummy in Action kene dre values" << std::endl; + // This function calls a member function with a pointer + // Very helpful site: + // https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types + 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]); } diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h index b0580b1..93f8cf6 100644 --- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h +++ b/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 diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index 198c572..b97a246 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/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->setRotationSpeed(0); + this->moveControl->setDrivingStatus(DrivingStatus::stop); } } -void TestMode::setSpeed(double speed) { - this->speed = speed; +void TestMode::setSpeed(int16_t speed) { + this->speed = (double) speed / 100.0; } -void TestMode::setRotationSpeed(double speed) { - this->rotationSpeed = speed; +void TestMode::setRotationSpeed(int16_t 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; - return true; -} + if (degree < 0) + 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) { - if (this->busy) - return false; + } else if (degree == 0) { + //Only forward or backward + this->moveControl->setRotationSpeed(0); - this->moveControl->setRotationSpeed(0); - this->moveControl->setSpeed(-this->speed); + if (cm < 0) + 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->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; + this->moveControl->setDrivingStatus(DrivingStatus::stop); 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; +} diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h index 12d3d40..818e784 100644 --- a/src/driveModi/Modi/TestMode/testMode.h +++ b/src/driveModi/Modi/TestMode/testMode.h @@ -21,19 +21,14 @@ class TestMode : public DriveModi { void loop() override; - void setSpeed(double speed); - void setRotationSpeed(double speed); + void setSpeed(int16_t speed); + void setRotationSpeed(int16_t speed); - bool driveForward(uint16_t cm); - bool driveBackward(uint16_t cm); + bool drive(int16_t cm = 0, int16_t degree = 0); - 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); + bool leftEngine(int16_t powerPercentage, int16_t seconds); + bool rightEngine(int16_t powerPercentage, int16_t seconds); + bool bothEngine(int16_t powerPercentage, int16_t seconds); private: MoveControl *moveControl; diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index afb78f4..9a6a451 100644 --- a/src/driveModi/driveManager.cpp +++ b/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() { diff --git a/src/main.cpp b/src/main.cpp index e6ca5aa..9e3f56a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #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); diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 356253c..88f0caa 100644 --- a/src/moveControl.cpp +++ b/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;