Skip to content

Q
qgroundcontrol
Unverified Commit a32d84f8 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #7383 from DonLakeFlyer/FenceReturnPoint 

Add support for geofence return point.
parents e2b3a235 0d5b7019
Show whitespace changes
Inline Side-by-side
Showing with 201 additions and 73 deletions
qgroundcontrol.qrc
View file @ a32d84f8
......@@ -212,6 +212,7 @@
</qresource>
<qresource prefix="/json">
<file alias="APMMavlinkStreamRate.SettingsGroup.json">src/Settings/APMMavlinkStreamRate.SettingsGroup.json</file>
<file alias="BreachReturn.FactMetaData.json">src/MissionManager/BreachReturn.FactMetaData.json</file>
<file alias="CameraCalc.FactMetaData.json">src/MissionManager/CameraCalc.FactMetaData.json</file>
<file alias="CameraSpec.FactMetaData.json">src/MissionManager/CameraSpec.FactMetaData.json</file>
<file alias="CorridorScan.SettingsGroup.json">src/MissionManager/CorridorScan.SettingsGroup.json</file>
......
src/MissionManager/BreachReturn.FactMetaData.json 0 → 100644
View file @ a32d84f8
[
{
"name": "Latitude",
"shortDescription": "Latitude of breach return point position",
"type": "double",
"decimalPlaces": 7
},
{
"name": "Longitude",
"shortDescription": "Longitude of breach return point position",
"type": "double",
"decimalPlaces": 7
},
{
"name": "Altitude",
"shortDescription": "Altitude of breach return point position (Rel)",
"type": "double",
"decimalPlaces": 2,
"units": "m"
}
]
src/MissionManager/GeoFenceController.cc
View file @ a32d84f8
......@@ -21,6 +21,8 @@
#include "QGCQGeoCoordinate.h"
#include "AppSettings.h"
#include "PlanMasterController.h"
#include "SettingsManager.h"
#include "AppSettings.h"
#ifndef __mobile__
#include "MainWindow.h"
......@@ -32,24 +34,43 @@
QGC_LOGGING_CATEGORY(GeoFenceControllerLog, "GeoFenceControllerLog")
QMap<QString, FactMetaData*> GeoFenceController::_metaDataMap;
const char* GeoFenceController::_jsonFileTypeValue = "GeoFence";
const char* GeoFenceController::_jsonBreachReturnKey = "breachReturn";
const char* GeoFenceController::_jsonPolygonsKey = "polygons";
const char* GeoFenceController::_jsonCirclesKey = "circles";
const char* GeoFenceController::_breachReturnAltitudeFactName = "Altitude";
const char* GeoFenceController::_px4ParamCircularFence = "GF_MAX_HOR_DIST";
GeoFenceController::GeoFenceController(PlanMasterController* masterController, QObject* parent)
: PlanElementController (masterController, parent)
, _geoFenceManager (_managerVehicle->geoFenceManager())
, _dirty (false)
, _breachReturnAltitudeFact (0, _breachReturnAltitudeFactName, FactMetaData::valueTypeDouble)
, _breachReturnDefaultAltitude (qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue().toDouble())
, _itemsRequested (false)
, _px4ParamCircularFenceFact(NULL)
, _px4ParamCircularFenceFact(nullptr)
{
if (_metaDataMap.isEmpty()) {
_metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/BreachReturn.FactMetaData.json"), nullptr /* metaDataParent */);
}
_breachReturnAltitudeFact.setMetaData(_metaDataMap[_breachReturnAltitudeFactName]);
_breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude);
connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
managerVehicleChanged(_managerVehicle);
connect(this, &GeoFenceController::breachReturnPointChanged, this, &GeoFenceController::_setDirty);
connect(&_breachReturnAltitudeFact, &Fact::rawValueChanged, this, &GeoFenceController::_setDirty);
connect(&_polygons, &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_setDirty);
connect(&_circles, &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_setDirty);
}
GeoFenceController::~GeoFenceController()
......@@ -79,26 +100,19 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn
}
}
void GeoFenceController::_signalAll(void)
{
emit breachReturnPointChanged(breachReturnPoint());
emit dirtyChanged(dirty());
emit supportedChanged(supported());
}
void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
{
if (_managerVehicle) {
_geoFenceManager->disconnect(this);
_managerVehicle->disconnect(this);
_managerVehicle->parameterManager()->disconnect(this);
_managerVehicle = NULL;
_geoFenceManager = NULL;
_managerVehicle = nullptr;
_geoFenceManager = nullptr;
}
_managerVehicle = managerVehicle;
if (!_managerVehicle) {
qWarning() << "GeoFenceController::managerVehicleChanged managerVehicle=NULL";
qWarning() << "GeoFenceController::managerVehicleChanged managerVehicle=nullptr";
return;
}
......@@ -114,7 +128,6 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
_parametersReady();
emit supportedChanged(supported());
_signalAll();
}
bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
......@@ -133,6 +146,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
{ JsonHelper::jsonVersionKey, QJsonValue::Double, true },
{ _jsonCirclesKey, QJsonValue::Array, true },
{ _jsonPolygonsKey, QJsonValue::Array, true },
{ _jsonBreachReturnKey, QJsonValue::Array, false },
};
if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
return false;
......@@ -144,7 +158,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
}
QJsonArray jsonPolygonArray = json[_jsonPolygonsKey].toArray();
for (const QJsonValue& jsonPolygonValue: jsonPolygonArray) {
for (const QJsonValue jsonPolygonValue: jsonPolygonArray) {
if (jsonPolygonValue.type() != QJsonValue::Object) {
errorString = tr("GeoFence polygon not stored as object");
return false;
......@@ -158,7 +172,7 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
}
QJsonArray jsonCircleArray = json[_jsonCirclesKey].toArray();
for (const QJsonValue& jsonCircleValue: jsonCircleArray) {
for (const QJsonValue jsonCircleValue: jsonCircleArray) {
if (jsonCircleValue.type() != QJsonValue::Object) {
errorString = tr("GeoFence circle not stored as object");
return false;
......@@ -171,8 +185,18 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
_circles.append(fenceCircle);
}
if (json.contains(_jsonBreachReturnKey)) {
if (!JsonHelper::loadGeoCoordinate(json[_jsonBreachReturnKey], true /* altitudeRequred */, _breachReturnPoint, errorString)) {
return false;
}
_breachReturnAltitudeFact.setRawValue(_breachReturnPoint.altitude());
} else {
_breachReturnPoint = QGeoCoordinate();
_breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude);
}
emit breachReturnPointChanged(_breachReturnPoint);
setDirty(false);
_signalAll();
return true;
}
......@@ -198,6 +222,14 @@ void GeoFenceController::save(QJsonObject& json)
jsonCircleArray.append(jsonCircle);
}
json[_jsonCirclesKey] = jsonCircleArray;
if (_breachReturnPoint.isValid()) {
QJsonValue jsonCoordinate;
_breachReturnPoint.setAltitude(_breachReturnAltitudeFact.rawValue().toDouble());
JsonHelper::saveGeoCoordinate(_breachReturnPoint, true /* writeAltitude */, jsonCoordinate);
json[_jsonBreachReturnKey] = jsonCoordinate;
}
}
void GeoFenceController::removeAll(void)
......@@ -305,6 +337,11 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP
{
_breachReturnPoint = breachReturnPoint;
emit breachReturnPointChanged(_breachReturnPoint);
if (_breachReturnPoint.isValid()) {
_breachReturnAltitudeFact.setRawValue(_breachReturnPoint.altitude());
} else {
_breachReturnAltitudeFact.setRawValue(_breachReturnDefaultAltitude);
}
}
void GeoFenceController::_managerLoadComplete(void)
......@@ -315,7 +352,6 @@ void GeoFenceController::_managerLoadComplete(void)
_setReturnPointFromManager(_geoFenceManager->breachReturnPoint());
_setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles());
setDirty(false);
_signalAll();
emit loadComplete();
}
_itemsRequested = false;
......@@ -476,7 +512,7 @@ void GeoFenceController::_parametersReady(void)
{
if (_px4ParamCircularFenceFact) {
_px4ParamCircularFenceFact->disconnect(this);
_px4ParamCircularFenceFact = NULL;
_px4ParamCircularFenceFact = nullptr;
}
if (_managerVehicle->isOfflineEditingVehicle() || !_managerVehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _px4ParamCircularFence)) {
......
src/MissionManager/GeoFenceController.h
View file @ a32d84f8
......@@ -27,12 +27,13 @@ class GeoFenceController : public PlanElementController
Q_OBJECT
public:
GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL);
GeoFenceController(PlanMasterController* masterController, QObject* parent = nullptr);
~GeoFenceController();
Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT)
Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT)
Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
Q_PROPERTY(Fact* breachReturnAltitude READ breachReturnAltitude CONSTANT)
// Hack to expose PX4 circular fence controlled by GF_MAX_HOR_DIST
Q_PROPERTY(double paramCircularFence READ paramCircularFence NOTIFY paramCircularFenceChanged)
......@@ -58,7 +59,8 @@ public:
/// Clears the interactive bit from all fence items
Q_INVOKABLE void clearAllInteractive(void);
double paramCircularFence(void);
double paramCircularFence (void);
Fact* breachReturnAltitude(void) { return &_breachReturnAltitudeFact; }
// Overrides from PlanElementController
bool supported (void) const final;
......@@ -101,16 +103,19 @@ private slots:
private:
void _init(void);
void _signalAll(void);
GeoFenceManager* _geoFenceManager;
bool _dirty;
QmlObjectListModel _polygons;
QmlObjectListModel _circles;
QGeoCoordinate _breachReturnPoint;
Fact _breachReturnAltitudeFact;
double _breachReturnDefaultAltitude;
bool _itemsRequested;
Fact* _px4ParamCircularFenceFact;
static QMap<QString, FactMetaData*> _metaDataMap;
static const char* _px4ParamCircularFence;
static const int _jsonCurrentVersion = 2;
......@@ -119,6 +124,8 @@ private:
static const char* _jsonBreachReturnKey;
static const char* _jsonPolygonsKey;
static const char* _jsonCirclesKey;
static const char* _breachReturnAltitudeFactName;
};
#endif
src/MissionManager/GeoFenceManager.cc
View file @ a32d84f8
......@@ -51,8 +51,6 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn,
QmlObjectListModel& polygons,
QmlObjectListModel& circles)
{
Q_UNUSED(breachReturn);
QList<MissionItem*> fenceItems;
_sendPolygons.clear();
......@@ -64,6 +62,7 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn,
for (int i=0; i<circles.count(); i++) {
_sendCircles.append(*circles.value<QGCFenceCircle*>(i));
}
_breachReturnPoint = breachReturn;
for (int i=0; i<_sendPolygons.count(); i++) {
const QGCFencePolygon& polygon = _sendPolygons[i];
......@@ -103,12 +102,30 @@ void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn,
fenceItems.append(item);
}
if (_breachReturnPoint.isValid()) {
MissionItem* item = new MissionItem(0,
MAV_CMD_NAV_FENCE_RETURN_POINT,
MAV_FRAME_GLOBAL_RELATIVE_ALT,
0, 0, 0, 0, // param 1-4 unused
breachReturn.latitude(),
breachReturn.longitude(),
breachReturn.altitude(),
false, // autocontinue
false, // isCurrentItem
this); // parent
fenceItems.append(item);
}
// Plan manager takes control of MissionItems, so no need to delete
_planManager.writeMissionItems(fenceItems);
}
void GeoFenceManager::removeAll(void)
{
_polygons.clear();
_circles.clear();
_breachReturnPoint = QGeoCoordinate();
_planManager.removeAll();
}
......@@ -117,6 +134,7 @@ void GeoFenceManager::_sendComplete(bool error)
if (error) {
_polygons.clear();
_circles.clear();
_breachReturnPoint = QGeoCoordinate();
} else {
_polygons = _sendPolygons;
_circles = _sendCircles;
......@@ -174,6 +192,8 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested)
}
QGCFenceCircle circle(QGeoCoordinate(item->param5(), item->param6()), item->param1(), command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION /* inclusion */);
_circles.append(circle);
} else if (command == MAV_CMD_NAV_FENCE_RETURN_POINT) {
_breachReturnPoint = QGeoCoordinate(item->param5(), item->param6(), item->param7());
} else {
emit error(UnsupportedCommand, tr("GeoFence load: Unsupported command %1").arg(item->command()));
break;
......@@ -183,6 +203,7 @@ void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested)
if (loadFailed) {
_polygons.clear();
_circles.clear();
_breachReturnPoint = QGeoCoordinate();
}
emit loadComplete();
......
src/MissionManager/RallyPointManager.cc
View file @ a32d84f8
......@@ -72,6 +72,7 @@ void RallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
void RallyPointManager::removeAll(void)
{
_rgPoints.clear();
_planManager.removeAll();
}
......
src/PlanView/GeoFenceEditor.qml
View file @ a32d84f8
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtQuick.Layouts 1.2
import QtPositioning 5.2
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
......@@ -53,7 +54,7 @@ QGCFlickable {
anchors.top: parent.top
anchors.left: parent.left
anchors.right: parent.right
spacing: ScreenTools.defaultFontPixelHeight / 2
spacing: _margin
QGCLabel {
anchors.left: parent.left
......@@ -68,7 +69,7 @@ QGCFlickable {
Column {
anchors.left: parent.left
anchors.right: parent.right
spacing: ScreenTools.defaultFontPixelHeight / 2
spacing: _margin
visible: myGeoFenceController.supported
Repeater {
......@@ -114,8 +115,7 @@ QGCFlickable {
}
QGCButton {
anchors.left: parent.left
anchors.right: parent.right
Layout.fillWidth: true
text: qsTr("Polygon Fence")
onClicked: {
......@@ -127,8 +127,7 @@ QGCFlickable {
}
QGCButton {
anchors.left: parent.left
anchors.right: parent.right
Layout.fillWidth: true
text: qsTr("Circular Fence")
onClicked: {
......@@ -150,8 +149,7 @@ QGCFlickable {
}
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
Layout.fillWidth: true
columns: 3
flow: GridLayout.TopToBottom
visible: polygonSection.checked && myGeoFenceController.polygons.count > 0
......@@ -302,6 +300,46 @@ QGCFlickable {
}
}
} // GridLayout
SectionHeader {
id: breachReturnSection
text: qsTr("Breach Return Point")
}
QGCButton {
text: qsTr("Add Breach Return Point")
visible: breachReturnSection.visible && !myGeoFenceController.breachReturnPoint.isValid
anchors.left: parent.left
anchors.right: parent.right
onClicked: myGeoFenceController.breachReturnPoint = flightMap.center
}
QGCButton {
text: qsTr("Remove Breach Return Point")
visible: breachReturnSection.visible && myGeoFenceController.breachReturnPoint.isValid
anchors.left: parent.left
anchors.right: parent.right
onClicked: myGeoFenceController.breachReturnPoint = QtPositioning.coordinate()
}
ColumnLayout {
anchors.left: parent.left
anchors.right: parent.right
spacing: _margin
visible: breachReturnSection.visible && myGeoFenceController.breachReturnPoint.isValid
QGCLabel {
text: qsTr("Altitude")
}
AltitudeFactTextField {
fact: myGeoFenceController.breachReturnAltitude
altitudeMode: QGroundControl.AltitudeModeRelative
}
}
}
}
}
......
src/PlanView/GeoFenceMapVisuals.qml
View file @ a32d84f8
......@@ -28,8 +28,8 @@ Item {
property bool planView: false ///< true: visuals showing in plan view
property var homePosition
//property var _breachReturnPointComponent
//property var _mouseAreaComponent
property var _breachReturnPointComponent
property var _breachReturnDragComponent
property var _paramCircleFenceComponent
property var _polygons: myGeoFenceController.polygons
property var _circles: myGeoFenceController.circles
......@@ -75,30 +75,19 @@ Item {
}
Component.onCompleted: {
//_breachReturnPointComponent = breachReturnPointComponent.createObject(map)
//map.addMapItem(_breachReturnPointComponent)
//_mouseAreaComponent = mouseAreaComponent.createObject(map)
_breachReturnPointComponent = breachReturnPointComponent.createObject(map)
map.addMapItem(_breachReturnPointComponent)
_breachReturnDragComponent = breachReturnDragComponent.createObject(map, { "itemIndicator": _breachReturnPointComponent })
_paramCircleFenceComponent = paramCircleFenceComponent.createObject(map)
map.addMapItem(_paramCircleFenceComponent)
}
Component.onDestruction: {
//_breachReturnPointComponent.destroy()
//_mouseAreaComponent.destroy()
_breachReturnPointComponent.destroy()
_breachReturnDragComponent.destroy()
_paramCircleFenceComponent.destroy()
}
// Mouse area to capture breach return point coordinate
Component {
id: mouseAreaComponent
MouseArea {
anchors.fill: map
visible: interactive
onClicked: myGeoFenceController.breachReturnPoint = map.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */)
}
}
Instantiator {
model: _polygons
......@@ -144,6 +133,19 @@ Item {
}
}
Component {
id: breachReturnDragComponent
MissionItemIndicatorDrag {
mapControl: map
itemCoordinate: myGeoFenceController.breachReturnPoint
//visible: itemCoordinate.isValid
onItemCoordinateChanged: myGeoFenceController.breachReturnPoint = itemCoordinate
}
}
// Breach return point
Component {
id: breachReturnPointComponent
......@@ -155,7 +157,8 @@ Item {
coordinate: myGeoFenceController.breachReturnPoint
sourceItem: MissionItemIndexLabel {
label: "B"
label: qsTr("B", "Breach Return Point item indicator")
checked: true
}
}
}
......
src/PlanView/PlanView.qml
View file @ a32d84f8
......@@ -938,9 +938,9 @@ QGCView {
(_planMasterController.offline ? "" : qsTr("This will also remove all items from the vehicle."))
function accept() {
if (_planMasterController.offline) {
masterController.removeAll()
_planMasterController.removeAll()
} else {
masterController.removeAllFromVehicle()
_planMasterController.removeAllFromVehicle()
}
hideDialog()
}
......@@ -1042,7 +1042,7 @@ QGCView {
QGCButton {
text: qsTr("New...")
Layout.fillWidth: true
enabled: _visualItems.count > 1
enabled: _planMasterController.containsItems
onClicked: {
dropPanel.hide()
_qgcView.showDialog(removeAllPromptDialog, qsTr("New Plan"), _qgcView.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No)
......@@ -1080,7 +1080,7 @@ QGCView {
QGCButton {
text: qsTr("Save As...")
Layout.fillWidth: true
enabled: !masterController.syncInProgress && _visualItems.count > 1
enabled: !masterController.syncInProgress && _planMasterController.containsItems
onClicked: {
dropPanel.hide()
masterController.saveToSelectedFile()
......@@ -1115,7 +1115,7 @@ QGCView {
QGCButton {
text: qsTr("Upload")
Layout.fillWidth: true
enabled: !masterController.offline && !masterController.syncInProgress && _visualItems.count > 1
enabled: !masterController.offline && !masterController.syncInProgress && _planMasterController.containsItems
visible: !QGroundControl.corePlugin.options.disableVehicleConnection
onClicked: {
dropPanel.hide()
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment