Browse Source
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/7670QGC4.4
Val Doroshchuk
4 years ago
committed by
Don Gagne
12 changed files with 335 additions and 0 deletions
@ -0,0 +1,71 @@
@@ -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) |
||||
} |
||||
} |
||||
@ -0,0 +1,114 @@
@@ -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 |
||||
} |
||||
} |
||||
|
||||
@ -0,0 +1,113 @@
@@ -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 |
||||
} |
||||
} |
||||
Loading…
Reference in new issue