added description and video to readme

- fix gyroscope
- fix init screen

.vscode/settings.json

@@ -1,5 +1,6 @@
 {
     "files.insertFinalNewline": true,
+    "editor.wordWrap": "on",
     "cSpell.ignoreWords": [
         "ledc",
         "write"
@@ -132,6 +133,7 @@
         "Doxygen",
         "Dutycycle",
         "gast",
+        "gnss",
         "GNSS",
         "GPGGA",
         "HSPI",
@@ -140,9 +142,12 @@
         "kleiax",
         "Lebennig",
         "microcontroller",
+        "Mqtt",
         "MQTT",
         "NMEA",
         "NMEAGPGGA",
+        "ntrip",
+        "Ntrip",
         "NTRIP",
         "pcnt",
         "pidl",
@@ -150,6 +155,8 @@
         "Punica",
         "RHEDE",
         "Rtcm",
+        "sapos",
+        "SAPOS",
         "Schalke",
         "Soln",
         "Systeminformation",

README.md

@@ -0,0 +1,5 @@
+# Autonomer Rover
+Testfahrt auf Youtube: https://youtu.be/GigIyo-vghE?si=jhProl1DQFUrcz7w
+
+# Beschreibung
+Vollständige Softwarelösung und Hardwareerweiterungen für einen Rover mit Skid-Antrieb. Das System wurde in C++ geschrieben und es wurden erweiterte Konzepte der Objektorientierung wie Polymorphie eingesetzt. Die Datenhaltung wurde erfolgreich von der Logik getrennt, hierfür wurde unter anderem eine API mit Express.js implementiert. Hardwareseitig wurde PWM für die Motorsteuerung, IC2 für verschiedene Sensoren und SPI für das GNSS-Modul eingesetzt. Zusätzlich wurden Encoder an den Motoren verbaut, um diese präzise zu steuern. Überdies wurde eine RTK-Referenzstation geplant und umgesetzt, dafür wurde die Antenne, ein weiteres GNSS-Modul und ein Embedded-Server installiert und konfiguriert. Durch das Zusammenspiel dieser Systeme kann der Rover Routen im niedrigen einstelligen Zentimeterbereich aufzeichnen und abfahren.

doc/Doxygen/Doxyfile

@@ -44,7 +44,7 @@ PROJECT_NUMBER         =
 # for a project that appears at the top of each page and should give viewer a
 # quick idea about the purpose of the project. Keep the description short.
 
-PROJECT_BRIEF          = "A small verhicle which should be drive a route with GPS"
+PROJECT_BRIEF          = "A small verhicle which should be drive a route with GNNS and RTK"
 
 # With the PROJECT_LOGO tag one can specify a logo or an icon that is included
 # in the documentation. The maximum height of the logo should not exceed 55

doc/TODO allgemein.txt

@@ -1,6 +1,8 @@
 Maybe:
     -> Time Measurement in component interface
     -> Time warnings
+    -> Blocking Battery Function in Battery class
+        - Callback function for output and shutdown things
 
 Do later:
     -> Program underfloorLighting
@@ -32,16 +34,12 @@ Do later:
         - navigation
         - autopilot
 
+Have to:
+    -> check new distance calculation between points
+    -> check new course calculation between points
+    -> menu route data und ui disconnecten
+    -> use routeCache in captureRoute and autopilot
+    -> make ntrip client useable without user and password
+    -> understand gyroscope offsets
 
-Do now:
-Code:
-    Fernbedienung!
-
-Latex:
-    Anhang: Liste mit allen Komponenten und Kurzbeschreibung
-    Unterschied funktionale und nicht funktionale Anforderungen
-    Overfull H-Boxen
-    Test Motor, Zeiten kontrolieren. Wenn einfach so dann bezug auf fehler bei Timing und freeRtos
-    Was mit den nicht erfüllten Anforderungen machen?
-    Hardware doppelt erklärt
 

include/networkConfig.h

@@ -14,16 +14,18 @@
 
 #include <stdint.h>
 
-#define HW1
+#define ZUHAUSE
+#define SAPOS_NI
+// #define RTK2GO
 
 #ifdef HOTSPOT
 namespace NetworkConfig
 {
-    const char ssid[] = "Kleiax Handy";
+    const char ssid[] = "Nubsi";
     const char password[] = "12345677";
-    const char ip[] = "192.168.43.4";
+    const char ip[] = "192.168.72.7";
     const char subnet[] = "255.255.255.0";
-    const char gateway[] = "192.168.43.1";
+    const char gateway[] = "192.168.72.228";
     const char dns[] = "8.8.8.8";
     const bool mqtt = false;
 }
@@ -56,6 +58,19 @@ namespace NetworkConfig
 }
 #endif // RHEDE
 
+#ifdef ZUHAUSE
+namespace NetworkConfig
+{
+    const char ssid[] = "Tuedelkram";
+    const char password[] = "3!Kaesehoch";
+    const char ip[] = "192.168.178.13";
+    const char subnet[] = "255.255.255.0";
+    const char gateway[] = "192.168.178.1";
+    const char dns[] = "8.8.8.8";
+    const bool mqtt = false;
+}
+#endif // ZUHAUSE
+
 namespace MqttConfig
 {
     const char server[] = "192.168.1.7";
@@ -64,13 +79,28 @@ namespace MqttConfig
     const char password[] = "p?{$_~5%hBM7wrcFkr55KWr#";
 }
 
+#ifdef RTK2GO
 namespace NtripConfig
 {
     const char host[] = "rtk2go.com";
     const uint16_t port = 2101;
-    // const char mountPoint[] = "GER-Dortmund";
+    const char mountPoint[] = "GER-Dortmund";
     // const char mountPoint[] = "GER-Papenburg";
-    const char mountPoint[] = "Rtkramerntrip";
+    // const char mountPoint[] = "Rtkramerntrip";
     const char user[] = "alklein1@gmx.de";
     const char password[] = "none";
+    const bool sendOwnPosition = false;
 }
+#endif // RTK2GO
+
+#ifdef SAPOS_NI
+namespace NtripConfig
+{
+    const char host[] = "sapos-ni-ntrip.de";
+    const uint16_t port = 2101;
+    const char mountPoint[] = "VRS_3_4G_NI";
+    const char user[] = "ni_FHDo01";
+    const char password[] = "ALdx-1-3e49";
+    const bool sendOwnPosition = true;
+}
+#endif // RTK2GO

lib/Component/component.cpp

@@ -19,7 +19,7 @@ void Component::loop()
             (*it)->loop();
         }
     }
-    this->runAsChild();F
+    this->runAsChild();
 
     if (this->onlyChilds)
     {

lib/Component/component.h

@@ -62,7 +62,7 @@ public:
      * If the component is deactivated the call of loop() ha no effect
      */
     void deactivate() { this->active = false; }
-    void activate() { this->active = false; }
+    void activate() { this->active = true; }
 
 protected:
     /**

lib/LcdWrapper/LcdWrapper.cpp

@@ -14,7 +14,7 @@
 LcdWrapper::LcdWrapper(LiquidCrystal_I2C *lcd)
     : lcd{lcd}, changed{false}
 {
-    this->clear();
+    // this->clear();
 }
 
 void LcdWrapper::run()

lib/MotorControl/motorControl.cpp

@@ -194,6 +194,7 @@ void MotorControl::setRealPower(int8_t power)
     }
 
     ledcWrite(this->pwmChannel, pwm_val);
+    std::cout << "MotorControl::serRealPower - pwm: " << static_cast<int>(pwm_val) << std::endl;
     this->dutycycle = pwm_val;
 }
 

lib/Navigation/navigation.h

@@ -35,7 +35,7 @@ struct CourseCorrection
 /**
  * @brief This class navigate an object
  *
- * The class use the given Route and the gps device
+ * The class use the given Route and the gnss device
  * to tell the driver in which direction he have to
  * be drive and the distance to the next checkpoint.
  *

lib/NtripClient/ntripClient.cpp

@@ -11,9 +11,13 @@
 
 #include "ntripClient.h"
 
-NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS *gps, const char *host, uint16_t port, const char *mountPoint, const char *user, const char *password)
+NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS *gnss, const char *host, uint16_t port, const char *mountPoint, const char *user, const char *password)
-    : gps{gps}, port{port}, host{host}, mountPoint{mountPoint}, user{user}, password{password}, ntripClient{new WiFiClient}, state{NTRIPClientStates::closeConnection}
+    : gnss{gnss}, port{port}, ntripClient{new WiFiClient}, state{NTRIPClientStates::closingConnection}
 {
+    strcpy(this->host, host);
+    strcpy(this->mountPoint, mountPoint);
+    strcpy(this->user, user);
+    strcpy(this->password, password);
     Component::loopDelay = NTRIPClient::loopDelay;
 }
 
@@ -26,10 +30,10 @@ void NTRIPClient::run()
 {
     switch (this->state)
     {
-    case NTRIPClientStates::openConnection:
+    case NTRIPClientStates::openingConnection:
         if (!this->activated)
         {
-            this->state = NTRIPClientStates::closeConnection;
+            this->state = NTRIPClientStates::closingConnection;
             break;
         }
 
@@ -37,33 +41,34 @@ void NTRIPClient::run()
         if (this->beginClient())
         {
             std::cout << "Connected to the NTRIP caster!" << std::endl;
-            this->state = NTRIPClientStates::pushData;
+            this->state = NTRIPClientStates::pushingData;
         }
         else
         {
             std::cout << "Failed!" << std::endl;
-            this->state = NTRIPClientStates::wait;
+            this->state = NTRIPClientStates::waiting;
+            this->lastReconnectTime = millis();
             this->activated = false;
         }
         break;
 
-    case NTRIPClientStates::pushData:
+    case NTRIPClientStates::pushingData:
         if (!processConnection() || !this->activated)
         {
-            this->state = NTRIPClientStates::closeConnection;
+            this->state = NTRIPClientStates::closingConnection;
         }
         break;
 
-    case NTRIPClientStates::closeConnection:
+    case NTRIPClientStates::closingConnection:
         std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
         this->closeConnection();
-        state = NTRIPClientStates::wait;
+        state = NTRIPClientStates::waiting;
         break;
 
-    case NTRIPClientStates::wait:
+    case NTRIPClientStates::waiting:
         if (this->activated)
         {
-            this->state = NTRIPClientStates::openConnection;
+            this->state = NTRIPClientStates::openingConnection;
         }
         else
         {
@@ -76,7 +81,7 @@ void NTRIPClient::run()
 
     default:
         std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
-        this->state = NTRIPClientStates::closeConnection;
+        this->state = NTRIPClientStates::closingConnection;
         break;
     }
 }
@@ -86,14 +91,14 @@ void NTRIPClient::runAsChild()
     this->pushGPGGA();
 }
 
-void NTRIPClient::gpsConfiguration()
+void NTRIPClient::gnssConfiguration()
 {
-    this->gps->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
+    this->gnss->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
-    this->gps->setPortInput(COM_PORT_SPI, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
+    this->gnss->setPortInput(COM_PORT_SPI, 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->gnss->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED);
-    this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
+    this->gnss->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
-    this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
+    this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI);
 }
 
 bool NTRIPClient::setActivated(bool state)
@@ -145,29 +150,71 @@ bool NTRIPClient::beginClient()
     std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl;
 
     // Generate the server request (GET)
-    snprintf(static_cast<char *>(serverRequest),
+    if (this->useNtripRev1)
-             this->bufferSize,
+    {
-             static_cast<const char *>("GET /%s HTTP/1.0\r\nUser-Agent: NTRIP SparkFun u-blox Client v1.0\r\n"),
+        snprintf(static_cast<char *>(serverRequest),
-             this->mountPoint);
+                 this->bufferSize,
+                 static_cast<const char *>("GET /%s HTTP/1.0\r\nHost: %s:%u\r\nUser-Agent: NTRIP KleiaxNtripClient/0.1\r\nAccept: */*\r\nConnection: close\r\n"),
+                 this->mountPoint,
+                 this->host,
+                 this->port);
+    }
+    else
+    {
+        snprintf(static_cast<char *>(serverRequest),
+                 this->bufferSize,
+                 static_cast<const char *>("GET /%s HTTP/1.1\r\nHost: %s:%u\r\nNtrip-Version: Ntrip/2.0\r\nUser-Agent: NTRIP KleiaxNtripClient/0.1\r\nAccept: */*\r\nConnection: close\r\n"),
+                 this->mountPoint,
+                 this->host,
+                 this->port);
+    }
 
-    // Credentials
+    // Add own Position if activated
-    const uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
+    if (this->transmitLocation && !this->useNtripRev1)
-    auto *userCredentials = new char[userCredentialsLength];
+    {
-    snprintf(static_cast<char *>(userCredentials), userCredentialsLength, static_cast<const char *>("%s:%s"), this->user, this->password);
+        std::cout << "NTRIPClient::beginClient - Add own position to server request." << std::endl;
+        auto *data = new NMEA_GGA_data_t;
+        const uint8_t res = this->gnss->getLatestNMEAGPGGA(data);
+        if (res > 0) // valid data
+        {
+            char positionUpdate[this->bufferSizePushGPGGA];
+            snprintf(static_cast<char *>(positionUpdate),
+                     this->bufferSizePushGPGGA,
+                     static_cast<const char *>("Ntrip-GGA: %s"),
+                     data->nmea);
+            strncat(static_cast<char *>(serverRequest), static_cast<const char *>(positionUpdate), this->bufferSize);
+        }
+        else
+        {
+            std::cout << "NTRIPClient::beginClient - No valid data to send position to caster." << std::endl;
+        }
+        delete data;
 
-    std::cout << "Sending credentials: " << userCredentials << std::endl;
+        this->lastGPGGAPushTime = millis();
+    }
 
-    // Encode
+    if (this->user && this->password)
-    const base64 base;
+    {
-    const String strEncodedCredentials = base64::encode(userCredentials);
+        // Credentials
-    delete userCredentials;
+        const uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
-    char encodedCredentials[strEncodedCredentials.length() + 1];
+        auto *userCredentials = new char[userCredentialsLength];
-    strEncodedCredentials.toCharArray(static_cast<char *>(encodedCredentials), sizeof(encodedCredentials));
+        snprintf(static_cast<char *>(userCredentials), userCredentialsLength, static_cast<const char *>("%s:%s"), this->user, this->password);
 
-    snprintf(credentials, sizeof(credentials), static_cast<const char *>("Authorization: Basic %s\r\n"), static_cast<const char *>(encodedCredentials));
+        std::cout << "Sending credentials: " << userCredentials << std::endl;
+
+        // Encode
+        const base64 base;
+        const String strEncodedCredentials = base64::encode(userCredentials);
+        delete userCredentials;
+        char encodedCredentials[strEncodedCredentials.length() + 1];
+        strEncodedCredentials.toCharArray(static_cast<char *>(encodedCredentials), sizeof(encodedCredentials));
+
+        snprintf(credentials, sizeof(credentials), static_cast<const char *>("Authorization: Basic %s\r\n"), static_cast<const char *>(encodedCredentials));
+
+        // Add the encoded credentials to the server request
+        strncat(static_cast<char *>(serverRequest), static_cast<const char *>(credentials), this->bufferSize);
+    }
 
-    // Add the encoded credentials to the server request
-    strncat(static_cast<char *>(serverRequest), static_cast<const char *>(credentials), this->bufferSize);
     strncat(static_cast<char *>(serverRequest), static_cast<const char *>("\r\n"), this->bufferSize);
 
     std::cout << static_cast<const char *>("serverRequest size: ")
@@ -273,7 +320,7 @@ bool NTRIPClient::processConnection()
         if (rtcmCount > 0)
         {
             this->lastReceivedRtcmTime = millis();
-            this->gps->pushRawData(static_cast<uint8_t *>(rtcmData), rtcmCount);
+            this->gnss->pushRawData(static_cast<uint8_t *>(rtcmData), rtcmCount);
             // std::cout << "Pushed " << rtcmCount << " RTCM bytes to ZED." << std::endl;
         }
     }
@@ -317,7 +364,7 @@ void NTRIPClient::checkAutoReconnect()
 
 void NTRIPClient::pushGPGGA()
 {
-    if (!this->transmitLocation && !this->activated)
+    if ((!this->transmitLocation || !this->activated) || !this->ntripClient->connected())
     {
         return;
     }
@@ -328,21 +375,26 @@ void NTRIPClient::pushGPGGA()
     }
     this->lastGPGGAPushTime = millis();
 
-    if (!this->ntripClient->connected())
-    {
-        std::cout << "Failed to pushing GGA to server: " << std::endl;
-    }
-
     auto *data = new NMEA_GGA_data_t;
-    const uint8_t res = this->gps->getLatestNMEAGPGGA(data);
+    const uint8_t res = this->gnss->getLatestNMEAGPGGA(data);
-    if (res == 2)
+    if (res > 0) // 2 means fresh data
     {
-        this->ntripClient->print(reinterpret_cast<const char *>(data));
+        char positionUpdate[this->bufferSizePushGPGGA];
+        snprintf(static_cast<char *>(positionUpdate),
+                 this->bufferSizePushGPGGA,
+                 static_cast<const char *>("%s"),
+                 data->nmea);
+        this->ntripClient->write(positionUpdate, strlen(positionUpdate));
+        std::cout << "Position update: " << positionUpdate << std::endl;
+    }
+    else
+    {
+        std::cout << "NTRIPClient::pushGPGGA - No valid data to send position to caster." << std::endl;
     }
     delete data;
 }
 
 bool NTRIPClient::isConnected()
 {
-    return this->state == NTRIPClientStates::pushData;
+    return this->state == NTRIPClientStates::pushingData;
 }

lib/NtripClient/ntripClient.h

@@ -27,10 +27,10 @@
  */
 enum NTRIPClientStates
 {
-    openConnection,
+    openingConnection,
-    pushData,
+    pushingData,
-    closeConnection,
+    closingConnection,
-    wait,
+    waiting,
     notAvailable
 };
 
@@ -47,21 +47,21 @@ public:
     /**
      * @brief Construct a new NTRIPClient object
      *
-     * @param gps The Gnss module
+     * @param gnss The Gnss module
      * @param host
      * @param port
      * @param mountPoint
      * @param user
      * @param password
      */
-    NTRIPClient(SFE_UBLOX_GNSS *gps, const char *host, uint16_t port, const char *mountPoint, const char *user, const char *password);
+    NTRIPClient(SFE_UBLOX_GNSS *gnss, const char *host, uint16_t port, const char *mountPoint, const char *user = nullptr, const char *password = nullptr);
     ~NTRIPClient();
 
     /**
      * @brief Configure the Gnss module to accept correction data
      *
      */
-    void gpsConfiguration();
+    void gnssConfiguration();
 
     /**
      * @brief Activate or deactivate the location transmission
@@ -82,6 +82,7 @@ public:
      */
     bool setActivated(bool state);
     void setAutoReconnect(bool state);
+    void useOutdatedNtripRev1 (bool rev1 = false ) { this->useNtripRev1 = rev1; }
 
     bool isConnected();
 
@@ -103,13 +104,14 @@ private:
     bool processConnection();
     void checkAutoReconnect();
 
-    SFE_UBLOX_GNSS *gps;
+    SFE_UBLOX_GNSS *gnss;
     WiFiClient *ntripClient;
     NTRIPClientStates state = NTRIPClientStates::notAvailable;
 
     bool transmitLocation = false;
     bool activated = true;
     bool autoReconnect = false;
+    bool useNtripRev1 = false;
 
     uint8_t reconnectAttemps = 0;
     uint16_t port;
@@ -118,15 +120,17 @@ private:
     uint32_t lastGPGGAPushTime = 0;
     uint32_t lastReconnectTime = 0;
 
-    const char *host;
+    static const uint8_t connectionDataLength = 32;
-    const char *mountPoint;
+    char host[connectionDataLength];
-    const char *user;
+    char mountPoint[connectionDataLength];
-    const char *password;
+    char user[connectionDataLength];
+    char password[connectionDataLength];
     const uint8_t maxReconnectAttemps = 10;
-    const uint16_t reconnectDelayTime = 1000;
+    const uint16_t reconnectDelayTime = 5000;
     const uint16_t timeOut = 10000;
     const uint16_t bufferSize = 512;
-    const uint16_t pushGPGGATime = 10000;
+    const uint16_t bufferSizePushGPGGA = 128;
+    const uint16_t pushGPGGATime = 5000;
 
     static constexpr uint8_t loopDelay = 20;
 };

lib/Point/point.cpp

@@ -56,14 +56,29 @@ bool Point::operator==(const Point &rhs) const
 
 double Point::distanceTo(const Coordinates &point) const
 {
-    const Coordinates begin = this->coordinates;
+    // Old implementation
-    const Coordinates end = point;
+    // const Coordinates begin = this->coordinates;
+    // const Coordinates end = point;
 
-    const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
+    // const double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
-    const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
+    // const double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
-    const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
+    // const double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
 
-    return sqrt(dx * dx + dy * dy);
+    // return sqrt(dx * dx + dy * dy);
+
+    double latStart = this->toRad(this->coordinates.lat);
+    double lonStart = this->toRad(this->coordinates.lon);
+    double latEnd = this->toRad(point.lat);
+    double lonEnd = this->toRad(point.lon);
+
+    const double earthRadius = 6371.0; //km
+    double cartesianX_1 = cos(latStart) * lonStart * earthRadius;
+    double cartesianY_1 = latStart * earthRadius;
+    double cartesianX_2 = cos(latEnd) * lonEnd * earthRadius;
+    double cartesianY_2 = latEnd * earthRadius;
+
+    return sqrt(pow(cartesianX_2 - cartesianX_1, 2)
+              + pow(cartesianY_2 - cartesianY_1, 2));
 }
 
 double Point::distanceTo(const Point &point) const
@@ -73,13 +88,24 @@ double Point::distanceTo(const Point &point) const
 
 int16_t Point::courseTo(const Coordinates &point) const
 {
-    const Coordinates begin = this->coordinates;
+    // const Coordinates begin = this->coordinates;
-    const Coordinates end = point;
+    // const Coordinates end = point;
 
-    const double phi = log(tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4));
+    // const double phi = log(tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4));
-    const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
+    // const double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
 
-    return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
+    // return static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
+
+    double latStart = this->toRad(this->coordinates.lat);
+    double lonStart = this->toRad(this->coordinates.lon);
+    double latEnd = this->toRad(point.lat);
+    double lonEnd = this->toRad(point.lon);
+
+    double y = sin(lonEnd - lonStart) * cos(latEnd);
+    double x = cos(latStart) * sin(latEnd) - sin(latStart) * cos(latEnd) * cos(lonEnd - lonStart);
+    double theta = atan2(y, x);
+
+    return static_cast<int16_t>(this->toDeg(theta)) % 360;
 }
 
 int16_t Point::courseTo(const Point &point) const

lib/Point/point.h

@@ -131,6 +131,17 @@ public:
 
 private:
     void init(uint32_t horizontalAccuracy, uint32_t creationTime);
+    double toRad (double degree) const
+    {
+        constexpr double radFactor = M_PI / 180.0;
+        return degree * radFactor;
+    };
+
+    double toDeg(double radiant) const
+    {
+        constexpr double degFactor = 180.0 / M_PI;
+        return radiant * degFactor;
+    };
 
     Accuracy accuracy = Accuracy::none;
     Coordinates coordinates{0, 0};

lib/Route/route.cpp

@@ -11,6 +11,8 @@
 
 #include "route.h"
 
+Route Route::routeCache = Route();
+
 void Route::addPointToRoute(Point point)
 {
     this->points.push_back(point);
@@ -88,6 +90,7 @@ Point Route::getPreviousPoint()
         this->currentPoint--;
         return *this->it;
     }
+
     return point;
 }
 
@@ -98,3 +101,7 @@ RouteInfo Route::getRouteInfo()
     info.currentPoint = this->currentPoint;
     return info;
 }
+
+Route& Route::getRouteCache() {
+    return Route::routeCache;
+}

lib/Route/route.h

@@ -100,12 +100,16 @@ public:
      */
     bool getStarted() const { return this->started; }
 
+    static Route& getRouteCache();
+
 private:
     uint16_t currentPoint = 0;
     bool started = false;
 
     std::list<Point> points;
     std::list<Point>::iterator it;
+
+    static Route routeCache;
 };
 
 #endif // ROUTE_H

lib/Sensors/sensorData.cpp

@@ -25,11 +25,12 @@ SensorData::~SensorData()
     delete this->ntripClient;
 }
 
-void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password)
+void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password, bool sendOwnPosition)
 {
     this->ntripClient = new NTRIPClient(this->gnss, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
-    this->ntripClient->gpsConfiguration();
+    this->ntripClient->gnssConfiguration();
     this->ntripClient->loop();
+    this->ntripClient->setTransmitLocation(sendOwnPosition);
     this->ntripClient->setActivated(false);
     this->isNtripInit = true;
     this->addChildComponent(this->ntripClient);
@@ -89,7 +90,7 @@ void SensorData::enableGyroscope()
     this->gyroscope->initialize();
     if (!this->gyroscope->testConnection())
     {
-        std::cout << "SensorData::enableGyroscope: Gyroskop is not conntected. Freeze!" << std::endl;
+        std::cout << "SensorData::enableGyroscope: Gyroscope is not connected. Freeze!" << std::endl;
         while (true)
         {
         }
@@ -107,7 +108,7 @@ void SensorData::enableGyroscope()
     {
         this->gyroscope->CalibrateAccel(6);
         this->gyroscope->CalibrateGyro(6);
-        this->gyroscope->PrintActiveOffsets();
+        // this->gyroscope->PrintActiveOffsets();
         this->gyroscope->setDMPEnabled(true);
     }
     else
@@ -264,7 +265,7 @@ void SensorData::initGnss()
     std::cout << "u-blox protocol version: " << static_cast<int>(versionHigh) << "." << static_cast<int>(versionLow) << std::endl;
 
     this->gnss->setSPIOutput(COM_TYPE_UBX);
-    this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
+    this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI);
     this->gnss->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
     this->gnss->setAutoPVTcallbackPtr(&(SensorData::savePVTdata));
     // SensorData::setOutputStatusPrintPVTdata(true);

lib/Sensors/sensorData.h

@@ -21,7 +21,7 @@
 
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 #include <QMC5883LCompass.h>
-#include <MPU6050_6Axis_MotionApps20.h>
+#include <MPU6050_6Axis_MotionApps612.h>
 #include "calcAzimuth.h"
 #include "ntripClient.h"
 
@@ -38,7 +38,7 @@ public:
     SensorData();
     ~SensorData();
 
-    void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);
+    void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password, bool sendOwnPosition = false);
 
     /**
      * @brief Enable the gnss module over spi

platformio.ini

@@ -18,6 +18,7 @@ upload_speed = 921600
 ; monitor_port = COM6
 lib_deps = 
 	https://git.kleiax.de/PlatformIO-Libs/SparkFunGNSS.git
+	https://git.kleiax.de/PlatformIO-Libs/Menu.git
 	knolleary/PubSubClient@^2.8
 	br3ttb/PID@^1.2.1
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
@@ -49,14 +50,3 @@ description = A Rover who should be drive a route by gps.
 ; 
 ; 
 ; 
-; 
-; 
-; 
-; 
-; 
-; 
-; 
-; 
-; 
-; 
-#

src/SpecialMenus/SensorData/menuSensorData.cpp

@@ -113,7 +113,7 @@ void MenuSensorData::printPage() const
             lineOne = "UNKNOWN";
         }
         const NTRIPClientStates status = this->sensorData->getNtripState();
-        if (status == NTRIPClientStates::pushData)
+        if (status == NTRIPClientStates::pushingData)
         {
             lineTwo.concat("enabled");
         }
@@ -150,3 +150,28 @@ void MenuSensorData::printPage() const
 
     this->print(lineOne, lineTwo);
 }
+
+void MenuSensorData::runCommand()
+{
+    switch (this->getCurrentPage())
+    {
+    case 5:
+        {
+            NTRIPClient* client = this->sensorData->getNtripClient();
+            NTRIPClientStates state = client->getClientState();
+            if (state == NTRIPClientStates::pushingData)
+            {
+                client->deactivate();
+            }
+            else if (state == NTRIPClientStates::waiting)
+            {
+                client->activate();
+            }
+        }
+        break;
+        
+    default:
+        
+        break;
+    }
+}

src/SpecialMenus/SensorData/menuSensorData.h

@@ -30,6 +30,7 @@ public:
 
 private:
     void printPage() const override;
+    void runCommand() override;
 
     SensorData *sensorData;
 };

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -120,7 +120,7 @@ void MenuAutopilot::printPage() const
         const uint8_t carrSoln = gpsData->flags.bits.carrSoln;
         lineOne = "GNSS: ";
 
-        if (status == NTRIPClientStates::pushData)
+        if (status == NTRIPClientStates::pushingData)
         {
             if (carrSoln == 0)
             {

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp

@@ -63,7 +63,7 @@ void MenuCaptureRoute::printPage() const
         NTRIPClientStates status = this->captureRoute->getSensorData()->getNtripClient()->getClientState();
         lineOne = "NTRIP Client is";
 
-        if (status == NTRIPClientStates::pushData)
+        if (status == NTRIPClientStates::pushingData)
         {
             lineTwo = "enabled";
         }

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -83,7 +83,7 @@ void Autopilot::run()
         this->selfDriving();
         break;
 
-    case SelfDrivingRotate:
+    case State::SelfDrivingRotate:
         this->checkButtonInput();
         this->rotate();
         break;

src/main.cpp

@@ -129,8 +129,9 @@ void setup()
                             NtripConfig::port,
                             static_cast<const char *>(NtripConfig::mountPoint),
                             static_cast<const char *>(NtripConfig::user),
-                            static_cast<const char *>(NtripConfig::password));
+                            static_cast<const char *>(NtripConfig::password),
-    // sensorData->enableGyroscope();
+                            NtripConfig::sendOwnPosition);
+    sensorData->enableGyroscope();
     driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());
 
     char wifiIndicator = 'X';