ken plan mehr

lib/NtripClient/ntripClient.cpp

@@ -0,0 +1,300 @@
+/**
+ * @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;
+    strcpy(this->host, host);
+    this->port = port;
+    strcpy(this->mountPoint, mountPoint);
+    strcpy(this->user, user);
+    strcpy(this->password, password);
+
+    this->ntripClient = new WiFiClient;
+}
+
+NTRIPClient::~NTRIPClient() {
+    delete this->ntripClient;
+}
+
+void NTRIPClient::loop() {
+    if (millis() - this->lastLoopTime < this->delayTime)
+        return;
+    this->lastLoopTime = millis();
+
+    this->gps->checkUblox();
+    this->gps->checkCallbacks();
+
+    if (millis() - this->lastGPGGAPushTime > this->pushGPGGATime) {
+        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)
+                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;
+                this->state = NTRIPClientStates::pushData;
+            } else {
+                uint8_t seconds = this->tryReconnectTime / 1000;
+                std::cout << "Could not connect to the caster. Trying again in " 
+                          << (int) seconds << " seconds." << std::endl;
+                this->lastNtripConnectTime = millis();
+            }
+            break;
+
+        case NTRIPClientStates::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;
+            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;
+    }
+}
+
+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);
+    // 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);
+
+    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) + 2;
+        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);
+    }
+
+    //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, "401") != nullptr)
+                httpStatusCode = 401;
+        }
+    }
+    response[responseIndex] = '\0';
+
+    // std::cout << "Caster response: " << response << std::endl;
+
+    if (httpStatusCode != 200) {
+        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;
+    }
+
+    std::cout << "Connected to: " << this->host << std::endl;
+    this->lastReceivedRtcmTime = millis();
+    return true;
+}
+
+void NTRIPClient::closeConnection() {
+    if (this->ntripClient->connected())
+        this->ntripClient->stop();
+    this->activated = false;
+    std::cout << "NtripClient disconnected from: " << this->host << std::endl;
+}
+
+bool NTRIPClient::processConnection() {
+    if (this->ntripClient->connected()) {
+        uint8_t rtcmData[this->bufferSize * 4];
+        uint16_t rtcmCount = 0;
+
+        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(carrSolnString, "None");
+    else if (carrSoln == 1)
+        strcpy(carrSolnString, "Floating");
+    else if (carrSoln == 2)
+        strcpy(carrSolnString, "Fixed");
+    else
+        strcpy(carrSolnString, "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;
+}
+
+bool NTRIPClient::isConnected() {
+    if (this->state == NTRIPClientStates::pushData)
+        return true;
+    return false;
+}

lib/NtripClient/ntripClient.h

@@ -0,0 +1,65 @@
+#ifndef NTRIP_CLIENT
+#define NTRIP_CLIENT
+
+#include <WiFiClient.h>
+#include <Arduino.h>
+#include <base64.h>
+#include <iostream>
+#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
+
+#include "debugTimes.h"
+
+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 setTransmitLocation(bool b) { this->transmitLocation = b; }
+        void setActivated(bool b) { this->activated = b; }
+
+        bool isConnected();
+        NTRIPClientStates getClientState() { return this->state; }
+
+        static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
+
+    private:
+        void pushGPGGA(NMEA_GGA_data_t *nmeaData);
+        bool beginClient();
+        void closeConnection();
+        bool processConnection();
+
+        SFE_UBLOX_GNSS* gps;
+        WiFiClient* ntripClient;
+        NTRIPClientStates state = NTRIPClientStates::closeConnection;
+
+        bool transmitLocation = true;
+        bool activated = true;
+
+        uint16_t port;
+        uint32_t lastReceivedRtcmTime = 0;
+        uint32_t lastNtripConnectTime = 0;
+        uint32_t lastGPGGAPushTime = 0;
+        uint32_t lastLoopTime = 0;
+
+        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;
+        const uint16_t bufferSize = 512;
+        const uint16_t pushGPGGATime = 10000;
+};
+
+#endif