added correction data for gnss

lib/Navigation/navigation.cpp

@@ -11,9 +11,21 @@
 
 #include "navigation.h"
 
-Navigation::Navigation(uint8_t rx, uint8_t tx, Route* route) {
-    this->gps = new TinyGPSPlus;
-    Serial1.begin(9600, SERIAL_8N1, rx, tx);
+Navigation::Navigation(Route* route) {
+    this->gps = new SFE_UBLOX_GNSS();
+    if (this->gps->begin() == false) {
+        std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
+        while (1);
+    }
+
+    uint8_t versionHigh = this->gps->getProtocolVersionHigh();
+    uint8_t versionLow = this->gps->getProtocolVersionLow();
+    std::cout << "Neo-M8Q protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
+    this->gps->setI2COutput(COM_TYPE_UBX);
+    this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
+
+    
+
     if (route)
         this->route = route;
     else
@@ -27,8 +39,7 @@ Navigation::~Navigation() {
 }
 
 void Navigation::loop() {
-    while (Serial1.available())
-        gps->encode(Serial1.read());
+    this->gps->checkUbloxI2C();
 }
 
 void Navigation::newRoute() {
@@ -65,7 +76,7 @@ CourseCorrection Navigation::getCourseCorrection() {
         }
     }
 
-    double currentCourse = gps->course.deg();
+    double currentCourse = this->gps->course.deg();
     int16_t correction = currentCourse - targetCourse;
     if (correction > 180)
         correction -= 360;
@@ -99,9 +110,10 @@ bool Navigation::addCurrentPosToRoute() {
 
 Point Navigation::currentLocation() {
     Point p;
-    if (this->gps->location.isValid()) {
-        p.lat = this->gps->location.lat();
-        p.lon = this->gps->location.lng();
+    //TODO: check if the position is possible the true location
+    if (this->gps) {
+        p.lat = this->gps->getLatitude();
+        p.lon = this->gps->getLongitude();
     }
     return p;
 }
@@ -123,12 +135,12 @@ bool Navigation::setTargetPoint(Point target) {
 
 double Navigation::distanceBetween(Point p1, Point p2) {
     if (p1.isValid() && p2.isValid())
-        return TinyGPSPlus::distanceBetween(p1.lat, p1.lon, p2.lat, p2.lon);
+        return NavigationUtils::distanceBetween(p1, p2);
     return -1;
 }
 
 double Navigation::courseTo(Point p1, Point p2) {
     if (p1.isValid() && p2.isValid())
-        return TinyGPSPlus::courseTo(p1.lat, p1.lon, p2.lat, p2.lon);
+        return NavigationUtils::courseTo(p1, p2);
     return -1;
 }

lib/Navigation/navigation.h

@@ -12,12 +12,13 @@
 #ifndef NAVIGATION_H
 #define NAVIGATION_H
 
-#include <TinyGPS++.h>
+#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
 #include <Arduino.h>
 #include <Wire.h>
 #include <iostream>
 
 #include "route.h"
+#include "navigationUtils.h"
 
 /**
  * @brief The minimal distance between Points
@@ -53,11 +54,9 @@ class Navigation {
         /**
          * @brief Construct a new Navigation object
          * 
-         * @param rx pin to the gps device
-         * @param tx pin to the gps device
          * @param route with which to navigate
          */
-        Navigation(uint8_t rx, uint8_t tx, Route* route = nullptr);
+        Navigation(Route* route = nullptr);
 
         /**
          * @brief Destroy the Navigation object
@@ -168,7 +167,7 @@ class Navigation {
          */
         static double courseTo(Point p1, Point p2);
 
-        TinyGPSPlus* gps;
+        SFE_UBLOX_GNSS* gps;
         Route* route = nullptr;
 
         Point lastPoint;

lib/Navigation/navigationUtils.cpp

@@ -0,0 +1,10 @@
+/**
+ * @file navigationUtils.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-03-24
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */

lib/Navigation/navigationUtils.h

@@ -0,0 +1,30 @@
+/**
+ * @file navigationUtils.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @version 0.1
+ * @date 2022-03-24
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef NAVIGATION_UTILS_H
+#define NAVIGATION_UTILS_H
+
+#include "route.h"
+
+class NavigationUtils {
+    public:
+        void getCurrentCourse(Point p1);
+        void setLastPoint(Point p1);
+
+        static double courseTo(Point p1, Point p2);
+        static double courseTo(double lat1, double lon1, double lat2, double lon2);
+        static double distanceBetween(Point p1, Point p2);
+        static double distanceBetween(double lat1, double lon1, double lat2, double lon2);
+
+    private:
+        Point lastPoint;
+};
+
+#endif // NAVIGATION_UTILS_H