use callback to save data from gnss module

lib/Navigation/navigation.cpp

@@ -12,6 +12,8 @@
 #include "navigation.h"
 
 bool Navigation::outputStatusPrintPVTdata = false;
+uint32_t Navigation::ubxUpdateTimeStatic = 0;
+UBX_NAV_PVT_data_t* Navigation::ubxDataStatic = nullptr;
 
 Navigation::Navigation(Route* route) {
     this->gps = new SFE_UBLOX_GNSS();
@@ -42,8 +44,9 @@ void Navigation::init(Route* route) {
     // std::cout << "Complete" << std::endl;
 
     this->gps->setSPIOutput(COM_TYPE_UBX);
+    this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
     this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
-    this->gps->setAutoPVTcallbackPtr(&(Navigation::printPVTdata));
+    this->gps->setAutoPVTcallbackPtr(&(Navigation::savePVTdata));
     // Navigation::setOutputStatusPrintPVTdata(true);
     this->gps->setNavigationFrequency(1);
     this->gps->setAutoPVT(true);
@@ -75,32 +78,11 @@ void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String
 }
 
 void Navigation::loop() {
-    static uint32_t lastTime = 0;
+    this->gps->checkUblox();
-
     this->gps->checkCallbacks();
-    if (this->gps->getPVT() && this->gps->getGnssFixOk()) {
-        this->updateCurrentLocation();
-        std::cout << "duration between: " << millis() - lastTime << std::endl;
-        lastTime = millis();
-    } else if (millis() - lastTime > 20000 && !this->gps->getGnssFixOk()) {
-        std::cout << "No Data from ZED arrived since a long time. Navigation::loop" << std::endl;
-        this->gps->flushPVT();
-        this->gps->softwareResetGNSSOnly();
-        lastTime = millis();
-
-        //std::cout
-        //     << "checkUblox: " << this->gps->checkUblox() << "  -|-  "
-        //     << "getGnssFixOk: " << this->gps->getGnssFixOk() << "  -|-  "
-        //     << "getHWstatus: " << this->gps->getHWstatus() << "  -|-  "
-        //     << "isConnected: " << this->gps->isConnected() << "  -|-  "
-        //     << std::endl;
-    }
 
     if (this->isNtripInit)
         this->ntripClient->loop();
-
-    if (millis() - this->lastMillis < this->timeToWait)
-        return;
 }
 
 void Navigation::newRoute() {
@@ -139,6 +121,7 @@ CourseCorrection Navigation::getCourseCorrection() {
 
     //FIXME: next line
     double currentCourse = 0;
+    
     //double currentCourse = this->gps->course.deg();
     int16_t correction = currentCourse - targetCourse;
     if (correction > 180)
@@ -152,8 +135,6 @@ CourseCorrection Navigation::getCourseCorrection() {
 }
 
 bool Navigation::addCurrentPosToRoute() {
-    // FIXME: Should I use updateCurrentLocation here? think overall
-    this->updateCurrentLocation();
     if (!this->currentPosition.isValid()) { return false; }
 
     // First Point
@@ -173,11 +154,19 @@ bool Navigation::addCurrentPosToRoute() {
 }
 
 void Navigation::updateCurrentLocation() {
-    //TODO: check if the position is possible the true location
+    if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
-    if (this->gps) {
+        return;
-        this->currentPosition.lat = this->gps->getLatitude();
+
-        this->currentPosition.lon = this->gps->getLongitude();
+    this->ubxData = Navigation::ubxDataStatic;
-    }
+    this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic;
+
+    Point p;
+    p.lon = this->ubxData->lon;
+    p.lat = this->ubxData->lat;
+    p.fixType = this->ubxData->fixType;
+    p.carrierSolution = this->ubxData->flags.all;
+
+    this->currentPosition = p;
 }
 
 bool Navigation::nextPoint() {
@@ -195,6 +184,10 @@ bool Navigation::setTargetPoint(Point target) {
     return false;
 }
 
+void Navigation::setOutputStatusPrintPVTdata(bool status) {
+    Navigation::outputStatusPrintPVTdata = status;
+}
+
 void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
     if (!Navigation::outputStatusPrintPVTdata)
         return;
@@ -242,6 +235,10 @@ void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
               << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
 }
 
-void Navigation::setOutputStatusPrintPVTdata(bool status) {
+void Navigation::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
-    Navigation::outputStatusPrintPVTdata = status;
+    Navigation::printPVTdata(ubxDataStruct);
+
+    Navigation::ubxDataStatic = ubxDataStruct;
+    Navigation::ubxUpdateTimeStatic = millis();
 }
+

lib/Navigation/navigation.h

@@ -125,14 +125,21 @@ class Navigation {
          */
         bool addCurrentPosToRoute();
 
+        // /**
+        //  * @brief Returns the GPS object
+        //  * 
+        //  * @return SFE_UBLOX_GNSS* 
+        //  */
+        // SFE_UBLOX_GNSS* getGPS() { return this->gps; }
+        
         /**
-         * @brief Returns the GPS object
+         * @brief Get the Ubx Data object
          * 
-         * @return SFE_UBLOX_GNSS* 
+         * This struct includes the most Data from the GNSS-Module.
+         * 
+         * @return UBX_NAV_PVT_data_t* 
          */
-        SFE_UBLOX_GNSS* getGPS() { return this->gps; }
+        UBX_NAV_PVT_data_t* getUbxData() { return this->ubxData; }
-        static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
-        static void setOutputStatusPrintPVTdata(bool status);
         
         /**
          * @brief Returns the NTRIPClient object
@@ -151,6 +158,8 @@ class Navigation {
          */
         RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
 
+        static void setOutputStatusPrintPVTdata(bool status);
+
     private:
         void updateCurrentLocation();
 
@@ -174,6 +183,7 @@ class Navigation {
         void init(Route* route);
 
         SFE_UBLOX_GNSS* gps;
+        UBX_NAV_PVT_data_t* ubxData = nullptr;
         Route* route = nullptr;
         NTRIPClient* ntripClient = nullptr;
 
@@ -181,6 +191,7 @@ class Navigation {
         Point targetPoint;
         Point currentPosition;
 
+
         bool navigationStarted = false;
         bool navigationFinished = false;
         bool isNtripInit = false;
@@ -193,6 +204,14 @@ class Navigation {
         uint8_t timeToWait = 200;
         uint16_t port;
         uint32_t lastMillis = 0;
+        uint32_t ubxUpdateTime = 0;
+
+        static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
+        static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
+
+        static UBX_NAV_PVT_data_t* ubxDataStatic;
+
+        static uint32_t ubxUpdateTimeStatic;
 
         static bool outputStatusPrintPVTdata;
 };

lib/Navigation/route.h

@@ -39,6 +39,9 @@ class Point{
 
         double lat = 0;
         double lon = 0;
+        uint8_t fixType = -1;
+        uint8_t carrierSolution = -1;
+        uint32_t horizontalAccuracy = -1;
 
         /**
          * @brief checks if to points are equal
@@ -52,12 +55,13 @@ class Point{
         }
 
         /**
-         * @brief Checks if the Point is valid
+         * @brief Checks if the Point is initalized.
          * 
          * @return true 
          * @return false 
          */
-        bool isValid() const { return this->lat + this->lon; }
+        bool isInit() const { return this->lat + this->lon; }
+        bool isValid() const { return (this->horizontalAccuracy < 5000) ? true : false; }
 
         double distanceTo(const Point& point) const;
         double courseTo(const Point& point) const;

src/SpecialMenus/GPS/menuGPS.cpp

@@ -12,8 +12,8 @@
 #include "menuGPS.h"
 
 MenuGPS::MenuGPS(DriveManager* driveManager) {
-    this->gps = driveManager->getNavigation()->getGPS();
+    this->navigation = driveManager->getNavigation();
-    this->ntripClient = driveManager->getNavigation()->getNTRIPClient();
+    this->ntripClient = this->navigation->getNTRIPClient();
     this->lastMillis = millis();
 }
 
@@ -64,19 +64,20 @@ void MenuGPS::update() {
 }
 
 void MenuGPS::printPage() const {
-    uint8_t fixType = this->gps->getFixType();
+    UBX_NAV_PVT_data_t* gpsData = this->navigation->getUbxData();
+    uint8_t fixType = gpsData->fixType;
 
     switch (this->currentPage) {
         case 0: {
-            uint8_t siv = this->gps->getSIV();
+            uint8_t siv = gpsData->numSV;
-            uint16_t hdop = this->gps->getHorizontalDOP();
+            uint32_t pDOP = gpsData->pDOP;
             if (this->lcd) {
                 lcd->clear();
                 lcd->setCursor(0, 0);
                 if (fixType) {
                     lcd->printf("Sats: %3.u", siv);
                     lcd->setCursor(0, 1);
-                    lcd->printf("HDOP: %u", hdop);
+                    lcd->printf("PDOP: %u", pDOP);
                 } else {
                     lcd->printf("Data isn't valid");
                     lcd->setCursor(0, 1);
@@ -85,15 +86,15 @@ void MenuGPS::printPage() const {
             }
                 if (fixType)
                     std::cout   << "Sats: " << (int) siv
-                                << " HDOP: " << (int) hdop << std::endl; 
+                                << " PDOP: " << (int) pDOP << std::endl; 
                 else
                     std::cout << "Data isn't valid or no GPS signal" << std::endl;
             }
             break;
         
         case 1: {
-            int32_t lat = this->gps->getLatitude();
+            int32_t lat = gpsData->lat;
-            int32_t lng = this->gps->getLongitude();
+            int32_t lng = gpsData->lon;
             if (this->lcd) {
                 lcd->clear();
                 lcd->setCursor(0, 0);
@@ -116,7 +117,7 @@ void MenuGPS::printPage() const {
             break;
 
         case 2: {
-            uint32_t unixEpoch = this->gps->getUnixEpoch();
+            uint32_t unixEpoch = gpsData->sec;
             
             if (this->lcd) {
                 lcd->clear();
@@ -154,7 +155,7 @@ void MenuGPS::printPage() const {
             break;
 
         case 4: {
-            uint32_t accuracy = this->gps->getHorizontalAccuracy();
+            uint32_t accuracy = gpsData->hAcc;
 
             if (this->lcd) {
                 lcd->clear();

src/SpecialMenus/GPS/menuGPS.h

@@ -16,6 +16,7 @@
 
 #include "menuControl.h"
 #include "driveModi/driveManager.h"
+#include "navigation.h"
 #include "ntripClient.h"
 
 /**
@@ -82,8 +83,8 @@ class MenuGPS : public MenuControl {
         void printPage() const;
         void runCommand() const;
 
-        SFE_UBLOX_GNSS* gps;
         NTRIPClient* ntripClient;
+        Navigation* navigation;
 
         const uint16_t delay = 5000;
         uint32_t lastMillis = 0;