ready to test capture Route and Autopiloz

lib/Navigation/navigation.cpp

@@ -11,13 +11,19 @@
 
 #include "navigation.h"
 
-Navigation::Navigation(uint8_t rx, uint8_t tx) {
+Navigation::Navigation(uint8_t rx, uint8_t tx, Route* route) {
     this->gps = new TinyGPSPlus;
     Serial1.begin(9600, SERIAL_8N1, rx, tx);
+    if (route)
+        this->route = route;
+    else
+        this->route = new Route();
 }
 
 Navigation::~Navigation() {
     delete this->gps;
+    if (this->route)
+        delete this->route;
 }
 
 void Navigation::loop() {
@@ -25,6 +31,12 @@ void Navigation::loop() {
         gps->encode(Serial1.read());
 }
 
+void Navigation::newRoute() {
+    if (this->route)
+        delete this->route;
+    this->route = new Route();
+}
+
 bool Navigation::startNavigation() {
     Point newTargetPoint = this->route->startRoute();
     this->navigationStarted = this->setTargetPoint(newTargetPoint);

lib/Navigation/navigation.h

@@ -26,7 +26,7 @@ struct CourseCorrection {
 };
 class Navigation {
     public:
-        Navigation(uint8_t rx, uint8_t tx);
+        Navigation(uint8_t rx, uint8_t tx, Route* route = nullptr);
         ~Navigation();
         void loop();
 
@@ -38,6 +38,7 @@ class Navigation {
         bool addCurrentPosToRoute();
 
         TinyGPSPlus* getGPS() { return this->gps; }
+        RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
 
     private:
         Point currentLocation();
@@ -47,7 +48,7 @@ class Navigation {
         static double courseTo(Point p1, Point p2);
 
         TinyGPSPlus* gps;
-        Route* route;
+        Route* route = nullptr;
 
         Point lastPoint;
         Point targetPoint;

lib/Navigation/route.cpp

@@ -23,14 +23,23 @@ Point Route::startRoute() {
     if (this->points.size() < 1)
         return Point(0, 0);
     *this->it = this->points.begin();
+    this->currentPoint = 1;
     return **this->it;
 }
 
 Point Route::getNextPoint() {
     if (*this->it != this->points.end()) {
         this->it++;
+        this->currentPoint++;
         return **this->it;
     }
     else
         return Point(0, 0);
+}
+
+RouteInfo Route::getRouteInfo() {
+    RouteInfo info;
+    info.totalPoints = this->points.size();
+    info.currentPoint = this->currentPoint;
+    return info;
 }

lib/Navigation/route.h

@@ -31,6 +31,11 @@ class Point{
         bool isValid() const { return this->lat + this->lon; }
 };
 
+struct RouteInfo{
+    uint16_t currentPoint;
+    uint16_t totalPoints;
+};
+
 class Route {
     public:
         Route();
@@ -38,10 +43,10 @@ class Route {
         Point startRoute();
         Point getNextPoint();
 
-        uint16_t getNumberOfPoints() { return this->count_points; }
+        RouteInfo getRouteInfo();
 
     private:
-        uint16_t count_points = 0;
+        uint16_t currentPoint = 0;
 
         std::list<Point> points;
         std::list<Point>::iterator* it;