diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index b445dae..677bda3 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -233,6 +233,9 @@ src/FlightDisplay/TerrainProgress.qml src/FlightDisplay/TelemetryValuesBar.qml src/FlightDisplay/VehicleWarnings.qml + src/FlightDisplay/ObstacleDistanceOverlay.qml + src/FlightDisplay/ObstacleDistanceOverlayMap.qml + src/FlightDisplay/ObstacleDistanceOverlayVideo.qml src/QmlControls/QGroundControl/FlightDisplay/qmldir src/FlightMap/MapItems/CameraTriggerIndicator.qml src/FlightMap/Widgets/CenterMapDropButton.qml diff --git a/src/AutoPilotPlugins/PX4/SafetyComponent.qml b/src/AutoPilotPlugins/PX4/SafetyComponent.qml index cc02769..7e04b57 100644 --- a/src/AutoPilotPlugins/PX4/SafetyComponent.qml +++ b/src/AutoPilotPlugins/PX4/SafetyComponent.qml @@ -176,6 +176,7 @@ SetupPage { if(_collisionPrevention) { _collisionPrevention.value = index > 0 ? 5 : -1 console.log('Collision prevention enabled: ' + _collisionPrevention.value) + showObstacleDistanceOverlayCheckBox.checked = _collisionPrevention.value > 0 } } } @@ -231,6 +232,15 @@ SetupPage { } } } + + FactCheckBox { + id: showObstacleDistanceOverlayCheckBox + text: qsTr("Show obstacle distance overlay") + visible: _showObstacleDistanceOverlay.visible + fact: _showObstacleDistanceOverlay + + property Fact _showObstacleDistanceOverlay: QGroundControl.settingsManager.flyViewSettings.showObstacleDistanceOverlay + } } } } diff --git a/src/FlightDisplay/CMakeLists.txt b/src/FlightDisplay/CMakeLists.txt index 29c5ce3..6aafa3c 100644 --- a/src/FlightDisplay/CMakeLists.txt +++ b/src/FlightDisplay/CMakeLists.txt @@ -46,5 +46,8 @@ add_custom_target(FligthDisplayQml VehicleWarnings.qml VirtualJoystick.qml VTOLChecklist.qml + ObstacleDistanceOverlay.qml + ObstacleDistanceOverlayMap.qml + ObstacleDistanceOverlayVideo.qml ) diff --git a/src/FlightDisplay/FlyViewMap.qml b/src/FlightDisplay/FlyViewMap.qml index 202824f..b00e283 100644 --- a/src/FlightDisplay/FlyViewMap.qml +++ b/src/FlightDisplay/FlyViewMap.qml @@ -223,6 +223,11 @@ FlightMap { planMasterController: _planMasterController } + ObstacleDistanceOverlayMap { + id: obstacleDistance + showText: !pipMode + } + // Add trajectory lines to the map MapPolyline { id: trajectoryPolyline diff --git a/src/FlightDisplay/FlyViewVideo.qml b/src/FlightDisplay/FlyViewVideo.qml index 551ba39..16f3821 100644 --- a/src/FlightDisplay/FlyViewVideo.qml +++ b/src/FlightDisplay/FlyViewVideo.qml @@ -83,4 +83,9 @@ Item { anchors.fill: parent vehicle: QGroundControl.multiVehicleManager.activeVehicle } + + ObstacleDistanceOverlayVideo { + id: obstacleDistance + showText: pipState.state === pipState.fullState + } } diff --git a/src/FlightDisplay/ObstacleDistanceOverlay.qml b/src/FlightDisplay/ObstacleDistanceOverlay.qml new file mode 100644 index 0000000..64dda7a --- /dev/null +++ b/src/FlightDisplay/ObstacleDistanceOverlay.qml @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.11 + +import QGroundControl 1.0 +import QGroundControl.SettingsManager 1.0 + +Canvas { + id: canvas + anchors.fill: parent + visible: QGroundControl.settingsManager.flyViewSettings.showObstacleDistanceOverlay.value > 0 && _activeVehicle && _activeVehicle.objectAvoidance.available + + property var showText: true + property var interval: 200 + property var _ranges: [] + property var _incrementDeg: 0 + property var _offsetDeg: 0 + property var _heading: 0 + property var _maxRadiusMeters: 0 + property var _rangesLen: 0 + property var _degToRangeIdx: function(deg, useHeading) { + return rangeIdx(deg, _incrementDeg, _offsetDeg, _rangesLen, useHeading ? _heading : 0) + } + property var _rangeToShow: function(range) { + const feets = QGroundControl.settingsManager.unitsSettings.horizontalDistanceUnits.value === UnitsSettings.HorizontalDistanceUnitsFeet + range = feets ? range * 3.2808399 : range + return range.toFixed(2) + } + + // Converts degrees to index from ranges + function rangeIdx(deg, increment, offset, len, heading) { + const i = (360 - heading + deg - offset) / increment + return (len + Math.ceil(i)) % len + } + + Timer { + interval: canvas.interval + running: true + repeat: true + onTriggered: canvas.requestPaint() + } + + onPaint: { + if (!_activeVehicle) + return + + var ctx = getContext("2d"); + ctx.reset() + + _ranges = _activeVehicle.objectAvoidance.distances + _incrementDeg = _activeVehicle.objectAvoidance.increment + if (_ranges.length == 0 || _incrementDeg == 0) + return + + _offsetDeg = _activeVehicle.objectAvoidance.angleOffset + _heading = _activeVehicle.heading.value + _maxRadiusMeters = _activeVehicle.objectAvoidance.maxDistance / 100 + _rangesLen = 360.0 / _incrementDeg + if (_rangesLen > _ranges.length) + _rangesLen = _ranges.length + + paintObstacleOverlay(ctx) + } +} diff --git a/src/FlightDisplay/ObstacleDistanceOverlayMap.qml b/src/FlightDisplay/ObstacleDistanceOverlayMap.qml new file mode 100644 index 0000000..b9a16fc --- /dev/null +++ b/src/FlightDisplay/ObstacleDistanceOverlayMap.qml @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.11 + +import QGroundControl 1.0 +import QGroundControl.SettingsManager 1.0 + +Item { + id: root + anchors.fill: parent + property var showText: obstacleDistance._showText + + function paintObstacleOverlay(ctx) { + const vehiclePoint = _root.fromCoordinate(_activeVehicleCoordinate, false) + const centerX = vehiclePoint.x + const centerY = vehiclePoint.y + const maxRadiusPixels = 0.9 * root.height / 2 // Max pixels to center + const minRadiusPixels = maxRadiusPixels * 0.2 + const metersPerPixelInCycle = (maxRadiusPixels - minRadiusPixels) / obstacleDistance._maxRadiusMeters + + const leftCoord = mapControl.toCoordinate(Qt.point(0, root.y), false) + const rightCoord = mapControl.toCoordinate(Qt.point(100, root.y), false) + const metersIn100Pixels = leftCoord.distanceTo(rightCoord) + const metersPerPixel = 100.0 / metersIn100Pixels + + var minGradPixels = minRadiusPixels + var maxGradPixels = maxRadiusPixels + var metersToPixels = metersPerPixelInCycle + if (metersIn100Pixels < 4) { + minGradPixels = 0 + maxGradPixels = obstacleDistance._maxRadiusMeters * metersPerPixel + metersToPixels = metersPerPixel + } + + var grad = ctx.createRadialGradient(centerX, centerY, minGradPixels, centerX, centerY, maxGradPixels) + grad.addColorStop(0, Qt.rgba(1, 0, 0, 1)) + grad.addColorStop(0.1, Qt.rgba(1, 0, 0, 0.7)) + grad.addColorStop(0.5, Qt.rgba(1, 0.64, 0, 0.7)) + grad.addColorStop(0.65, Qt.rgba(1, 0.64, 0, 0.3)) + grad.addColorStop(0.95, Qt.rgba(0, 1, 0, 0.3)) + grad.addColorStop(1, Qt.rgba(0, 1, 0, 0)) + + var points = [] + const height = minRadiusPixels / 8 + for (var i = 0; i < obstacleDistance._rangesLen; ++i) { + const deg = i * obstacleDistance._incrementDeg + const rad = deg * Math.PI / 180.0 + const m = obstacleDistance._ranges[obstacleDistance._degToRangeIdx(deg, true)] / 100.0 + const pixels = minGradPixels + m * metersToPixels + const outerX = centerX + pixels * Math.cos(rad) + const outerY = centerY + pixels * Math.sin(rad) + const innerX = centerX + (pixels - height) * Math.cos(rad) + const innerY = centerY + (pixels - height) * Math.sin(rad) + + points.push({'outer_x': outerX, 'outer_y': outerY, 'inner_x': innerX, 'inner_y': innerY, 'range': m}) + } + + ctx.strokeStyle = Qt.rgba(0, 0, 0, 0.8) + ctx.font = "bold 22px sans-serif" + ctx.lineWidth = 2; + var mPrev = -1 + for (var i = 0; i < points.length; i += 3) { + const i3 = (i + 3) % points.length // catch the line from the last to the first point + + ctx.beginPath() + ctx.fillStyle = grad + ctx.moveTo(points[i].inner_x, points[i].inner_y) + ctx.lineTo(points[i].outer_x, points[i].outer_y) + ctx.bezierCurveTo( + points[i + 1].outer_x, points[i + 1].outer_y, + points[i + 2].outer_x, points[i + 2].outer_y, + points[i3].outer_x, points[i3].outer_y) + ctx.lineTo(points[i3].inner_x, points[i3].inner_y) + ctx.bezierCurveTo( + points[i3].inner_x, points[i3].inner_y, + points[i + 2].inner_x, points[i + 2].inner_y, + points[i + 1].inner_x, points[i + 1].inner_y) + ctx.fill() + + if (showText) { + var iMin = i + for (var k = iMin + 1; k < iMin + 2; ++k) { + const idx = k % points.length + if (points[idx].range < points[iMin].range) + iMin = idx + } + + var m = points[iMin].range + if (m < obstacleDistance._maxRadiusMeters && Math.abs(m - mPrev) > 2.0) { + const textX = points[iMin].inner_x + const textY = points[iMin].inner_y + + ctx.fillStyle = Qt.rgba(1, 1, 1, 0.9) + const text = obstacleDistance._rangeToShow(m) + ctx.strokeText(text, textX, textY) + ctx.fillText(text, textX, textY) + mPrev = m + } + } + } + } + + ObstacleDistanceOverlay { + id: obstacleDistance + } +} + diff --git a/src/FlightDisplay/ObstacleDistanceOverlayVideo.qml b/src/FlightDisplay/ObstacleDistanceOverlayVideo.qml new file mode 100644 index 0000000..2d51c1e --- /dev/null +++ b/src/FlightDisplay/ObstacleDistanceOverlayVideo.qml @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.11 + +import QGroundControl 1.0 +import QGroundControl.SettingsManager 1.0 + +Item { + id: root + anchors.fill: parent + property var showText: obstacleDistance._showText + + function drawSegment(ctx, range, centerX, centerY, lengthFrom, lengthTo, radFrom, radTo, grad) { + const topSrcX = centerX + lengthFrom * Math.cos(radFrom) + const topSrcY = centerY + lengthFrom * Math.sin(radFrom) + const topDstX = centerX + lengthFrom * Math.cos(radTo) + const topDstY = centerY + lengthFrom * Math.sin(radTo) + + ctx.save() + ctx.setTransform(2, 0, 0, 1, -centerX, 0) + ctx.beginPath() + ctx.fillStyle = grad + ctx.moveTo(topSrcX, topSrcY) + ctx.arc(centerX, centerY, lengthFrom, radFrom, radTo, false) + ctx.arc(centerX, centerY, lengthTo, radTo, radFrom, true) + ctx.lineTo(topSrcX, topSrcY) + ctx.fill() + ctx.closePath() + ctx.restore() + + if (range > 0) { + ctx.save() + ctx.setTransform(2, 0, 0, 2, -centerX, 0) + ctx.beginPath() + ctx.strokeStyle = Qt.rgba(0, 0, 0, 0.8) + ctx.font = "bold 10px sans-serif" + ctx.lineWidth = 2 + ctx.fillStyle = Qt.rgba(1, 1, 1, 0.8) + const text = obstacleDistance._rangeToShow(range) + const textX = topSrcX + (topDstX - topSrcX) / 2 + const textY = topSrcY + (topDstY - topSrcY) / 2 + ctx.strokeText(text, textX, textY / 2) + ctx.fillText(text, textX, textY / 2) + ctx.closePath() + ctx.restore() + } + } + + function paintObstacleOverlay(ctx) { + const centerX = root.width / 2 + const centerY = root.height / 2 + const maxRadiusPixels = 0.9 * root.height / 2 // Max pixels to center + const minRadiusPixels = maxRadiusPixels * 0.2 // Min radius in pixels to center + const minGradPixels = minRadiusPixels + const maxGradPixels = maxRadiusPixels + const segmentHeightPixels = minRadiusPixels / 8 + const levelMeters = 10 + const levelNum = obstacleDistance._maxRadiusMeters / levelMeters + + var grad = ctx.createRadialGradient(centerX, centerY, maxGradPixels - segmentHeightPixels * levelNum * 2, centerX, centerY, maxGradPixels) + grad.addColorStop(0, Qt.rgba(1, 0, 0, 0.9)) + grad.addColorStop(0.1, Qt.rgba(1, 0, 0, 0.3)) + grad.addColorStop(0.5, Qt.rgba(1, 0.64, 0, 0.3)) + grad.addColorStop(0.65, Qt.rgba(1, 0.64, 0, 0.2)) + grad.addColorStop(0.95, Qt.rgba(0, 1, 0, 0.1)) + grad.addColorStop(1, Qt.rgba(0, 1, 0, 0)) + + const segNum = 16 + const incDeg = 360 / segNum + for (var s = 0; s < segNum; ++s) { + const deg = s * 360.0 / segNum + const rad = deg * Math.PI / 180.0 + const i = obstacleDistance._degToRangeIdx(deg, false) + const iNext = obstacleDistance._degToRangeIdx(deg + incDeg, false) + var rangeMin = obstacleDistance._maxRadiusMeters + + const end = i < iNext ? iNext : obstacleDistance._rangesLen + iNext + for (var ii = i; ii < end; ++ii) { + const r = obstacleDistance._ranges[ii % obstacleDistance._rangesLen] / 100 + if (r < rangeMin) + rangeMin = r + } + + const lengthFrom = maxRadiusPixels + const radFrom = rad + const radTo = radFrom + incDeg * Math.PI / 180.0 - 0.03 + var range = obstacleDistance._maxRadiusMeters + for (var ii = 0; ii < levelNum; ++ii) { + const from = lengthFrom - ii * segmentHeightPixels * 2 + const to = from - segmentHeightPixels + const rangeInLevel = obstacleDistance._maxRadiusMeters - (ii + 1) * levelMeters + const isLast = ii >= levelNum - 1 + if (rangeMin > rangeInLevel || isLast) + range = rangeMin + const rangeToShow = showText && range < obstacleDistance._maxRadiusMeters ? range : 0 + drawSegment(ctx, rangeToShow, centerX, centerY, from, to, radFrom, radTo, grad) + if (range == rangeMin) + break + } + } + } + + ObstacleDistanceOverlay { + id: obstacleDistance + } +} diff --git a/src/QmlControls/QGroundControl/FlightDisplay/qmldir b/src/QmlControls/QGroundControl/FlightDisplay/qmldir index 727ac69..f44dd8c 100644 --- a/src/QmlControls/QGroundControl/FlightDisplay/qmldir +++ b/src/QmlControls/QGroundControl/FlightDisplay/qmldir @@ -33,3 +33,6 @@ ProximityRadarVideoView 1.0 ProximityRadarVideoView.qml TerrainProgress 1.0 TerrainProgress.qml TelemetryValuesBar 1.0 TelemetryValuesBar.qml VehicleWarnings 1.0 VehicleWarnings.qml +ObstacleDistanceOverlay 1.0 ObstacleDistanceOverlay.qml +ObstacleDistanceOverlayMap 1.0 ObstacleDistanceOverlayMap.qml +ObstacleDistanceOverlayVideo 1.0 ObstacleDistanceOverlayVideo.qml diff --git a/src/Settings/FlyView.SettingsGroup.json b/src/Settings/FlyView.SettingsGroup.json index 582bbb1..c5bbeb5 100644 --- a/src/Settings/FlyView.SettingsGroup.json +++ b/src/Settings/FlyView.SettingsGroup.json @@ -54,6 +54,12 @@ "default": true }, { + "name": "showObstacleDistanceOverlay", + "shortDesc": "Show obstacle distance overlay on map and video.", + "type": "bool", + "default": false +}, +{ "name": "maxGoToLocationDistance", "shortDesc": "Maximum distance allowed for Go To Location.", "type": "double", diff --git a/src/Settings/FlyViewSettings.cc b/src/Settings/FlyViewSettings.cc index e99fae5..5b8fb41 100644 --- a/src/Settings/FlyViewSettings.cc +++ b/src/Settings/FlyViewSettings.cc @@ -26,3 +26,4 @@ DECLARE_SETTINGSFACT(FlyViewSettings, lockNoseUpCompass) DECLARE_SETTINGSFACT(FlyViewSettings, maxGoToLocationDistance) DECLARE_SETTINGSFACT(FlyViewSettings, keepMapCenteredOnVehicle) DECLARE_SETTINGSFACT(FlyViewSettings, showSimpleCameraControl) +DECLARE_SETTINGSFACT(FlyViewSettings, showObstacleDistanceOverlay) diff --git a/src/Settings/FlyViewSettings.h b/src/Settings/FlyViewSettings.h index 71b08df..237ad32 100644 --- a/src/Settings/FlyViewSettings.h +++ b/src/Settings/FlyViewSettings.h @@ -28,4 +28,5 @@ public: DEFINE_SETTINGFACT(maxGoToLocationDistance) DEFINE_SETTINGFACT(keepMapCenteredOnVehicle) DEFINE_SETTINGFACT(showSimpleCameraControl) + DEFINE_SETTINGFACT(showObstacleDistanceOverlay) };