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