From e965e5c7a3c729f41bc592e8f8e483a850fc49b3 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 12 Feb 2020 19:24:21 +0100 Subject: [PATCH] remainingDistance and remainingTimeAdded to MissionController, still testing. --- src/FlightDisplay/FlightDisplayView.qml | 6 +- src/FlightDisplay/FlightDisplayViewMap.qml | 8 +- src/FlightDisplay/FlightDisplayWimaMenu.qml | 51 +++++- src/FlightMap/Widgets/MapFitFunctions.qml | 12 +- src/MissionManager/MissionController.cc | 181 +++++++++++++++++++- src/MissionManager/MissionController.h | 18 +- src/QmlControls/QGroundControlQmlGlobal.h | 36 ++-- 7 files changed, 280 insertions(+), 32 deletions(-) diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index f3a5abd7f..07194b4b5 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -42,6 +42,7 @@ QGCView { property bool activeVehicleJoystickEnabled: _activeVehicle ? _activeVehicle.joystickEnabled : false property var _planMasterController: masterController + property var _wimaController: wimaController property var _missionController: _planMasterController.missionController property var _geoFenceController: _planMasterController.geoFenceController property var _rallyPointController: _planMasterController.rallyPointController @@ -610,8 +611,9 @@ QGCView { maxHeight: parent.height - toolStrip.height - toolStrip.y - ScreenTools.defaultFontPixelHeight*4 - wimaController: wimaController - planMasterController: masterController + wimaController: _wimaController + planMasterController: _planMasterController + missionController: _missionController onInitSmartRTL: { if (wimaController.checkSmartRTLPreCondition()) { diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index b98f173da..ff1089c5c 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -225,8 +225,8 @@ FlightMap { missionItems: wimaController.missionItems path: wimaController.waypointPath showItems: _wimaEnabled && _showAllWimaItems - zOrderWP: QGroundControl.zOrderWaypointIndicators-2 - zOrderLines: QGroundControl.zOrderWaypointLines-2 + zOrderWP: QGroundControl.zOrderWimaAllWaypointIndicators + zOrderLines: QGroundControl.zOrderWimaAllWaypointLines color: "gray" } // current Items @@ -236,8 +236,8 @@ FlightMap { missionItems: wimaController.currentMissionItems path: wimaController.currentWaypointPath showItems: _wimaEnabled && _showCurrentWimaItems - zOrderWP: QGroundControl.zOrderWaypointIndicators-1 - zOrderLines: QGroundControl.zOrderWaypointLines-1 + zOrderWP: QGroundControl.zOrderWimaCurrentWaypointIndicators + zOrderLines: QGroundControl.zOrderWimaCurrentWaypointLines color: "green" } diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 8303de75d..8e9b4e3d6 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -27,6 +27,7 @@ Item { property var wimaController // must be provided by the user property var planMasterController // must be provided by the user + property var missionController // must be provided by the user readonly property alias missionReadyForStart: _private.missionReadyForStart property bool _controllerValid: planMasterController !== undefined @@ -358,7 +359,10 @@ Item { Layout.fillWidth: true } QGCLabel { - text: wimaController.phaseDistance >= 0 ? wimaController.phaseDistance.toFixed(2) + " m": "" + + property var phaseDistance: wimaController.phaseDistance + + text: phaseDistance >= 0 ? phaseDistance.toFixed(2) + " m": "-.-" wrapMode: Text.WordWrap Layout.fillWidth: true } @@ -369,11 +373,52 @@ Item { Layout.fillWidth: true } QGCLabel { - text: wimaController.phaseDuration >= 0 ? getTime(wimaController.phaseDuration) : "" + property var phaseDuration: wimaController.phaseDuration + + text: phaseDuration >= 0 ? getTime(phaseDuration) : "-.-" wrapMode: Text.WordWrap Layout.fillWidth: true } + QGCLabel { + text: qsTr("Current Waypoint: ") + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + QGCLabel { + property var currentMissionIndex: missionController.currentMissionIndex + + text: currentMissionIndex > 0 ? currentMissionIndex : "-.-" + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + + QGCLabel { + text: qsTr("Remaining Distance: ") + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + QGCLabel { + property var remainingDistance: missionController.remainingDistance + + text: remainingDistance >= 0 ? remainingDistance.toFixed(2) + " m" : "-.-" + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + + QGCLabel { + text: qsTr("Remaining Time: ") + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + QGCLabel { + property var remainingTime: missionController.remainingTime + + text: remainingTime >= 0 ? getTime(remainingTime) : "-.-" + wrapMode: Text.WordWrap + Layout.fillWidth: true + } + QGCLabel { // space text: "" Layout.columnSpan: 2 } @@ -382,7 +427,7 @@ Item { Item { // spacer width: 6 - height: 1 + height: 5 } } diff --git a/src/FlightMap/Widgets/MapFitFunctions.qml b/src/FlightMap/Widgets/MapFitFunctions.qml index 21a56b3ec..0dfc62b75 100644 --- a/src/FlightMap/Widgets/MapFitFunctions.qml +++ b/src/FlightMap/Widgets/MapFitFunctions.qml @@ -81,10 +81,14 @@ Item { east = Math.min(east + ((map.width - mapFitViewport.right) * lonDegreesPerPixel), 360) // Back off on zoom level - east = Math.min(east * 1.0000075, 360) - north = Math.min(north * 1.0000075, 180) - west = west * 0.9999925 - south = south * 0.9999925 +// var alpha1 = 1.0000075 // original value +// var alpha2= 0.9999925 // original value + var alpha1 = 1 + var alpha2 = 1 + east = Math.min(east * alpha1, 360) + north = Math.min(north * alpha1, 180) + west = west * alpha2 + south = south * alpha2 // Fit the map region to the new bounding rect var topLeftCoord = QtPositioning.coordinate(north - 90.0, west - 180.0) diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 60bdb0bf8..b458e2d92 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -82,6 +82,7 @@ MissionController::MissionController(PlanMasterController* masterController, QOb managerVehicleChanged(_managerVehicle); _updateTimer.setSingleShot(true); connect(&_updateTimer, &QTimer::timeout, this, &MissionController::_updateTimeout); + } MissionController::~MissionController() @@ -1786,6 +1787,8 @@ 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); @@ -2141,6 +2144,182 @@ int MissionController::currentPlanViewIndex(void) const return _currentPlanViewIndex; } +bool MissionController::remainingDistanceAndTime(double &remainingDistance, double &remainingTime, int missionIndex) const +{ + if ( _visualItems == nullptr + || _visualItems->count() < 1 + || !_flyView + || missionIndex < 1 + || _visualItems->count() < missionIndex) { + return false; + } + + remainingDistance = 0; + remainingTime = 0; + + bool firstCoordinateItem = true; + VisualMissionItem* lastCoordinateItem = qobject_cast(_visualItems->get(0)); + + + bool showHomePosition = _settingsItem->coordinate().isValid(); + + // If home position is valid we can calculate distances between all waypoints. + // If home position is not valid we can only calculate distances between waypoints which are + // both relative altitude. + + // No values for first item + lastCoordinateItem->setAltDifference(0.0); + lastCoordinateItem->setAzimuth(0.0); + lastCoordinateItem->setDistance(0.0); + + const double homePositionAltitude = _settingsItem->coordinate().altitude(); + + bool linkStartToHome = false; + bool linkEndToHome = false; + + if (showHomePosition) { + SimpleMissionItem* lastItem = _visualItems->value(_visualItems->count() - 1); + if (lastItem && int(lastItem->command()) == MAV_CMD_NAV_RETURN_TO_LAUNCH) { + linkEndToHome = true; + } else { + linkEndToHome = _settingsItem->missionEndRTL(); + } + } + + // determine speed at waypoint with index currentMissionIdx-1 + double currentSpeed = _settingsItem->specifiedFlightSpeed(); + currentSpeed = !qIsNaN(currentSpeed) ? currentSpeed : 5; + for (int i = 1; i < missionIndex-1; ++i) { + SimpleMissionItem* simpleItem = _visualItems->value(i); + + if (simpleItem != nullptr) { + double speed = simpleItem->specifiedFlightSpeed(); + if (!qIsNaN(speed)) + currentSpeed = speed; + } + } + + // 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)); + SimpleMissionItem* simpleItem = qobject_cast(item); + + // Assume the worst + item->setAzimuth(0.0); + item->setDistance(0.0); + + // Look for speed changed + double newSpeed = item->specifiedFlightSpeed(); + if (!qIsNaN(newSpeed)) { + currentSpeed = newSpeed; + } + + // Link back to home if first item is takeoff and we have home position + if (firstCoordinateItem && simpleItem && (simpleItem->command() == MAV_CMD_NAV_TAKEOFF || simpleItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF)) { + if (showHomePosition) { + linkStartToHome = true; + if (_controllerVehicle->multiRotor() || _controllerVehicle->vtol()) { + // We have to special case takeoff, assuming vehicle takes off straight up to specified altitude + + QGeoCoordinate itemCoordinate = item->exitCoordinate(); + QGeoCoordinate settingsCoordinate = _settingsItem->coordinate(); + if (item->exitCoordinateHasRelativeAltitude()) { + itemCoordinate.setAltitude(homePositionAltitude + settingsCoordinate.altitude()); + } + + double altDifference = settingsCoordinate.altitude() - itemCoordinate.altitude(); + + double takeoffTime = qAbs(altDifference) / _appSettings->offlineEditingAscentSpeed()->rawValue().toDouble(); + remainingTime += takeoffTime; + } + } + } + + remainingTime += item->additionalTimeDelay(); + + if (item->specifiesCoordinate() && !item->isStandaloneCoordinate()) { + firstCoordinateItem = false; + + if (lastCoordinateItem != _settingsItem || linkStartToHome) { + // This is a subsequent waypoint or we are forcing the first waypoint back to home + + QGeoCoordinate currentCoord = item->coordinate(); + QGeoCoordinate prevCoord = lastCoordinateItem->exitCoordinate(); + + if (item != _settingsItem && item->coordinateHasRelativeAltitude()) { + currentCoord.setAltitude(homePositionAltitude + currentCoord.altitude()); + } + if (lastCoordinateItem != _settingsItem && lastCoordinateItem->exitCoordinateHasRelativeAltitude()) { + prevCoord.setAltitude(homePositionAltitude + prevCoord.altitude()); + } + + double distance = prevCoord.distanceTo(currentCoord); + double time = distance / currentSpeed; + + remainingDistance += distance; + remainingTime += time; + } + + lastCoordinateItem = item; + } + } + + if (linkEndToHome && lastCoordinateItem != _settingsItem) { + + QGeoCoordinate currentCoord = lastCoordinateItem->coordinate(); + QGeoCoordinate prevCoord = _settingsItem->exitCoordinate(); + + if (lastCoordinateItem != _settingsItem && lastCoordinateItem->coordinateHasRelativeAltitude()) { + currentCoord.setAltitude(homePositionAltitude + currentCoord.altitude()); + } + + double altDifference = currentCoord.altitude() - prevCoord.altitude(); + double distance = currentCoord.distanceTo(prevCoord); + + double time = distance / currentSpeed; + double landTime = qAbs(altDifference) / _appSettings->offlineEditingDescentSpeed()->rawValue().toDouble(); + + remainingDistance += distance; + remainingTime += time + landTime; + } + + 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; @@ -2149,7 +2328,7 @@ VisualMissionItem* MissionController::currentPlanViewItem(void) const void MissionController::setCurrentPlanViewIndex(int sequenceNumber, bool force) { if(_visualItems && (force || sequenceNumber != _currentPlanViewIndex)) { - _currentPlanViewItem = NULL; + _currentPlanViewItem = nullptr; _currentPlanViewIndex = -1; for (int i = 0; i < _visualItems->count(); i++) { VisualMissionItem* pVI = qobject_cast(_visualItems->get(i)); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index edc70d462..b3a7d61ab 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -81,6 +81,8 @@ public: Q_PROPERTY(int missionItemCount READ missionItemCount NOTIFY missionItemCountChanged) ///< True mission item command count (only valid in Fly View) Q_PROPERTY(int currentMissionIndex READ currentMissionIndex NOTIFY currentMissionIndexChanged) Q_PROPERTY(int resumeMissionIndex READ resumeMissionIndex NOTIFY resumeMissionIndexChanged) ///< Returns the item index two which a mission should be resumed. -1 indicates resume mission not available. + Q_PROPERTY(double remainingDistance READ remainingDistance NOTIFY remainingDistanceChanged) + Q_PROPERTY(double remainingTime READ remainingTime NOTIFY remainingTimeChanged) Q_PROPERTY(int currentPlanViewIndex READ currentPlanViewIndex NOTIFY currentPlanViewIndexChanged) Q_PROPERTY(VisualMissionItem* currentPlanViewItem READ currentPlanViewItem NOTIFY currentPlanViewItemChanged) @@ -202,10 +204,15 @@ public: QString corridorScanComplexItemName (void) const { return patternCorridorScanName; } QString structureScanComplexItemName (void) const { return patternStructureScanName; } - int missionItemCount (void) const { return _missionItemCount; } - int currentMissionIndex (void) const; - int resumeMissionIndex (void) const; - int currentPlanViewIndex (void) const; + int missionItemCount (void) const { return _missionItemCount; } + 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; double missionDistance (void) const { return _missionFlightStatus.totalDistance; } double missionTime (void) const { return _missionFlightStatus.totalTime; } @@ -215,6 +222,7 @@ public: double missionCruiseTime (void) const { return _missionFlightStatus.cruiseTime; } double missionMaxTelemetry (void) const { return _missionFlightStatus.maxTelemetryDistance; } + int batteryChangePoint (void) const { return _missionFlightStatus.batteryChangePoint; } ///< -1 for not supported, 0 for not needed int batteriesRequired (void) const { return _missionFlightStatus.batteriesRequired; } ///< -1 for not supported @@ -249,6 +257,8 @@ signals: void currentPlanViewItemChanged (void); void missionBoundingCubeChanged (void); void missionItemCountChanged (int missionItemCount); + void remainingDistanceChanged (void); + void remainingTimeChanged (void); private slots: void _newMissionItemsAvailableFromVehicle(bool removeAllRequested); diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 7ec8538cf..6de7f04d0 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -73,13 +73,17 @@ public: Q_PROPERTY(bool px4ProFirmwareSupported READ px4ProFirmwareSupported CONSTANT) Q_PROPERTY(int apmFirmwareSupported READ apmFirmwareSupported CONSTANT) - Q_PROPERTY(qreal zOrderTopMost READ zOrderTopMost CONSTANT) ///< z order for top most items, toolbar, main window sub view - Q_PROPERTY(qreal zOrderWidgets READ zOrderWidgets CONSTANT) ///< z order value to widgets, for example: zoom controls, hud widgetss - Q_PROPERTY(qreal zOrderMapItems READ zOrderMapItems CONSTANT) - Q_PROPERTY(qreal zOrderVehicles READ zOrderVehicles CONSTANT) - Q_PROPERTY(qreal zOrderWaypointIndicators READ zOrderWaypointIndicators CONSTANT) - Q_PROPERTY(qreal zOrderTrajectoryLines READ zOrderTrajectoryLines CONSTANT) - Q_PROPERTY(qreal zOrderWaypointLines READ zOrderWaypointLines CONSTANT) + Q_PROPERTY(qreal zOrderTopMost READ zOrderTopMost CONSTANT) ///< z order for top most items, toolbar, main window sub view + Q_PROPERTY(qreal zOrderWidgets READ zOrderWidgets CONSTANT) ///< z order value to widgets, for example: zoom controls, hud widgetss + Q_PROPERTY(qreal zOrderMapItems READ zOrderMapItems CONSTANT) + Q_PROPERTY(qreal zOrderVehicles READ zOrderVehicles CONSTANT) + Q_PROPERTY(qreal zOrderWaypointIndicators READ zOrderWaypointIndicators CONSTANT) + Q_PROPERTY(qreal zOrderWimaAllWaypointIndicators READ zOrderWimaAllWaypointIndicators CONSTANT) + Q_PROPERTY(qreal zOrderWimaCurrentWaypointIndicators READ zOrderWimaCurrentWaypointIndicators CONSTANT) + Q_PROPERTY(qreal zOrderTrajectoryLines READ zOrderTrajectoryLines CONSTANT) + Q_PROPERTY(qreal zOrderWaypointLines READ zOrderWaypointLines CONSTANT) + Q_PROPERTY(qreal zOrderWimaAllWaypointLines READ zOrderWimaAllWaypointLines CONSTANT) + Q_PROPERTY(qreal zOrderWimaCurrentWaypointLines READ zOrderWimaCurrentWaypointLines CONSTANT) //------------------------------------------------------------------------- // MavLink Protocol @@ -170,13 +174,17 @@ public: bool taisyncSupported () { return false; } #endif - qreal zOrderTopMost () { return 1000; } - qreal zOrderWidgets () { return 100; } - qreal zOrderMapItems () { return 50; } - qreal zOrderWaypointIndicators () { return 40; } - qreal zOrderVehicles () { return 30; } - qreal zOrderTrajectoryLines () { return 20; } - qreal zOrderWaypointLines () { return 10; } + qreal zOrderTopMost () { return 1000; } + qreal zOrderWidgets () { return 200; } + qreal zOrderMapItems () { return 150; } + qreal zOrderWaypointIndicators () { return 80; } + qreal zOrderVehicles () { return 70; } + qreal zOrderTrajectoryLines () { return 60; } + qreal zOrderWaypointLines () { return 50; } + qreal zOrderWimaCurrentWaypointIndicators () { return 40; } + qreal zOrderWimaAllWaypointIndicators () { return 30; } + qreal zOrderWimaCurrentWaypointLines () { return 20; } + qreal zOrderWimaAllWaypointLines () { return 10; } bool isVersionCheckEnabled () { return _toolbox->mavlinkProtocol()->versionCheckEnabled(); } int mavlinkSystemID () { return _toolbox->mavlinkProtocol()->getSystemId(); } -- 2.22.0