diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 54af4762bbbad6442ce1960cfef98346ee263f07..8de299f5961348232fd58ec696aa357f5bb7726c 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -39,6 +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 QGC_LOGGING_CATEGORY(MissionControllerLog, "MissionControllerLog") @@ -83,6 +84,9 @@ MissionController::MissionController(PlanMasterController* masterController, QOb _updateTimer.setSingleShot(true); connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout); + _remainingDistanceTimeTimer.setInterval(REMAINING_DIST_TIME_UPDATE_INTERVAL); + connect(&_remainingDistanceTimeTimer, &QTimer::timeout, this, &MissionController::distanceTimeToMissionEnd); + } MissionController::~MissionController() @@ -1668,6 +1672,29 @@ void MissionController::_recalcAll(void) _recalcAllWithClickCoordinate(emptyCoord); } +void MissionController::_enableDisableRemainingDistTimeCalculation(bool flying) +{ + if (flying) { + _remainingDistanceTimeTimer.start(); + } else { + _remainingDistanceTimeTimer.stop(); + } +} + +void MissionController::_updateRemainingDistanceTime() +{ + double dist = 0; + double time = 0; + bool success = distanceTimeToMissionEnd(dist, time, _missionManager->currentIndex() /*missionIndex >= 1*/); + if (success) { + _remainingTime = time; + _remainingDistance = dist; + + emit remainingTimeChanged(); + emit remainingDistanceChanged(); + } +} + /// Initializes a new set of mission items void MissionController::_initAllVisualItems(void) { @@ -1787,8 +1814,6 @@ void MissionController::managerVehicleChanged(Vehicle* managerVehicle) connect(_missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); connect(_missionManager, &MissionManager::progressPct, this, &MissionController::_progressPctChanged); connect(_missionManager, &MissionManager::currentIndexChanged, this, &MissionController::_currentMissionIndexChanged); - connect(_missionManager, &MissionManager::currentIndexChanged, this, &MissionController::remainingDistanceChanged); - connect(_missionManager, &MissionManager::currentIndexChanged, this, &MissionController::remainingTimeChanged); connect(_missionManager, &MissionManager::lastCurrentIndexChanged, this, &MissionController::resumeMissionIndexChanged); connect(_missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady); connect(_missionManager, &MissionManager::resumeMissionUploadFail, this, &MissionController::resumeMissionUploadFail); @@ -1796,6 +1821,7 @@ void MissionController::managerVehicleChanged(Vehicle* managerVehicle) connect(_managerVehicle, &Vehicle::defaultCruiseSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); connect(_managerVehicle, &Vehicle::defaultHoverSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); connect(_managerVehicle, &Vehicle::vehicleTypeChanged, this, &MissionController::complexMissionItemNamesChanged); + connect(_managerVehicle, &Vehicle::flyingChanged, this, &MissionController::_enableDisableRemainingDistTimeCalculation); if (!_masterController->offline()) { _managerVehicleHomePositionChanged(_managerVehicle->homePosition()); @@ -2144,8 +2170,9 @@ int MissionController::currentPlanViewIndex(void) const return _currentPlanViewIndex; } -bool MissionController::remainingDistanceAndTime(double &remainingDistance, double &remainingTime, int missionIndex) const +bool MissionController::distanceTimeToMissionEnd(double &remainingDistance, double &remainingTime, int missionIndex, bool useVehiclePosition) const { + // input check if ( _visualItems == nullptr || _visualItems->count() < 1 || !_flyView @@ -2154,6 +2181,17 @@ bool MissionController::remainingDistanceAndTime(double &remainingDistance, doub return false; } + // check if vehicle is flying and fetch vehicle coordinate + QGeoCoordinate vehiclePosition; + if (_managerVehicle && _managerVehicle->flying() && useVehiclePosition) { + vehiclePosition = _managerVehicle->coordinate(); + } else { + if (useVehiclePosition) { + useVehiclePosition = false; + qWarning("MissionController::distanceTimeToMissionEnd(): vehicle position can't be used. Either no vehicle connected, or vehicle not flying."); + } + } + remainingDistance = 0; remainingTime = 0; @@ -2199,6 +2237,17 @@ bool MissionController::remainingDistanceAndTime(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; + } + } + // iterate over mission items starting at currentMissionIdx-1 and determine time and distance for (int i=missionIndex-1; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); @@ -2286,40 +2335,6 @@ bool MissionController::remainingDistanceAndTime(double &remainingDistance, doub return true; } -double MissionController::remainingDistance(int missionIndex) const -{ - double remainingDistance = 0, remainingTime = 0; - if (remainingDistanceAndTime(remainingDistance, remainingTime, missionIndex)) - return remainingDistance; - - return -1; -} - -double MissionController::remainingTime(int missionIndex) const -{ - double remainingDistance = 0, remainingTime = 0; - if (remainingDistanceAndTime(remainingDistance, remainingTime, missionIndex)) - return remainingTime; - - return -1; -} - -double MissionController::remainingDistance() const -{ - if (_missionManager != nullptr) - return remainingDistance(_missionManager->currentIndex()); - - return -1; -} - -double MissionController::remainingTime() const -{ - if (_missionManager != nullptr) - return remainingTime(_missionManager->currentIndex()); - - return -1; -} - VisualMissionItem* MissionController::currentPlanViewItem(void) const { return _currentPlanViewItem; diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index b3a7d61ab72fafff63ed2b37284f3548ef64a3e8..a38e3563f59418dcbcbf0997e9774dfd6e67c4e4 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -208,11 +208,13 @@ public: int currentMissionIndex (void) const; int resumeMissionIndex (void) const; int currentPlanViewIndex (void) const; - bool remainingDistanceAndTime (double &remainingDistance, double &remainingTime, int missionIndex) const; - double remainingDistance (int missionIndex) const; - double remainingTime (int missionIndex) const; - double remainingDistance () const; - double remainingTime () const; + // distance and time to mission end (this function does not take into account current vehicle position, if useVehiclePosition == false) + bool distanceTimeToMissionEnd (double &remainingDistance, double &remainingTime, int missionIndex, bool useVehiclePosition) const; + + // distance and time to mission end (this function does (!) take into account current vehicle position) + double remainingDistance () const { return _remainingDistance; } + // distance and time to mission end (this function does (!) take into account current vehicle position) + double remainingTime () const { return _remainingTime; } double missionDistance (void) const { return _missionFlightStatus.totalDistance; } double missionTime (void) const { return _missionFlightStatus.totalTime; } @@ -276,6 +278,9 @@ private slots: void _updateTimeout(); void _complexBoundingBoxChanged(); void _recalcAll(void); + void _enableDisableRemainingDistTimeCalculation(bool flying); + // updates the fields _remainingTime and_remainingDistance (this function does (!) take into account current vehicle position) + void _updateRemainingDistanceTime(); private: void _init(void); @@ -330,8 +335,11 @@ private: int _currentPlanViewIndex; VisualMissionItem* _currentPlanViewItem; QTimer _updateTimer; + QTimer _remainingDistanceTimeTimer; QGCGeoBoundingCube _travelBoundingCube; QGeoCoordinate _takeoffCoordinate; + double _remainingTime; // estimated Time until mission end + double _remainingDistance; // estimated Distance to mission end static const char* _settingsGroup;