gps now over spi

lib/Navigation/navigation.cpp

@@ -11,6 +11,8 @@
 
 #include "navigation.h"
 
+bool Navigation::outputStatusPrintPVTdata = false;
+
 Navigation::Navigation(Route* route) {
     this->gps = new SFE_UBLOX_GNSS();
     if (this->gps->begin() == false) {
@@ -20,9 +22,9 @@ Navigation::Navigation(Route* route) {
     this->init(route);
 }
 
-Navigation::Navigation(SPIClass spiPort, uint8_t csPin, Route* route) {
+Navigation::Navigation(SPIClass* spiPort, uint8_t csPin, Route* route) {
     this->gps = new SFE_UBLOX_GNSS();
-    if (this->gps->begin(spiPort, csPin, 4000000) == false) {
+    if (this->gps->begin(*spiPort, csPin, 4000000) == false) {
         std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl;
         while (1);
     }
@@ -34,10 +36,17 @@ void Navigation::init(Route* route) {
     uint8_t versionLow = this->gps->getProtocolVersionLow();
     std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
 
-    this->gps->setI2COutput(COM_TYPE_UBX);
-    this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
+    // std::cout << "Set GNSS Module to factory settings... ";
+    // this->gps->factoryReset();
+    // delay(5000);
+    // std::cout << "Complete" << std::endl;
 
+    this->gps->setSPIOutput(COM_TYPE_UBX);
+    this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
+    this->gps->setAutoPVTcallbackPtr(&(Navigation::printPVTdata));
+    // Navigation::setOutputStatusPrintPVTdata(true);
     this->gps->setNavigationFrequency(1);
+    this->gps->setAutoPVT(true);
 
     if (route)
         this->route = route;
@@ -64,13 +73,32 @@ void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String
 }
 
 void Navigation::loop() {
-    //FIXME: Hier sollte funktion zum aktualisieren des GPS Moduls stehen
+    static uint32_t lastTime = 0;
 
-    if (millis() - this->lastMillis < this->timeToWait)
-        return;
+    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) {
+        std::cout << "Keine Daten mehr!" << 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() {
@@ -90,9 +118,9 @@ bool Navigation::startNavigation() {
 CourseCorrection Navigation::getCourseCorrection() {
     CourseCorrection courseCorrection = {0, 0};
 
-    Point currentPos = this->currentLocation();
-    double targetCourse = Navigation::courseTo(currentPos, this->targetPoint);
-    double distance = Navigation::distanceBetween(currentPos, this->targetPoint);
+    this->updateCurrentLocation();
+    double targetCourse = this->currentPosition.courseTo(this->targetPoint);
+    double distance = this->currentPosition.distanceTo(this->targetPoint);
 
     if (targetCourse < 0 || distance < 0 || this->navigationFinished)
         return courseCorrection;
@@ -122,33 +150,32 @@ CourseCorrection Navigation::getCourseCorrection() {
 }
 
 bool Navigation::addCurrentPosToRoute() {
-    Point p = this->currentLocation();
-    if (!p.isValid()) { return false; }
+    // FIXME: Should I use updateCurrentLocation here? think overall
+    this->updateCurrentLocation();
+    if (!this->currentPosition.isValid()) { return false; }
 
     // First Point
     if (this->route->getRouteInfo().totalPoints == 0) {
-        this->route->addPointToRoute(p);
-        this->lastPoint = p;
+        this->route->addPointToRoute(this->currentPosition);
+        this->lastPoint = this->currentPosition;
         return true;
     }
 
     // Ever Point after the first
-    if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetween(p, this->lastPoint)) {
-        this->route->addPointToRoute(p);
-        this->lastPoint = p;
+    if (MIN_DISTANCE_BETWEEN_POINTS <= this->currentPosition.distanceTo(this->lastPoint)) {
+        this->route->addPointToRoute(this->currentPosition);
+        this->lastPoint = this->currentPosition;
         return true;
     }
     return false;
 }
 
-Point Navigation::currentLocation() {
-    Point p;
+void Navigation::updateCurrentLocation() {
     //TODO: check if the position is possible the true location
     if (this->gps) {
-        p.lat = this->gps->getLatitude();
-        p.lon = this->gps->getLongitude();
+        this->currentPosition.lat = this->gps->getLatitude();
+        this->currentPosition.lon = this->gps->getLongitude();
     }
-    return p;
 }
 
 bool Navigation::nextPoint() {
@@ -166,14 +193,53 @@ bool Navigation::setTargetPoint(Point target) {
     return false;
 }
 
-double Navigation::distanceBetween(Point p1, Point p2) {
-    if (p1.isValid() && p2.isValid())
-        return NavigationUtils::distanceBetween(p1, p2);
-    return -1;
+void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
+    if (!Navigation::outputStatusPrintPVTdata)
+        return;
+
+    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;
 }
 
-double Navigation::courseTo(Point p1, Point p2) {
-    if (p1.isValid() && p2.isValid())
-        return NavigationUtils::courseTo(p1, p2);
-    return -1;
+void Navigation::setOutputStatusPrintPVTdata(bool status) {
+    Navigation::outputStatusPrintPVTdata = status;
 }