Introduce obstacle distance overlay

It receives mavlink message obstacle_distance and shows distances in a circular array fashion.

Added a setting in Safety/Object Detection section to show the overlay.
Can be used independently to collision prevention features.

There are two overlays: Video and Map overlay
In different styles.

The styles could be changed in particular qml file.

https://github.com/mavlink/qgroundcontrol/issues/7670

qgroundcontrol.qrc

@@ -233,6 +233,9 @@
         <file alias="QGroundControl/FlightDisplay/TerrainProgress.qml">src/FlightDisplay/TerrainProgress.qml</file>
         <file alias="QGroundControl/FlightDisplay/TelemetryValuesBar.qml">src/FlightDisplay/TelemetryValuesBar.qml</file>
         <file alias="QGroundControl/FlightDisplay/VehicleWarnings.qml">src/FlightDisplay/VehicleWarnings.qml</file>
+        <file alias="QGroundControl/FlightDisplay/ObstacleDistanceOverlay.qml">src/FlightDisplay/ObstacleDistanceOverlay.qml</file>
+        <file alias="QGroundControl/FlightDisplay/ObstacleDistanceOverlayMap.qml">src/FlightDisplay/ObstacleDistanceOverlayMap.qml</file>
+        <file alias="QGroundControl/FlightDisplay/ObstacleDistanceOverlayVideo.qml">src/FlightDisplay/ObstacleDistanceOverlayVideo.qml</file>
         <file alias="QGroundControl/FlightDisplay/qmldir">src/QmlControls/QGroundControl/FlightDisplay/qmldir</file>
         <file alias="QGroundControl/FlightMap/CameraTriggerIndicator.qml">src/FlightMap/MapItems/CameraTriggerIndicator.qml</file>
         <file alias="QGroundControl/FlightMap/CenterMapDropButton.qml">src/FlightMap/Widgets/CenterMapDropButton.qml</file>

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
+                            }
                         }
                     }
                 }

src/FlightDisplay/CMakeLists.txt

@@ -46,5 +46,8 @@ add_custom_target(FligthDisplayQml
 		VehicleWarnings.qml
 		VirtualJoystick.qml
 		VTOLChecklist.qml
+		ObstacleDistanceOverlay.qml
+		ObstacleDistanceOverlayMap.qml
+		ObstacleDistanceOverlayVideo.qml
 )
 

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

src/FlightDisplay/FlyViewVideo.qml

@@ -83,4 +83,9 @@ Item {
         anchors.fill:   parent
         vehicle:        QGroundControl.multiVehicleManager.activeVehicle
     }
+
+    ObstacleDistanceOverlayVideo {
+        id: obstacleDistance
+        showText: pipState.state === pipState.fullState
+    }
 }

src/FlightDisplay/ObstacleDistanceOverlay.qml

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * 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)
+    }
+}

src/FlightDisplay/ObstacleDistanceOverlayMap.qml

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * 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
+    }
+}
+

src/FlightDisplay/ObstacleDistanceOverlayVideo.qml

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * 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
+    }
+}

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

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",

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)

src/Settings/FlyViewSettings.h

@@ -28,4 +28,5 @@ public:
     DEFINE_SETTINGFACT(maxGoToLocationDistance)
     DEFINE_SETTINGFACT(keepMapCenteredOnVehicle)
     DEFINE_SETTINGFACT(showSimpleCameraControl)
+    DEFINE_SETTINGFACT(showObstacleDistanceOverlay)
 };