diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 8de299f5961348232fd58ec696aa357f5bb7726c..4a6dc13c3a747aee1e6f2f6f74f0d289b8ec9dc1 100644 --- a/src/MissionManager/MissionController.cc +++ b/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(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(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(_visualItems->get(i)); SimpleMissionItem* simpleItem = qobject_cast(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; } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index a38e3563f59418dcbcbf0997e9774dfd6e67c4e4..992a4e53e94c793cfbc13afd7a005d01955b4202 100644 --- a/src/MissionManager/MissionController.h +++ b/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: diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 2a23d5a530ebdcb859e64faf06f94ece1ace04df..ab744092a1d1dc3f5f638dfba67325fe8995353d 100644 --- a/src/Wima/WimaPlaner.cc +++ b/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 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::high_resolution_clock::now()-startTime).count(); - qWarning() << "WimaPlaner::calcShortestPath: time " << duration << " us"; - return retVal; }