From 377b0ea03b4655adb7b0e5e47edb283c9383734d Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sun, 18 Dec 2022 10:04:05 +0100 Subject: [PATCH] use callback to save data from gnss module --- lib/Navigation/navigation.cpp | 61 +++++++++++++++----------------- lib/Navigation/navigation.h | 29 ++++++++++++--- lib/Navigation/route.h | 8 +++-- src/SpecialMenus/GPS/menuGPS.cpp | 23 ++++++------ src/SpecialMenus/GPS/menuGPS.h | 3 +- 5 files changed, 73 insertions(+), 51 deletions(-) diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 3193d69..5981cde 100644 --- a/lib/Navigation/navigation.cpp +++ b/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 (this->gps) { - this->currentPosition.lat = this->gps->getLatitude(); - this->currentPosition.lon = this->gps->getLongitude(); - } + if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime) + return; + + 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) { - Navigation::outputStatusPrintPVTdata = status; +void Navigation::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { + Navigation::printPVTdata(ubxDataStruct); + + Navigation::ubxDataStatic = ubxDataStruct; + Navigation::ubxUpdateTimeStatic = millis(); } + diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 6608ba3..51eb9cb 100644 --- a/lib/Navigation/navigation.h +++ b/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; } - static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct); - static void setOutputStatusPrintPVTdata(bool status); + UBX_NAV_PVT_data_t* getUbxData() { return this->ubxData; } /** * @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; }; diff --git a/lib/Navigation/route.h b/lib/Navigation/route.h index 7cd8a62..1003ce4 100644 --- a/lib/Navigation/route.h +++ b/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; diff --git a/src/SpecialMenus/GPS/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp index 7c26242..2537f44 100644 --- a/src/SpecialMenus/GPS/menuGPS.cpp +++ b/src/SpecialMenus/GPS/menuGPS.cpp @@ -12,8 +12,8 @@ #include "menuGPS.h" MenuGPS::MenuGPS(DriveManager* driveManager) { - this->gps = driveManager->getNavigation()->getGPS(); - this->ntripClient = driveManager->getNavigation()->getNTRIPClient(); + this->navigation = driveManager->getNavigation(); + 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(); - uint16_t hdop = this->gps->getHorizontalDOP(); + uint8_t siv = gpsData->numSV; + 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 lng = this->gps->getLongitude(); + int32_t lat = gpsData->lat; + 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(); diff --git a/src/SpecialMenus/GPS/menuGPS.h b/src/SpecialMenus/GPS/menuGPS.h index 12cc50c..312d175 100644 --- a/src/SpecialMenus/GPS/menuGPS.h +++ b/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;