remaining dist time in mission controller edited

src/MissionManager/MissionController.cc

@@ -39,7 +39,7 @@
 #endif
 
 #define UPDATE_TIMEOUT 5000 ///< How often we check for bounding box changes
-#define REMAINING_DIST_TIME_UPDATE_INTERVAL 100 ///< How often we check for bounding box changes
+#define REMAINING_DIST_TIME_UPDATE_INTERVAL 300 ///< How often we check for bounding box changes
 
 QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog")
 
@@ -78,6 +78,8 @@ MissionController::MissionController(PlanMasterController* masterController, QOb
     , _progressPct                  (0)
     , _currentPlanViewIndex         (-1)
     , _currentPlanViewItem          (nullptr)
+    , _remainingTime                (-1)
+    , _remainingDistance            (-1)
 {
     _resetMissionFlightStatus();
     managerVehicleChanged(_managerVehicle);
@@ -85,7 +87,7 @@ MissionController::MissionController(PlanMasterController* masterController, QOb
     connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout);
 
     _remainingDistanceTimeTimer.setInterval(REMAINING_DIST_TIME_UPDATE_INTERVAL);
-    connect(&_remainingDistanceTimeTimer, &QTimer::timeout, this, &MissionController::distanceTimeToMissionEnd);
+    connect(&_remainingDistanceTimeTimer, &QTimer::timeout, this, &MissionController::_updateRemainingDistanceTime);
 
 }
 
@@ -1674,10 +1676,16 @@ void MissionController::_recalcAll(void)
 
 void MissionController::_enableDisableRemainingDistTimeCalculation(bool flying)
 {
+    qWarning() << "MissionController::_enableDisableRemainingDistTimeCalculation(): current index: " << _missionManager->currentIndex();
     if (flying) {
         _remainingDistanceTimeTimer.start();
     } else {
         _remainingDistanceTimeTimer.stop();
+        _remainingTime = -1;
+        _remainingDistance = -1;
+
+        emit remainingTimeChanged();
+        emit remainingDistanceChanged();
     }
 }
 
@@ -1685,7 +1693,7 @@ void MissionController::_updateRemainingDistanceTime()
 {
     double dist = 0;
     double time = 0;
-    bool success = distanceTimeToMissionEnd(dist, time, _missionManager->currentIndex() /*missionIndex >= 1*/);
+    bool success = distanceTimeToMissionEnd(dist, time, _missionManager->currentIndex() /*missionIndex >= 1*/, true /* useVehiclePostion */);
     if (success) {
         _remainingTime      = time;
         _remainingDistance  = dist;
@@ -2180,7 +2188,8 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
         || missionIndex > _visualItems->count()) {
         return false;
     }
-
+    qWarning("\n\n");
+    qWarning() << "MissionController::distanceTimeToMissionEnd: missionIndex" << missionIndex;
     // check if vehicle is flying and fetch vehicle coordinate
     QGeoCoordinate vehiclePosition;
     if (_managerVehicle && _managerVehicle->flying() && useVehiclePosition) {
@@ -2238,20 +2247,26 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
     }
 
     if (useVehiclePosition) {
-        SimpleMissionItem* simpleItem = _visualItems->value<SimpleMissionItem*>(missionIndex-1);
-        if (simpleItem != nullptr) {
-            double dist = vehiclePosition.distanceTo(simpleItem->coordinate());
-            double time = dist/currentSpeed;
-
-            remainingDistance   += dist;
-            remainingTime       += time;
+        // find valid coordinate
+        for (int i = missionIndex; i < _visualItems->count(); ++i) {
+            SimpleMissionItem* simpleItem = _visualItems->value<SimpleMissionItem*>(missionIndex);
+            if (simpleItem != nullptr && simpleItem->specifiesCoordinate()) {
+                double dist = vehiclePosition.distanceTo(simpleItem->coordinate());
+                double time = dist/currentSpeed;
+
+                remainingDistance   += dist;
+                remainingTime       += time;
+                break;
+            }
         }
     }
 
     // iterate over mission items starting at currentMissionIdx-1 and determine time and distance
-    for (int i=missionIndex-1; i<_visualItems->count(); i++) {
+    for (int i=missionIndex; i<_visualItems->count(); i++) {
         VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
         SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
+        qWarning() << "MissionController::distanceTimeToMissionEnd: i:" << i;
+        qWarning() << "MissionController::distanceTimeToMissionEnd: coordinate:" << item->coordinate();
 
         // Assume the worst
         item->setAzimuth(0.0);
@@ -2291,6 +2306,7 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
 
             if (lastCoordinateItem != _settingsItem || linkStartToHome) {
                 // This is a subsequent waypoint or we are forcing the first waypoint back to home
+                qWarning() << "MissionController::distanceTimeToMissionEnd: entered";
 
                 QGeoCoordinate  currentCoord =  item->coordinate();
                 QGeoCoordinate  prevCoord =     lastCoordinateItem->exitCoordinate();
@@ -2311,6 +2327,9 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
 
             lastCoordinateItem = item;
         }
+
+
+        qWarning() << "MissionController::distanceTimeToMissionEnd: dist: " << remainingDistance;
     }
 
     if (linkEndToHome && lastCoordinateItem != _settingsItem) {
@@ -2332,6 +2351,7 @@ bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, doub
         remainingTime       += time + landTime;
     }
 
+    qWarning() << "MissionController::distanceTimeToMissionEnd: end dist: " << remainingDistance;
     return true;
 }
 

src/MissionManager/MissionController.h

@@ -279,7 +279,10 @@ private slots:
     void _complexBoundingBoxChanged();
     void _recalcAll(void);
     void _enableDisableRemainingDistTimeCalculation(bool flying);
+
     // updates the fields _remainingTime and_remainingDistance (this function does (!) take into account current vehicle position)
+    // This function is supposed to be called by _remainingDistanceTimeTimer::timeout() exclusively. Calling it in a different way might cause
+    // undesired behaviour
     void _updateRemainingDistanceTime();
 
 private:

src/Wima/WimaPlaner.cc

@@ -480,7 +480,6 @@ void WimaPlaner::recalcPolygonInteractivity(int index)
 
 bool WimaPlaner::calcArrivalAndReturnPath()
 {
-    static int counter = 0;
     setReadyForSync(false);
 
     // extract old survey data
@@ -595,8 +594,6 @@ bool WimaPlaner::calcArrivalAndReturnPath()
         _missionController->setCurrentPlanViewIndex(missionItems->indexOf(_circularSurvey), false);
     setSyncronizedWithControllerFalse();
     setReadyForSync(true);
-    counter++;
-    qWarning() << "WimaPlaner::calcArrivalAndReturnPath(): " << counter;
 
     return true;
 }
@@ -662,17 +659,12 @@ bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordin
     using namespace GeoUtilities;
     using namespace PolygonCalculus;
     QVector<QPointF> path2D;
-
-    auto startTime = std::chrono::high_resolution_clock::now();
     bool retVal = PolygonCalculus::shortestPath(
                                    toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)),
                                    /*start point*/ QPointF(0,0),
                                    /*destination*/ toCartesian2D(destination, start),
                                    /*shortest path*/ path2D);
     path.append(toGeo(path2D, /*origin*/ start));
-    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now()-startTime).count();
-    qWarning() << "WimaPlaner::calcShortestPath: time " << duration << " us";
-
 
     return  retVal;
 }