Commit c39e9c7f authored by Don Gagne's avatar Don Gagne

Initial GeoFence implementation

parent 32cfa15c
......@@ -273,12 +273,16 @@ HEADERS += \
src/LogCompressor.h \
src/MG.h \
src/MissionManager/ComplexMissionItem.h \
src/MissionManager/GeoFenceController.h \
src/MissionManager/GeoFenceManager.h \
src/MissionManager/QGCMapPolygon.h \
src/MissionManager/MissionCommandList.h \
src/MissionManager/MissionCommandTree.h \
src/MissionManager/MissionCommandUIInfo.h \
src/MissionManager/MissionController.h \
src/MissionManager/MissionItem.h \
src/MissionManager/MissionManager.h \
src/MissionManager/PlanElementController.h \
src/MissionManager/SimpleMissionItem.h \
src/MissionManager/SurveyMissionItem.h \
src/MissionManager/VisualMissionItem.h \
......@@ -434,12 +438,16 @@ SOURCES += \
src/LogCompressor.cc \
src/main.cc \
src/MissionManager/ComplexMissionItem.cc \
src/MissionManager/GeoFenceController.cc \
src/MissionManager/GeoFenceManager.cc \
src/MissionManager/QGCMapPolygon.cc \
src/MissionManager/MissionCommandList.cc \
src/MissionManager/MissionCommandTree.cc \
src/MissionManager/MissionCommandUIInfo.cc \
src/MissionManager/MissionController.cc \
src/MissionManager/MissionItem.cc \
src/MissionManager/MissionManager.cc \
src/MissionManager/PlanElementController.cc \
src/MissionManager/SimpleMissionItem.cc \
src/MissionManager/SurveyMissionItem.cc \
src/MissionManager/VisualMissionItem.cc \
......
......@@ -92,6 +92,7 @@
<file alias="QGroundControl/Controls/ViewWidget.qml">src/ViewWidgets/ViewWidget.qml</file>
<file alias="SimpleItemEditor.qml">src/MissionEditor/SimpleItemEditor.qml</file>
<file alias="SurveyItemEditor.qml">src/MissionEditor/SurveyItemEditor.qml</file>
<file alias="GeoFenceEditor.qml">src/MissionEditor/GeoFenceEditor.qml</file>
<file alias="QGroundControl/FactControls/FactBitmask.qml">src/FactSystem/FactControls/FactBitmask.qml</file>
<file alias="QGroundControl/FactControls/FactCheckBox.qml">src/FactSystem/FactControls/FactCheckBox.qml</file>
<file alias="QGroundControl/FactControls/FactComboBox.qml">src/FactSystem/FactControls/FactComboBox.qml</file>
......@@ -107,6 +108,7 @@
<file alias="VirtualJoystick.qml">src/FlightDisplay/VirtualJoystick.qml</file>
<file alias="QGroundControl/FlightMap/qmldir">src/FlightMap/qmldir</file>
<file alias="QGroundControl/FlightMap/FlightMap.qml">src/FlightMap/FlightMap.qml</file>
<file alias="QGroundControl/FlightMap/QGCMapPolygonControls.qml">src/MissionEditor/QGCMapPolygonControls.qml</file>
<file alias="QGroundControl/FlightMap/MapScale.qml">src/FlightMap/MapScale.qml</file>
<file alias="QGroundControl/FlightMap/MissionItemIndicator.qml">src/FlightMap/MapItems/MissionItemIndicator.qml</file>
<file alias="QGroundControl/FlightMap/MissionItemView.qml">src/FlightMap/MapItems/MissionItemView.qml</file>
......
......@@ -58,6 +58,11 @@ FlightMap {
Component.onCompleted: start(false /* editMode */)
}
GeoFenceController {
id: _geoFenceController
Component.onCompleted: start(false /* editMode */)
}
// Add trajectory points to the map
MapItemView {
model: _mainIsMap ? _activeVehicle ? _activeVehicle.trajectoryPoints : 0 : 0
......@@ -96,6 +101,20 @@ FlightMap {
model: _mainIsMap ? _missionController.waypointLines : 0
}
// GeoFence polygon
MapPolygon {
border.color: "#80FF0000"
border.width: 3
path: _geoFenceController.polygon.path
}
// GeoFence breach return point
MapQuickItem {
anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2)
coordinate: _geoFenceController.breachReturnPoint
sourceItem: MissionItemIndexLabel { label: "F" }
}
// GoTo here waypoint
MapQuickItem {
coordinate: _gotoHereCoordinate
......
......@@ -198,30 +198,17 @@ Map {
property bool adjustingPolygon: false
property bool polygonReady: polygonDrawerPolygon.path.length > 3 ///< true: enough points have been captured to create a closed polygon
/// New polygon capture has started
signal polygonCaptureStarted
/// Polygon capture is complete
/// @param coordinates Map coordinates for the polygon points
signal polygonCaptureFinished(var coordinates)
/// Polygon adjustment has begun
signal polygonAdjustStarted
/// Polygon Vertex coordinate has been adjusted
signal polygonAdjustVertex(int vertexIndex, var vertexCoordinate)
/// Polygon adjustment finished
signal polygonAdjustFinished
property var _callbackObject
property var _vertexDragList: []
/// Begin capturing a new polygon
/// polygonCaptureStarted will be signalled
function startCapturePolygon() {
function startCapturePolygon(callback) {
polygonDrawer._callbackObject = callback
polygonDrawer.drawingPolygon = true
polygonDrawer._clearPolygon()
polygonDrawer.polygonCaptureStarted()
polygonDrawer._callbackObject.polygonCaptureStarted()
}
/// Finish capturing the polygon
......@@ -236,11 +223,12 @@ Map {
polygonPath.pop() // get rid of drag coordinate
polygonDrawer._clearPolygon()
polygonDrawer.drawingPolygon = false
polygonDrawer.polygonCaptureFinished(polygonPath)
polygonDrawer._callbackObject.polygonCaptureFinished(polygonPath)
return true
}
function startAdjustPolygon(vertexCoordinates) {
function startAdjustPolygon(callback, vertexCoordinates) {
polygonDraw._callbackObject = callback
polygonDrawer.adjustingPolygon = true
for (var i=0; i<vertexCoordinates.length; i++) {
var mapItem = Qt.createQmlObject(
......@@ -268,7 +256,7 @@ Map {
"" +
" function updateCoordinate() { " +
" vertexDrag.coordinate = _map.toCoordinate(Qt.point(vertexDrag.x + _halfSideLength, vertexDrag.y + _halfSideLength), false); " +
" polygonDrawer.polygonAdjustVertex(vertexDrag.index, vertexDrag.coordinate); " +
" polygonDrawer._callbackObject.polygonAdjustVertex(vertexDrag.index, vertexDrag.coordinate); " +
" } " +
"" +
" function updatePosition() { " +
......@@ -299,7 +287,7 @@ Map {
mapItem.index = i
mapItem.updatePosition()
polygonDrawer._vertexDragList.push(mapItem)
polygonDrawer.polygonAdjustStarted()
polygonDrawer._callbackObject.polygonAdjustStarted()
}
}
......@@ -309,7 +297,7 @@ Map {
polygonDrawer._vertexDragList[i].destroy()
}
polygonDrawer._vertexDragList = []
polygonDrawer.polygonAdjustFinished()
polygonDrawer._callbackObject.polygonAdjustFinished()
}
function _clearPolygon() {
......
......@@ -23,3 +23,6 @@ MissionItemIndicator 1.0 MissionItemIndicator.qml
MissionItemView 1.0 MissionItemView.qml
MissionLineView 1.0 MissionLineView.qml
VehicleMapItem 1.0 VehicleMapItem.qml
# Editor controls
QGCMapPolygonControls 1.0 QGCMapPolygonControls.qml
import QtQuick 2.2
import QtQuick.Controls 1.2
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0
import QGroundControl.FactControls 1.0
import QGroundControl.Palette 1.0
import QGroundControl.FlightMap 1.0
QGCFlickable {
id: root
width: availableWidth
height: Math.min(availableHeight, geoFenceEditorRect.height)
contentHeight: geoFenceEditorRect.height
clip: true
readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 12)
readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2
readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2
property var polygon: geoFenceController.polygon
Connections {
target: geoFenceController.polygon
onPathChanged: {
if (geoFenceController.polygon.path.length > 2) {
geoFenceController.breachReturnPoint = geoFenceController.polygon.center()
}
}
}
Rectangle {
id: geoFenceEditorRect
width: parent.width
height: geoFenceItems.y + geoFenceItems.height + (_margin * 2)
radius: _radius
color: qgcPal.buttonHighlight
QGCLabel {
id: geoFenceLabel
anchors.margins: _margin
anchors.left: parent.left
anchors.top: parent.top
text: qsTr("Geo-Fence (WIP careful!)")
color: "black"
}
Rectangle {
id: geoFenceItems
anchors.margins: _margin
anchors.left: parent.left
anchors.right: parent.right
anchors.top: geoFenceLabel.bottom
height: editorColumn.height + (_margin * 2)
color: qgcPal.windowShadeDark
radius: _radius
Column {
id: editorColumn
anchors.margins: _margin
anchors.top: parent.top
anchors.left: parent.left
anchors.right: parent.right
spacing: _margin
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: qsTr("Click in map to set breach return point.")
}
QGCLabel { text: qsTr("Fence Settings:") }
Rectangle {
anchors.left: parent.left
anchors.right: parent.right
height: 1
color: qgcPal.text
}
QGCLabel {
text: qsTr("Must be connected to Vehicle to change fence settings.")
visible: !QGroundControl.multiVehicleManager.activeVehicle
}
Repeater {
model: geoFenceController.params
Item {
width: editorColumn.width
height: textField.height
QGCLabel {
id: textFieldLabel
anchors.baseline: textField.baseline
text: modelData.name
}
FactTextField {
id: textField
anchors.right: parent.right
width: _editFieldWidth
showUnits: true
fact: modelData
}
}
}
QGCMapPolygonControls {
anchors.left: parent.left
anchors.right: parent.right
flightMap: editorMap
polygon: root.polygon
sectionLabel: qsTr("Fence Polygon:")
}
}
}
}
}
This diff is collapsed.
import QtQuick 2.2
import QtQuick.Controls 1.2
import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0
import QGroundControl.Palette 1.0
/// Controls for drawing/editing map polygon
Column {
id: root
spacing: _margin
property var sectionLabel: qsTr("Polygon:") ///< Section label
property var flightMap ///< Must be set to FlightMap control
property var polygon ///< Must be set to MapPolygon
property real _margin: ScreenTools.defaultFontPixelWidth / 2
function polygonCaptureStarted() {
polygon.clear()
}
function polygonCaptureFinished(coordinates) {
polygon.path = coordinates
}
function polygonAdjustVertex(vertexIndex, vertexCoordinate) {
polygon.adjustCoordinate(vertexIndex, vertexCoordinate)
}
function polygonAdjustStarted() { }
function polygonAdjustFinished() { }
QGCLabel { text: sectionLabel }
Rectangle {
anchors.left: parent.left
anchors.right: parent.right
height: 1
color: qgcPal.text
}
Row {
spacing: ScreenTools.defaultFontPixelWidth
QGCButton {
text: flightMap.polygonDraw.drawingPolygon ? qsTr("Finish Draw") : qsTr("Draw")
visible: !flightMap.polygonDraw.adjustingPolygon
enabled: ((flightMap.polygonDraw.drawingPolygon && flightMap.polygonDraw.polygonReady) || !flightMap.polygonDraw.drawingPolygon)
onClicked: {
if (flightMap.polygonDraw.drawingPolygon) {
flightMap.polygonDraw.finishCapturePolygon()
} else {
flightMap.polygonDraw.startCapturePolygon(root)
}
}
}
QGCButton {
text: flightMap.polygonDraw.adjustingPolygon ? qsTr("Finish Adjust") : qsTr("Adjust")
visible: polygon.path.length > 0 && !flightMap.polygonDraw.drawingPolygon
onClicked: {
if (flightMap.polygonDraw.adjustingPolygon) {
flightMap.polygonDraw.finishAdjustPolygon()
} else {
flightMap.polygonDraw.startAdjustPolygon(root, polygon.path)
}
}
}
}
}
......@@ -54,22 +54,23 @@ Rectangle {
missionItem.cameraTriggerDistance.rawValue = cameraTriggerDistance
}
Connections {
target: editorMap.polygonDraw
onPolygonCaptureStarted: {
missionItem.clearPolygon()
}
function polygonCaptureStarted() {
missionItem.clearPolygon()
}
onPolygonCaptureFinished: {
for (var i=0; i<coordinates.length; i++) {
missionItem.addPolygonCoordinate(coordinates[i])
}
function polygonCaptureFinished(coordinates) {
for (var i=0; i<coordinates.length; i++) {
missionItem.addPolygonCoordinate(coordinates[i])
}
}
onPolygonAdjustVertex: missionItem.adjustPolygonCoordinate(vertexIndex, vertexCoordinate)
function polygonAdjustVertex(vertexIndex, vertexCoordinate) {
missionItem.adjustPolygonCoordinate(vertexIndex, vertexCoordinate)
}
function polygonAdjustStarted() { }
function polygonAdjustFinished() { }
QGCPalette { id: qgcPal; colorGroupEnabled: true }
ExclusiveGroup {
......@@ -229,7 +230,7 @@ Rectangle {
if (editorMap.polygonDraw.drawingPolygon) {
editorMap.polygonDraw.finishCapturePolygon()
} else {
editorMap.polygonDraw.startCapturePolygon()
editorMap.polygonDraw.startCapturePolygon(_root)
}
}
}
......@@ -242,7 +243,7 @@ Rectangle {
if (editorMap.polygonDraw.adjustingPolygon) {
editorMap.polygonDraw.finishAdjustPolygon()
} else {
editorMap.polygonDraw.startAdjustPolygon(missionItem.polygonPath)
editorMap.polygonDraw.startAdjustPolygon(_root, missionItem.polygonPath)
}
}
}
......
/****************************************************************************
*
* (c) 2009-2016 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.
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#include "GeoFenceController.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"
#include "ParameterLoader.h"
QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog")
GeoFenceController::GeoFenceController(QObject* parent)
: PlanElementController(parent)
, _dirty(false)
{
_clearGeoFence();
}
GeoFenceController::~GeoFenceController()
{
}
void GeoFenceController::start(bool editMode)
{
qCDebug(GeoFenceControllerLog) << "start editMode" << editMode;
PlanElementController::start(editMode);
connect(_multiVehicleMgr, &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &GeoFenceController::_parameterReadyVehicleAvailableChanged);
connect(&_geoFence.polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
}
void GeoFenceController::setFenceType(GeoFenceTypeEnum fenceType)
{
if (_geoFence.fenceType != (GeoFenceManager::GeoFenceType_t)fenceType) {
_geoFence.fenceType = (GeoFenceManager::GeoFenceType_t)fenceType;
emit fenceTypeChanged(fenceType);
}
}
void GeoFenceController::setCircleRadius(float circleRadius)
{
if (qFuzzyCompare(_geoFence.circleRadius, circleRadius)) {
_geoFence.circleRadius = circleRadius;
emit circleRadiusChanged(circleRadius);
}
}
void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint)
{
if (_geoFence.breachReturnPoint != breachReturnPoint) {
_geoFence.breachReturnPoint = breachReturnPoint;
emit breachReturnPointChanged(breachReturnPoint);
}
}
void GeoFenceController::_setParams(void)
{
if (_params.count() == 0 && _activeVehicle && _multiVehicleMgr->parameterReadyVehicleAvailable()) {
QStringList skipList;
skipList << QStringLiteral("FENCE_TOTAL") << QStringLiteral("FENCE_ENABLE");
QStringList allNames = _activeVehicle->autopilotPlugin()->parameterNames(-1);
foreach (const QString& paramName, allNames) {
if (paramName.startsWith(QStringLiteral("FENCE_")) && !skipList.contains(paramName)) {
_params << QVariant::fromValue(_activeVehicle->autopilotPlugin()->getParameterFact(-1, paramName));
}
}
emit paramsChanged();
}
}
void GeoFenceController::_activeVehicleBeingRemoved(void)
{
_clearGeoFence();
_params.clear();
emit paramsChanged();
_activeVehicle->geoFenceManager()->disconnect(this);
}
void GeoFenceController::_activeVehicleSet(void)
{
connect(_activeVehicle->geoFenceManager(), &GeoFenceManager::newGeoFenceAvailable, this, &GeoFenceController::_newGeoFenceAvailable);
_setParams();
if (_activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) {
// We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle.
// We don't request mission items for new vehicles since that will happen autamatically.
loadFromVehicle();
}
}
void GeoFenceController::_parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable)
{
Q_UNUSED(parameterReadyVehicleAvailable);
_setParams();
}
void GeoFenceController::_newGeoFenceAvailable(void)
{
_setGeoFence(_activeVehicle->geoFenceManager()->geoFence());
setDirty(false);
}
void GeoFenceController::loadFromFilePicker(void)
{
}
void GeoFenceController::loadFromFile(const QString& filename)
{
Q_UNUSED(filename);
}
void GeoFenceController::saveToFilePicker(void)
{
}
void GeoFenceController::saveToFile(const QString& filename)
{
Q_UNUSED(filename);
}
void GeoFenceController::removeAll(void)
{
_clearGeoFence();
}
void GeoFenceController::loadFromVehicle(void)
{
if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) {
_activeVehicle->geoFenceManager()->requestGeoFence();
} else {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress();
}
}
void GeoFenceController::sendToVehicle(void)
{
if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) {
// FIXME: Hack
setFenceType(GeoFencePolygon);
setDirty(false);
_geoFence.polygon.setDirty(false);
_activeVehicle->geoFenceManager()->setGeoFence(_geoFence);
} else {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress();
}
}
void GeoFenceController::_clearGeoFence(void)
{
setFenceType(GeoFenceNone);
setCircleRadius(0.0);
setBreachReturnPoint(QGeoCoordinate());
_geoFence.polygon.clear();
}
void GeoFenceController::_setGeoFence(const GeoFenceManager::GeoFence_t& geoFence)
{
_clearGeoFence();
setFenceType(static_cast<GeoFenceTypeEnum>(geoFence.fenceType));
setCircleRadius(geoFence.circleRadius);
setBreachReturnPoint(geoFence.breachReturnPoint);
_geoFence.polygon = geoFence.polygon;
}
bool GeoFenceController::syncInProgress(void) const
{
if (_activeVehicle) {
return _activeVehicle->geoFenceManager()->inProgress();
} else {
return false;
}
}
bool GeoFenceController::dirty(void) const
{
return _dirty | _geoFence.polygon.dirty();
}
void GeoFenceController::setDirty(bool dirty)
{
if (dirty != _dirty) {
_dirty = dirty;
if (!dirty) {
_geoFence.polygon.setDirty(dirty);
}
emit dirtyChanged(dirty);
}
}
void GeoFenceController::_polygonDirtyChanged(bool dirty)
{
if (dirty) {
setDirty(true);
}
}
/****************************************************************************
*
* (c) 2009-2016 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.
*
****************************************************************************/
#ifndef GeoFenceController_H
#define GeoFenceController_H
#include "PlanElementController.h"
#include "GeoFenceManager.h"
#include "QGCMapPolygon.h"
#include "Vehicle.h"
#include "MultiVehicleManager.h"
#include "QGCLoggingCategory.h"
Q_DECLARE_LOGGING_CATEGORY(GeoFenceControllerLog)
class GeoFenceController : public PlanElementController
{
Q_OBJECT
public:
GeoFenceController(QObject* parent = NULL);
~GeoFenceController();
enum GeoFenceTypeEnum {
GeoFenceNone = GeoFenceManager::GeoFenceNone,
GeoFenceCircle = GeoFenceManager::GeoFenceCircle,
GeoFencePolygon = GeoFenceManager::GeoFencePolygon,
};
Q_PROPERTY(GeoFenceTypeEnum fenceType READ fenceType WRITE setFenceType NOTIFY fenceTypeChanged)
Q_PROPERTY(float circleRadius READ circleRadius WRITE setCircleRadius NOTIFY circleRadiusChanged)
Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT)
Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged)
void start (bool editMode) final;
void loadFromVehicle (void) final;
void sendToVehicle (void) final;
void loadFromFilePicker (void) final;
void loadFromFile (const QString& filename) final;
void saveToFilePicker (void) final;
void saveToFile (const QString& filename) final;
void removeAll (void) final;
bool syncInProgress (void) const final;
bool dirty (void) const final;
void setDirty (bool dirty) final;
GeoFenceTypeEnum fenceType (void) const { return (GeoFenceTypeEnum)_geoFence.fenceType; }
float circleRadius (void) const { return _geoFence.circleRadius; }
QGCMapPolygon* polygon (void) { return &_geoFence.polygon; }
QGeoCoordinate breachReturnPoint (void) const { return _geoFence.breachReturnPoint; }
QVariantList params (void) { return _params; }
void setFenceType(GeoFenceTypeEnum fenceType);
void setCircleRadius(float circleRadius);
void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
signals:
void fenceTypeChanged (GeoFenceTypeEnum fenceType);
void circleRadiusChanged (float circleRadius);
void polygonPathChanged (const QVariantList& polygonPath);
void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
void paramsChanged (void);
private slots:
void _parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable);
void _newGeoFenceAvailable(void);
void _polygonDirtyChanged(bool dirty);
private:
void _setParams(void);
void _clearGeoFence(void);
void _setGeoFence(const GeoFenceManager::GeoFence_t& geoFence);
void _activeVehicleBeingRemoved(void) final;
void _activeVehicleSet(void) final;
bool _dirty;
GeoFenceManager::GeoFence_t _geoFence;
QVariantList _params;