new ntrip client

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;
 }
-
-
-

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;
 
 };

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);
+        }
     }
 }
 

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;
 }

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{
-  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