ready to test capture Route and Autopiloz

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;
 }