Don Gagne
9 years ago
committed by
GitHub
24 changed files with 1504 additions and 289 deletions
@ -0,0 +1,122 @@
@@ -0,0 +1,122 @@
|
||||
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:") |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,73 @@
@@ -0,0 +1,73 @@
|
||||
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) |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -0,0 +1,213 @@
@@ -0,0 +1,213 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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); |
||||
} |
||||
} |
||||
@ -0,0 +1,89 @@
@@ -0,0 +1,89 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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; |
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,254 @@
@@ -0,0 +1,254 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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 "GeoFenceManager.h" |
||||
#include "Vehicle.h" |
||||
#include "FirmwarePlugin.h" |
||||
#include "MAVLinkProtocol.h" |
||||
#include "QGCApplication.h" |
||||
|
||||
QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog") |
||||
|
||||
const char* GeoFenceManager::_fenceTotalParam = "FENCE_TOTAL"; |
||||
const char* GeoFenceManager::_fenceActionParam = "FENCE_ACTION"; |
||||
|
||||
GeoFenceManager::GeoFenceManager(Vehicle* vehicle) |
||||
: _vehicle(vehicle) |
||||
, _readTransactionInProgress(false) |
||||
, _writeTransactionInProgress(false) |
||||
{ |
||||
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &GeoFenceManager::_mavlinkMessageReceived); |
||||
} |
||||
|
||||
GeoFenceManager::~GeoFenceManager() |
||||
{ |
||||
|
||||
} |
||||
|
||||
void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg) |
||||
{ |
||||
qCDebug(GeoFenceManagerLog) << "Sending error" << errorCode << errorMsg; |
||||
|
||||
emit error(errorCode, errorMsg); |
||||
} |
||||
|
||||
void GeoFenceManager::setGeoFence(const GeoFence_t& geoFence) |
||||
{ |
||||
if (_readTransactionInProgress) { |
||||
_sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress.")); |
||||
return; |
||||
} |
||||
|
||||
if (!_geoFenceSupported()) { |
||||
return; |
||||
} |
||||
|
||||
// Validate
|
||||
switch (geoFence.fenceType) { |
||||
case GeoFencePolygon: |
||||
if (geoFence.polygon.count() < 3) { |
||||
_sendError(TooFewPoints, QStringLiteral("Geo-Fence polygon must contain at least 3 points.")); |
||||
return; |
||||
} |
||||
if (geoFence.polygon.count() > std::numeric_limits<uint8_t>::max()) { |
||||
_sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(geoFence.polygon.count())); |
||||
return; |
||||
} |
||||
break; |
||||
case GeoFenceCircle: |
||||
if (geoFence.circleRadius <= 0.0) { |
||||
_sendError(InvalidCircleRadius, QStringLiteral("Geo-Fence circle radius must be greater than 0.")); |
||||
return; |
||||
} |
||||
default: |
||||
break; |
||||
} |
||||
|
||||
_geoFence.fenceType = geoFence.fenceType; |
||||
_geoFence.circleRadius = geoFence.circleRadius; |
||||
_geoFence.polygon = geoFence.polygon; |
||||
_geoFence.breachReturnPoint = geoFence.breachReturnPoint; |
||||
|
||||
if (_geoFence.fenceType != GeoFencePolygon) { |
||||
// Circle is just params, so no more work to do
|
||||
return; |
||||
} |
||||
|
||||
emit newGeoFenceAvailable(); |
||||
|
||||
// First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air
|
||||
Fact* fenceActionFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceActionParam); |
||||
_savedWriteFenceAction = fenceActionFact->rawValue(); |
||||
fenceActionFact->setRawValue(0); |
||||
|
||||
// Fence total param includes:
|
||||
// index 0: breach return
|
||||
// last index: polygon close (same as first polygon point)
|
||||
_vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_geoFence.polygon.count() + 2); |
||||
|
||||
// FIXME: No validation of correct fence received
|
||||
// Send points:
|
||||
// breach return
|
||||
// polygon fence points
|
||||
// polygon close
|
||||
// Put back previous fence action to start geofence again
|
||||
_sendFencePoint(0); |
||||
for (uint8_t index=0; index<_geoFence.polygon.count(); index++) { |
||||
_sendFencePoint(index + 1); |
||||
} |
||||
_sendFencePoint(_geoFence.polygon.count() + 1); |
||||
fenceActionFact->setRawValue(_savedWriteFenceAction); |
||||
} |
||||
|
||||
void GeoFenceManager::requestGeoFence(void) |
||||
{ |
||||
_clearGeoFence(); |
||||
|
||||
if (!_geoFenceSupported()) { |
||||
return; |
||||
} |
||||
|
||||
// Point 0: Breach return point
|
||||
// Point [1,N]: Polygon points
|
||||
// Point N+1: Close polygon point (same as point 1)
|
||||
int cFencePoints = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt(); |
||||
qCDebug(GeoFenceManagerLog) << "GeoFenceManager::requestGeoFence" << cFencePoints; |
||||
if (cFencePoints == 0) { |
||||
// No fence, no more work to do, fence data has already been cleared
|
||||
return; |
||||
} |
||||
if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < 6)) { |
||||
_sendError(TooFewPoints, QStringLiteral("Geo-Fence information from Vehicle has too few points: %1").arg(cFencePoints)); |
||||
return; |
||||
} |
||||
if (cFencePoints > std::numeric_limits<uint8_t>::max()) { |
||||
_sendError(TooManyPoints, QStringLiteral("Geo-Fence information from Vehicle has too many points: %1").arg(cFencePoints)); |
||||
return; |
||||
} |
||||
|
||||
_readTransactionInProgress = true; |
||||
_cReadFencePoints = cFencePoints; |
||||
_currentFencePoint = 0; |
||||
|
||||
_requestFencePoint(_currentFencePoint); |
||||
} |
||||
|
||||
/// Called when a new mavlink message for out vehicle is received
|
||||
void GeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message) |
||||
{ |
||||
if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) { |
||||
mavlink_fence_point_t fencePoint; |
||||
|
||||
mavlink_msg_fence_point_decode(&message, &fencePoint); |
||||
if (fencePoint.idx != _currentFencePoint) { |
||||
// FIXME: Protocol out of whack
|
||||
qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint; |
||||
return; |
||||
} |
||||
|
||||
if (fencePoint.idx == 0) { |
||||
_geoFence.breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng); |
||||
qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _geoFence.breachReturnPoint; |
||||
_requestFencePoint(++_currentFencePoint); |
||||
} else if (fencePoint.idx < _cReadFencePoints - 1) { |
||||
QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng); |
||||
_geoFence.polygon.addCoordinate(polyCoord); |
||||
qCDebug(GeoFenceManagerLog) << "From vehicle: polygon point" << fencePoint.idx << polyCoord; |
||||
if (fencePoint.idx < _cReadFencePoints - 2) { |
||||
// Still more points to request
|
||||
_requestFencePoint(++_currentFencePoint); |
||||
} else { |
||||
// We've finished collecting fence points
|
||||
_readTransactionInProgress = false; |
||||
emit newGeoFenceAvailable(); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
void GeoFenceManager::_clearGeoFence(void) |
||||
{ |
||||
_geoFence.fenceType = GeoFenceNone; |
||||
_geoFence.circleRadius = 0.0; |
||||
_geoFence.polygon.clear(); |
||||
_geoFence.breachReturnPoint = QGeoCoordinate(); |
||||
emit newGeoFenceAvailable(); |
||||
} |
||||
|
||||
void GeoFenceManager::_requestFencePoint(uint8_t pointIndex) |
||||
{ |
||||
mavlink_message_t msg; |
||||
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); |
||||
|
||||
qCDebug(GeoFenceManagerLog) << "GeoFenceManager::_requestFencePoint" << pointIndex; |
||||
mavlink_msg_fence_fetch_point_pack(mavlink->getSystemId(), |
||||
mavlink->getComponentId(), |
||||
&msg, |
||||
_vehicle->id(), |
||||
_vehicle->defaultComponentId(), |
||||
pointIndex); |
||||
_vehicle->sendMessageOnPriorityLink(msg); |
||||
} |
||||
|
||||
void GeoFenceManager::_sendFencePoint(uint8_t pointIndex) |
||||
{ |
||||
mavlink_message_t msg; |
||||
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); |
||||
|
||||
QGeoCoordinate fenceCoord; |
||||
if (pointIndex == 0) { |
||||
fenceCoord = _geoFence.breachReturnPoint; |
||||
} else if (pointIndex - 1 < _geoFence.polygon.count()) { |
||||
fenceCoord = _geoFence.polygon[pointIndex - 1]; |
||||
} else { |
||||
// Polygon close point
|
||||
fenceCoord = _geoFence.polygon[0]; |
||||
} |
||||
|
||||
mavlink_msg_fence_point_pack(mavlink->getSystemId(), |
||||
mavlink->getComponentId(), |
||||
&msg, |
||||
_vehicle->id(), |
||||
_vehicle->defaultComponentId(), |
||||
pointIndex, // Index of point to set
|
||||
_geoFence.polygon.count() + 2, // Total point count, +1 for breach in index 0, +1 polygon close in last index
|
||||
fenceCoord.latitude(), |
||||
fenceCoord.longitude()); |
||||
_vehicle->sendMessageOnPriorityLink(msg); |
||||
} |
||||
|
||||
bool GeoFenceManager::inProgress(void) const |
||||
{ |
||||
return _readTransactionInProgress || _writeTransactionInProgress; |
||||
} |
||||
|
||||
bool GeoFenceManager::_geoFenceSupported(void) |
||||
{ |
||||
// FIXME: MockLink doesn't support geo fence yet
|
||||
if (qgcApp()->runningUnitTests()) { |
||||
return false; |
||||
} |
||||
|
||||
// FIXME: Hack to get around lack of plugin-ized version of code
|
||||
if (_vehicle->apmFirmware()) { |
||||
if (!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) || |
||||
!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) { |
||||
_sendError(InternalError, QStringLiteral("Vehicle does not support Geo-Fence implementation.")); |
||||
return false; |
||||
} else { |
||||
return true; |
||||
} |
||||
} else { |
||||
return false; |
||||
} |
||||
} |
||||
@ -0,0 +1,99 @@
@@ -0,0 +1,99 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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 GeoFenceManager_H |
||||
#define GeoFenceManager_H |
||||
|
||||
#include <QObject> |
||||
#include <QTimer> |
||||
#include <QGeoCoordinate> |
||||
|
||||
#include "MissionItem.h" |
||||
#include "QGCMAVLink.h" |
||||
#include "QGCLoggingCategory.h" |
||||
#include "LinkInterface.h" |
||||
#include "QGCMapPolygon.h" |
||||
|
||||
class Vehicle; |
||||
|
||||
Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog) |
||||
|
||||
class GeoFenceManager : public QObject |
||||
{ |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
GeoFenceManager(Vehicle* vehicle); |
||||
~GeoFenceManager(); |
||||
|
||||
typedef enum { |
||||
GeoFenceNone, |
||||
GeoFenceCircle, |
||||
GeoFencePolygon, |
||||
} GeoFenceType_t; |
||||
|
||||
typedef struct { |
||||
GeoFenceType_t fenceType; |
||||
float circleRadius; |
||||
QGCMapPolygon polygon; |
||||
QGeoCoordinate breachReturnPoint; |
||||
} GeoFence_t; |
||||
|
||||
bool inProgress(void) const; |
||||
|
||||
/// Request the geo fence from the vehicle
|
||||
void requestGeoFence(void); |
||||
|
||||
/// Returns the current geofence settings
|
||||
const GeoFence_t& geoFence(void) const { return _geoFence; } |
||||
|
||||
/// Set and send the specified geo fence to the vehicle
|
||||
void setGeoFence(const GeoFence_t& geoFence); |
||||
|
||||
/// Error codes returned in error signal
|
||||
typedef enum { |
||||
InternalError, |
||||
TooFewPoints, ///< Too few points for valid geofence
|
||||
TooManyPoints, ///< Too many points for valid geofence
|
||||
InvalidCircleRadius, |
||||
} ErrorCode_t; |
||||
|
||||
signals: |
||||
void newGeoFenceAvailable(void); |
||||
void inProgressChanged(bool inProgress); |
||||
void error(int errorCode, const QString& errorMsg); |
||||
|
||||
private slots: |
||||
void _mavlinkMessageReceived(const mavlink_message_t& message); |
||||
//void _ackTimeout(void);
|
||||
|
||||
private: |
||||
void _sendError(ErrorCode_t errorCode, const QString& errorMsg); |
||||
void _clearGeoFence(void); |
||||
void _requestFencePoint(uint8_t pointIndex); |
||||
void _sendFencePoint(uint8_t pointIndex); |
||||
bool _geoFenceSupported(void); |
||||
|
||||
private: |
||||
Vehicle* _vehicle; |
||||
|
||||
bool _readTransactionInProgress; |
||||
bool _writeTransactionInProgress; |
||||
|
||||
uint8_t _cReadFencePoints; |
||||
uint8_t _currentFencePoint; |
||||
QVariant _savedWriteFenceAction; |
||||
|
||||
GeoFence_t _geoFence; |
||||
|
||||
static const char* _fenceTotalParam; |
||||
static const char* _fenceActionParam; |
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,48 @@
@@ -0,0 +1,48 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "PlanElementController.h" |
||||
#include "QGCApplication.h" |
||||
|
||||
PlanElementController::PlanElementController(QObject* parent) |
||||
: QObject(parent) |
||||
, _activeVehicle(NULL) |
||||
, _multiVehicleMgr(qgcApp()->toolbox()->multiVehicleManager()) |
||||
, _editMode(false) |
||||
{ |
||||
|
||||
} |
||||
|
||||
PlanElementController::~PlanElementController() |
||||
{ |
||||
|
||||
} |
||||
|
||||
void PlanElementController::start(bool editMode) |
||||
{ |
||||
_editMode = editMode; |
||||
connect(_multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &PlanElementController::_activeVehicleChanged); |
||||
_activeVehicleChanged(_multiVehicleMgr->activeVehicle()); |
||||
} |
||||
|
||||
void PlanElementController::_activeVehicleChanged(Vehicle* activeVehicle) |
||||
{ |
||||
if (_activeVehicle) { |
||||
_activeVehicleBeingRemoved(); |
||||
} |
||||
|
||||
_activeVehicle = activeVehicle; |
||||
|
||||
if (_activeVehicle) { |
||||
_activeVehicleSet(); |
||||
} |
||||
|
||||
// Whenever vehicle changes we need to update syncInProgress
|
||||
emit syncInProgressChanged(syncInProgress()); |
||||
} |
||||
@ -0,0 +1,71 @@
@@ -0,0 +1,71 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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 PlanElementController_H |
||||
#define PlanElementController_H |
||||
|
||||
#include <QObject> |
||||
|
||||
#include "Vehicle.h" |
||||
#include "MultiVehicleManager.h" |
||||
|
||||
/// This is the abstract base clas for Plan Element controllers.
|
||||
/// Examples of plan elements are: missions (MissionController), geofence (GeoFenceController)
|
||||
class PlanElementController : public QObject |
||||
{ |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
PlanElementController(QObject* parent = NULL); |
||||
~PlanElementController(); |
||||
|
||||
/// true: information is currently being saved/sent, false: no active save/send in progress
|
||||
Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) |
||||
|
||||
/// true: unsaved/sent changes are present, false: no changes since last save/send
|
||||
Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) |
||||
|
||||
/// Should be called immediately upon Component.onCompleted.
|
||||
/// @param editMode true: controller being used in Plan view, false: controller being used in Fly view
|
||||
Q_INVOKABLE virtual void start(bool editMode); |
||||
|
||||
Q_INVOKABLE virtual void loadFromVehicle(void) = 0; |
||||
Q_INVOKABLE virtual void sendToVehicle(void) = 0; |
||||
Q_INVOKABLE virtual void loadFromFilePicker(void) = 0; |
||||
Q_INVOKABLE virtual void loadFromFile(const QString& filename) = 0; |
||||
Q_INVOKABLE virtual void saveToFilePicker(void) = 0; |
||||
Q_INVOKABLE virtual void saveToFile(const QString& filename) = 0; |
||||
Q_INVOKABLE virtual void removeAll(void) = 0; |
||||
|
||||
virtual bool syncInProgress (void) const = 0; |
||||
virtual bool dirty (void) const = 0; |
||||
virtual void setDirty (bool dirty) = 0; |
||||
|
||||
signals: |
||||
void syncInProgressChanged (bool syncInProgress); |
||||
void dirtyChanged (bool dirty); |
||||
|
||||
protected: |
||||
Vehicle* _activeVehicle; |
||||
MultiVehicleManager* _multiVehicleMgr; |
||||
bool _editMode; |
||||
|
||||
/// Called when the current active vehicle has been removed. Derived classes should override
|
||||
/// to implement custom behavior.
|
||||
virtual void _activeVehicleBeingRemoved(void) = 0; |
||||
|
||||
/// Called when a new active vehicle has been set. Derived classes should override
|
||||
/// to implement custom behavior.
|
||||
virtual void _activeVehicleSet(void) = 0; |
||||
|
||||
private slots: |
||||
void _activeVehicleChanged(Vehicle* activeVehicle); |
||||
}; |
||||
|
||||
#endif |
||||
@ -0,0 +1,108 @@
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "QGCMapPolygon.h" |
||||
#include "QGCGeo.h" |
||||
|
||||
#include <QGeoRectangle> |
||||
#include <QDebug> |
||||
#include <QPolygon> |
||||
|
||||
QGCMapPolygon::QGCMapPolygon(QObject* parent) |
||||
: QObject(parent) |
||||
, _dirty(false) |
||||
{ |
||||
|
||||
} |
||||
|
||||
const QGCMapPolygon& QGCMapPolygon::operator=(const QGCMapPolygon& other) |
||||
{ |
||||
_polygonPath = other._polygonPath; |
||||
setDirty(true); |
||||
|
||||
emit pathChanged(); |
||||
|
||||
return *this; |
||||
} |
||||
|
||||
void QGCMapPolygon::clear(void) |
||||
{ |
||||
// Bug workaround, see below
|
||||
while (_polygonPath.count() > 1) { |
||||
_polygonPath.takeLast(); |
||||
} |
||||
emit pathChanged(); |
||||
|
||||
// Although this code should remove the polygon from the map it doesn't. There appears
|
||||
// to be a bug in QGCMapPolygon which causes it to not be redrawn if the list is empty. So
|
||||
// we work around it by using the code above to remove all but the last point which in turn
|
||||
// will cause the polygon to go away.
|
||||
_polygonPath.clear(); |
||||
|
||||
setDirty(true); |
||||
} |
||||
|
||||
void QGCMapPolygon::addCoordinate(const QGeoCoordinate coordinate) |
||||
{ |
||||
_polygonPath << QVariant::fromValue(coordinate); |
||||
emit pathChanged(); |
||||
setDirty(true); |
||||
} |
||||
|
||||
void QGCMapPolygon::adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate) |
||||
{ |
||||
_polygonPath[vertexIndex] = QVariant::fromValue(coordinate); |
||||
emit pathChanged(); |
||||
setDirty(true); |
||||
} |
||||
|
||||
void QGCMapPolygon::setDirty(bool dirty) |
||||
{ |
||||
if (_dirty != dirty) { |
||||
_dirty = dirty; |
||||
emit dirtyChanged(dirty); |
||||
} |
||||
} |
||||
|
||||
QGeoCoordinate QGCMapPolygon::center(void) const |
||||
{ |
||||
QPolygonF polygon; |
||||
|
||||
QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>(); |
||||
|
||||
foreach(const QVariant& coordVar, _polygonPath) { |
||||
double y, x, down; |
||||
|
||||
convertGeoToNed(coordVar.value<QGeoCoordinate>(), tangentOrigin, &y, &x, &down); |
||||
polygon << QPointF(x, -y); |
||||
} |
||||
|
||||
QGeoCoordinate centerCoord; |
||||
QPointF centerPoint = polygon.boundingRect().center(); |
||||
convertNedToGeo(-centerPoint.y(), centerPoint.x(), 0, tangentOrigin, ¢erCoord); |
||||
|
||||
return centerCoord; |
||||
} |
||||
|
||||
void QGCMapPolygon::setPath(const QList<QGeoCoordinate>& path) |
||||
{ |
||||
_polygonPath.clear(); |
||||
foreach(const QGeoCoordinate& coord, path) { |
||||
_polygonPath << QVariant::fromValue(coord); |
||||
} |
||||
setDirty(true); |
||||
emit pathChanged(); |
||||
} |
||||
|
||||
void QGCMapPolygon::setPath(const QVariantList& path) |
||||
{ |
||||
_polygonPath = path; |
||||
setDirty(true); |
||||
emit pathChanged(); |
||||
} |
||||
@ -0,0 +1,53 @@
@@ -0,0 +1,53 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* (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 QGCMapPolygon_H |
||||
#define QGCMapPolygon_H |
||||
|
||||
#include <QObject> |
||||
#include <QGeoCoordinate> |
||||
#include <QVariantList> |
||||
|
||||
class QGCMapPolygon : public QObject |
||||
{ |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
QGCMapPolygon(QObject* parent = NULL); |
||||
|
||||
const QGCMapPolygon& operator=(const QGCMapPolygon& other); |
||||
|
||||
Q_PROPERTY(QVariantList path READ path WRITE setPath NOTIFY pathChanged) |
||||
Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) |
||||
|
||||
Q_INVOKABLE void clear(void); |
||||
Q_INVOKABLE void addCoordinate(const QGeoCoordinate coordinate); |
||||
Q_INVOKABLE void adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate); |
||||
Q_INVOKABLE QGeoCoordinate center(void) const; |
||||
Q_INVOKABLE int count(void) const { return _polygonPath.count(); } |
||||
|
||||
const QVariantList path(void) const { return _polygonPath; } |
||||
void setPath(const QList<QGeoCoordinate>& path); |
||||
void setPath(const QVariantList& path); |
||||
|
||||
const QGeoCoordinate operator[](int index) { return _polygonPath[index].value<QGeoCoordinate>(); } |
||||
|
||||
bool dirty(void) const { return _dirty; } |
||||
void setDirty(bool dirty); |
||||
|
||||
signals: |
||||
void pathChanged(void); |
||||
void dirtyChanged(bool dirty); |
||||
|
||||
private: |
||||
QVariantList _polygonPath; |
||||
bool _dirty; |
||||
}; |
||||
|
||||
#endif |
||||
Loading…
Reference in new issue