new funcs in navigation to export instructions

lib/Navigation/navigation.cpp

@@ -25,17 +25,89 @@ void Navigation::loop() {
         gps->encode(Serial1.read());
 }
 
-Point Navigation::addCurrentPosToRoute() {
+bool Navigation::startNavigation() {
+    Point newTargetPoint = this->route->startRoute();
+    this->navigationStarted = this->setTargetPoint(newTargetPoint);
+    if (this->navigationStarted)
+        this->navigationFinished = false;
+    return this->navigationStarted;
+}
+
+CourseCorrection Navigation::getCourseCorrection() {
+    CourseCorrection courseCorrection = {0, 0};
+
+    Point currentPos = this->currentLocation();
+    double targetCourse = Navigation::courseTo(currentPos, this->targetPoint);
+    double distance = Navigation::distanceBetwenn(currentPos, this->targetPoint);
+
+    if (targetCourse < 0 || distance < 0 || this->navigationFinished)
+        return courseCorrection;
+
+    // Check if I need a new Point
+    if (distance < MIN_DISTANCE_BETWEEN_POINTS) {
+        bool goOn = this->nextPoint();
+        if (!goOn) {
+            this->navigationFinished = true;
+            this->navigationStarted = false;
+            return courseCorrection; // End of navigation
+        }
+    }
+
+    double currentCourse = gps->course.deg();
+    int16_t correction = currentCourse - targetCourse;
+    if (correction > 180)
+        correction -= 360;
+    else if (correction < -180)
+        correction += 360;
+
+    courseCorrection.correction = correction;
+    courseCorrection.distance = distance;
+    return courseCorrection;
+}
+
+bool Navigation::addCurrentPosToRoute() {
+    Point p = currentLocation();
+    if (!p.isValid()) { return false; }
+    if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetwenn(p, this->lastPoint)) {
+        this->route->addPointToRoute(p);
+        this->lastPoint = p;
+        return true;
+    }
+    return false;
+}
+
+Point Navigation::currentLocation() {
     Point p;
-
-
-
-    if (this->gps->location.isUpdated() && this->gps->location.isValid()) {
+    if (this->gps->location.isValid()) {
         p.lat = this->gps->location.lat();
         p.lon = this->gps->location.lng();
-        if (MIN_DISTANCE_BETWEEN_POINTS <= 
-            TinyGPSPlus::distanceBetween(p.lat, p.lon, this->lastPoint.lat, this->lastPoint.lon))
-            this->route->addPointToRoute(p);
     }
     return p;
 }
+
+bool Navigation::nextPoint() {
+    if (!this->navigationStarted)
+        return false;
+    Point newTargetPoint = this->route->getNextPoint();
+    return this->setTargetPoint(newTargetPoint);
+}
+
+bool Navigation::setTargetPoint(Point target) {
+    if (target.isValid()) {
+        this->targetPoint = target;
+        return true;
+    }
+    return false;
+}
+
+double Navigation::distanceBetwenn(Point p1, Point p2) {
+    if (p1.isValid() && p2.isValid())
+        return TinyGPSPlus::distanceBetween(p1.lat, p1.lon, p2.lat, p2.lon);
+    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 -1;
+}