optimized navigation

src/driveModi/driveManager.cpp

@@ -52,9 +52,11 @@ void DriveManager::nextModus() {
 void DriveManager::changeModus(Modi modus) {
     this->currentModus = modus;
 
-    //Set moveControl to a safe state
     delete this->currentModusPtr;
+    //Reset Route to first point
+    this->navigation->startNavigation();
 
+    //Set moveControl to a safe state
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationSpeed(0);
     this->moveControl->setDrivingStatus(DrivingStatus::stop);