diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 10bd5e234ccaaab73df8fbea107e19b850c834f1..cb5825b276a4041092cb0702efdec7fbf97baafc 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -509,7 +509,11 @@ HEADERS += \
src/MissionManager/MissionManager.h \
src/MissionManager/MissionSettingsItem.h \
src/MissionManager/PlanElementController.h \
+ src/MissionManager/PlanManager.h \
src/MissionManager/PlanMasterController.h \
+ src/MissionManager/QGCFenceCircle.h \
+ src/MissionManager/QGCFencePolygon.h \
+ src/MissionManager/QGCMapCircle.h \
src/MissionManager/QGCMapPolygon.h \
src/MissionManager/RallyPoint.h \
src/MissionManager/RallyPointController.h \
@@ -689,7 +693,11 @@ SOURCES += \
src/MissionManager/MissionManager.cc \
src/MissionManager/MissionSettingsItem.cc \
src/MissionManager/PlanElementController.cc \
+ src/MissionManager/PlanManager.cc \
src/MissionManager/PlanMasterController.cc \
+ src/MissionManager/QGCFenceCircle.cc \
+ src/MissionManager/QGCFencePolygon.cc \
+ src/MissionManager/QGCMapCircle.cc \
src/MissionManager/QGCMapPolygon.cc \
src/MissionManager/RallyPoint.cc \
src/MissionManager/RallyPointController.cc \
@@ -906,9 +914,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.h \
src/AutoPilotPlugins/APM/APMTuningComponent.h \
src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
- src/FirmwarePlugin/APM/APMGeoFenceManager.h \
src/FirmwarePlugin/APM/APMParameterMetaData.h \
- src/FirmwarePlugin/APM/APMRallyPointManager.h \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \
@@ -933,9 +939,7 @@ APMFirmwarePlugin {
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc \
src/AutoPilotPlugins/APM/APMTuningComponent.cc \
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
- src/FirmwarePlugin/APM/APMGeoFenceManager.cc \
src/FirmwarePlugin/APM/APMParameterMetaData.cc \
- src/FirmwarePlugin/APM/APMRallyPointManager.cc \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc \
@@ -974,7 +978,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.h \
src/AutoPilotPlugins/PX4/SensorsComponentController.h \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \
- src/FirmwarePlugin/PX4/PX4GeoFenceManager.h \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \
SOURCES += \
@@ -995,7 +998,6 @@ PX4FirmwarePlugin {
src/AutoPilotPlugins/PX4/SensorsComponent.cc \
src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \
- src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \
}
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 7c2fcf20375c7a3fa50152a89f6ecc1c0a5b710c..974c2eb57f2e0f4ade902d293d8db77999f10fe5 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -90,6 +90,7 @@
<file alias="QGroundControl/Controls/QGCLabel.qml">src/QmlControls/QGCLabel.qml</file>
<file alias="QGroundControl/Controls/QGCListView.qml">src/QmlControls/QGCListView.qml</file>
<file alias="QGroundControl/Controls/QGCMapLabel.qml">src/QmlControls/QGCMapLabel.qml</file>
+ <file alias="QGroundControl/Controls/QGCMapCircleVisuals.qml">src/MissionManager/QGCMapCircleVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMapPolygonVisuals.qml">src/MissionManager/QGCMapPolygonVisuals.qml</file>
<file alias="QGroundControl/Controls/QGCMouseArea.qml">src/QmlControls/QGCMouseArea.qml</file>
<file alias="QGroundControl/Controls/QGCMovableItem.qml">src/QmlControls/QGCMovableItem.qml</file>
@@ -199,6 +200,7 @@
<file alias="AutoConnect.SettingsGroup.json">src/Settings/AutoConnect.SettingsGroup.json</file>
<file alias="FlightMap.SettingsGroup.json">src/Settings/FlightMap.SettingsGroup.json</file>
<file alias="Guided.SettingsGroup.json">src/Settings/Guided.SettingsGroup.json</file>
+ <file alias="QGCMapCircle.Facts.json">src/MissionManager/QGCMapCircle.Facts.json</file>
<file alias="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
<file alias="Survey.SettingsGroup.json">src/MissionManager/Survey.SettingsGroup.json</file>
<file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
index 19faca6667f2154f8249ecde20c2c720b2bda0d1..aa27e876b13d59e85ec7fc9ce35cfa0bbd5c55ac 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -17,8 +17,6 @@
#include "FirmwarePlugin.h"
#include "QGCLoggingCategory.h"
#include "APMParameterMetaData.h"
-#include "APMGeoFenceManager.h"
-#include "APMRallyPointManager.h"
#include <QAbstractSocket>
@@ -93,8 +91,6 @@ public:
QString internalParameterMetaDataFile (Vehicle* vehicle) final;
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final;
- GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) final { return new APMGeoFenceManager(vehicle); }
- RallyPointManager* newRallyPointManager (Vehicle* vehicle) final { return new APMRallyPointManager(vehicle); }
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc
deleted file mode 100644
index 5fd028903fce54c1c597f69b14401ace6ee68cf7..0000000000000000000000000000000000000000
--- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc
+++ /dev/null
@@ -1,337 +0,0 @@
-/****************************************************************************
- *
- * (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 "APMGeoFenceManager.h"
-#include "Vehicle.h"
-#include "FirmwarePlugin.h"
-#include "MAVLinkProtocol.h"
-#include "QGCApplication.h"
-#include "ParameterManager.h"
-#include "QmlObjectListModel.h"
-#include "QGCQGeoCoordinate.h"
-
-const char* APMGeoFenceManager::_fenceTotalParam = "FENCE_TOTAL";
-const char* APMGeoFenceManager::_fenceActionParam = "FENCE_ACTION";
-const char* APMGeoFenceManager::_fenceEnableParam = "FENCE_ENABLE";
-
-APMGeoFenceManager::APMGeoFenceManager(Vehicle* vehicle)
- : GeoFenceManager(vehicle)
- , _fenceSupported(false)
- , _circleEnabled(false)
- , _polygonSupported(false)
- , _polygonEnabled(false)
- , _breachReturnSupported(vehicle->fixedWing())
- , _firstParamLoadComplete(false)
- , _readTransactionInProgress(false)
- , _writeTransactionInProgress(false)
- , _fenceTypeFact(NULL)
- , _fenceEnableFact(NULL)
- , _circleRadiusFact(NULL)
-{
- connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived);
- connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &APMGeoFenceManager::_parametersReady);
-
- if (_vehicle->parameterManager()->parametersReady()) {
- _parametersReady();
- }
-}
-
-APMGeoFenceManager::~APMGeoFenceManager()
-{
-
-}
-
-void APMGeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon)
-{
- if (_vehicle->isOfflineEditingVehicle()) {
- return;
- }
-
- if (_readTransactionInProgress) {
- _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress."));
- return;
- }
-
- if (!_fenceSupported) {
- return;
- }
-
- // Validate
- int validatedPolygonCount = 0;
- if (polygon.count() >= 3) {
- validatedPolygonCount = polygon.count();
- }
- if (polygon.count() > std::numeric_limits<uint8_t>::max()) {
- _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count()));
- validatedPolygonCount = 0;
- }
-
- _breachReturnPoint = breachReturn;
- _polygon.clear();
- if (validatedPolygonCount) {
- for (int i=0; i<polygon.count(); i++) {
- _polygon.append(polygon.value<QGCQGeoCoordinate*>(i)->coordinate());
- }
- }
-
- // Total point count, +1 polygon close in last index, +1 for breach in index 0
- _cWriteFencePoints = validatedPolygonCount ? validatedPolygonCount + 1 + 1 : 0;
- qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::sendToVehicle validatedPolygonCount:_cWriteFencePoints" << validatedPolygonCount << _cWriteFencePoints;
- _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints);
-
- // FIXME: No validation of correct fence received
- for (uint8_t index=0; index<_cWriteFencePoints; index++) {
- _sendFencePoint(index);
- }
-
- emit sendComplete(false /* error */);
-}
-
-void APMGeoFenceManager::loadFromVehicle(void)
-{
- if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) {
- return;
- }
-
- _breachReturnPoint = QGeoCoordinate();
- _polygon.clear();
-
- if (!_fenceSupported) {
- emit loadComplete(_breachReturnPoint, _polygon);
- return;
- }
-
- // Point 0: Breach return point (always sent, but supported by ArduPlane only)
- // Point [1,N]: Polygon points
- // Point N+1: Close polygon point (same as point 1)
- int cFencePoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt();
- int minFencePoints = 5;
- qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints;
- if (cFencePoints == 0) {
- // No fence
- emit loadComplete(_breachReturnPoint, _polygon);
- return;
- }
- if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < minFencePoints)) {
- _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 our vehicle is received
-void APMGeoFenceManager::_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);
- qCDebug(GeoFenceManagerLog) << "From vehicle fence_point: idx:lat:lng" << fencePoint.idx << fencePoint.lat << fencePoint.lng;
-
- if (fencePoint.idx != _currentFencePoint) {
- // FIXME: Protocol out of whack
- _readTransactionInProgress = false;
- emit inProgressChanged(inProgress());
- qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint;
- return;
- }
-
- if (fencePoint.idx == 0) {
- _breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng);
- qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _breachReturnPoint;
- _requestFencePoint(++_currentFencePoint);
- } else if (fencePoint.idx < _cReadFencePoints - 1) {
- QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng);
- _polygon.append(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
- qCDebug(GeoFenceManagerLog) << "Fence point load complete";
- _readTransactionInProgress = false;
- emit loadComplete(_breachReturnPoint, _polygon);
- emit inProgressChanged(inProgress());
- }
- }
- }
-}
-
-void APMGeoFenceManager::_requestFencePoint(uint8_t pointIndex)
-{
- mavlink_message_t msg;
- MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-
- qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::_requestFencePoint" << pointIndex;
- mavlink_msg_fence_fetch_point_pack_chan(mavlink->getSystemId(),
- mavlink->getComponentId(),
- _vehicle->priorityLink()->mavlinkChannel(),
- &msg,
- _vehicle->id(),
- _vehicle->defaultComponentId(),
- pointIndex);
- _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
-}
-
-void APMGeoFenceManager::_sendFencePoint(uint8_t pointIndex)
-{
- mavlink_message_t msg;
- MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-
- QGeoCoordinate fenceCoord;
- if (pointIndex == 0) {
- fenceCoord = breachReturnPoint();
- } else if (pointIndex == _cWriteFencePoints - 1) {
- // Polygon close point
- fenceCoord = _polygon[0];
- } else {
- // Polygon point
- fenceCoord = _polygon[pointIndex - 1];
- }
-
- // Total point count, +1 polygon close in last index, +1 for breach in index 0
- uint8_t totalPointCount = _polygon.count() + 1 + 1;
-
- mavlink_msg_fence_point_pack_chan(mavlink->getSystemId(),
- mavlink->getComponentId(),
- _vehicle->priorityLink()->mavlinkChannel(),
- &msg,
- _vehicle->id(),
- _vehicle->defaultComponentId(),
- pointIndex, // Index of point to set
- totalPointCount,
- fenceCoord.latitude(),
- fenceCoord.longitude());
- _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
-}
-
-bool APMGeoFenceManager::inProgress(void) const
-{
- return _readTransactionInProgress || _writeTransactionInProgress;
-}
-
-void APMGeoFenceManager::_setCircleEnabled(bool circleEnabled)
-{
- if (circleEnabled != _circleEnabled) {
- _circleEnabled = circleEnabled;
- emit circleEnabledChanged(circleEnabled);
- }
-}
-
-void APMGeoFenceManager::_setPolygonEnabled(bool polygonEnabled)
-{
- if (polygonEnabled != _polygonEnabled) {
- _polygonEnabled = polygonEnabled;
- emit polygonEnabledChanged(polygonEnabled);
- }
-}
-
-void APMGeoFenceManager::_updateEnabledFlags(void)
-{
- bool fenceEnabled = false;
-
- if (_fenceSupported) {
- if (_fenceEnableFact) {
- fenceEnabled = _fenceEnableFact->rawValue().toBool();
- } else {
- fenceEnabled = true;
- }
-
- bool newCircleEnabled = fenceEnabled && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 2);
- _setCircleEnabled(newCircleEnabled);
-
- bool newPolygonEnabled = _fenceSupported && fenceEnabled &&
- ((_vehicle->multiRotor() && _fenceTypeFact && (_fenceTypeFact->rawValue().toInt() & 4)) ||
- _vehicle->fixedWing());
- _setPolygonEnabled(newPolygonEnabled);
- } else {
- _setCircleEnabled(false);
- _setPolygonEnabled(false);
- }
-}
-
-void APMGeoFenceManager::_parametersReady(void)
-{
- if (!_firstParamLoadComplete) {
- _firstParamLoadComplete = true;
-
- _fenceSupported = _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) &&
- _vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceActionParam) &&
- !qgcApp()->runningUnitTests();
-
- if (_fenceSupported) {
- QStringList paramNames;
- QStringList paramLabels;
-
- _polygonSupported = true;
-
- if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, _fenceEnableParam)) {
- _fenceEnableFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _fenceEnableParam);
- connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
- }
-
- if (_vehicle->multiRotor()) {
- _breachReturnSupported = false;
- _fenceTypeFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE"));
- connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateEnabledFlags);
-
- _circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS"));
-
- paramNames << QStringLiteral("FENCE_ENABLE") << QStringLiteral("FENCE_TYPE") << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_ALT_MAX")
- << QStringLiteral("FENCE_RADIUS") << QStringLiteral("FENCE_MARGIN");
- paramLabels << QStringLiteral("Enabled:") << QStringLiteral("Type:") << QStringLiteral("Breach Action:") << QStringLiteral("Max Altitude:")
- << QStringLiteral("Radius:") << QStringLiteral("Margin:");
- } if (_vehicle->fixedWing()) {
- _breachReturnSupported = true;
- paramNames << QStringLiteral("FENCE_ACTION") << QStringLiteral("FENCE_MINALT") << QStringLiteral("FENCE_MAXALT") << QStringLiteral("FENCE_RETALT")
- << QStringLiteral("FENCE_AUTOENABLE") << QStringLiteral("FENCE_RET_RALLY");
- paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Min Altitude:") << QStringLiteral("Max Altitude:") << QStringLiteral("Return Altitude:")
- << QStringLiteral("Auto-Enable:") << QStringLiteral("Return to Rally:");
- }
-
- _params.clear();
- _paramLabels.clear();
- for (int i=0; i<paramNames.count(); i++) {
- QString paramName = paramNames[i];
- if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) {
- Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName);
- _params << QVariant::fromValue(paramFact);
- _paramLabels << paramLabels[i];
- }
- }
- emit paramsChanged(_params);
- emit paramLabelsChanged(_paramLabels);
-
- emit breachReturnSupportedChanged(_breachReturnSupported);
- emit polygonSupportedChanged(_polygonSupported);
- }
-
- _updateEnabledFlags();
- }
-}
-
-void APMGeoFenceManager::removeAll(void)
-{
- qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::removeAll";
-
- QmlObjectListModel emptyPolygon;
-
- sendToVehicle(_breachReturnPoint, emptyPolygon);
- emit removeAllComplete(false /* error */);
-}
diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.h b/src/FirmwarePlugin/APM/APMGeoFenceManager.h
deleted file mode 100644
index a092e412f8e5737135c44e9249af460b74c17d1c..0000000000000000000000000000000000000000
--- a/src/FirmwarePlugin/APM/APMGeoFenceManager.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/****************************************************************************
- *
- * (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 APMGeoFenceManager_H
-#define APMGeoFenceManager_H
-
-#include "GeoFenceManager.h"
-#include "QGCMAVLink.h"
-#include "FactSystem.h"
-
-class QmlObjectListModel;
-
-class APMGeoFenceManager : public GeoFenceManager
-{
- Q_OBJECT
-
-public:
- APMGeoFenceManager(Vehicle* vehicle);
- ~APMGeoFenceManager();
-
- // Overrides from GeoFenceManager
- bool inProgress (void) const final;
- void loadFromVehicle (void) final;
- void sendToVehicle (const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon) final;
- bool polygonSupported (void) const final { return _polygonSupported; }
- bool polygonEnabled (void) const final { return _polygonEnabled; }
- bool breachReturnSupported (void) const final { return _breachReturnSupported; }
- bool circleEnabled (void) const { return _circleEnabled; }
- Fact* circleRadiusFact (void) const { return _circleRadiusFact; }
- QVariantList params (void) const final { return _params; }
- QStringList paramLabels (void) const final { return _paramLabels; }
- void removeAll (void) final;
-
-private slots:
- void _mavlinkMessageReceived (const mavlink_message_t& message);
- void _parametersReady (void);
-
-private:
- void _requestFencePoint (uint8_t pointIndex);
- void _sendFencePoint (uint8_t pointIndex);
- void _updateEnabledFlags(void);
- void _setCircleEnabled (bool circleEnabled);
- void _setPolygonEnabled (bool polygonEnabled);
-
-private:
- bool _fenceSupported;
- bool _circleEnabled;
- bool _polygonSupported;
- bool _polygonEnabled;
- bool _breachReturnSupported;
- bool _firstParamLoadComplete;
-
- QVariantList _params;
- QStringList _paramLabels;
-
- bool _readTransactionInProgress;
- bool _writeTransactionInProgress;
-
- uint8_t _cReadFencePoints;
- uint8_t _cWriteFencePoints;
- uint8_t _currentFencePoint;
-
- Fact* _fenceTypeFact;
- Fact* _fenceEnableFact;
- Fact* _circleRadiusFact;
-
- static const char* _fenceTotalParam;
- static const char* _fenceActionParam;
- static const char* _fenceEnableParam;
-};
-
-#endif
diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc
deleted file mode 100644
index 86011d98ec54e9633e096b6bfff050fc3338a7c8..0000000000000000000000000000000000000000
--- a/src/FirmwarePlugin/APM/APMRallyPointManager.cc
+++ /dev/null
@@ -1,166 +0,0 @@
-/****************************************************************************
- *
- * (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 "APMRallyPointManager.h"
-#include "Vehicle.h"
-#include "FirmwarePlugin.h"
-#include "MAVLinkProtocol.h"
-#include "QGCApplication.h"
-#include "ParameterManager.h"
-
-const char* APMRallyPointManager::_rallyTotalParam = "RALLY_TOTAL";
-
-APMRallyPointManager::APMRallyPointManager(Vehicle* vehicle)
- : RallyPointManager(vehicle)
- , _readTransactionInProgress(false)
- , _writeTransactionInProgress(false)
-{
- connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMRallyPointManager::_mavlinkMessageReceived);
-}
-
-APMRallyPointManager::~APMRallyPointManager()
-{
-
-}
-
-void APMRallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
-{
- if (_vehicle->isOfflineEditingVehicle() || !rallyPointsSupported()) {
- return;
- }
-
- if (_readTransactionInProgress) {
- _sendError(InternalError, QStringLiteral("Rally Point write attempted while read in progress."));
- return;
- }
-
- _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->setRawValue(rgPoints.count());
-
- // FIXME: No validation of correct point received
- _rgPoints = rgPoints;
- for (uint8_t index=0; index<_rgPoints.count(); index++) {
- _sendRallyPoint(index);
- }
-
- emit sendComplete(false /* error */);
-}
-
-void APMRallyPointManager::loadFromVehicle(void)
-{
- if (_vehicle->isOfflineEditingVehicle() || _readTransactionInProgress) {
- return;
- }
-
- _rgPoints.clear();
-
- if (!rallyPointsSupported()) {
- emit loadComplete(QList<QGeoCoordinate>());
- return;
- }
-
- _cReadRallyPoints = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _rallyTotalParam)->rawValue().toInt();
- qCDebug(RallyPointManagerLog) << "APMRallyPointManager::loadFromVehicle - point count" << _cReadRallyPoints;
- if (_cReadRallyPoints == 0) {
- emit loadComplete(_rgPoints);
- return;
- }
-
- _currentRallyPoint = 0;
- _readTransactionInProgress = true;
- _requestRallyPoint(_currentRallyPoint);
-}
-
-/// Called when a new mavlink message for out vehicle is received
-void APMRallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& message)
-{
- if (message.msgid == MAVLINK_MSG_ID_RALLY_POINT) {
- mavlink_rally_point_t rallyPoint;
-
- mavlink_msg_rally_point_decode(&message, &rallyPoint);
- qCDebug(RallyPointManagerLog) << "From vehicle rally_point: idx:lat:lng:alt" << rallyPoint.idx << rallyPoint.lat << rallyPoint.lng << rallyPoint.alt;
-
- if (rallyPoint.idx != _currentRallyPoint) {
- // FIXME: Protocol out of whack
- _readTransactionInProgress = false;
- emit inProgressChanged(inProgress());
- qCWarning(RallyPointManagerLog) << "Indices out of sync" << rallyPoint.idx << _currentRallyPoint;
- return;
- }
-
- QGeoCoordinate point((float)rallyPoint.lat / 1e7, (float)rallyPoint.lng / 1e7, rallyPoint.alt);
- _rgPoints.append(point);
- if (rallyPoint.idx < _cReadRallyPoints - 1) {
- // Still more points to request
- _requestRallyPoint(++_currentRallyPoint);
- } else {
- // We've finished collecting rally points
- qCDebug(RallyPointManagerLog) << "Rally point load complete";
- _readTransactionInProgress = false;
- emit loadComplete(_rgPoints);
- emit inProgressChanged(inProgress());
- }
- }
-}
-
-void APMRallyPointManager::_requestRallyPoint(uint8_t pointIndex)
-{
- mavlink_message_t msg;
- MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-
- qCDebug(RallyPointManagerLog) << "APMRallyPointManager::_requestRallyPoint" << pointIndex;
- mavlink_msg_rally_fetch_point_pack_chan(mavlink->getSystemId(),
- mavlink->getComponentId(),
- _vehicle->priorityLink()->mavlinkChannel(),
- &msg,
- _vehicle->id(),
- _vehicle->defaultComponentId(),
- pointIndex);
- _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
-}
-
-void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex)
-{
- mavlink_message_t msg;
- MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-
- QGeoCoordinate point = _rgPoints[pointIndex];
- mavlink_msg_rally_point_pack_chan(mavlink->getSystemId(),
- mavlink->getComponentId(),
- _vehicle->priorityLink()->mavlinkChannel(),
- &msg,
- _vehicle->id(),
- _vehicle->defaultComponentId(),
- pointIndex,
- _rgPoints.count(),
- point.latitude() * 1e7,
- point.longitude() * 1e7,
- point.altitude(),
- 0, 0, 0); // break_alt, land_dir, flags
- _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
-}
-
-bool APMRallyPointManager::inProgress(void) const
-{
- return _readTransactionInProgress || _writeTransactionInProgress;
-}
-
-bool APMRallyPointManager::rallyPointsSupported(void) const
-{
- return _vehicle->parameterManager()->parameterExists(_vehicle->defaultComponentId(), _rallyTotalParam);
-}
-
-void APMRallyPointManager::removeAll(void)
-{
- qCDebug(RallyPointManagerLog) << "APMRallyPointManager::removeAll";
-
- QList<QGeoCoordinate> noRallyPoints;
-
- sendToVehicle(noRallyPoints);
- emit removeAllComplete(false /* error */);
-}
diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.h b/src/FirmwarePlugin/APM/APMRallyPointManager.h
deleted file mode 100644
index 74fc7012c02bb07fbae1c19eac576177d48d2e01..0000000000000000000000000000000000000000
--- a/src/FirmwarePlugin/APM/APMRallyPointManager.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/****************************************************************************
- *
- * (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 APMRallyPointManager_H
-#define APMRallyPointManager_H
-
-#include "RallyPointManager.h"
-#include "QGCMAVLink.h"
-
-class APMRallyPointManager : public RallyPointManager
-{
- Q_OBJECT
-
-public:
- APMRallyPointManager(Vehicle* vehicle);
- ~APMRallyPointManager();
-
- // Overrides from RallyPointManager
- bool inProgress (void) const final;
- void loadFromVehicle (void) final;
- void sendToVehicle (const QList<QGeoCoordinate>& rgPoints) final;
- bool rallyPointsSupported (void) const final;
-
- void removeAll (void);
-
- QString editorQml(void) const final { return QStringLiteral("qrc:/FirmwarePlugin/APM/APMRallyPointEditor.qml"); }
-
-private slots:
- void _mavlinkMessageReceived(const mavlink_message_t& message);
-
-private:
- void _requestRallyPoint(uint8_t pointIndex);
- void _sendRallyPoint(uint8_t pointIndex);
-
-private:
- bool _readTransactionInProgress;
- bool _writeTransactionInProgress;
-
- uint8_t _cReadRallyPoints;
- uint8_t _currentRallyPoint;
-
- static const char* _rallyTotalParam;
-};
-
-#endif
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
index c8752af7630ab03ff21d04f420a09ebae620086f..9a8fb896d3bcfe994081c252631945a64b06a273 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
@@ -278,12 +278,6 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0;
}
-QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle)
-{
- Q_UNUSED(vehicle);
- return QStringLiteral("FENCE_RADIUS");
-}
-
bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
if (vehicle->isOfflineEditingVehicle()) {
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
index 89e164657a9f50d0b545aab33e15dcd21555ce58..e21d4f6d09a051080b9f9d507a44daa644623a14 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
@@ -67,7 +67,6 @@ public:
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
bool multiRotorCoaxialMotors (Vehicle* vehicle) final;
bool multiRotorXConfig (Vehicle* vehicle) final;
- QString geoFenceRadiusParam (Vehicle* vehicle) final;
QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
QString pauseFlightMode (void) const override { return QString("Brake"); }
QString missionFlightMode (void) const override { return QString("Auto"); }
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index 0aae9367fc4a99ed278206da3a5f1c6aa97767eb..1df9f74fd60d0233457b9ecb635e86a6d7168f07 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -235,15 +235,6 @@ public:
/// @return true: X confiuration, false: Plus configuration
virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; }
- /// Returns a newly created geofence manager for this vehicle.
- virtual GeoFenceManager* newGeoFenceManager(Vehicle* vehicle) { return new GeoFenceManager(vehicle); }
-
- /// Returns the parameter which holds the fence circle radius if supported.
- virtual QString geoFenceRadiusParam(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); }
-
- /// Returns a newly created rally point manager for this vehicle.
- virtual RallyPointManager* newRallyPointManager(Vehicle* vehicle) { return new RallyPointManager(vehicle); }
-
/// Return the resource file which contains the set of params loaded for offline editing.
virtual QString offlineEditingParamFile(Vehicle* vehicle) { Q_UNUSED(vehicle); return QString(); }
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
index bda3803a9848fce269f7ab5eb5e75f84f071bddf..8e4a8de55f02039deb4a42b959b286948e61a7f9 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
@@ -17,7 +17,6 @@
#include "FirmwarePlugin.h"
#include "ParameterManager.h"
#include "PX4ParameterMetaData.h"
-#include "PX4GeoFenceManager.h"
class PX4FirmwarePlugin : public FirmwarePlugin
{
@@ -62,7 +61,6 @@ public:
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) override { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile) final;
bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override;
- GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) override { return new PX4GeoFenceManager(vehicle); }
QString offlineEditingParamFile(Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/PX4/PX4.OfflineEditing.params"); }
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc
deleted file mode 100644
index fec0bfd2fb52e999b54cc73b25740af0fcaf0a80..0000000000000000000000000000000000000000
--- a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc
+++ /dev/null
@@ -1,60 +0,0 @@
-/****************************************************************************
- *
- * (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 "PX4GeoFenceManager.h"
-#include "Vehicle.h"
-#include "FirmwarePlugin.h"
-#include "ParameterManager.h"
-
-PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
- : GeoFenceManager(vehicle)
- , _firstParamLoadComplete(false)
- , _circleRadiusFact(NULL)
-{
- connect(_vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &PX4GeoFenceManager::_parametersReady);
-
- if (_vehicle->parameterManager()->parametersReady()) {
- _parametersReady();
- }
-}
-
-PX4GeoFenceManager::~PX4GeoFenceManager()
-{
-
-}
-
-void PX4GeoFenceManager::_parametersReady(void)
-{
- if (!_firstParamLoadComplete) {
- _firstParamLoadComplete = true;
-
- _circleRadiusFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("GF_MAX_HOR_DIST"));
- emit circleRadiusFactChanged(_circleRadiusFact);
-
- QStringList paramNames;
- QStringList paramLabels;
-
- paramNames << QStringLiteral("GF_ACTION") << QStringLiteral("GF_MAX_HOR_DIST") << QStringLiteral("GF_MAX_VER_DIST");
- paramLabels << QStringLiteral("Breach Action:") << QStringLiteral("Radius:") << QStringLiteral("Max Altitude:");
-
- _params.clear();
- _paramLabels.clear();
- for (int i=0; i<paramNames.count(); i++) {
- QString paramName = paramNames[i];
- if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, paramName)) {
- Fact* paramFact = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, paramName);
- _params << QVariant::fromValue(paramFact);
- _paramLabels << paramLabels[i];
- }
- }
-
- emit paramsChanged(_params);
- emit paramLabelsChanged(_paramLabels);
- }
-}
diff --git a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h b/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h
deleted file mode 100644
index 7a87f2cbf234ed348ad342c4c6b1b3cc25de3361..0000000000000000000000000000000000000000
--- a/src/FirmwarePlugin/PX4/PX4GeoFenceManager.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- * (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 PX4GeoFenceManager_H
-#define PX4GeoFenceManager_H
-
-#include "GeoFenceManager.h"
-#include "QGCMAVLink.h"
-#include "FactSystem.h"
-
-class PX4GeoFenceManager : public GeoFenceManager
-{
- Q_OBJECT
-
-public:
- PX4GeoFenceManager(Vehicle* vehicle);
- ~PX4GeoFenceManager();
-
- // Overrides from GeoFenceManager
- bool circleEnabled (void) const { return true; }
- Fact* circleRadiusFact (void) const { return _circleRadiusFact; }
- QVariantList params (void) const final { return _params; }
- QStringList paramLabels (void) const final { return _paramLabels; }
-
-private slots:
- void _parametersReady(void);
-
-private:
- bool _firstParamLoadComplete;
- Fact* _circleRadiusFact;
- QVariantList _params;
- QStringList _paramLabels;
-};
-
-#endif
diff --git a/src/FlightMap/qmldir b/src/FlightMap/qmldir
index 9afe7d25f264f96ba86619235822be5661b9a941..bf11b6fc816e8ca4cddb00251aeac48ce530abbc 100644
--- a/src/FlightMap/qmldir
+++ b/src/FlightMap/qmldir
@@ -29,6 +29,3 @@ MissionItemView 1.0 MissionItemView.qml
MissionLineView 1.0 MissionLineView.qml
PolygonEditor 1.0 PolygonEditor.qml
VehicleMapItem 1.0 VehicleMapItem.qml
-
-# Editor controls
-QGCMapPolygonControls 1.0 QGCMapPolygonControls.qml
diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc
index b8fca7ec94726ef58b8ebf1d9b610b8d564b2326..64980ba51b7d02ae0d6b3455edffd2c8c5d341dd 100644
--- a/src/MissionManager/GeoFenceController.cc
+++ b/src/MissionManager/GeoFenceController.cc
@@ -39,11 +39,10 @@ GeoFenceController::GeoFenceController(PlanMasterController* masterController, Q
: PlanElementController(masterController, parent)
, _geoFenceManager(_managerVehicle->geoFenceManager())
, _dirty(false)
- , _mapPolygon(this)
, _itemsRequested(false)
{
- connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
- connect(_mapPolygon.qmlPathModel(), &QmlObjectListModel::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
+ connect(&_polygons, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
+ connect(&_circles, &QmlObjectListModel::countChanged, this, &GeoFenceController::_updateContainsItems);
managerVehicleChanged(_managerVehicle);
}
@@ -77,21 +76,16 @@ void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturn
void GeoFenceController::_signalAll(void)
{
- emit breachReturnSupportedChanged(breachReturnSupported());
emit breachReturnPointChanged(breachReturnPoint());
- emit circleEnabledChanged(circleEnabled());
- emit circleRadiusFactChanged(circleRadiusFact());
- emit polygonEnabledChanged(polygonEnabled());
- emit polygonSupportedChanged(polygonSupported());
emit dirtyChanged(dirty());
- emit paramsChanged(params());
- emit paramLabelsChanged(paramLabels());
+ emit supportedChanged(supported());
}
void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
{
if (_managerVehicle) {
_geoFenceManager->disconnect(this);
+ _managerVehicle->disconnect(this);
_managerVehicle = NULL;
_geoFenceManager = NULL;
}
@@ -103,21 +97,23 @@ void GeoFenceController::managerVehicleChanged(Vehicle* managerVehicle)
}
_geoFenceManager = _managerVehicle->geoFenceManager();
- connect(_geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged);
- connect(_geoFenceManager, &GeoFenceManager::circleEnabledChanged, this, &GeoFenceController::circleEnabledChanged);
- connect(_geoFenceManager, &GeoFenceManager::circleRadiusFactChanged, this, &GeoFenceController::circleRadiusFactChanged);
- connect(_geoFenceManager, &GeoFenceManager::polygonEnabledChanged, this, &GeoFenceController::polygonEnabledChanged);
- connect(_geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &GeoFenceController::_managerLoadComplete);
connect(_geoFenceManager, &GeoFenceManager::sendComplete, this, &GeoFenceController::_managerSendComplete);
connect(_geoFenceManager, &GeoFenceManager::removeAllComplete, this, &GeoFenceController::_managerRemoveAllComplete);
connect(_geoFenceManager, &GeoFenceManager::inProgressChanged, this, &GeoFenceController::syncInProgressChanged);
+ connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged);
+
+ emit supportedChanged(supported());
_signalAll();
}
bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
{
+ Q_UNUSED(json);
+ Q_UNUSED(errorString);
+
+#if 0
QString errorStr;
QString errorMessage = tr("GeoFence: %1");
@@ -135,12 +131,15 @@ bool GeoFenceController::load(const QJsonObject& json, QString& errorString)
setDirty(false);
_signalAll();
+#endif
return true;
}
void GeoFenceController::save(QJsonObject& json)
{
+ Q_UNUSED(json);
+#if 0
json[JsonHelper::jsonVersionKey] = 1;
if (_breachReturnPoint.isValid()) {
@@ -150,12 +149,14 @@ void GeoFenceController::save(QJsonObject& json)
}
_mapPolygon.saveToJson(json);
+#endif
}
void GeoFenceController::removeAll(void)
-{
+{
setBreachReturnPoint(QGeoCoordinate());
- _mapPolygon.clear();
+ _polygons.clearAndDeleteContents();
+ _circles.clearAndDeleteContents();
}
void GeoFenceController::removeAllFromVehicle(void)
@@ -189,8 +190,7 @@ void GeoFenceController::sendToVehicle(void)
qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress";
} else {
qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle";
- _geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel());
- _mapPolygon.setDirty(false);
+ _geoFenceManager->sendToVehicle(_breachReturnPoint, _polygons, _circles);
setDirty(false);
}
}
@@ -211,7 +211,14 @@ void GeoFenceController::setDirty(bool dirty)
if (dirty != _dirty) {
_dirty = dirty;
if (!dirty) {
- _mapPolygon.setDirty(dirty);
+ for (int i=0; i<_polygons.count(); i++) {
+ QGCFencePolygon* polygon = _polygons.value<QGCFencePolygon*>(i);
+ polygon->setDirty(false);
+ }
+ for (int i=0; i<_circles.count(); i++) {
+ QGCFenceCircle* circle = _circles.value<QGCFenceCircle*>(i);
+ circle->setDirty(false);
+ }
}
emit dirtyChanged(dirty);
}
@@ -224,53 +231,26 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty)
}
}
-bool GeoFenceController::breachReturnSupported(void) const
-{
- return _geoFenceManager->breachReturnSupported();
-}
-
-bool GeoFenceController::circleEnabled(void) const
-{
- return _geoFenceManager->circleEnabled();
-}
-
-Fact* GeoFenceController::circleRadiusFact(void) const
-{
- return _geoFenceManager->circleRadiusFact();
-}
-
-bool GeoFenceController::polygonSupported(void) const
-{
- return _geoFenceManager->polygonSupported();
-}
-
-bool GeoFenceController::polygonEnabled(void) const
-{
- return _geoFenceManager->polygonEnabled();
-}
-
-QVariantList GeoFenceController::params(void) const
-{
- return _geoFenceManager->params();
-}
-
-QStringList GeoFenceController::paramLabels(void) const
-{
- return _geoFenceManager->paramLabels();
-}
-
void GeoFenceController::_setDirty(void)
{
setDirty(true);
}
-void GeoFenceController::_setPolygonFromManager(const QList<QGeoCoordinate>& polygon)
+void GeoFenceController::_setFenceFromManager(const QList<QGCFencePolygon>& polygons,
+ const QList<QGCFenceCircle>& circles)
{
- _mapPolygon.clear();
- for (int i=0; i<polygon.count(); i++) {
- _mapPolygon.appendVertex(polygon[i]);
+ _polygons.clearAndDeleteContents();
+ _circles.clearAndDeleteContents();
+
+ for (int i=0; i<polygons.count(); i++) {
+ _polygons.append(new QGCFencePolygon(polygons[i], this));
}
- _mapPolygon.setDirty(false);
+
+ for (int i=0; i<circles.count(); i++) {
+ _circles.append(new QGCFenceCircle(circles[i], this));
+ }
+
+ setDirty(false);
}
void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnPoint)
@@ -279,13 +259,13 @@ void GeoFenceController::_setReturnPointFromManager(QGeoCoordinate breachReturnP
emit breachReturnPointChanged(_breachReturnPoint);
}
-void GeoFenceController::_managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon)
+void GeoFenceController::_managerLoadComplete(void)
{
// Fly view always reloads on _loadComplete
// Plan view only reloads on _loadComplete if specifically requested
if (!_editMode || _itemsRequested) {
- _setReturnPointFromManager(breachReturn);
- _setPolygonFromManager(polygon);
+ _setReturnPointFromManager(_geoFenceManager->breachReturnPoint());
+ _setFenceFromManager(_geoFenceManager->polygons(), _geoFenceManager->circles());
setDirty(false);
_signalAll();
emit loadComplete();
@@ -311,7 +291,7 @@ void GeoFenceController::_managerRemoveAllComplete(bool error)
bool GeoFenceController::containsItems(void) const
{
- return _mapPolygon.count() > 2;
+ return _polygons.count() > 0 || _circles.count() > 0;
}
void GeoFenceController::_updateContainsItems(void)
@@ -339,8 +319,97 @@ bool GeoFenceController::showPlanFromManagerVehicle(void)
// Fake a _loadComplete with the current items
qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
_itemsRequested = true;
- _managerLoadComplete(_geoFenceManager->breachReturnPoint(), _geoFenceManager->polygon());
+ _managerLoadComplete();
return false;
}
}
}
+
+void GeoFenceController::addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight)
+{
+ QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude());
+ QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude());
+
+ double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0;
+ double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0;
+
+ QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180);
+ QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90);
+ QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude());
+
+ // Initial polygon is inset to take 3/4s of viewport with max width/height of 3000 meters
+ halfWidthMeters = qMin(halfWidthMeters * 0.75, 1500.0);
+ halfHeightMeters = qMin(halfHeightMeters * 0.75, 1500.0);
+
+ // Initial polygon has max width and height of 3000 meters
+ topLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0);
+ topRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0);
+ bottomLeft = center.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180);
+ bottomRight = center.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180);
+
+ QGCFencePolygon* polygon = new QGCFencePolygon(true /* inclusion */, this);
+ polygon->appendVertex(topLeft);
+ polygon->appendVertex(topRight);
+ polygon->appendVertex(bottomRight);
+ polygon->appendVertex(bottomLeft);
+ _polygons.append(polygon);
+
+ clearAllInteractive();
+ polygon->setInteractive(true);
+}
+
+void GeoFenceController::addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight)
+{
+ QGeoCoordinate topRight(topLeft.latitude(), bottomRight.longitude());
+ QGeoCoordinate bottomLeft(bottomRight.latitude(), topLeft.longitude());
+
+ // Initial radius is inset to take 3/4s of viewport and max of 1500 meters
+ double halfWidthMeters = topLeft.distanceTo(topRight) / 2.0;
+ double halfHeightMeters = topLeft.distanceTo(bottomLeft) / 2.0;
+ double radius = qMin(qMin(halfWidthMeters, halfHeightMeters) * 0.75, 1500.0);
+
+ QGeoCoordinate centerLeftEdge = topLeft.atDistanceAndAzimuth(halfHeightMeters, 180);
+ QGeoCoordinate centerTopEdge = topLeft.atDistanceAndAzimuth(halfWidthMeters, 90);
+ QGeoCoordinate center(centerLeftEdge.latitude(), centerTopEdge.longitude());
+
+ QGCFenceCircle* circle = new QGCFenceCircle(center, radius, true /* inclusion */, this);
+ _circles.append(circle);
+
+ clearAllInteractive();
+ circle->setInteractive(true);
+}
+
+void GeoFenceController::deletePolygon(int index)
+{
+ if (index < 0 || index > _polygons.count() - 1) {
+ return;
+ }
+
+ QGCFencePolygon* polygon = qobject_cast<QGCFencePolygon*>(_polygons.removeAt(index));
+ polygon->deleteLater();
+}
+
+void GeoFenceController::deleteCircle(int index)
+{
+ if (index < 0 || index > _circles.count() - 1) {
+ return;
+ }
+
+ QGCFenceCircle* circle = qobject_cast<QGCFenceCircle*>(_circles.removeAt(index));
+ circle->deleteLater();
+}
+
+void GeoFenceController::clearAllInteractive(void)
+{
+ for (int i=0; i<_polygons.count(); i++) {
+ _polygons.value<QGCFencePolygon*>(i)->setInteractive(false);
+ }
+ for (int i=0; i<_circles.count(); i++) {
+ _circles.value<QGCFenceCircle*>(i)->setInteractive(false);
+ }
+}
+
+bool GeoFenceController::supported(void) const
+{
+ return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_managerVehicle->maxProtoVersion() >= 200);
+}
diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h
index 7019cba3133a79e6a7f0c63c63c7b3492e990f6b..dd13cec9256fc0493932468321570935d343fbf4 100644
--- a/src/MissionManager/GeoFenceController.h
+++ b/src/MissionManager/GeoFenceController.h
@@ -12,7 +12,8 @@
#include "PlanElementController.h"
#include "GeoFenceManager.h"
-#include "QGCMapPolygon.h"
+#include "QGCFencePolygon.h"
+#include "QGCFenceCircle.h"
#include "Vehicle.h"
#include "MultiVehicleManager.h"
#include "QGCLoggingCategory.h"
@@ -29,21 +30,32 @@ public:
GeoFenceController(PlanMasterController* masterController, QObject* parent = NULL);
~GeoFenceController();
- Q_PROPERTY(QGCMapPolygon* mapPolygon READ mapPolygon CONSTANT)
- Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
+ Q_PROPERTY(QmlObjectListModel* polygons READ polygons CONSTANT)
+ Q_PROPERTY(QmlObjectListModel* circles READ circles CONSTANT)
+ Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
- // The following properties are reflections of properties from GeoFenceManager
- Q_PROPERTY(bool circleEnabled READ circleEnabled NOTIFY circleEnabledChanged)
- Q_PROPERTY(Fact* circleRadiusFact READ circleRadiusFact NOTIFY circleRadiusFactChanged)
- Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged)
- Q_PROPERTY(bool polygonEnabled READ polygonEnabled NOTIFY polygonEnabledChanged)
- Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged)
- Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged)
- Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged)
+ /// Add a new inclusion polygon to the fence
+ /// @param topLeft - Top left coordinate or map viewport
+ /// @param topLeft - Bottom right left coordinate or map viewport
+ Q_INVOKABLE void addInclusionPolygon(QGeoCoordinate topLeft, QGeoCoordinate bottomRight);
- Q_INVOKABLE void addPolygon (void) { emit addInitialFencePolygon(); }
- Q_INVOKABLE void removePolygon (void) { _mapPolygon.clear(); }
+ /// Add a new inclusion circle to the fence
+ /// @param topLeft - Top left coordinate or map viewport
+ /// @param topLeft - Bottom right left coordinate or map viewport
+ Q_INVOKABLE void addInclusionCircle(QGeoCoordinate topLeft, QGeoCoordinate bottomRight);
+ /// Deletes the specified polygon from the polygon list
+ /// @param index Index of poygon to delete
+ Q_INVOKABLE void deletePolygon(int index);
+
+ /// Deletes the specified circle from the circle list
+ /// @param index Index of circle to delete
+ Q_INVOKABLE void deleteCircle(int index);
+
+ /// Clears the interactive bit from all fence items
+ Q_INVOKABLE void clearAllInteractive(void);
+
+ bool supported (void) const final;
void start (bool editMode) final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
@@ -58,15 +70,9 @@ public:
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
- bool circleEnabled (void) const;
- Fact* circleRadiusFact (void) const;
- bool polygonSupported (void) const;
- bool polygonEnabled (void) const;
- bool breachReturnSupported (void) const;
- QVariantList params (void) const;
- QStringList paramLabels (void) const;
- QGCMapPolygon* mapPolygon (void) { return &_mapPolygon; }
- QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
+ QmlObjectListModel* polygons (void) { return &_polygons; }
+ QmlObjectListModel* circles (void) { return &_circles; }
+ QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
@@ -74,21 +80,14 @@ signals:
void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
void editorQmlChanged (QString editorQml);
void loadComplete (void);
- void addInitialFencePolygon (void);
- void circleEnabledChanged (bool circleEnabled);
- void circleRadiusFactChanged (Fact* circleRadiusFact);
- void polygonSupportedChanged (bool polygonSupported);
- void polygonEnabledChanged (bool polygonEnabled);
- void breachReturnSupportedChanged (bool breachReturnSupported);
- void paramsChanged (QVariantList params);
- void paramLabelsChanged (QStringList paramLabels);
private slots:
void _polygonDirtyChanged(bool dirty);
void _setDirty(void);
- void _setPolygonFromManager(const QList<QGeoCoordinate>& polygon);
+ void _setFenceFromManager(const QList<QGCFencePolygon>& polygons,
+ const QList<QGCFenceCircle>& circles);
void _setReturnPointFromManager(QGeoCoordinate breachReturnPoint);
- void _managerLoadComplete(const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
+ void _managerLoadComplete(void);
void _updateContainsItems(void);
void _managerSendComplete(bool error);
void _managerRemoveAllComplete(bool error);
@@ -99,7 +98,8 @@ private:
GeoFenceManager* _geoFenceManager;
bool _dirty;
- QGCMapPolygon _mapPolygon;
+ QmlObjectListModel _polygons;
+ QmlObjectListModel _circles;
QGeoCoordinate _breachReturnPoint;
bool _itemsRequested;
diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc
index 357e3b768b335c2aa7f75bdfd2a76c74eadf1a59..448be9eb03b4d79462a1eec083b950d2a7e40f46 100644
--- a/src/MissionManager/GeoFenceManager.cc
+++ b/src/MissionManager/GeoFenceManager.cc
@@ -10,14 +10,22 @@
#include "GeoFenceManager.h"
#include "Vehicle.h"
#include "QmlObjectListModel.h"
+#include "ParameterManager.h"
+#include "QGCMapPolygon.h"
+#include "QGCMapCircle.h"
QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog")
GeoFenceManager::GeoFenceManager(Vehicle* vehicle)
- : QObject(vehicle)
- , _vehicle(vehicle)
+ : _vehicle (vehicle)
+ , _planManager (vehicle, MAV_MISSION_TYPE_FENCE)
+ , _firstParamLoadComplete (false)
{
-
+ connect(&_planManager, &PlanManager::inProgressChanged, this, &GeoFenceManager::inProgressChanged);
+ connect(&_planManager, &PlanManager::error, this, &GeoFenceManager::error);
+ connect(&_planManager, &PlanManager::removeAllComplete, this, &GeoFenceManager::removeAllComplete);
+ connect(&_planManager, &PlanManager::sendComplete, this, &GeoFenceManager::_sendComplete);
+ connect(&_planManager, &PlanManager::newMissionItemsAvailable, this, &GeoFenceManager::_planManagerLoadComplete);
}
GeoFenceManager::~GeoFenceManager()
@@ -25,30 +33,161 @@ GeoFenceManager::~GeoFenceManager()
}
-void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
+bool GeoFenceManager::inProgress(void) const
{
- qCDebug(GeoFenceManagerLog) << "Sending error" << errorCode << errorMsg;
-
- emit error(errorCode, errorMsg);
+ return _planManager.inProgress();
}
void GeoFenceManager::loadFromVehicle(void)
{
- // No geofence support in unknown vehicle
- emit loadComplete(QGeoCoordinate(), QList<QGeoCoordinate>());
+ _planManager.loadFromVehicle();
}
-void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon)
+void GeoFenceManager::sendToVehicle(const QGeoCoordinate& breachReturn,
+ QmlObjectListModel& polygons,
+ QmlObjectListModel& circles)
{
- // No geofence support in unknown vehicle
Q_UNUSED(breachReturn);
- Q_UNUSED(polygon);
- emit sendComplete(false /* error */);
-}
+ QList<MissionItem*> fenceItems;
+
+ _sendPolygons.clear();
+ _sendCircles.clear();
+
+ for (int i=0; i<polygons.count(); i++) {
+ _sendPolygons.append(*polygons.value<QGCFencePolygon*>(i));
+ }
+ for (int i=0; i<circles.count(); i++) {
+ _sendCircles.append(*circles.value<QGCFenceCircle*>(i));
+ }
+
+ for (int i=0; i<_sendPolygons.count(); i++) {
+ const QGCFencePolygon& polygon = _sendPolygons[i];
+
+ for (int j=0; j<polygon.count(); j++) {
+ const QGeoCoordinate& vertex = polygon.path()[j].value<QGeoCoordinate>();
+
+ MissionItem* item = new MissionItem(0,
+ polygon.inclusion() ? MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION : MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
+ MAV_FRAME_GLOBAL,
+ polygon.count(), // vertex count
+ 0, 0, 0, // param 2-4 unused
+ vertex.latitude(),
+ vertex.longitude(),
+ 0, // param 7 unused
+ false, // autocontinue
+ false, // isCurrentItem
+ this); // parent
+ fenceItems.append(item);
+ }
+ }
+
+ for (int i=0; i<_sendCircles.count(); i++) {
+ QGCFenceCircle& circle = _sendCircles[i];
+
+ MissionItem* item = new MissionItem(0,
+ circle.inclusion() ? MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION : MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
+ MAV_FRAME_GLOBAL,
+ circle.radius()->rawValue().toDouble(),
+ 0, 0, 0, // param 2-4 unused
+ circle.center().latitude(),
+ circle.center().longitude(),
+ 0, // param 7 unused
+ false, // autocontinue
+ false, // isCurrentItem
+ this); // parent
+ fenceItems.append(item);
+ }
+
+ _planManager.writeMissionItems(fenceItems);
+
+ for (int i=0; i<fenceItems.count(); i++) {
+ fenceItems[i]->deleteLater();
+ }
+}
void GeoFenceManager::removeAll(void)
{
- // No geofence support in unknown vehicle
- emit removeAllComplete(false /* error */);
+ _planManager.removeAll();
+}
+
+void GeoFenceManager::_sendComplete(bool error)
+{
+ if (error) {
+ _polygons.clear();
+ _circles.clear();
+ } else {
+ _polygons = _sendPolygons;
+ _circles = _sendCircles;
+ }
+ _sendPolygons.clear();
+ _sendCircles.clear();
+ emit sendComplete(error);
+}
+
+void GeoFenceManager::_planManagerLoadComplete(bool removeAllRequested)
+{
+ bool loadFailed = false;
+
+ Q_UNUSED(removeAllRequested);
+
+ _polygons.clear();
+ _circles.clear();
+
+ MAV_CMD expectedCommand = (MAV_CMD)0;
+ int expectedVertexCount = 0;
+ QGCFencePolygon nextPolygon(true /* inclusion */);
+ const QList<MissionItem*>& fenceItems = _planManager.missionItems();
+
+ for (int i=0; i<fenceItems.count(); i++) {
+ MissionItem* item = fenceItems[i];
+
+ MAV_CMD command = item->command();
+
+ if (command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
+ if (nextPolygon.count() == 0) {
+ // Starting a new polygon
+ expectedVertexCount = item->param1();
+ expectedCommand = command;
+ } else if (expectedVertexCount != item->param1()){
+ // In the middle of a polygon, but count suddenly changed
+ emit error(BadPolygonItemFormat, tr("GeoFence load: Vertex count change mid-polygon - actual:expected").arg(item->param1()).arg(expectedVertexCount));
+ break;
+ } if (expectedCommand != command) {
+ // Command changed before last polygon was completely loaded
+ emit error(BadPolygonItemFormat, tr("GeoFence load: Polygon type changed before last load complete - actual:expected").arg(command).arg(expectedCommand));
+ break;
+ }
+ nextPolygon.appendVertex(QGeoCoordinate(item->param5(), item->param6()));
+ if (nextPolygon.count() == expectedVertexCount) {
+ // Polygon is complete
+ nextPolygon.setInclusion(command == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION);
+ _polygons.append(nextPolygon);
+ nextPolygon.clear();
+ }
+ } else if (command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || command == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) {
+ if (nextPolygon.count() != 0) {
+ // Incomplete polygon
+ emit error(IncompletePolygonLoad, tr("GeoFence load: Incomplete polygon loaded"));
+ break;
+ }
+ QGCFenceCircle circle(QGeoCoordinate(item->param5(), item->param6()), item->param1(), command == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION /* inclusion */);
+ _circles.append(circle);
+ } else {
+ emit error(UnsupportedCommand, tr("GeoFence load: Unsupported command %1").arg(item->command()));
+ break;
+ }
+ }
+
+ if (loadFailed) {
+ _polygons.clear();
+ _circles.clear();
+ }
+
+ emit loadComplete();
+}
+
+bool GeoFenceManager::supported(void) const
+{
+ return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) && (_vehicle->maxProtoVersion() >= 200);
}
diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h
index a1ea66ac861f2f1d3118fb21be6aca636ab952d2..9c7c4e41fd31cdf3d415e6932da8041a7a9e6054 100644
--- a/src/MissionManager/GeoFenceManager.h
+++ b/src/MissionManager/GeoFenceManager.h
@@ -15,9 +15,13 @@
#include "QGCLoggingCategory.h"
#include "FactSystem.h"
+#include "PlanManager.h"
+#include "QGCFencePolygon.h"
+#include "QGCFenceCircle.h"
class Vehicle;
class QmlObjectListModel;
+class PlanManager;
Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog)
@@ -31,80 +35,67 @@ public:
GeoFenceManager(Vehicle* vehicle);
~GeoFenceManager();
+ /// Returns true if GeoFence is supported by this vehicle
+ virtual bool supported(void) const;
+
/// Returns true if the manager is currently communicating with the vehicle
- virtual bool inProgress(void) const { return false; }
+ virtual bool inProgress(void) const;
/// Load the current settings from the vehicle
/// Signals loadComplete when done
virtual void loadFromVehicle(void);
- /// Send the current settings to the vehicle
+ /// Send the geofence settings to the vehicle
/// Signals sendComplete when done
- virtual void sendToVehicle(const QGeoCoordinate& breachReturn, QmlObjectListModel& polygon);
+ virtual void sendToVehicle(const QGeoCoordinate& breachReturn, ///< Breach return point
+ QmlObjectListModel& polygons, ///< List of QGCFencePolygons
+ QmlObjectListModel& circles); ///< List of QGCFenceCircles
/// Remove all fence related items from vehicle (does not affect parameters)
/// Signals removeAllComplete when done
virtual void removeAll(void);
- /// Returns true if this vehicle support polygon fence
- /// Signal: polygonSupportedChanged
- virtual bool polygonSupported(void) const { return false; }
-
/// Returns true if polygon fence is currently enabled on this vehicle
/// Signal: polygonEnabledChanged
- virtual bool polygonEnabled(void) const { return false; }
-
- /// Returns true if breach return is supported by this vehicle
- /// Signal: breachReturnSupportedChanged
- virtual bool breachReturnSupported(void) const { return false; }
-
- /// Returns a list of parameter facts that relate to geofence support for the vehicle
- /// Signal: paramsChanged
- virtual QVariantList params(void) const { return QVariantList(); }
+ virtual bool polygonEnabled(void) const { return true; }
- /// Returns the user visible labels for the paremeters returned by params method
- /// Signal: paramLabelsChanged
- virtual QStringList paramLabels(void) const { return QStringList(); }
-
- /// Returns true if circular fence is currently enabled on vehicle
- /// Signal: circleEnabledChanged
- virtual bool circleEnabled(void) const { return false; }
-
- /// Returns the fact which controls the fence circle radius. NULL if not supported
- /// Signal: circleRadiusFactChanged
- virtual Fact* circleRadiusFact(void) const { return NULL; }
-
- QList<QGeoCoordinate> polygon (void) const { return _polygon; }
- QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
+ const QList<QGCFencePolygon>& polygons(void) { return _polygons; }
+ const QList<QGCFenceCircle>& circles(void) { return _circles; }
+ const QGeoCoordinate& breachReturnPoint(void) const { return _breachReturnPoint; }
/// Error codes returned in error signal
typedef enum {
InternalError,
- TooFewPoints, ///< Too few points for valid geofence
- TooManyPoints, ///< Too many points for valid geofence
+ PolygonTooFewPoints, ///< Too few points for valid fence polygon
+ PolygonTooManyPoints, ///< Too many points for valid fence polygon
+ IncompletePolygonLoad, ///< Incomplete polygon loaded
+ UnsupportedCommand, ///< Usupported command in mission type
+ BadPolygonItemFormat, ///< Error re-creating polygons from mission items
InvalidCircleRadius,
} ErrorCode_t;
signals:
- void loadComplete (const QGeoCoordinate& breachReturn, const QList<QGeoCoordinate>& polygon);
+ void loadComplete (void);
void inProgressChanged (bool inProgress);
void error (int errorCode, const QString& errorMsg);
- void paramsChanged (QVariantList params);
- void paramLabelsChanged (QStringList paramLabels);
- void circleEnabledChanged (bool circleEnabled);
- void circleRadiusFactChanged (Fact* circleRadiusFact);
- void polygonSupportedChanged (bool polygonSupported);
- void polygonEnabledChanged (bool polygonEnabled);
- void breachReturnSupportedChanged (bool breachReturnSupported);
void removeAllComplete (bool error);
void sendComplete (bool error);
-protected:
+private slots:
+ void _sendComplete (bool error);
+ void _planManagerLoadComplete (bool removeAllRequested);
+
+private:
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
Vehicle* _vehicle;
- QList<QGeoCoordinate> _polygon;
+ PlanManager _planManager;
+ QList<QGCFencePolygon> _polygons;
+ QList<QGCFenceCircle> _circles;
QGeoCoordinate _breachReturnPoint;
+ bool _firstParamLoadComplete;
+ QList<QGCFencePolygon> _sendPolygons;
+ QList<QGCFenceCircle> _sendCircles;
};
#endif
diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h
index daba4ffb6b0218ae4dc4e154a8d644c6c67f3ac0..54362623dd4bb502edcb157456f98d03292d8876 100644
--- a/src/MissionManager/MissionController.h
+++ b/src/MissionManager/MissionController.h
@@ -110,6 +110,7 @@ public:
bool loadTextFile(QFile& file, QString& errorString);
// Overrides from PlanElementController
+ bool supported (void) const final { return true; };
void start (bool editMode) final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc
index 5b3372f8aaf0de6235ebb68bed3c207edd7e1765..1a2d0115608b8b85a2a8ae9d476d835279c243ba 100644
--- a/src/MissionManager/MissionManager.cc
+++ b/src/MissionManager/MissionManager.cc
@@ -22,110 +22,15 @@
QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
MissionManager::MissionManager(Vehicle* vehicle)
- : _vehicle(vehicle)
- , _dedicatedLink(NULL)
- , _ackTimeoutTimer(NULL)
- , _expectedAck(AckNone)
- , _transactionInProgress(TransactionNone)
- , _resumeMission(false)
- , _lastMissionRequest(-1)
- , _currentMissionIndex(-1)
- , _lastCurrentIndex(-1)
-{
- connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived);
-
- _ackTimeoutTimer = new QTimer(this);
- _ackTimeoutTimer->setSingleShot(true);
- _ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds);
-
- connect(_ackTimeoutTimer, &QTimer::timeout, this, &MissionManager::_ackTimeout);
-}
-
-MissionManager::~MissionManager()
+ : PlanManager(vehicle, MAV_MISSION_TYPE_MISSION)
{
}
-void MissionManager::_writeMissionItemsWorker(void)
-{
- _lastMissionRequest = -1;
-
- emit progressPct(0);
-
- qCDebug(MissionManagerLog) << "writeMissionItems count:" << _writeMissionItems.count();
-
- // Prime write list
- _itemIndicesToWrite.clear();
- for (int i=0; i<_writeMissionItems.count(); i++) {
- _itemIndicesToWrite << i;
- }
-
- _transactionInProgress = TransactionWrite;
- _retryCount = 0;
- emit inProgressChanged(true);
- _writeMissionCount();
-}
-
-
-void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
-{
- if (_vehicle->isOfflineEditingVehicle()) {
- return;
- }
-
- if (inProgress()) {
- qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress";
- return;
- }
-
- _clearAndDeleteWriteMissionItems();
-
- bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
-
- int firstIndex = skipFirstItem ? 1 : 0;
-
- for (int i=firstIndex; i<missionItems.count(); i++) {
- MissionItem* item = new MissionItem(*missionItems[i]);
- _writeMissionItems.append(item);
-
- item->setIsCurrentItem(i == firstIndex);
-
- if (skipFirstItem) {
- // Home is in sequence 0, remainder of items start at sequence 1
- item->setSequenceNumber(item->sequenceNumber() - 1);
- if (item->command() == MAV_CMD_DO_JUMP) {
- item->setParam1((int)item->param1() - 1);
- }
- }
- }
-
- _writeMissionItemsWorker();
-}
-
-/// This begins the write sequence with the vehicle. This may be called during a retry.
-void MissionManager::_writeMissionCount(void)
+MissionManager::~MissionManager()
{
- qCDebug(MissionManagerLog) << "_writeMissionCount count:_retryCount" << _writeMissionItems.count() << _retryCount;
-
- mavlink_message_t message;
- mavlink_mission_count_t missionCount;
-
- memset(&missionCount, 0, sizeof(missionCount));
- missionCount.target_system = _vehicle->id();
- missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER;
- missionCount.count = _writeMissionItems.count();
-
- _dedicatedLink = _vehicle->priorityLink();
- mavlink_msg_mission_count_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &message,
- &missionCount);
- _vehicle->sendMessageOnLink(_dedicatedLink, message);
- _startAckTimeout(AckMissionRequest);
}
-
void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly)
{
if (inProgress()) {
@@ -166,809 +71,6 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
emit inProgressChanged(true);
}
-void MissionManager::loadFromVehicle(void)
-{
- if (_vehicle->isOfflineEditingVehicle()) {
- return;
- }
-
- qCDebug(MissionManagerLog) << "loadFromVehicle read sequence";
-
- if (inProgress()) {
- qCDebug(MissionManagerLog) << "loadFromVehicle called while transaction in progress";
- return;
- }
-
- _retryCount = 0;
- _transactionInProgress = TransactionRead;
- emit inProgressChanged(true);
- _requestList();
-}
-
-/// Internal call to request list of mission items. May be called during a retry sequence.
-void MissionManager::_requestList(void)
-{
- qCDebug(MissionManagerLog) << "_requestList retry count" << _retryCount;
-
- mavlink_message_t message;
- mavlink_mission_request_list_t request;
-
- memset(&request, 0, sizeof(request));
-
- _itemIndicesToRead.clear();
- _clearMissionItems();
-
- request.target_system = _vehicle->id();
- request.target_component = MAV_COMP_ID_MISSIONPLANNER;
-
- _dedicatedLink = _vehicle->priorityLink();
- mavlink_msg_mission_request_list_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &message,
- &request);
-
- _vehicle->sendMessageOnLink(_dedicatedLink, message);
- _startAckTimeout(AckMissionCount);
-}
-
-void MissionManager::_ackTimeout(void)
-{
- if (_expectedAck == AckNone) {
- return;
- }
-
- switch (_expectedAck) {
- case AckNone:
- qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone";
- _sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone");
- break;
- case AckMissionCount:
- // MISSION_COUNT message expected
- if (_retryCount > _maxRetryCount) {
- _sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded."));
- _finishTransaction(false);
- } else {
- _retryCount++;
- qCDebug(MissionManagerLog) << "Retrying REQUEST_LIST retry Count" << _retryCount;
- _requestList();
- }
- break;
- case AckMissionItem:
- // MISSION_ITEM expected
- if (_retryCount > _maxRetryCount) {
- _sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded."));
- _finishTransaction(false);
- } else {
- _retryCount++;
- qCDebug(MissionManagerLog) << "Retrying MISSION_REQUEST retry Count" << _retryCount;
- _requestNextMissionItem();
- }
- break;
- case AckMissionRequest:
- // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
- if (_itemIndicesToWrite.count() == 0) {
- // Vehicle did not send final MISSION_ACK at end of sequence
- _sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack."));
- _finishTransaction(false);
- } else if (_itemIndicesToWrite[0] == 0) {
- // Vehicle did not respond to MISSION_COUNT, try again
- if (_retryCount > _maxRetryCount) {
- _sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded."));
- _finishTransaction(false);
- } else {
- _retryCount++;
- qCDebug(MissionManagerLog) << "Retrying MISSION_COUNT retry Count" << _retryCount;
- _writeMissionCount();
- }
- } else {
- // Vehicle did not request all items from ground station
- _sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck)));
- _expectedAck = AckNone;
- _finishTransaction(false);
- }
- break;
- case AckMissionClearAll:
- // MISSION_ACK expected
- if (_retryCount > _maxRetryCount) {
- _sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded."));
- _finishTransaction(false);
- } else {
- _retryCount++;
- qCDebug(MissionManagerLog) << "Retrying MISSION_CLEAR_ALL retry Count" << _retryCount;
- _removeAllWorker();
- }
- break;
- case AckGuidedItem:
- // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
- default:
- _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck)));
- _expectedAck = AckNone;
- _finishTransaction(false);
- }
-}
-
-void MissionManager::_startAckTimeout(AckType_t ack)
-{
- _expectedAck = ack;
- _ackTimeoutTimer->start();
-}
-
-/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped.
-/// @return true: received ack matches expected ack
-bool MissionManager::_checkForExpectedAck(AckType_t receivedAck)
-{
- if (receivedAck == _expectedAck) {
- _expectedAck = AckNone;
- _ackTimeoutTimer->stop();
- return true;
- } else {
- if (_expectedAck == AckNone) {
- // Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using
- // spurious MISSION_ITEMs.
- } else {
- // We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers.
- // Whatever it is we let the ack timeout handle any error output to the user.
- qCDebug(MissionManagerLog) << QString("Out of sequence ack expected:received %1:%2").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck));
- }
- return false;
- }
-}
-
-void MissionManager::_readTransactionComplete(void)
-{
- qCDebug(MissionManagerLog) << "_readTransactionComplete read sequence complete";
-
- mavlink_message_t message;
- mavlink_mission_ack_t missionAck;
-
- memset(&missionAck, 0, sizeof(missionAck));
-
- missionAck.target_system = _vehicle->id();
- missionAck.target_component = MAV_COMP_ID_MISSIONPLANNER;
- missionAck.type = MAV_MISSION_ACCEPTED;
-
- mavlink_msg_mission_ack_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &message,
- &missionAck);
-
- _vehicle->sendMessageOnLink(_dedicatedLink, message);
-
- _finishTransaction(true);
-}
-
-void MissionManager::_handleMissionCount(const mavlink_message_t& message)
-{
- mavlink_mission_count_t missionCount;
-
- if (!_checkForExpectedAck(AckMissionCount)) {
- return;
- }
-
- _retryCount = 0;
-
- mavlink_msg_mission_count_decode(&message, &missionCount);
- qCDebug(MissionManagerLog) << "_handleMissionCount count:" << missionCount.count;
-
- if (missionCount.count == 0) {
- _readTransactionComplete();
- } else {
- // Prime read list
- for (int i=0; i<missionCount.count; i++) {
- _itemIndicesToRead << i;
- }
- _requestNextMissionItem();
- }
-}
-
-void MissionManager::_requestNextMissionItem(void)
-{
- if (_itemIndicesToRead.count() == 0) {
- _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
- return;
- }
-
- qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount;
-
- mavlink_message_t message;
- if (_vehicle->supportsMissionItemInt()) {
- mavlink_mission_request_int_t missionRequest;
-
- memset(&missionRequest, 0, sizeof(missionRequest));
- missionRequest.target_system = _vehicle->id();
- missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
- missionRequest.seq = _itemIndicesToRead[0];
-
- mavlink_msg_mission_request_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &message,
- &missionRequest);
- } else {
- mavlink_mission_request_t missionRequest;
-
- memset(&missionRequest, 0, sizeof(missionRequest));
- missionRequest.target_system = _vehicle->id();
- missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
- missionRequest.seq = _itemIndicesToRead[0];
-
- mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &message,
- &missionRequest);
- }
-
- _vehicle->sendMessageOnLink(_dedicatedLink, message);
- _startAckTimeout(AckMissionItem);
-}
-
-void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
-{
- MAV_CMD command;
- MAV_FRAME frame;
- double param1;
- double param2;
- double param3;
- double param4;
- double param5;
- double param6;
- double param7;
- bool autoContinue;
- bool isCurrentItem;
- int seq;
-
- if (missionItemInt) {
- mavlink_mission_item_int_t missionItem;
- mavlink_msg_mission_item_int_decode(&message, &missionItem);
-
- command = (MAV_CMD)missionItem.command,
- frame = (MAV_FRAME)missionItem.frame,
- param1 = missionItem.param1;
- param2 = missionItem.param2;
- param3 = missionItem.param3;
- param4 = missionItem.param4;
- param5 = (double)missionItem.x / qPow(10.0, 7.0);
- param6 = (double)missionItem.y / qPow(10.0, 7.0);
- param7 = (double)missionItem.z;
- autoContinue = missionItem.autocontinue;
- isCurrentItem = missionItem.current;
- seq = missionItem.seq;
- } else {
- mavlink_mission_item_t missionItem;
- mavlink_msg_mission_item_decode(&message, &missionItem);
-
- command = (MAV_CMD)missionItem.command,
- frame = (MAV_FRAME)missionItem.frame,
- param1 = missionItem.param1;
- param2 = missionItem.param2;
- param3 = missionItem.param3;
- param4 = missionItem.param4;
- param5 = missionItem.x;
- param6 = missionItem.y;
- param7 = missionItem.z;
- autoContinue = missionItem.autocontinue;
- isCurrentItem = missionItem.current;
- seq = missionItem.seq;
- }
-
- // We don't support editing ALT_INT frames so change on the way in.
- if (frame == MAV_FRAME_GLOBAL_INT) {
- frame = MAV_FRAME_GLOBAL;
- } else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
- frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
- }
-
-
- bool ardupilotHomePositionUpdate = false;
- if (!_checkForExpectedAck(AckMissionItem)) {
- if (_vehicle->apmFirmware() && seq == 0) {
- ardupilotHomePositionUpdate = true;
- } else {
- qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem;
- return;
- }
- }
-
- qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate;
-
- if (ardupilotHomePositionUpdate) {
- QGeoCoordinate newHomePosition(param5, param6, param7);
- _vehicle->_setHomePosition(newHomePosition);
- return;
- }
-
- if (_itemIndicesToRead.contains(seq)) {
- _itemIndicesToRead.removeOne(seq);
-
- MissionItem* item = new MissionItem(seq,
- command,
- frame,
- param1,
- param2,
- param3,
- param4,
- param5,
- param6,
- param7,
- autoContinue,
- isCurrentItem,
- this);
-
- if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
- // Home is in position 0
- item->setParam1((int)item->param1() + 1);
- }
-
- _missionItems.append(item);
- } else {
- qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << seq;
- // We have to put the ack timeout back since it was removed above
- _startAckTimeout(AckMissionItem);
- return;
- }
-
- emit progressPct((double)seq / (double)_missionItems.count());
-
- _retryCount = 0;
- if (_itemIndicesToRead.count() == 0) {
- _readTransactionComplete();
- } else {
- _requestNextMissionItem();
- }
-}
-
-void MissionManager::_clearMissionItems(void)
-{
- _itemIndicesToRead.clear();
- _clearAndDeleteMissionItems();
-}
-
-void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
-{
- mavlink_mission_request_t missionRequest;
-
- if (!_checkForExpectedAck(AckMissionRequest)) {
- return;
- }
-
- mavlink_msg_mission_request_decode(&message, &missionRequest);
- qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber" << missionRequest.seq;
-
- if (missionRequest.seq > _writeMissionItems.count() - 1) {
- _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq));
- _finishTransaction(false);
- return;
- }
-
- emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count());
-
- _lastMissionRequest = missionRequest.seq;
- if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
- qCDebug(MissionManagerLog) << "_handleMissionRequest sequence number requested which has already been sent, sending again:" << missionRequest.seq;
- } else {
- _itemIndicesToWrite.removeOne(missionRequest.seq);
- }
-
- MissionItem* item = _writeMissionItems[missionRequest.seq];
- qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:command" << missionRequest.seq << item->command();
-
- mavlink_message_t messageOut;
- if (missionItemInt) {
- mavlink_mission_item_int_t missionItem;
-
- memset(&missionItem, 0, sizeof(missionItem));
- missionItem.target_system = _vehicle->id();
- missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER;
- missionItem.seq = missionRequest.seq;
- missionItem.command = item->command();
- missionItem.param1 = item->param1();
- missionItem.param2 = item->param2();
- missionItem.param3 = item->param3();
- missionItem.param4 = item->param4();
- missionItem.x = item->param5() * qPow(10.0, 7.0);
- missionItem.y = item->param6() * qPow(10.0, 7.0);
- missionItem.z = item->param7();
- missionItem.frame = item->frame();
- missionItem.current = missionRequest.seq == 0;
- missionItem.autocontinue = item->autoContinue();
-
- mavlink_msg_mission_item_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &messageOut,
- &missionItem);
- } else {
- mavlink_mission_item_t missionItem;
-
- memset(&missionItem, 0, sizeof(missionItem));
- missionItem.target_system = _vehicle->id();
- missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER;
- missionItem.seq = missionRequest.seq;
- missionItem.command = item->command();
- missionItem.param1 = item->param1();
- missionItem.param2 = item->param2();
- missionItem.param3 = item->param3();
- missionItem.param4 = item->param4();
- missionItem.x = item->param5();
- missionItem.y = item->param6();
- missionItem.z = item->param7();
- missionItem.frame = item->frame();
- missionItem.current = missionRequest.seq == 0;
- missionItem.autocontinue = item->autoContinue();
-
- mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &messageOut,
- &missionItem);
- }
-
- _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
- _startAckTimeout(AckMissionRequest);
-}
-
-void MissionManager::_handleMissionAck(const mavlink_message_t& message)
-{
- mavlink_mission_ack_t missionAck;
-
- // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
- // type of a protocol sequence we are in.
- AckType_t savedExpectedAck = _expectedAck;
-
- // We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
- // a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
- // matter what.
- if (!_checkForExpectedAck(_expectedAck)) {
- return;
- }
-
- mavlink_msg_mission_ack_decode(&message, &missionAck);
-
- qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
-
- switch (savedExpectedAck) {
- case AckNone:
- // State machine is idle. Vehicle is confused.
- _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
- break;
- case AckMissionCount:
- // MISSION_COUNT message expected
- _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
- _finishTransaction(false);
- break;
- case AckMissionItem:
- // MISSION_ITEM expected
- _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
- _finishTransaction(false);
- break;
- case AckMissionRequest:
- // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
- if (missionAck.type == MAV_MISSION_ACCEPTED) {
- if (_itemIndicesToWrite.count() == 0) {
- qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
- _finishTransaction(true);
- } else {
- _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count()));
- _finishTransaction(false);
- }
- } else {
- _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
- _finishTransaction(false);
- }
- break;
- case AckMissionClearAll:
- // MISSION_ACK expected
- if (missionAck.type != MAV_MISSION_ACCEPTED) {
- _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
- }
- _finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED);
- break;
- case AckGuidedItem:
- // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
- if (missionAck.type == MAV_MISSION_ACCEPTED) {
- qCDebug(MissionManagerLog) << "_handleMissionAck guided mode item accepted";
- _finishTransaction(true);
- } else {
- _sendError(VehicleError, QString("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
- _finishTransaction(false);
- }
- break;
- }
-}
-/// Called when a new mavlink message for out vehicle is received
-void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
-{
- switch (message.msgid) {
- case MAVLINK_MSG_ID_MISSION_COUNT:
- _handleMissionCount(message);
- break;
-
- case MAVLINK_MSG_ID_MISSION_ITEM:
- _handleMissionItem(message, false /* missionItemInt */);
- break;
-
- case MAVLINK_MSG_ID_MISSION_ITEM_INT:
- _handleMissionItem(message, true /* missionItemInt */);
- break;
-
- case MAVLINK_MSG_ID_MISSION_REQUEST:
- _handleMissionRequest(message, false /* missionItemInt */);
- break;
-
- case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
- _handleMissionRequest(message, true /* missionItemInt */);
- break;
-
- case MAVLINK_MSG_ID_MISSION_ACK:
- _handleMissionAck(message);
- break;
-
- case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
- // FIXME: NYI
- break;
-
- case MAVLINK_MSG_ID_MISSION_CURRENT:
- _handleMissionCurrent(message);
- break;
- }
-}
-
-void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
-{
- qCDebug(MissionManagerLog) << "Sending error" << errorCode << errorMsg;
-
- emit error(errorCode, errorMsg);
-}
-
-QString MissionManager::_ackTypeToString(AckType_t ackType)
-{
- switch (ackType) {
- case AckNone:
- return QString("No Ack");
- case AckMissionCount:
- return QString("MISSION_COUNT");
- case AckMissionItem:
- return QString("MISSION_ITEM");
- case AckMissionRequest:
- return QString("MISSION_REQUEST");
- case AckGuidedItem:
- return QString("Guided Mode Item");
- default:
- qWarning(MissionManagerLog) << "Fell off end of switch statement";
- return QString("QGC Internal Error");
- }
-}
-
-QString MissionManager::_lastMissionReqestString(MAV_MISSION_RESULT result)
-{
- if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) {
- MissionItem* item = _writeMissionItems[_lastMissionRequest];
-
- switch (result) {
- case MAV_MISSION_UNSUPPORTED_FRAME:
- return QString(". Frame: %1").arg(item->frame());
- case MAV_MISSION_UNSUPPORTED:
- {
- const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command());
- QString friendlyName;
- QString rawName;
- if (uiInfo) {
- friendlyName = uiInfo->friendlyName();
- rawName = uiInfo->rawName();
- }
- return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command());
- }
- case MAV_MISSION_INVALID_PARAM1:
- return QString(". Param1: %1").arg(item->param1());
- case MAV_MISSION_INVALID_PARAM2:
- return QString(". Param2: %1").arg(item->param2());
- case MAV_MISSION_INVALID_PARAM3:
- return QString(". Param3: %1").arg(item->param3());
- case MAV_MISSION_INVALID_PARAM4:
- return QString(". Param4: %1").arg(item->param4());
- case MAV_MISSION_INVALID_PARAM5_X:
- return QString(". Param5: %1").arg(item->param5());
- case MAV_MISSION_INVALID_PARAM6_Y:
- return QString(". Param6: %1").arg(item->param6());
- case MAV_MISSION_INVALID_PARAM7:
- return QString(". Param7: %1").arg(item->param7());
- case MAV_MISSION_INVALID_SEQUENCE:
- return QString(". Sequence: %1").arg(item->sequenceNumber());
- default:
- break;
- }
- }
-
- return QString();
-}
-
-QString MissionManager::_missionResultToString(MAV_MISSION_RESULT result)
-{
- QString resultString;
- QString lastRequestString = _lastMissionReqestString(result);
-
- switch (result) {
- case MAV_MISSION_ACCEPTED:
- resultString = QString("Mission accepted (MAV_MISSION_ACCEPTED)");
- break;
- case MAV_MISSION_ERROR:
- resultString = QString("Unspecified error (MAV_MISSION_ERROR)");
- break;
- case MAV_MISSION_UNSUPPORTED_FRAME:
- resultString = QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)");
- break;
- case MAV_MISSION_UNSUPPORTED:
- resultString = QString("Command is not supported (MAV_MISSION_UNSUPPORTED)");
- break;
- case MAV_MISSION_NO_SPACE:
- resultString = QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)");
- break;
- case MAV_MISSION_INVALID:
- resultString = QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)");
- break;
- case MAV_MISSION_INVALID_PARAM1:
- resultString = QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)");
- break;
- case MAV_MISSION_INVALID_PARAM2:
- resultString = QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)");
- break;
- case MAV_MISSION_INVALID_PARAM3:
- resultString = QString("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)");
- break;
- case MAV_MISSION_INVALID_PARAM4:
- resultString = QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)");
- break;
- case MAV_MISSION_INVALID_PARAM5_X:
- resultString = QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)");
- break;
- case MAV_MISSION_INVALID_PARAM6_Y:
- resultString = QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)");
- break;
- case MAV_MISSION_INVALID_PARAM7:
- resultString = QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)");
- break;
- case MAV_MISSION_INVALID_SEQUENCE:
- resultString = QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)");
- break;
- case MAV_MISSION_DENIED:
- resultString = QString("Not accepting any mission commands (MAV_MISSION_DENIED)");
- break;
- default:
- qWarning(MissionManagerLog) << "Fell off end of switch statement";
- resultString = QString("QGC Internal Error");
- }
-
- return resultString + lastRequestString;
-}
-
-void MissionManager::_finishTransaction(bool success)
-{
- emit progressPct(1);
-
- _itemIndicesToRead.clear();
- _itemIndicesToWrite.clear();
-
- // First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete.
- TransactionType_t currentTransactionType = _transactionInProgress;
- _transactionInProgress = TransactionNone;
- if (currentTransactionType != TransactionNone) {
- _transactionInProgress = TransactionNone;
- qDebug() << "inProgressChanged";
- emit inProgressChanged(false);
- }
-
- switch (currentTransactionType) {
- case TransactionRead:
- if (!success) {
- // Read from vehicle failed, clear partial list
- _clearAndDeleteMissionItems();
- }
- emit newMissionItemsAvailable(false);
- break;
- case TransactionWrite:
- if (success) {
- // Write succeeded, update internal list to be current
- _currentMissionIndex = -1;
- _lastCurrentIndex = -1;
- emit currentIndexChanged(-1);
- emit lastCurrentIndexChanged(-1);
- _clearAndDeleteMissionItems();
- for (int i=0; i<_writeMissionItems.count(); i++) {
- _missionItems.append(_writeMissionItems[i]);
- }
- _writeMissionItems.clear();
- } else {
- // Write failed, throw out the write list
- _clearAndDeleteWriteMissionItems();
- }
- emit sendComplete(!success /* error */);
- break;
- case TransactionRemoveAll:
- emit removeAllComplete(!success /* error */);
- break;
- default:
- break;
- }
-
- if (_resumeMission) {
- _resumeMission = false;
- if (success) {
- emit resumeMissionReady();
- } else {
- emit resumeMissionUploadFail();
- }
- }
-}
-
-bool MissionManager::inProgress(void)
-{
- return _transactionInProgress != TransactionNone;
-}
-
-void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
-{
- mavlink_mission_current_t missionCurrent;
-
- mavlink_msg_mission_current_decode(&message, &missionCurrent);
-
- if (missionCurrent.seq != _currentMissionIndex) {
- qCDebug(MissionManagerLog) << "_handleMissionCurrent currentIndex:" << missionCurrent.seq;
- _currentMissionIndex = missionCurrent.seq;
- emit currentIndexChanged(_currentMissionIndex);
- }
-
- if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
- qCDebug(MissionManagerLog) << "_handleMissionCurrent lastCurrentIndex:" << _currentMissionIndex;
- _lastCurrentIndex = _currentMissionIndex;
- emit lastCurrentIndexChanged(_lastCurrentIndex);
- }
-}
-
-void MissionManager::_removeAllWorker(void)
-{
- mavlink_message_t message;
-
- qCDebug(MissionManagerLog) << "_removeAllWorker";
-
- emit progressPct(0);
-
- _dedicatedLink = _vehicle->priorityLink();
- mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
- qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
- _dedicatedLink->mavlinkChannel(),
- &message,
- _vehicle->id(),
- MAV_COMP_ID_MISSIONPLANNER,
- MAV_MISSION_TYPE_MISSION);
- _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
- _startAckTimeout(AckMissionClearAll);
-}
-
-void MissionManager::removeAll(void)
-{
- if (inProgress()) {
- return;
- }
-
- qCDebug(MissionManagerLog) << "removeAll";
-
- _clearAndDeleteMissionItems();
-
- _currentMissionIndex = -1;
- _lastCurrentIndex = -1;
- emit currentIndexChanged(-1);
- emit lastCurrentIndexChanged(-1);
-
- _transactionInProgress = TransactionRemoveAll;
- _retryCount = 0;
- emit inProgressChanged(true);
-
- _removeAllWorker();
-}
-
void MissionManager::generateResumeMission(int resumeIndex)
{
if (_vehicle->isOfflineEditingVehicle()) {
@@ -1129,20 +231,3 @@ void MissionManager::generateResumeMission(int resumeIndex)
resumeMission[i]->deleteLater();
}
}
-
-void MissionManager::_clearAndDeleteMissionItems(void)
-{
- for (int i=0; i<_missionItems.count(); i++) {
- _missionItems[i]->deleteLater();
- }
- _missionItems.clear();
-}
-
-
-void MissionManager::_clearAndDeleteWriteMissionItems(void)
-{
- for (int i=0; i<_writeMissionItems.count(); i++) {
- _writeMissionItems[i]->deleteLater();
- }
- _writeMissionItems.clear();
-}
diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h
index 6c6a792a7c7433bbe2daf525d1c447d28734a4ed..bf6936c6aba9fefc7b5c41e964091f25be46060c 100644
--- a/src/MissionManager/MissionManager.h
+++ b/src/MissionManager/MissionManager.h
@@ -11,22 +11,11 @@
#ifndef MissionManager_H
#define MissionManager_H
-#include <QObject>
-#include <QLoggingCategory>
-#include <QThread>
-#include <QMutex>
-#include <QTimer>
-
-#include "MissionItem.h"
-#include "QGCMAVLink.h"
-#include "QGCLoggingCategory.h"
-#include "LinkInterface.h"
-
-class Vehicle;
+#include "PlanManager.h"
Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog)
-class MissionManager : public QObject
+class MissionManager : public PlanManager
{
Q_OBJECT
@@ -34,128 +23,14 @@ public:
MissionManager(Vehicle* vehicle);
~MissionManager();
- bool inProgress(void);
- const QList<MissionItem*>& missionItems(void) { return _missionItems; }
-
- /// Current mission item as reported by MISSION_CURRENT
- int currentIndex(void) const { return _currentMissionIndex; }
-
- /// Last current mission item reported while in Mission flight mode
- int lastCurrentIndex(void) const { return _lastCurrentIndex; }
-
- /// Load the mission items from the vehicle
- /// Signals newMissionItemsAvailable when done
- void loadFromVehicle(void);
-
- /// Writes the specified set of mission items to the vehicle
- /// @param missionItems Items to send to vehicle
- /// Signals sendComplete when done
- void writeMissionItems(const QList<MissionItem*>& missionItems);
-
/// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
/// @param gotoCoord Coordinate to move to
/// @param altChangeOnly true: only altitude change, false: lat/lon/alt change
void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly);
- /// Removes all mission items from vehicle
- /// Signals removeAllComplete when done
- void removeAll(void);
-
/// Generates a new mission which starts from the specified index. It will include all the CMD_DO items
/// from mission start to resumeIndex in the generate mission.
void generateResumeMission(int resumeIndex);
-
- /// Error codes returned in error signal
- typedef enum {
- InternalError,
- AckTimeoutError, ///< Timed out waiting for response from vehicle
- ProtocolOrderError, ///< Incorrect protocol sequence from vehicle
- RequestRangeError, ///< Vehicle requested item out of range
- ItemMismatchError, ///< Vehicle returned item with seq # different than requested
- VehicleError, ///< Vehicle returned error
- MissingRequestsError, ///< Vehicle did not request all items during write sequence
- MaxRetryExceeded, ///< Retry failed
- } ErrorCode_t;
-
- // These values are public so the unit test can set appropriate signal wait times
- static const int _ackTimeoutMilliseconds = 1000;
- static const int _maxRetryCount = 5;
-
-signals:
- void newMissionItemsAvailable(bool removeAllRequested);
- void inProgressChanged(bool inProgress);
- void error(int errorCode, const QString& errorMsg);
- void currentIndexChanged(int currentIndex);
- void lastCurrentIndexChanged(int lastCurrentIndex);
- void resumeMissionReady(void);
- void resumeMissionUploadFail(void);
- void progressPct(double progressPercentPct);
- void removeAllComplete (bool error);
- void sendComplete (bool error);
-
-private slots:
- void _mavlinkMessageReceived(const mavlink_message_t& message);
- void _ackTimeout(void);
-
-private:
- typedef enum {
- AckNone, ///< State machine is idle
- AckMissionCount, ///< MISSION_COUNT message expected
- AckMissionItem, ///< MISSION_ITEM expected
- AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
- AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
- AckGuidedItem, ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
- } AckType_t;
-
- typedef enum {
- TransactionNone,
- TransactionRead,
- TransactionWrite,
- TransactionRemoveAll
- } TransactionType_t;
-
- void _startAckTimeout(AckType_t ack);
- bool _checkForExpectedAck(AckType_t receivedAck);
- void _readTransactionComplete(void);
- void _handleMissionCount(const mavlink_message_t& message);
- void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
- void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
- void _handleMissionAck(const mavlink_message_t& message);
- void _handleMissionCurrent(const mavlink_message_t& message);
- void _requestNextMissionItem(void);
- void _clearMissionItems(void);
- void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
- QString _ackTypeToString(AckType_t ackType);
- QString _missionResultToString(MAV_MISSION_RESULT result);
- void _finishTransaction(bool success);
- void _requestList(void);
- void _writeMissionCount(void);
- void _writeMissionItemsWorker(void);
- void _clearAndDeleteMissionItems(void);
- void _clearAndDeleteWriteMissionItems(void);
- QString _lastMissionReqestString(MAV_MISSION_RESULT result);
- void _removeAllWorker(void);
-
-private:
- Vehicle* _vehicle;
- LinkInterface* _dedicatedLink;
-
- QTimer* _ackTimeoutTimer;
- AckType_t _expectedAck;
- int _retryCount;
-
- TransactionType_t _transactionInProgress;
- bool _resumeMission;
- QList<int> _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle
- QList<int> _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle
- int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST
-
- QMutex _dataMutex;
-
- QList<MissionItem*> _missionItems; ///< Set of mission items on vehicle
- QList<MissionItem*> _writeMissionItems; ///< Set of mission items currently being written to vehicle
- int _currentMissionIndex;
- int _lastCurrentIndex;
};
#endif
diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h
index 090ea2a4617d967bfafd7aa00d17340ddebf33f3..ffe29497831e3633b304b467f9985f9f271866b6 100644
--- a/src/MissionManager/PlanElementController.h
+++ b/src/MissionManager/PlanElementController.h
@@ -27,6 +27,7 @@ public:
PlanElementController(PlanMasterController* masterController, QObject* parent = NULL);
~PlanElementController();
+ Q_PROPERTY(bool supported READ supported NOTIFY supportedChanged) ///< true: Element is supported by Vehicle
Q_PROPERTY(bool containsItems READ containsItems NOTIFY containsItemsChanged) ///< true: Elemement is non-empty
Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) ///< true: information is currently being saved/sent, false: no active save/send in progress
Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) ///< true: unsaved/sent changes are present, false: no changes since last save/send
@@ -41,10 +42,11 @@ public:
virtual void removeAll (void) = 0; ///< Removes all from controller only
virtual bool showPlanFromManagerVehicle (void) = 0; /// true: controller is waiting for the current load to complete
- virtual bool containsItems (void) const = 0;
- virtual bool syncInProgress (void) const = 0;
- virtual bool dirty (void) const = 0;
- virtual void setDirty (bool dirty) = 0;
+ virtual bool supported (void) const = 0;
+ virtual bool containsItems (void) const = 0;
+ virtual bool syncInProgress (void) const = 0;
+ virtual bool dirty (void) const = 0;
+ virtual void setDirty (bool dirty) = 0;
/// Sends the current plan element to the vehicle
/// Signals sendComplete when done
@@ -58,6 +60,7 @@ public:
virtual void managerVehicleChanged(Vehicle* managerVehicle) = 0;
signals:
+ void supportedChanged (bool supported);
void containsItemsChanged (bool containsItems);
void syncInProgressChanged (bool syncInProgress);
void dirtyChanged (bool dirty);
diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc
new file mode 100644
index 0000000000000000000000000000000000000000..b132c5228e97d0e3eb89fac46901374177edec89
--- /dev/null
+++ b/src/MissionManager/PlanManager.cc
@@ -0,0 +1,970 @@
+/****************************************************************************
+ *
+ * (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 "PlanManager.h"
+#include "Vehicle.h"
+#include "FirmwarePlugin.h"
+#include "MAVLinkProtocol.h"
+#include "QGCApplication.h"
+#include "MissionCommandTree.h"
+#include "MissionCommandUIInfo.h"
+
+QGC_LOGGING_CATEGORY(PlanManagerLog, "PlanManagerLog")
+
+PlanManager::PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType)
+ : _vehicle(vehicle)
+ , _planType(planType)
+ , _dedicatedLink(NULL)
+ , _ackTimeoutTimer(NULL)
+ , _expectedAck(AckNone)
+ , _transactionInProgress(TransactionNone)
+ , _resumeMission(false)
+ , _lastMissionRequest(-1)
+ , _currentMissionIndex(-1)
+ , _lastCurrentIndex(-1)
+{
+ _ackTimeoutTimer = new QTimer(this);
+ _ackTimeoutTimer->setSingleShot(true);
+ _ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds);
+
+ connect(_ackTimeoutTimer, &QTimer::timeout, this, &PlanManager::_ackTimeout);
+}
+
+PlanManager::~PlanManager()
+{
+
+}
+
+void PlanManager::_writeMissionItemsWorker(void)
+{
+ _lastMissionRequest = -1;
+
+ emit progressPct(0);
+
+ qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 count:").arg(_planTypeString()) << _writeMissionItems.count();
+
+ // Prime write list
+ _itemIndicesToWrite.clear();
+ for (int i=0; i<_writeMissionItems.count(); i++) {
+ _itemIndicesToWrite << i;
+ }
+
+ _transactionInProgress = TransactionWrite;
+ _retryCount = 0;
+ emit inProgressChanged(true);
+ _connectToMavlink();
+ _writeMissionCount();
+}
+
+
+void PlanManager::writeMissionItems(const QList<MissionItem*>& missionItems)
+{
+ if (_vehicle->isOfflineEditingVehicle()) {
+ return;
+ }
+
+ if (inProgress()) {
+ qCDebug(PlanManagerLog) << QStringLiteral("writeMissionItems %1 called while transaction in progress").arg(_planTypeString());
+ return;
+ }
+
+ _clearAndDeleteWriteMissionItems();
+
+ bool skipFirstItem = _planType == MAV_MISSION_TYPE_MISSION && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle();
+
+ int firstIndex = skipFirstItem ? 1 : 0;
+
+ for (int i=firstIndex; i<missionItems.count(); i++) {
+ MissionItem* item = new MissionItem(*missionItems[i]);
+ _writeMissionItems.append(item);
+
+ item->setIsCurrentItem(i == firstIndex);
+
+ if (skipFirstItem) {
+ // Home is in sequence 0, remainder of items start at sequence 1
+ item->setSequenceNumber(item->sequenceNumber() - 1);
+ if (item->command() == MAV_CMD_DO_JUMP) {
+ item->setParam1((int)item->param1() - 1);
+ }
+ }
+ }
+
+ _writeMissionItemsWorker();
+}
+
+/// This begins the write sequence with the vehicle. This may be called during a retry.
+void PlanManager::_writeMissionCount(void)
+{
+ qCDebug(PlanManagerLog) << QStringLiteral("_writeMissionCount %1 count:_retryCount").arg(_planTypeString()) << _writeMissionItems.count() << _retryCount;
+
+ mavlink_message_t message;
+
+ _dedicatedLink = _vehicle->priorityLink();
+ mavlink_msg_mission_count_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &message,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ _writeMissionItems.count(),
+ _planType);
+
+ _vehicle->sendMessageOnLink(_dedicatedLink, message);
+ _startAckTimeout(AckMissionRequest);
+}
+
+void PlanManager::loadFromVehicle(void)
+{
+ if (_vehicle->isOfflineEditingVehicle()) {
+ return;
+ }
+
+ qCDebug(PlanManagerLog) << QStringLiteral("loadFromVehicle %1 read sequence").arg(_planTypeString());
+
+ if (inProgress()) {
+ qCDebug(PlanManagerLog) << QStringLiteral("loadFromVehicle %1 called while transaction in progress").arg(_planTypeString());
+ return;
+ }
+
+ _retryCount = 0;
+ _transactionInProgress = TransactionRead;
+ emit inProgressChanged(true);
+ _connectToMavlink();
+ _requestList();
+}
+
+/// Internal call to request list of mission items. May be called during a retry sequence.
+void PlanManager::_requestList(void)
+{
+ qCDebug(PlanManagerLog) << QStringLiteral("_requestList %1 _planType:_retryCount").arg(_planTypeString()) << _planType << _retryCount;
+
+ mavlink_message_t message;
+
+ _itemIndicesToRead.clear();
+ _clearMissionItems();
+
+ _dedicatedLink = _vehicle->priorityLink();
+ mavlink_msg_mission_request_list_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &message,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ _planType);
+
+ _vehicle->sendMessageOnLink(_dedicatedLink, message);
+ _startAckTimeout(AckMissionCount);
+}
+
+void PlanManager::_ackTimeout(void)
+{
+ if (_expectedAck == AckNone) {
+ return;
+ }
+
+ switch (_expectedAck) {
+ case AckNone:
+ qCWarning(PlanManagerLog) << QStringLiteral("_ackTimeout %1 timeout with AckNone").arg(_planTypeString());
+ _sendError(InternalError, "Internal error occurred during Mission Item communication: _ackTimeOut:_expectedAck == AckNone");
+ break;
+ case AckMissionCount:
+ // MISSION_COUNT message expected
+ if (_retryCount > _maxRetryCount) {
+ _sendError(VehicleError, QStringLiteral("Mission request list failed, maximum retries exceeded."));
+ _finishTransaction(false);
+ } else {
+ _retryCount++;
+ qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 REQUEST_LIST retry Count").arg(_planTypeString()) << _retryCount;
+ _requestList();
+ }
+ break;
+ case AckMissionItem:
+ // MISSION_ITEM expected
+ if (_retryCount > _maxRetryCount) {
+ _sendError(VehicleError, QStringLiteral("Mission read failed, maximum retries exceeded."));
+ _finishTransaction(false);
+ } else {
+ _retryCount++;
+ qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_REQUEST retry Count").arg(_planTypeString()) << _retryCount;
+ _requestNextMissionItem();
+ }
+ break;
+ case AckMissionRequest:
+ // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+ if (_itemIndicesToWrite.count() == 0) {
+ // Vehicle did not send final MISSION_ACK at end of sequence
+ _sendError(VehicleError, QStringLiteral("Mission write failed, vehicle failed to send final ack."));
+ _finishTransaction(false);
+ } else if (_itemIndicesToWrite[0] == 0) {
+ // Vehicle did not respond to MISSION_COUNT, try again
+ if (_retryCount > _maxRetryCount) {
+ _sendError(VehicleError, QStringLiteral("Mission write mission count failed, maximum retries exceeded."));
+ _finishTransaction(false);
+ } else {
+ _retryCount++;
+ qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_COUNT retry Count").arg(_planTypeString()) << _retryCount;
+ _writeMissionCount();
+ }
+ } else {
+ // Vehicle did not request all items from ground station
+ _sendError(AckTimeoutError, QString("Vehicle did not request all items from ground station: %1").arg(_ackTypeToString(_expectedAck)));
+ _expectedAck = AckNone;
+ _finishTransaction(false);
+ }
+ break;
+ case AckMissionClearAll:
+ // MISSION_ACK expected
+ if (_retryCount > _maxRetryCount) {
+ _sendError(VehicleError, QStringLiteral("Mission remove all, maximum retries exceeded."));
+ _finishTransaction(false);
+ } else {
+ _retryCount++;
+ qCDebug(PlanManagerLog) << QStringLiteral("Retrying %1 MISSION_CLEAR_ALL retry Count").arg(_planTypeString()) << _retryCount;
+ _removeAllWorker();
+ }
+ break;
+ case AckGuidedItem:
+ // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+ default:
+ _sendError(AckTimeoutError, QString("Vehicle did not respond to mission item communication: %1").arg(_ackTypeToString(_expectedAck)));
+ _expectedAck = AckNone;
+ _finishTransaction(false);
+ }
+}
+
+void PlanManager::_startAckTimeout(AckType_t ack)
+{
+ _expectedAck = ack;
+ _ackTimeoutTimer->start();
+}
+
+/// Checks the received ack against the expected ack. If they match the ack timeout timer will be stopped.
+/// @return true: received ack matches expected ack
+bool PlanManager::_checkForExpectedAck(AckType_t receivedAck)
+{
+ if (receivedAck == _expectedAck) {
+ _expectedAck = AckNone;
+ _ackTimeoutTimer->stop();
+ return true;
+ } else {
+ if (_expectedAck == AckNone) {
+ // Don't worry about unexpected mission commands, just ignore them; ArduPilot updates home position using
+ // spurious MISSION_ITEMs.
+ } else {
+ // We just warn in this case, this could be crap left over from a previous transaction or the vehicle going bonkers.
+ // Whatever it is we let the ack timeout handle any error output to the user.
+ qCDebug(PlanManagerLog) << QString("Out of sequence ack expected:received %1:%2 %1").arg(_ackTypeToString(_expectedAck)).arg(_ackTypeToString(receivedAck)).arg(_planTypeString());
+ }
+ return false;
+ }
+}
+
+void PlanManager::_readTransactionComplete(void)
+{
+ qCDebug(PlanManagerLog) << "_readTransactionComplete read sequence complete";
+
+ mavlink_message_t message;
+
+ mavlink_msg_mission_ack_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &message,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ MAV_MISSION_ACCEPTED,
+ _planType);
+
+ _vehicle->sendMessageOnLink(_dedicatedLink, message);
+
+ _finishTransaction(true);
+}
+
+void PlanManager::_handleMissionCount(const mavlink_message_t& message)
+{
+ mavlink_mission_count_t missionCount;
+
+ mavlink_msg_mission_count_decode(&message, &missionCount);
+
+ if (missionCount.mission_type != _planType) {
+ // if there was a previous transaction with a different mission_type, it can happen that we receive
+ // a stale message here, for example when the MAV ran into a timeout and sent a message twice
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCount %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionCount.mission_type;
+ return;
+ }
+
+ if (!_checkForExpectedAck(AckMissionCount)) {
+ return;
+ }
+
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCount %1 count:").arg(_planTypeString()) << missionCount.count;
+
+ _retryCount = 0;
+
+ if (missionCount.count == 0) {
+ _readTransactionComplete();
+ } else {
+ // Prime read list
+ for (int i=0; i<missionCount.count; i++) {
+ _itemIndicesToRead << i;
+ }
+ _requestNextMissionItem();
+ }
+}
+
+void PlanManager::_requestNextMissionItem(void)
+{
+ if (_itemIndicesToRead.count() == 0) {
+ _sendError(InternalError, "Internal Error: Call to Vehicle _requestNextMissionItem with no more indices to read");
+ return;
+ }
+
+ qCDebug(PlanManagerLog) << QStringLiteral("_requestNextMissionItem %1 sequenceNumber:retry").arg(_planTypeString()) << _itemIndicesToRead[0] << _retryCount;
+
+ mavlink_message_t message;
+ if (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_INT) {
+ mavlink_msg_mission_request_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &message,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ _itemIndicesToRead[0],
+ _planType);
+ } else {
+ mavlink_msg_mission_request_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &message,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ _itemIndicesToRead[0],
+ _planType);
+ }
+
+ _vehicle->sendMessageOnLink(_dedicatedLink, message);
+ _startAckTimeout(AckMissionItem);
+}
+
+void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
+{
+ MAV_CMD command;
+ MAV_FRAME frame;
+ double param1;
+ double param2;
+ double param3;
+ double param4;
+ double param5;
+ double param6;
+ double param7;
+ bool autoContinue;
+ bool isCurrentItem;
+ int seq;
+
+ if (missionItemInt) {
+ mavlink_mission_item_int_t missionItem;
+ mavlink_msg_mission_item_int_decode(&message, &missionItem);
+
+ command = (MAV_CMD)missionItem.command,
+ frame = (MAV_FRAME)missionItem.frame,
+ param1 = missionItem.param1;
+ param2 = missionItem.param2;
+ param3 = missionItem.param3;
+ param4 = missionItem.param4;
+ param5 = (double)missionItem.x / qPow(10.0, 7.0);
+ param6 = (double)missionItem.y / qPow(10.0, 7.0);
+ param7 = (double)missionItem.z;
+ autoContinue = missionItem.autocontinue;
+ isCurrentItem = missionItem.current;
+ seq = missionItem.seq;
+ } else {
+ mavlink_mission_item_t missionItem;
+ mavlink_msg_mission_item_decode(&message, &missionItem);
+
+ command = (MAV_CMD)missionItem.command,
+ frame = (MAV_FRAME)missionItem.frame,
+ param1 = missionItem.param1;
+ param2 = missionItem.param2;
+ param3 = missionItem.param3;
+ param4 = missionItem.param4;
+ param5 = missionItem.x;
+ param6 = missionItem.y;
+ param7 = missionItem.z;
+ autoContinue = missionItem.autocontinue;
+ isCurrentItem = missionItem.current;
+ seq = missionItem.seq;
+ }
+
+ // We don't support editing ALT_INT frames so change on the way in.
+ if (frame == MAV_FRAME_GLOBAL_INT) {
+ frame = MAV_FRAME_GLOBAL;
+ } else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
+ frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+ }
+
+
+ bool ardupilotHomePositionUpdate = false;
+ if (!_checkForExpectedAck(AckMissionItem)) {
+ if (_vehicle->apmFirmware() && seq == 0 && _planType == MAV_MISSION_TYPE_MISSION) {
+ ardupilotHomePositionUpdate = true;
+ } else {
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 dropping spurious item seq:command:current").arg(_planTypeString()) << seq << command << isCurrentItem;
+ return;
+ }
+ }
+
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 seq:command:current:ardupilotHomePositionUpdate").arg(_planTypeString()) << seq << command << isCurrentItem << ardupilotHomePositionUpdate;
+
+ if (ardupilotHomePositionUpdate) {
+ QGeoCoordinate newHomePosition(param5, param6, param7);
+ _vehicle->_setHomePosition(newHomePosition);
+ return;
+ }
+
+ if (_itemIndicesToRead.contains(seq)) {
+ _itemIndicesToRead.removeOne(seq);
+
+ MissionItem* item = new MissionItem(seq,
+ command,
+ frame,
+ param1,
+ param2,
+ param3,
+ param4,
+ param5,
+ param6,
+ param7,
+ autoContinue,
+ isCurrentItem,
+ this);
+
+ if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
+ // Home is in position 0
+ item->setParam1((int)item->param1() + 1);
+ }
+
+ _missionItems.append(item);
+ } else {
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionItem %1 mission item received item index which was not requested, disregrarding:").arg(_planTypeString()) << seq;
+ // We have to put the ack timeout back since it was removed above
+ _startAckTimeout(AckMissionItem);
+ return;
+ }
+
+ emit progressPct((double)seq / (double)_missionItems.count());
+
+ _retryCount = 0;
+ if (_itemIndicesToRead.count() == 0) {
+ _readTransactionComplete();
+ } else {
+ _requestNextMissionItem();
+ }
+}
+
+void PlanManager::_clearMissionItems(void)
+{
+ _itemIndicesToRead.clear();
+ _clearAndDeleteMissionItems();
+}
+
+void PlanManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
+{
+ mavlink_mission_request_t missionRequest;
+
+ mavlink_msg_mission_request_decode(&message, &missionRequest);
+
+ if (missionRequest.mission_type != _planType) {
+ // if there was a previous transaction with a different mission_type, it can happen that we receive
+ // a stale message here, for example when the MAV ran into a timeout and sent a message twice
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionRequest.mission_type;
+ return;
+ }
+
+ if (!_checkForExpectedAck(AckMissionRequest)) {
+ return;
+ }
+
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber").arg(_planTypeString()) << missionRequest.seq;
+
+ if (missionRequest.seq > _writeMissionItems.count() - 1) {
+ _sendError(RequestRangeError, QString("Vehicle requested item outside range, count:request %1:%2. Send to Vehicle failed.").arg(_writeMissionItems.count()).arg(missionRequest.seq));
+ _finishTransaction(false);
+ return;
+ }
+
+ emit progressPct((double)missionRequest.seq / (double)_writeMissionItems.count());
+
+ _lastMissionRequest = missionRequest.seq;
+ if (!_itemIndicesToWrite.contains(missionRequest.seq)) {
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequence number requested which has already been sent, sending again:").arg(_planTypeString()) << missionRequest.seq;
+ } else {
+ _itemIndicesToWrite.removeOne(missionRequest.seq);
+ }
+
+ MissionItem* item = _writeMissionItems[missionRequest.seq];
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionRequest %1 sequenceNumber:command").arg(_planTypeString()) << missionRequest.seq << item->command();
+
+ mavlink_message_t messageOut;
+ if (missionItemInt) {
+ mavlink_msg_mission_item_int_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &messageOut,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ missionRequest.seq,
+ item->frame(),
+ item->command(),
+ missionRequest.seq == 0,
+ item->autoContinue(),
+ item->param1(),
+ item->param2(),
+ item->param3(),
+ item->param4(),
+ item->param5() * qPow(10.0, 7.0),
+ item->param6() * qPow(10.0, 7.0),
+ item->param7(),
+ _planType);
+ } else {
+ mavlink_msg_mission_item_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &messageOut,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ missionRequest.seq,
+ item->frame(),
+ item->command(),
+ missionRequest.seq == 0,
+ item->autoContinue(),
+ item->param1(),
+ item->param2(),
+ item->param3(),
+ item->param4(),
+ item->param5(),
+ item->param6(),
+ item->param7(),
+ _planType);
+ }
+
+ _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
+ _startAckTimeout(AckMissionRequest);
+}
+
+void PlanManager::_handleMissionAck(const mavlink_message_t& message)
+{
+ mavlink_mission_ack_t missionAck;
+
+ mavlink_msg_mission_ack_decode(&message, &missionAck);
+ if (missionAck.mission_type != _planType) {
+ // if there was a previous transaction with a different mission_type, it can happen that we receive
+ // a stale message here, for example when the MAV ran into a timeout and sent a message twice
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 Incorrect mission_type received expected:actual").arg(_planTypeString()) << _planType << missionAck.mission_type;
+ return;
+ }
+
+ // Save the retry ack before calling _checkForExpectedAck since we'll need it to determine what
+ // type of a protocol sequence we are in.
+ AckType_t savedExpectedAck = _expectedAck;
+
+ // We can get a MISSION_ACK with an error at any time, so if the Acks don't match it is not
+ // a protocol sequence error. Call _checkForExpectedAck with _retryAck so it will succeed no
+ // matter what.
+ if (!_checkForExpectedAck(_expectedAck)) {
+ return;
+ }
+
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 type:").arg(_planTypeString()) << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
+
+ switch (savedExpectedAck) {
+ case AckNone:
+ // State machine is idle. Vehicle is confused.
+ _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
+ break;
+ case AckMissionCount:
+ // MISSION_COUNT message expected
+ _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
+ _finishTransaction(false);
+ break;
+ case AckMissionItem:
+ // MISSION_ITEM expected
+ _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
+ _finishTransaction(false);
+ break;
+ case AckMissionRequest:
+ // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+ if (missionAck.type == MAV_MISSION_ACCEPTED) {
+ if (_itemIndicesToWrite.count() == 0) {
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck write sequence complete").arg(_planTypeString());
+ _finishTransaction(true);
+ } else {
+ _sendError(MissingRequestsError, QString("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count()));
+ _finishTransaction(false);
+ }
+ } else {
+ _sendError(VehicleError, QString("Vehicle returned error: %1.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
+ _finishTransaction(false);
+ }
+ break;
+ case AckMissionClearAll:
+ // MISSION_ACK expected
+ if (missionAck.type != MAV_MISSION_ACCEPTED) {
+ _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle remove all failed.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
+ }
+ _finishTransaction(missionAck.type == MAV_MISSION_ACCEPTED);
+ break;
+ case AckGuidedItem:
+ // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+ if (missionAck.type == MAV_MISSION_ACCEPTED) {
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck %1 guided mode item accepted").arg(_planTypeString());
+ _finishTransaction(true);
+ } else {
+ _sendError(VehicleError, QString("Vehicle returned error: %1. %2Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
+ _finishTransaction(false);
+ }
+ break;
+ }
+}
+/// Called when a new mavlink message for out vehicle is received
+void PlanManager::_mavlinkMessageReceived(const mavlink_message_t& message)
+{
+ switch (message.msgid) {
+ case MAVLINK_MSG_ID_MISSION_COUNT:
+ _handleMissionCount(message);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ITEM:
+ _handleMissionItem(message, false /* missionItemInt */);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ITEM_INT:
+ _handleMissionItem(message, true /* missionItemInt */);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST:
+ _handleMissionRequest(message, false /* missionItemInt */);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
+ _handleMissionRequest(message, true /* missionItemInt */);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ACK:
+ _handleMissionAck(message);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
+ // FIXME: NYI
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_CURRENT:
+ _handleMissionCurrent(message);
+ break;
+ }
+}
+
+void PlanManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
+{
+ qCDebug(PlanManagerLog) << QStringLiteral("Sending %1 error").arg(_planTypeString()) << errorCode << errorMsg;
+
+ emit error(errorCode, errorMsg);
+}
+
+QString PlanManager::_ackTypeToString(AckType_t ackType)
+{
+ switch (ackType) {
+ case AckNone:
+ return QString("No Ack");
+ case AckMissionCount:
+ return QString("MISSION_COUNT");
+ case AckMissionItem:
+ return QString("MISSION_ITEM");
+ case AckMissionRequest:
+ return QString("MISSION_REQUEST");
+ case AckGuidedItem:
+ return QString("Guided Mode Item");
+ default:
+ qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString());
+ return QString("QGC Internal Error");
+ }
+}
+
+QString PlanManager::_lastMissionReqestString(MAV_MISSION_RESULT result)
+{
+ if (_lastMissionRequest != -1 && _lastMissionRequest >= 0 && _lastMissionRequest < _writeMissionItems.count()) {
+ MissionItem* item = _writeMissionItems[_lastMissionRequest];
+
+ switch (result) {
+ case MAV_MISSION_UNSUPPORTED_FRAME:
+ return QString(". Frame: %1").arg(item->frame());
+ case MAV_MISSION_UNSUPPORTED:
+ {
+ const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(_vehicle, item->command());
+ QString friendlyName;
+ QString rawName;
+ if (uiInfo) {
+ friendlyName = uiInfo->friendlyName();
+ rawName = uiInfo->rawName();
+ }
+ return QString(". Command: (%1, %2, %3)").arg(friendlyName).arg(rawName).arg(item->command());
+ }
+ case MAV_MISSION_INVALID_PARAM1:
+ return QString(". Param1: %1").arg(item->param1());
+ case MAV_MISSION_INVALID_PARAM2:
+ return QString(". Param2: %1").arg(item->param2());
+ case MAV_MISSION_INVALID_PARAM3:
+ return QString(". Param3: %1").arg(item->param3());
+ case MAV_MISSION_INVALID_PARAM4:
+ return QString(". Param4: %1").arg(item->param4());
+ case MAV_MISSION_INVALID_PARAM5_X:
+ return QString(". Param5: %1").arg(item->param5());
+ case MAV_MISSION_INVALID_PARAM6_Y:
+ return QString(". Param6: %1").arg(item->param6());
+ case MAV_MISSION_INVALID_PARAM7:
+ return QString(". Param7: %1").arg(item->param7());
+ case MAV_MISSION_INVALID_SEQUENCE:
+ return QString(". Sequence: %1").arg(item->sequenceNumber());
+ default:
+ break;
+ }
+ }
+
+ return QString();
+}
+
+QString PlanManager::_missionResultToString(MAV_MISSION_RESULT result)
+{
+ QString resultString;
+ QString lastRequestString = _lastMissionReqestString(result);
+
+ switch (result) {
+ case MAV_MISSION_ACCEPTED:
+ resultString = QString("Mission accepted (MAV_MISSION_ACCEPTED)");
+ break;
+ case MAV_MISSION_ERROR:
+ resultString = QString("Unspecified error (MAV_MISSION_ERROR)");
+ break;
+ case MAV_MISSION_UNSUPPORTED_FRAME:
+ resultString = QString("Coordinate frame is not supported (MAV_MISSION_UNSUPPORTED_FRAME)");
+ break;
+ case MAV_MISSION_UNSUPPORTED:
+ resultString = QString("Command is not supported (MAV_MISSION_UNSUPPORTED)");
+ break;
+ case MAV_MISSION_NO_SPACE:
+ resultString = QString("Mission item exceeds storage space (MAV_MISSION_NO_SPACE)");
+ break;
+ case MAV_MISSION_INVALID:
+ resultString = QString("One of the parameters has an invalid value (MAV_MISSION_INVALID)");
+ break;
+ case MAV_MISSION_INVALID_PARAM1:
+ resultString = QString("Param1 has an invalid value (MAV_MISSION_INVALID_PARAM1)");
+ break;
+ case MAV_MISSION_INVALID_PARAM2:
+ resultString = QString("Param2 has an invalid value (MAV_MISSION_INVALID_PARAM2)");
+ break;
+ case MAV_MISSION_INVALID_PARAM3:
+ resultString = QString("Param3 has an invalid value (MAV_MISSION_INVALID_PARAM3)");
+ break;
+ case MAV_MISSION_INVALID_PARAM4:
+ resultString = QString("Param4 has an invalid value (MAV_MISSION_INVALID_PARAM4)");
+ break;
+ case MAV_MISSION_INVALID_PARAM5_X:
+ resultString = QString("X/Param5 has an invalid value (MAV_MISSION_INVALID_PARAM5_X)");
+ break;
+ case MAV_MISSION_INVALID_PARAM6_Y:
+ resultString = QString("Y/Param6 has an invalid value (MAV_MISSION_INVALID_PARAM6_Y)");
+ break;
+ case MAV_MISSION_INVALID_PARAM7:
+ resultString = QString("Param7 has an invalid value (MAV_MISSION_INVALID_PARAM7)");
+ break;
+ case MAV_MISSION_INVALID_SEQUENCE:
+ resultString = QString("Received mission item out of sequence (MAV_MISSION_INVALID_SEQUENCE)");
+ break;
+ case MAV_MISSION_DENIED:
+ resultString = QString("Not accepting any mission commands (MAV_MISSION_DENIED)");
+ break;
+ default:
+ qWarning(PlanManagerLog) << QStringLiteral("Fell off end of switch statement %1").arg(_planTypeString());
+ resultString = QString("QGC Internal Error");
+ }
+
+ return resultString + lastRequestString;
+}
+
+void PlanManager::_finishTransaction(bool success)
+{
+ emit progressPct(1);
+ _disconnectFromMavlink();
+
+ _itemIndicesToRead.clear();
+ _itemIndicesToWrite.clear();
+
+ // First thing we do is clear the transaction. This way inProgesss is off when we signal transaction complete.
+ TransactionType_t currentTransactionType = _transactionInProgress;
+ _transactionInProgress = TransactionNone;
+ if (currentTransactionType != TransactionNone) {
+ _transactionInProgress = TransactionNone;
+ qCDebug(PlanManagerLog) << QStringLiteral("inProgressChanged %1").arg(_planTypeString());
+ emit inProgressChanged(false);
+ }
+
+ switch (currentTransactionType) {
+ case TransactionRead:
+ if (!success) {
+ // Read from vehicle failed, clear partial list
+ _clearAndDeleteMissionItems();
+ }
+ emit newMissionItemsAvailable(false);
+ break;
+ case TransactionWrite:
+ if (success) {
+ // Write succeeded, update internal list to be current
+ _currentMissionIndex = -1;
+ _lastCurrentIndex = -1;
+ emit currentIndexChanged(-1);
+ emit lastCurrentIndexChanged(-1);
+ _clearAndDeleteMissionItems();
+ for (int i=0; i<_writeMissionItems.count(); i++) {
+ _missionItems.append(_writeMissionItems[i]);
+ }
+ _writeMissionItems.clear();
+ } else {
+ // Write failed, throw out the write list
+ _clearAndDeleteWriteMissionItems();
+ }
+ emit sendComplete(!success /* error */);
+ break;
+ case TransactionRemoveAll:
+ emit removeAllComplete(!success /* error */);
+ break;
+ default:
+ break;
+ }
+
+ if (_resumeMission) {
+ _resumeMission = false;
+ if (success) {
+ emit resumeMissionReady();
+ } else {
+ emit resumeMissionUploadFail();
+ }
+ }
+}
+
+bool PlanManager::inProgress(void) const
+{
+ return _transactionInProgress != TransactionNone;
+}
+
+void PlanManager::_handleMissionCurrent(const mavlink_message_t& message)
+{
+ mavlink_mission_current_t missionCurrent;
+
+ mavlink_msg_mission_current_decode(&message, &missionCurrent);
+
+ if (missionCurrent.seq != _currentMissionIndex) {
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 currentIndex:").arg(_planTypeString()) << missionCurrent.seq;
+ _currentMissionIndex = missionCurrent.seq;
+ emit currentIndexChanged(_currentMissionIndex);
+ }
+
+ if (_vehicle->flightMode() == _vehicle->missionFlightMode() && _currentMissionIndex != _lastCurrentIndex) {
+ qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionCurrent %1 lastCurrentIndex:").arg(_planTypeString()) << _currentMissionIndex;
+ _lastCurrentIndex = _currentMissionIndex;
+ emit lastCurrentIndexChanged(_lastCurrentIndex);
+ }
+}
+
+void PlanManager::_removeAllWorker(void)
+{
+ mavlink_message_t message;
+
+ qCDebug(PlanManagerLog) << "_removeAllWorker";
+
+ emit progressPct(0);
+
+ _connectToMavlink();
+ _dedicatedLink = _vehicle->priorityLink();
+ mavlink_msg_mission_clear_all_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
+ qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
+ _dedicatedLink->mavlinkChannel(),
+ &message,
+ _vehicle->id(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ _planType);
+ _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
+ _startAckTimeout(AckMissionClearAll);
+}
+
+void PlanManager::removeAll(void)
+{
+ if (inProgress()) {
+ return;
+ }
+
+ qCDebug(PlanManagerLog) << QStringLiteral("removeAll").arg(_planTypeString());
+
+ _clearAndDeleteMissionItems();
+
+ _currentMissionIndex = -1;
+ _lastCurrentIndex = -1;
+ emit currentIndexChanged(-1);
+ emit lastCurrentIndexChanged(-1);
+
+ _transactionInProgress = TransactionRemoveAll;
+ _retryCount = 0;
+ emit inProgressChanged(true);
+
+ _removeAllWorker();
+}
+
+void PlanManager::_clearAndDeleteMissionItems(void)
+{
+ for (int i=0; i<_missionItems.count(); i++) {
+ _missionItems[i]->deleteLater();
+ }
+ _missionItems.clear();
+}
+
+
+void PlanManager::_clearAndDeleteWriteMissionItems(void)
+{
+ for (int i=0; i<_writeMissionItems.count(); i++) {
+ _writeMissionItems[i]->deleteLater();
+ }
+ _writeMissionItems.clear();
+}
+
+void PlanManager::_connectToMavlink(void)
+{
+ connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived);
+}
+
+void PlanManager::_disconnectFromMavlink(void)
+{
+ disconnect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &PlanManager::_mavlinkMessageReceived);
+}
+
+QString PlanManager::_planTypeString(void)
+{
+ switch (_planType) {
+ case MAV_MISSION_TYPE_MISSION:
+ return QStringLiteral("T:Mission");
+ case MAV_MISSION_TYPE_FENCE:
+ return QStringLiteral("T:GeoFence");
+ case MAV_MISSION_TYPE_RALLY:
+ return QStringLiteral("T:Rally");
+ default:
+ qWarning() << "Unknown plan type" << _planType;
+ return QStringLiteral("T:Unknown");
+ }
+}
diff --git a/src/MissionManager/PlanManager.h b/src/MissionManager/PlanManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..43f696c14ed9f7f6a8d5834c5cb311070a89e747
--- /dev/null
+++ b/src/MissionManager/PlanManager.h
@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ * (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 PlanManager_H
+#define PlanManager_H
+
+#include <QObject>
+#include <QLoggingCategory>
+#include <QTimer>
+
+#include "MissionItem.h"
+#include "QGCMAVLink.h"
+#include "QGCLoggingCategory.h"
+#include "LinkInterface.h"
+
+class Vehicle;
+
+Q_DECLARE_LOGGING_CATEGORY(PlanManagerLog)
+
+/// The PlanManager class is the base class for the Mission, GeoFence and Rally Point managers. All of which use the
+/// new mavlink v2 mission protocol.
+class PlanManager : public QObject
+{
+ Q_OBJECT
+
+public:
+ PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType);
+ ~PlanManager();
+
+ bool inProgress(void) const;
+ const QList<MissionItem*>& missionItems(void) { return _missionItems; }
+
+ /// Current mission item as reported by MISSION_CURRENT
+ int currentIndex(void) const { return _currentMissionIndex; }
+
+ /// Last current mission item reported while in Mission flight mode
+ int lastCurrentIndex(void) const { return _lastCurrentIndex; }
+
+ /// Load the mission items from the vehicle
+ /// Signals newMissionItemsAvailable when done
+ void loadFromVehicle(void);
+
+ /// Writes the specified set of mission items to the vehicle
+ /// @param missionItems Items to send to vehicle
+ /// Signals sendComplete when done
+ void writeMissionItems(const QList<MissionItem*>& missionItems);
+
+ /// Removes all mission items from vehicle
+ /// Signals removeAllComplete when done
+ void removeAll(void);
+
+ /// Error codes returned in error signal
+ typedef enum {
+ InternalError,
+ AckTimeoutError, ///< Timed out waiting for response from vehicle
+ ProtocolOrderError, ///< Incorrect protocol sequence from vehicle
+ RequestRangeError, ///< Vehicle requested item out of range
+ ItemMismatchError, ///< Vehicle returned item with seq # different than requested
+ VehicleError, ///< Vehicle returned error
+ MissingRequestsError, ///< Vehicle did not request all items during write sequence
+ MaxRetryExceeded, ///< Retry failed
+ MissionTypeMismatch, ///< MAV_MISSION_TYPE does not match _planType
+ } ErrorCode_t;
+
+ // These values are public so the unit test can set appropriate signal wait times
+ static const int _ackTimeoutMilliseconds = 1000;
+ static const int _maxRetryCount = 5;
+
+signals:
+ void newMissionItemsAvailable (bool removeAllRequested);
+ void inProgressChanged (bool inProgress);
+ void error (int errorCode, const QString& errorMsg);
+ void currentIndexChanged (int currentIndex);
+ void lastCurrentIndexChanged (int lastCurrentIndex);
+ void progressPct (double progressPercentPct);
+ void removeAllComplete (bool error);
+ void sendComplete (bool error);
+ void resumeMissionReady (void);
+ void resumeMissionUploadFail (void);
+
+private slots:
+ void _mavlinkMessageReceived(const mavlink_message_t& message);
+ void _ackTimeout(void);
+
+protected:
+ typedef enum {
+ AckNone, ///< State machine is idle
+ AckMissionCount, ///< MISSION_COUNT message expected
+ AckMissionItem, ///< MISSION_ITEM expected
+ AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
+ AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
+ AckGuidedItem, ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
+ } AckType_t;
+
+ typedef enum {
+ TransactionNone,
+ TransactionRead,
+ TransactionWrite,
+ TransactionRemoveAll
+ } TransactionType_t;
+
+ void _startAckTimeout(AckType_t ack);
+ bool _checkForExpectedAck(AckType_t receivedAck);
+ void _readTransactionComplete(void);
+ void _handleMissionCount(const mavlink_message_t& message);
+ void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
+ void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
+ void _handleMissionAck(const mavlink_message_t& message);
+ void _handleMissionCurrent(const mavlink_message_t& message);
+ void _requestNextMissionItem(void);
+ void _clearMissionItems(void);
+ void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
+ QString _ackTypeToString(AckType_t ackType);
+ QString _missionResultToString(MAV_MISSION_RESULT result);
+ void _finishTransaction(bool success);
+ void _requestList(void);
+ void _writeMissionCount(void);
+ void _writeMissionItemsWorker(void);
+ void _clearAndDeleteMissionItems(void);
+ void _clearAndDeleteWriteMissionItems(void);
+ QString _lastMissionReqestString(MAV_MISSION_RESULT result);
+ void _removeAllWorker(void);
+ void _connectToMavlink(void);
+ void _disconnectFromMavlink(void);
+ QString _planTypeString(void);
+
+protected:
+ Vehicle* _vehicle;
+ MAV_MISSION_TYPE _planType;
+ LinkInterface* _dedicatedLink;
+
+ QTimer* _ackTimeoutTimer;
+ AckType_t _expectedAck;
+ int _retryCount;
+
+ TransactionType_t _transactionInProgress;
+ bool _resumeMission;
+ QList<int> _itemIndicesToWrite; ///< List of mission items which still need to be written to vehicle
+ QList<int> _itemIndicesToRead; ///< List of mission items which still need to be requested from vehicle
+ int _lastMissionRequest; ///< Index of item last requested by MISSION_REQUEST
+
+ QList<MissionItem*> _missionItems; ///< Set of mission items on vehicle
+ QList<MissionItem*> _writeMissionItems; ///< Set of mission items currently being written to vehicle
+ int _currentMissionIndex;
+ int _lastCurrentIndex;
+};
+
+#endif
diff --git a/src/MissionManager/PlanMasterController.cc b/src/MissionManager/PlanMasterController.cc
index 797bd59cc6d4e16926f1b976fd0e0a387f2ea02f..6b9e453f6f7ddf619733f5d895b2e51e79b4a6ee 100644
--- a/src/MissionManager/PlanMasterController.cc
+++ b/src/MissionManager/PlanMasterController.cc
@@ -177,8 +177,14 @@ void PlanMasterController::_loadMissionComplete(void)
if (_editMode && _loadGeoFence) {
_loadGeoFence = false;
_loadRallyPoints = true;
- qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle";
- _geoFenceController.loadFromVehicle();
+ if (_geoFenceController.supported()) {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete _geoFenceController.loadFromVehicle";
+ _geoFenceController.loadFromVehicle();
+ } else {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete GeoFence not supported skipping";
+ _geoFenceController.removeAll();
+ _loadGeoFenceComplete();
+ }
setDirty(false);
}
}
@@ -187,8 +193,14 @@ void PlanMasterController::_loadGeoFenceComplete(void)
{
if (_editMode && _loadRallyPoints) {
_loadRallyPoints = false;
- qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle";
- _rallyPointController.loadFromVehicle();
+ if (_rallyPointController.supported()) {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadGeoFenceComplete _rallyPointController.loadFromVehicle";
+ _rallyPointController.loadFromVehicle();
+ } else {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::_loadMissionComplete Rally Points not supported skipping";
+ _rallyPointController.removeAll();
+ _loadRallyPointsComplete();
+ }
setDirty(false);
}
}
@@ -204,10 +216,15 @@ void PlanMasterController::_loadRallyPointsComplete(void)
void PlanMasterController::_sendMissionComplete(void)
{
if (_sendGeoFence) {
- qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start fence sendToVehicle";
_sendGeoFence = false;
_sendRallyPoints = true;
- _geoFenceController.sendToVehicle();
+ if (_geoFenceController.supported()) {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start GeoFence sendToVehicle";
+ _geoFenceController.sendToVehicle();
+ } else {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle GeoFence not supported skipping";
+ _sendGeoFenceComplete();
+ }
setDirty(false);
}
}
@@ -215,16 +232,21 @@ void PlanMasterController::_sendMissionComplete(void)
void PlanMasterController::_sendGeoFenceComplete(void)
{
if (_sendRallyPoints) {
- qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle";
_sendRallyPoints = false;
- _rallyPointController.sendToVehicle();
+ if (_rallyPointController.supported()) {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle start rally sendToVehicle";
+ _rallyPointController.sendToVehicle();
+ } else {
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Points not support skipping";
+ _sendRallyPointsComplete();
+ }
}
}
void PlanMasterController::_sendRallyPointsComplete(void)
{
if (_syncInProgress) {
- qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle rally point send complete";
+ qCDebug(PlanMasterControllerLog) << "PlanMasterController::sendToVehicle Rally Point send complete";
_syncInProgress = false;
emit syncInProgressChanged(false);
}
@@ -361,8 +383,12 @@ void PlanMasterController::removeAllFromVehicle(void)
{
if (!offline()) {
_missionController.removeAllFromVehicle();
- _geoFenceController.removeAllFromVehicle();
- _rallyPointController.removeAllFromVehicle();
+ if (_geoFenceController.supported()) {
+ _geoFenceController.removeAllFromVehicle();
+ }
+ if (_rallyPointController.supported()) {
+ _rallyPointController.removeAllFromVehicle();
+ }
setDirty(false);
} else {
qWarning() << "PlanMasterController::removeAllFromVehicle called while offline";
diff --git a/src/MissionManager/QGCFenceCircle.cc b/src/MissionManager/QGCFenceCircle.cc
new file mode 100644
index 0000000000000000000000000000000000000000..3c1e1b658fd66ae1218c95e15f1e35cc4532a8f6
--- /dev/null
+++ b/src/MissionManager/QGCFenceCircle.cc
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ * (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 "QGCFenceCircle.h"
+#include "JsonHelper.h"
+
+const char* QGCFenceCircle::_jsonInclusionKey = "inclusion";
+
+QGCFenceCircle::QGCFenceCircle(QObject* parent)
+ : QGCMapCircle (parent)
+ , _inclusion (true)
+{
+ _init();
+}
+
+QGCFenceCircle::QGCFenceCircle(const QGeoCoordinate& center, double radius, bool inclusion, QObject* parent)
+ : QGCMapCircle (center, radius, parent)
+ , _inclusion (inclusion)
+{
+ _init();
+}
+
+QGCFenceCircle::QGCFenceCircle(const QGCFenceCircle& other, QObject* parent)
+ : QGCMapCircle (other, parent)
+ , _inclusion (other._inclusion)
+{
+ _init();
+}
+
+void QGCFenceCircle::_init(void)
+{
+ connect(this, &QGCFenceCircle::inclusionChanged, this, &QGCFenceCircle::_setDirty);
+}
+
+const QGCFenceCircle& QGCFenceCircle::operator=(const QGCFenceCircle& other)
+{
+ QGCMapCircle::operator=(other);
+
+ setInclusion(other._inclusion);
+
+ return *this;
+}
+
+void QGCFenceCircle::_setDirty(void)
+{
+ setDirty(true);
+}
+
+void QGCFenceCircle::saveToJson(QJsonObject& json)
+{
+ QGCMapCircle::saveToJson(json);
+
+ json[_jsonInclusionKey] = _inclusion;
+}
+
+bool QGCFenceCircle::loadFromJson(const QJsonObject& json, QString& errorString)
+{
+ if (!QGCMapCircle::loadFromJson(json, errorString)) {
+ return false;
+ }
+
+ errorString.clear();
+
+ QList<JsonHelper::KeyValidateInfo> keyInfoList = {
+ { _jsonInclusionKey, QJsonValue::Bool, true },
+ };
+ if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
+ return false;
+ }
+
+ setInclusion(json[_jsonInclusionKey].toBool());
+
+ return true;
+}
+
+void QGCFenceCircle::setInclusion(bool inclusion)
+{
+ if (inclusion != _inclusion) {
+ _inclusion = inclusion;
+ emit inclusionChanged(inclusion);
+ }
+}
diff --git a/src/MissionManager/QGCFenceCircle.h b/src/MissionManager/QGCFenceCircle.h
new file mode 100644
index 0000000000000000000000000000000000000000..92d6d3ea1febcb81d538c409764f668d3f12aa89
--- /dev/null
+++ b/src/MissionManager/QGCFenceCircle.h
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "QGCMapCircle.h"
+
+/// The QGCFenceCircle class provides a cicle used by GeoFence support.
+class QGCFenceCircle : public QGCMapCircle
+{
+ Q_OBJECT
+
+public:
+ QGCFenceCircle(QObject* parent = NULL);
+ QGCFenceCircle(const QGeoCoordinate& center, double radius, bool inclusion, QObject* parent = NULL);
+ QGCFenceCircle(const QGCFenceCircle& other, QObject* parent = NULL);
+
+ const QGCFenceCircle& operator=(const QGCFenceCircle& other);
+
+ Q_PROPERTY(bool inclusion READ inclusion WRITE setInclusion NOTIFY inclusionChanged)
+
+ /// Saves the QGCFenceCircle to the json object.
+ /// @param json Json object to save to
+ void saveToJson(QJsonObject& json);
+
+ /// Load a QGCFenceCircle from json
+ /// @param json Json object to load from
+ /// @param errorString Error string if return is false
+ /// @return true: success, false: failure (errorString set)
+ bool loadFromJson(const QJsonObject& json, QString& errorString);
+
+ // Property methods
+
+ bool inclusion (void) const { return _inclusion; }
+ void setInclusion (bool inclusion);
+
+signals:
+ void inclusionChanged(bool inclusion);
+
+private slots:
+ void _setDirty(void);
+
+private:
+ void _init(void);
+
+ bool _inclusion;
+
+ static const char* _jsonInclusionKey;
+};
diff --git a/src/MissionManager/QGCFencePolygon.cc b/src/MissionManager/QGCFencePolygon.cc
new file mode 100644
index 0000000000000000000000000000000000000000..1524b67f5c5060da61a0d76c697f008002204ce1
--- /dev/null
+++ b/src/MissionManager/QGCFencePolygon.cc
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * (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 "QGCFencePolygon.h"
+#include "JsonHelper.h"
+
+const char* QGCFencePolygon::_jsonInclusionKey = "inclusion";
+
+QGCFencePolygon::QGCFencePolygon(bool inclusion, QObject* parent)
+ : QGCMapPolygon (parent)
+ , _inclusion (inclusion)
+{
+ _init();
+}
+
+QGCFencePolygon::QGCFencePolygon(const QGCFencePolygon& other, QObject* parent)
+ : QGCMapPolygon (other, parent)
+ , _inclusion (other._inclusion)
+{
+ _init();
+}
+
+void QGCFencePolygon::_init(void)
+{
+ connect(this, &QGCFencePolygon::inclusionChanged, this, &QGCFencePolygon::_setDirty);
+}
+
+const QGCFencePolygon& QGCFencePolygon::operator=(const QGCFencePolygon& other)
+{
+ QGCMapPolygon::operator=(other);
+
+ setInclusion(other._inclusion);
+
+ return *this;
+}
+
+void QGCFencePolygon::_setDirty(void)
+{
+ setDirty(true);
+}
+
+void QGCFencePolygon::saveToJson(QJsonObject& json)
+{
+ QGCMapPolygon::saveToJson(json);
+
+ json[_jsonInclusionKey] = _inclusion;
+}
+
+bool QGCFencePolygon::loadFromJson(const QJsonObject& json, bool required, QString& errorString)
+{
+ if (!QGCMapPolygon::loadFromJson(json, required, errorString)) {
+ return false;
+ }
+
+ errorString.clear();
+
+ QList<JsonHelper::KeyValidateInfo> keyInfoList = {
+ { _jsonInclusionKey, QJsonValue::Bool, true },
+ };
+ if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
+ return false;
+ }
+
+ setInclusion(json[_jsonInclusionKey].toBool());
+
+ return true;
+}
+
+void QGCFencePolygon::setInclusion(bool inclusion)
+{
+ if (inclusion != _inclusion) {
+ _inclusion = inclusion;
+ emit inclusionChanged(inclusion);
+ }
+}
diff --git a/src/MissionManager/QGCFencePolygon.h b/src/MissionManager/QGCFencePolygon.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c8ed56398da2ffac11a519dcdb4cf3d7e4506d0
--- /dev/null
+++ b/src/MissionManager/QGCFencePolygon.h
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "QGCMapPolygon.h"
+
+/// The QGCFencePolygon class provides a polygon used by GeoFence support.
+class QGCFencePolygon : public QGCMapPolygon
+{
+ Q_OBJECT
+
+public:
+ QGCFencePolygon(bool inclusion, QObject* parent = NULL);
+ QGCFencePolygon(const QGCFencePolygon& other, QObject* parent = NULL);
+
+ const QGCFencePolygon& operator=(const QGCFencePolygon& other);
+
+ Q_PROPERTY(bool inclusion READ inclusion WRITE setInclusion NOTIFY inclusionChanged)
+
+ /// Saves the QGCFencePolygon to the json object.
+ /// @param json Json object to save to
+ void saveToJson(QJsonObject& json);
+
+ /// Load a QGCFencePolygon from json
+ /// @param json Json object to load from
+ /// @param required true: no polygon in object will generate error
+ /// @param errorString Error string if return is false
+ /// @return true: success, false: failure (errorString set)
+ bool loadFromJson(const QJsonObject& json, bool required, QString& errorString);
+
+ // Property methods
+
+ bool inclusion (void) const { return _inclusion; }
+ void setInclusion (bool inclusion);
+
+signals:
+ void inclusionChanged (bool inclusion);
+
+private slots:
+ void _setDirty(void);
+
+private:
+ void _init(void);
+
+ bool _inclusion;
+
+ static const char* _jsonInclusionKey;
+};
diff --git a/src/MissionManager/QGCMapCircle.Facts.json b/src/MissionManager/QGCMapCircle.Facts.json
new file mode 100644
index 0000000000000000000000000000000000000000..f2ded5e5efea285b70279064ba7ed926e4d11456
--- /dev/null
+++ b/src/MissionManager/QGCMapCircle.Facts.json
@@ -0,0 +1,10 @@
+[
+{
+ "name": "Radius",
+ "shortDescription": "Radius for geofence circle.",
+ "type": "double",
+ "decimalPlaces": 2,
+ "min": 0.1,
+ "units": "m"
+}
+]
diff --git a/src/MissionManager/QGCMapCircle.cc b/src/MissionManager/QGCMapCircle.cc
new file mode 100644
index 0000000000000000000000000000000000000000..143956c8e25cf9f8b6221d8ba88ed9771d845bb9
--- /dev/null
+++ b/src/MissionManager/QGCMapCircle.cc
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ * (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 "QGCMapCircle.h"
+#include "QGCGeo.h"
+#include "JsonHelper.h"
+#include "QGCQGeoCoordinate.h"
+
+#include <QGeoRectangle>
+#include <QDebug>
+#include <QJsonArray>
+
+const char* QGCMapCircle::jsonCircleKey = "circle";
+const char* QGCMapCircle::_jsonCenterKey = "center";
+const char* QGCMapCircle::_jsonRadiusKey = "radius";
+const char* QGCMapCircle::_radiusFactName = "Radius";
+
+QGCMapCircle::QGCMapCircle(QObject* parent)
+ : QObject (parent)
+ , _dirty (false)
+ , _interactive (false)
+{
+ _init();
+}
+
+QGCMapCircle::QGCMapCircle(const QGeoCoordinate& center, double radius, QObject* parent)
+ : QObject (parent)
+ , _dirty (false)
+ , _center (center)
+ , _radius (FactSystem::defaultComponentId, _radiusFactName, FactMetaData::valueTypeDouble)
+ , _interactive (false)
+{
+ _radius.setRawValue(radius);
+ _init();
+}
+
+QGCMapCircle::QGCMapCircle(const QGCMapCircle& other, QObject* parent)
+ : QObject (parent)
+ , _dirty (false)
+ , _center (other._center)
+ , _radius (FactSystem::defaultComponentId, _radiusFactName, FactMetaData::valueTypeDouble)
+ , _interactive (false)
+{
+ _radius.setRawValue(other._radius.rawValue());
+ _init();
+}
+
+const QGCMapCircle& QGCMapCircle::operator=(const QGCMapCircle& other)
+{
+ setCenter(other._center);
+ _radius.setRawValue(other._radius.rawValue());
+ setDirty(true);
+
+ return *this;
+}
+
+void QGCMapCircle::_init(void)
+{
+ _nameToMetaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/QGCMapCircle.Facts.json"), this);
+ _radius.setMetaData(_nameToMetaDataMap[_radiusFactName]);
+
+ connect(this, &QGCMapCircle::centerChanged, this, &QGCMapCircle::_setDirty);
+ connect(&_radius, &Fact::rawValueChanged, this, &QGCMapCircle::_setDirty);
+}
+
+void QGCMapCircle::setDirty(bool dirty)
+{
+ if (_dirty != dirty) {
+ _dirty = dirty;
+ emit dirtyChanged(dirty);
+ }
+}
+
+void QGCMapCircle::saveToJson(QJsonObject& json)
+{
+ QJsonValue jsonValue;
+ QJsonObject circleObject;
+
+ JsonHelper::saveGeoCoordinate(_center, false /* writeAltitude*/, jsonValue);
+ circleObject.insert(_jsonCenterKey, jsonValue);
+ circleObject.insert(_jsonRadiusKey, _radius.rawValue().toDouble());
+
+ json.insert(jsonCircleKey, circleObject);
+}
+
+bool QGCMapCircle::loadFromJson(const QJsonObject& json, QString& errorString)
+{
+ errorString.clear();
+
+ QList<JsonHelper::KeyValidateInfo> circleKeyInfo = {
+ { jsonCircleKey, QJsonValue::Object, true },
+ };
+ if (!JsonHelper::validateKeys(json, circleKeyInfo, errorString)) {
+ return false;
+ }
+
+ QJsonObject circleObject = json[jsonCircleKey].toObject();
+
+ QList<JsonHelper::KeyValidateInfo> circleObjectKeyInfo = {
+ { _jsonCenterKey, QJsonValue::Array, true },
+ { _jsonRadiusKey, QJsonValue::Double, true },
+ };
+ if (!JsonHelper::validateKeys(circleObject, circleObjectKeyInfo, errorString)) {
+ return false;
+ }
+
+ QGeoCoordinate center;
+ if (!JsonHelper::loadGeoCoordinate(json[_jsonCenterKey], false /* altitudeRequired */, center, errorString)) {
+ return false;
+ }
+ setCenter(center);
+ _radius.setRawValue(json[_jsonRadiusKey].toDouble());
+
+ return true;
+}
+
+void QGCMapCircle::setCenter(QGeoCoordinate newCenter)
+{
+ if (newCenter != _center) {
+ _center = newCenter;
+ setDirty(true);
+ emit centerChanged(newCenter);
+ }
+}
+
+void QGCMapCircle::_setDirty(void)
+{
+ setDirty(true);
+}
+
+void QGCMapCircle::setInteractive(bool interactive)
+{
+ if (_interactive != interactive) {
+ _interactive = interactive;
+ emit interactiveChanged(interactive);
+ }
+}
+
diff --git a/src/MissionManager/QGCMapCircle.h b/src/MissionManager/QGCMapCircle.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1f1e8184a5e808f3b2ae5f0c5c7aec7946ad2f8
--- /dev/null
+++ b/src/MissionManager/QGCMapCircle.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <QObject>
+#include <QGeoCoordinate>
+#include <QVariantList>
+#include <QPolygon>
+
+#include "QmlObjectListModel.h"
+#include "FactSystem.h"
+
+/// The QGCMapCircle represents a circular area which can be displayed on a Map control.
+class QGCMapCircle : public QObject
+{
+ Q_OBJECT
+
+public:
+ QGCMapCircle(QObject* parent = NULL);
+ QGCMapCircle(const QGeoCoordinate& center, double radius, QObject* parent = NULL);
+ QGCMapCircle(const QGCMapCircle& other, QObject* parent = NULL);
+
+ const QGCMapCircle& operator=(const QGCMapCircle& other);
+
+ Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged)
+ Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged)
+ Q_PROPERTY(Fact* radius READ radius CONSTANT)
+ Q_PROPERTY(bool interactive READ interactive WRITE setInteractive NOTIFY interactiveChanged)
+
+ /// Saves the polygon to the json object.
+ /// @param json Json object to save to
+ void saveToJson(QJsonObject& json);
+
+ /// Load a circle from json
+ /// @param json Json object to load from
+ /// @param errorString Error string if return is false
+ /// @return true: success, false: failure (errorString set)
+ bool loadFromJson(const QJsonObject& json, QString& errorString);
+
+ // Property methods
+
+ bool dirty (void) const { return _dirty; }
+ QGeoCoordinate center (void) const { return _center; }
+ Fact* radius (void) { return &_radius; }
+ bool interactive (void) const { return _interactive; }
+
+ void setDirty (bool dirty);
+ void setCenter (QGeoCoordinate newCenter);
+ void setInteractive (bool interactive);
+
+ static const char* jsonCircleKey;
+
+signals:
+ void dirtyChanged (bool dirty);
+ void centerChanged (QGeoCoordinate center);
+ void interactiveChanged (bool interactive);
+
+private slots:
+ void _setDirty(void);
+
+private:
+ void _init(void);
+
+ bool _dirty;
+ QGeoCoordinate _center;
+ Fact _radius;
+ bool _interactive;
+
+ QMap<QString, FactMetaData*> _nameToMetaDataMap;
+
+ static const char* _jsonCenterKey;
+ static const char* _jsonRadiusKey;
+ static const char* _radiusFactName;
+};
diff --git a/src/MissionManager/QGCMapCircleVisuals.qml b/src/MissionManager/QGCMapCircleVisuals.qml
new file mode 100644
index 0000000000000000000000000000000000000000..c7388a38f5405176d50192c405184692babc7a18
--- /dev/null
+++ b/src/MissionManager/QGCMapCircleVisuals.qml
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ * (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.
+ *
+ ****************************************************************************/
+
+import QtQuick 2.3
+import QtQuick.Controls 1.2
+import QtLocation 5.3
+import QtPositioning 5.3
+
+import QGroundControl 1.0
+import QGroundControl.ScreenTools 1.0
+import QGroundControl.Palette 1.0
+import QGroundControl.Controls 1.0
+import QGroundControl.FlightMap 1.0
+
+/// QGCMapCircle map visuals
+Item {
+ id: _root
+
+ property var mapControl ///< Map control to place item in
+ property var mapCircle ///< QGCMapCircle object
+ property bool interactive: mapCircle.interactive /// true: user can manipulate polygon
+ property color interiorColor: "transparent"
+ property real interiorOpacity: 1
+ property int borderWidth: 0
+ property color borderColor: "black"
+
+ property var _circleComponent
+ property var _centerDragHandleComponent
+
+ function addVisuals() {
+ _circleComponent = circleComponent.createObject(mapControl)
+ mapControl.addMapItem(_circleComponent)
+ }
+
+ function removeVisuals() {
+ _circleComponent.destroy()
+ }
+
+ function addHandles() {
+ if (!_centerDragHandleComponent) {
+ _centerDragHandleComponent = centerDragHandleComponent.createObject(mapControl)
+ }
+ }
+
+ function removeHandles() {
+ if (_centerDragHandleComponent) {
+ _centerDragHandleComponent.destroy()
+ _centerDragHandleComponent = undefined
+ }
+ }
+
+ onInteractiveChanged: {
+ if (interactive) {
+ addHandles()
+ } else {
+ removeHandles()
+ }
+ }
+
+ Component.onCompleted: {
+ addVisuals()
+ if (interactive) {
+ addHandles()
+ }
+ }
+
+ Component.onDestruction: {
+ removeVisuals()
+ removeHandles()
+ }
+
+ Component {
+ id: circleComponent
+
+ MapCircle {
+ color: interiorColor
+ opacity: interiorOpacity
+ border.color: borderColor
+ border.width: borderWidth
+ center: mapCircle.center
+ radius: mapCircle.radius.rawValue
+ }
+ }
+
+ Component {
+ id: dragHandleComponent
+
+ MapQuickItem {
+ id: mapQuickItem
+ anchorPoint.x: dragHandle.width / 2
+ anchorPoint.y: dragHandle.height / 2
+ z: QGroundControl.zOrderMapItems + 2
+
+ sourceItem: Rectangle {
+ id: dragHandle
+ width: ScreenTools.defaultFontPixelHeight * 1.5
+ height: width
+ radius: width / 2
+ color: "white"
+ opacity: .90
+ }
+ }
+ }
+
+ Component {
+ id: centerDragAreaComponent
+
+ MissionItemIndicatorDrag {
+ onItemCoordinateChanged: mapCircle.center = itemCoordinate
+ }
+ }
+
+ Component {
+ id: centerDragHandleComponent
+
+ Item {
+ property var dragHandle
+ property var dragArea
+
+ Component.onCompleted: {
+ dragHandle = dragHandleComponent.createObject(mapControl)
+ dragHandle.coordinate = Qt.binding(function() { return mapCircle.center })
+ mapControl.addMapItem(dragHandle)
+ dragArea = centerDragAreaComponent.createObject(mapControl, { "itemIndicator": dragHandle, "itemCoordinate": mapCircle.center })
+ }
+
+ Component.onDestruction: {
+ dragHandle.destroy()
+ dragArea.destroy()
+ }
+ }
+ }
+}
+
diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc
index a7c5ca9ae01194587dfaa5dce7628b84c5e1c6c3..aef9372ab181be432975d30b04e182f8e396caed 100644
--- a/src/MissionManager/QGCMapPolygon.cc
+++ b/src/MissionManager/QGCMapPolygon.cc
@@ -18,18 +18,49 @@
const char* QGCMapPolygon::jsonPolygonKey = "polygon";
-QGCMapPolygon::QGCMapPolygon(QObject* newCoordParent, QObject* parent)
- : QObject(parent)
- , _newCoordParent(newCoordParent)
- , _dirty(false)
- , _centerDrag(false)
- , _ignoreCenterUpdates(false)
+QGCMapPolygon::QGCMapPolygon(QObject* parent)
+ : QObject (parent)
+ , _dirty (false)
+ , _centerDrag (false)
+ , _ignoreCenterUpdates (false)
+ , _interactive (false)
+{
+ _init();
+}
+
+QGCMapPolygon::QGCMapPolygon(const QGCMapPolygon& other, QObject* parent)
+ : QObject (parent)
+ , _dirty (false)
+ , _centerDrag (false)
+ , _ignoreCenterUpdates (false)
+ , _interactive (false)
+{
+ *this = other;
+
+ _init();
+}
+
+void QGCMapPolygon::_init(void)
{
connect(&_polygonModel, &QmlObjectListModel::dirtyChanged, this, &QGCMapPolygon::_polygonModelDirtyChanged);
connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_polygonModelCountChanged);
connect(&_polygonModel, &QmlObjectListModel::countChanged, this, &QGCMapPolygon::_updateCenter);
}
+const QGCMapPolygon& QGCMapPolygon::operator=(const QGCMapPolygon& other)
+{
+ clear();
+
+ QVariantList vertices = other.path();
+ for (int i=0; i<vertices.count(); i++) {
+ appendVertex(vertices[i].value<QGeoCoordinate>());
+ }
+
+ setDirty(true);
+
+ return *this;
+}
+
void QGCMapPolygon::clear(void)
{
// Bug workaround, see below
@@ -132,7 +163,7 @@ void QGCMapPolygon::setPath(const QList<QGeoCoordinate>& path)
_polygonModel.clearAndDeleteContents();
foreach(const QGeoCoordinate& coord, path) {
_polygonPath.append(QVariant::fromValue(coord));
- _polygonModel.append(new QGCQGeoCoordinate(coord, _newCoordParent));
+ _polygonModel.append(new QGCQGeoCoordinate(coord, this));
}
setDirty(true);
@@ -145,7 +176,7 @@ void QGCMapPolygon::setPath(const QVariantList& path)
_polygonModel.clearAndDeleteContents();
for (int i=0; i<_polygonPath.count(); i++) {
- _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), _newCoordParent));
+ _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), this));
}
setDirty(true);
@@ -179,7 +210,7 @@ bool QGCMapPolygon::loadFromJson(const QJsonObject& json, bool required, QString
}
for (int i=0; i<_polygonPath.count(); i++) {
- _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), _newCoordParent));
+ _polygonModel.append(new QGCQGeoCoordinate(_polygonPath[i].value<QGeoCoordinate>(), this));
}
setDirty(false);
@@ -225,7 +256,7 @@ void QGCMapPolygon::splitPolygonSegment(int vertexIndex)
void QGCMapPolygon::appendVertex(const QGeoCoordinate& coordinate)
{
_polygonPath.append(QVariant::fromValue(coordinate));
- _polygonModel.append(new QGCQGeoCoordinate(coordinate, _newCoordParent));
+ _polygonModel.append(new QGCQGeoCoordinate(coordinate, this));
emit pathChanged();
}
@@ -315,3 +346,11 @@ void QGCMapPolygon::setCenterDrag(bool centerDrag)
emit centerDragChanged(centerDrag);
}
}
+
+void QGCMapPolygon::setInteractive(bool interactive)
+{
+ if (_interactive != interactive) {
+ _interactive = interactive;
+ emit interactiveChanged(interactive);
+ }
+}
diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h
index 445df1e7ae404ae39ffc67a574879f133ee02af3..b2990dbe3f11ec4d59754871860db04f1875ee25 100644
--- a/src/MissionManager/QGCMapPolygon.h
+++ b/src/MissionManager/QGCMapPolygon.h
@@ -24,14 +24,18 @@ class QGCMapPolygon : public QObject
Q_OBJECT
public:
- QGCMapPolygon(QObject* newCoordParent, QObject* parent = NULL);
+ QGCMapPolygon(QObject* parent = NULL);
+ QGCMapPolygon(const QGCMapPolygon& other, QObject* parent = NULL);
- Q_PROPERTY(int count READ count NOTIFY countChanged)
- Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged)
- Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT)
- Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged)
- Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged)
- Q_PROPERTY(bool centerDrag READ centerDrag WRITE setCenterDrag NOTIFY centerDragChanged)
+ const QGCMapPolygon& operator=(const QGCMapPolygon& other);
+
+ Q_PROPERTY(int count READ count NOTIFY countChanged)
+ Q_PROPERTY(QVariantList path READ path NOTIFY pathChanged)
+ Q_PROPERTY(QmlObjectListModel* pathModel READ qmlPathModel CONSTANT)
+ Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged)
+ Q_PROPERTY(QGeoCoordinate center READ center WRITE setCenter NOTIFY centerChanged)
+ Q_PROPERTY(bool centerDrag READ centerDrag WRITE setCenterDrag NOTIFY centerDragChanged)
+ Q_PROPERTY(bool interactive READ interactive WRITE setInteractive NOTIFY interactiveChanged)
Q_INVOKABLE void clear(void);
Q_INVOKABLE void appendVertex(const QGeoCoordinate& coordinate);
@@ -69,6 +73,7 @@ public:
void setDirty (bool dirty);
QGeoCoordinate center (void) const { return _center; }
bool centerDrag (void) const { return _centerDrag; }
+ bool interactive (void) const { return _interactive; }
QVariantList path (void) const { return _polygonPath; }
QmlObjectListModel* qmlPathModel(void) { return &_polygonModel; }
@@ -78,6 +83,7 @@ public:
void setPath (const QVariantList& path);
void setCenter (QGeoCoordinate newCenter);
void setCenterDrag (bool centerDrag);
+ void setInteractive (bool interactive);
static const char* jsonPolygonKey;
@@ -88,6 +94,7 @@ signals:
void cleared (void);
void centerChanged (QGeoCoordinate center);
void centerDragChanged (bool centerDrag);
+ void interactiveChanged (bool interactive);
private slots:
void _polygonModelCountChanged(int count);
@@ -95,17 +102,18 @@ private slots:
void _updateCenter(void);
private:
+ void _init(void);
QPolygonF _toPolygonF(void) const;
QGeoCoordinate _coordFromPointF(const QPointF& point) const;
QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const;
- QObject* _newCoordParent;
QVariantList _polygonPath;
QmlObjectListModel _polygonModel;
bool _dirty;
QGeoCoordinate _center;
bool _centerDrag;
bool _ignoreCenterUpdates;
+ bool _interactive;
};
#endif
diff --git a/src/MissionManager/QGCMapPolygonTest.cc b/src/MissionManager/QGCMapPolygonTest.cc
index f6871872aba166c1a0d1a095eef8368fae1b2838..4f366d8c2084f7a8c035d77bacd1dc5a53ecadfe 100644
--- a/src/MissionManager/QGCMapPolygonTest.cc
+++ b/src/MissionManager/QGCMapPolygonTest.cc
@@ -32,7 +32,7 @@ void QGCMapPolygonTest::init(void)
_rgModelSignals[modelCountChangedIndex] = SIGNAL(countChanged(int));
_rgModelSignals[modelDirtyChangedIndex] = SIGNAL(dirtyChanged(bool));
- _mapPolygon = new QGCMapPolygon(this, this);
+ _mapPolygon = new QGCMapPolygon(this);
_pathModel = _mapPolygon->qmlPathModel();
QVERIFY(_pathModel);
diff --git a/src/MissionManager/QGCMapPolygonVisuals.qml b/src/MissionManager/QGCMapPolygonVisuals.qml
index 5d630a7a27cdc33ec6fe2f5a5ef3f3c7b711e6d4..36496a4023a26d98e8da99f35a225241cb8ffd4e 100644
--- a/src/MissionManager/QGCMapPolygonVisuals.qml
+++ b/src/MissionManager/QGCMapPolygonVisuals.qml
@@ -24,16 +24,16 @@ Item {
property var mapControl ///< Map control to place item in
property var mapPolygon ///< QGCMapPolygon object
- property bool interactive: true /// true: user can manipulate polygon
+ property bool interactive: mapPolygon.interactive
property color interiorColor: "transparent"
property real interiorOpacity: 1
property int borderWidth: 0
property color borderColor: "black"
- property var _polygonComponent
- property var _dragHandlesComponent
- property var _splitHandlesComponent
- property var _centerDragHandleComponent
+ property var _polygonComponent
+ property var _dragHandlesComponent
+ property var _splitHandlesComponent
+ property var _centerDragHandleComponent
property real _zorderDragHandle: QGroundControl.zOrderMapItems + 3 // Highest to prevent splitting when items overlap
property real _zorderSplitHandle: QGroundControl.zOrderMapItems + 2
@@ -111,7 +111,7 @@ Item {
}
Component.onCompleted: {
- mapPolygonVisuals.addVisuals()
+ addVisuals()
if (interactive) {
addHandles()
}
diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc
index c719e11f7ac0d8af22e54ed213665247e778056c..bc88fdce93efa183f0aa5fa6cf744dc5c18f419e 100644
--- a/src/MissionManager/RallyPointController.cc
+++ b/src/MissionManager/RallyPointController.cc
@@ -58,6 +58,7 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
{
if (_managerVehicle) {
_rallyPointManager->disconnect(this);
+ _managerVehicle->disconnect(this);
_managerVehicle = NULL;
_rallyPointManager = NULL;
}
@@ -74,7 +75,9 @@ void RallyPointController::managerVehicleChanged(Vehicle* managerVehicle)
connect(_rallyPointManager, &RallyPointManager::removeAllComplete, this, &RallyPointController::_managerRemoveAllComplete);
connect(_rallyPointManager, &RallyPointManager::inProgressChanged, this, &RallyPointController::syncInProgressChanged);
- emit rallyPointsSupportedChanged(rallyPointsSupported());
+ connect(_managerVehicle, &Vehicle::capabilityBitsChanged, this, &RallyPointController::supportedChanged);
+
+ emit supportedChanged(supported());
}
bool RallyPointController::load(const QJsonObject& json, QString& errorString)
@@ -234,9 +237,9 @@ void RallyPointController::addPoint(QGeoCoordinate point)
setDirty(true);
}
-bool RallyPointController::rallyPointsSupported(void) const
+bool RallyPointController::supported(void) const
{
- return _rallyPointManager->rallyPointsSupported();
+ return (_managerVehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_managerVehicle->maxProtoVersion() >= 200);
}
void RallyPointController::removePoint(QObject* rallyPoint)
diff --git a/src/MissionManager/RallyPointController.h b/src/MissionManager/RallyPointController.h
index 7a2c945817ebe07d43ca7620d8c6439578a11534..7df2dc79e8ad56cb1a5dc77ba67a10e153c2ed3a 100644
--- a/src/MissionManager/RallyPointController.h
+++ b/src/MissionManager/RallyPointController.h
@@ -29,7 +29,6 @@ public:
RallyPointController(PlanMasterController* masterController, QObject* parent = NULL);
~RallyPointController();
- Q_PROPERTY(bool rallyPointsSupported READ rallyPointsSupported NOTIFY rallyPointsSupportedChanged)
Q_PROPERTY(QmlObjectListModel* points READ points CONSTANT)
Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
Q_PROPERTY(QObject* currentRallyPoint READ currentRallyPoint WRITE setCurrentRallyPoint NOTIFY currentRallyPointChanged)
@@ -37,6 +36,7 @@ public:
Q_INVOKABLE void addPoint(QGeoCoordinate point);
Q_INVOKABLE void removePoint(QObject* rallyPoint);
+ bool supported (void) const final;
void save (QJsonObject& json) final;
bool load (const QJsonObject& json, QString& errorString) final;
void loadFromVehicle (void) final;
@@ -50,7 +50,6 @@ public:
void managerVehicleChanged (Vehicle* managerVehicle) final;
bool showPlanFromManagerVehicle (void) final;
- bool rallyPointsSupported (void) const;
QmlObjectListModel* points (void) { return &_points; }
QString editorQml (void) const;
QObject* currentRallyPoint (void) const { return _currentRallyPoint; }
@@ -58,7 +57,6 @@ public:
void setCurrentRallyPoint(QObject* rallyPoint);
signals:
- void rallyPointsSupportedChanged(bool rallyPointsSupported);
void currentRallyPointChanged(QObject* rallyPoint);
void loadComplete(void);
diff --git a/src/MissionManager/RallyPointManager.cc b/src/MissionManager/RallyPointManager.cc
index d101b4e890bb78fa30cc00b8aac8814b5c77ebe4..072ac4ae0d8d96157b507197d815e224be5d843d 100644
--- a/src/MissionManager/RallyPointManager.cc
+++ b/src/MissionManager/RallyPointManager.cc
@@ -49,3 +49,8 @@ void RallyPointManager::removeAll(void)
// No support in generic vehicle
emit removeAllComplete(false /* error */);
}
+
+bool RallyPointManager::supported(void) const
+{
+ return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200);
+}
diff --git a/src/MissionManager/RallyPointManager.h b/src/MissionManager/RallyPointManager.h
index db46f18fe39cea75b0b9503959d20c621f50337d..cf9e1747d502c331912383aa726f74fd84743884 100644
--- a/src/MissionManager/RallyPointManager.h
+++ b/src/MissionManager/RallyPointManager.h
@@ -29,6 +29,9 @@ public:
RallyPointManager(Vehicle* vehicle);
~RallyPointManager();
+ /// Returns true if GeoFence is supported by this vehicle
+ virtual bool supported(void) const;
+
/// Returns true if the manager is currently communicating with the vehicle
virtual bool inProgress(void) const { return false; }
@@ -44,8 +47,6 @@ public:
/// Signals removeAllCompleted when done
virtual void removeAll(void);
- virtual bool rallyPointsSupported(void) const { return false; }
-
QList<QGeoCoordinate> points(void) const { return _rgPoints; }
virtual QString editorQml(void) const { return QStringLiteral("qrc:/FirmwarePlugin/RallyPointEditor.qml"); }
diff --git a/src/PlanView/GeoFenceEditor.qml b/src/PlanView/GeoFenceEditor.qml
index 114f85ef348dd62b36fb17687bc1ea23689bc8e9..dd592b5fe6ef150d52a78da19cbf77468424d466 100644
--- a/src/PlanView/GeoFenceEditor.qml
+++ b/src/PlanView/GeoFenceEditor.qml
@@ -1,5 +1,6 @@
import QtQuick 2.3
import QtQuick.Controls 1.2
+import QtQuick.Layouts 1.2
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
@@ -10,8 +11,8 @@ import QGroundControl.FactControls 1.0
QGCFlickable {
id: root
width: availableWidth
- height: Math.min(availableHeight, geoFenceEditorRect.height)
- contentHeight: geoFenceEditorRect.height
+ height: availableHeight
+ contentHeight: editorColumn.height
clip: true
property real availableWidth
@@ -23,99 +24,317 @@ QGCFlickable {
readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2
readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2
- property var polygon: myGeoFenceController.polygon
+ Column {
+ id: editorColumn
+ anchors.left: parent.left
+ anchors.right: parent.right
- Rectangle {
- id: geoFenceEditorRect
- width: parent.width
- height: geoFenceItems.y + geoFenceItems.height + (_margin * 2)
- radius: _radius
- color: qgcPal.missionItemEditor
+ Rectangle {
+ id: geoFenceEditorRect
+ width: parent.width
+ height: geoFenceItems.y + geoFenceItems.height + (_margin * 2)
+ radius: _radius
+ color: qgcPal.missionItemEditor
- QGCLabel {
- id: geoFenceLabel
- anchors.margins: _margin
- anchors.left: parent.left
- anchors.top: parent.top
- text: qsTr("GeoFence")
- color: "black"
- }
+ QGCLabel {
+ id: geoFenceLabel
+ anchors.margins: _margin
+ anchors.left: parent.left
+ anchors.top: parent.top
+ text: qsTr("GeoFence")
+ color: "black"
+ }
- Rectangle {
- id: geoFenceItems
- anchors.margins: _margin
- anchors.left: parent.left
- anchors.right: parent.right
- anchors.top: geoFenceLabel.bottom
- height: fenceColumn.y + fenceColumn.height + (_margin * 2)
- color: qgcPal.windowShadeDark
- radius: _radius
-
- Column {
- id: fenceColumn
+ Rectangle {
+ id: geoFenceItems
anchors.margins: _margin
anchors.left: parent.left
anchors.right: parent.right
- spacing: ScreenTools.defaultFontPixelHeight / 2
+ anchors.top: geoFenceLabel.bottom
+ height: fenceColumn.y + fenceColumn.height + (_margin * 2)
+ color: qgcPal.windowShadeDark
+ radius: _radius
- QGCLabel {
- id: geoLabel
+ Column {
+ id: fenceColumn
+ anchors.margins: _margin
+ anchors.top: parent.top
anchors.left: parent.left
anchors.right: parent.right
- wrapMode: Text.WordWrap
- font.pointSize: ScreenTools.smallFontPointSize
- text: qsTr("GeoFencing allows you to set a virtual ‘fence’ around the area you want to fly in.")
- }
+ spacing: ScreenTools.defaultFontPixelHeight / 2
- Repeater {
- model: myGeoFenceController.params
+ QGCLabel {
+ anchors.left: parent.left
+ anchors.right: parent.right
+ wrapMode: Text.WordWrap
+ font.pointSize: myGeoFenceController.supported ? ScreenTools.smallFontPointSize : ScreenTools.defaultFontPointSize
+ text: myGeoFenceController.supported ?
+ qsTr("GeoFencing allows you to set a virtual ‘fence’ around the area you want to fly in.") :
+ qsTr("This vehicle does not support GeoFence.")
+ }
- Item {
- width: fenceColumn.width
- height: textField.height
+ Column {
+ anchors.left: parent.left
+ anchors.right: parent.right
+ spacing: ScreenTools.defaultFontPixelHeight / 2
+ visible: myGeoFenceController.supported
- property bool showCombo: modelData.enumStrings.length > 0
+ Repeater {
+ model: myGeoFenceController.params
- QGCLabel {
- id: textFieldLabel
- anchors.baseline: textField.baseline
- text: myGeoFenceController.paramLabels[index]
+ Item {
+ width: fenceColumn.width
+ height: textField.height
+
+ property bool showCombo: modelData.enumStrings.length > 0
+
+ QGCLabel {
+ id: textFieldLabel
+ anchors.baseline: textField.baseline
+ text: myGeoFenceController.paramLabels[index]
+ }
+
+ FactTextField {
+ id: textField
+ anchors.right: parent.right
+ width: _editFieldWidth
+ showUnits: true
+ fact: modelData
+ visible: !parent.showCombo
+ }
+
+ FactComboBox {
+ id: comboField
+ anchors.right: parent.right
+ width: _editFieldWidth
+ indexModel: false
+ fact: showCombo ? modelData : _nullFact
+ visible: parent.showCombo
+
+ property var _nullFact: Fact { }
+ }
+ }
+ }
+
+ SectionHeader {
+ id: insertSection
+ text: qsTr("Insert GeoFence")
}
- FactTextField {
- id: textField
+ QGCButton {
+ anchors.left: parent.left
anchors.right: parent.right
- width: _editFieldWidth
- showUnits: true
- fact: modelData
- visible: !parent.showCombo
+ text: qsTr("Polygon Fence")
+
+ onClicked: {
+ var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height)
+ var topLeftCoord = flightMap.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */)
+ var bottomRightCoord = flightMap.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */)
+ myGeoFenceController.addInclusionPolygon(topLeftCoord, bottomRightCoord)
+ }
}
- FactComboBox {
- id: comboField
+ QGCButton {
+ anchors.left: parent.left
anchors.right: parent.right
- width: _editFieldWidth
- indexModel: false
- fact: showCombo ? modelData : _nullFact
- visible: parent.showCombo
+ text: qsTr("Circular Fence")
- property var _nullFact: Fact { }
+ onClicked: {
+ var rect = Qt.rect(flightMap.centerViewport.x, flightMap.centerViewport.y, flightMap.centerViewport.width, flightMap.centerViewport.height)
+ var topLeftCoord = flightMap.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */)
+ var bottomRightCoord = flightMap.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */)
+ myGeoFenceController.addInclusionCircle(topLeftCoord, bottomRightCoord)
+ }
}
- }
- }
- QGCButton {
- text: qsTr("Add fence polygon")
- visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count === 0
- onClicked: myGeoFenceController.addPolygon()
- }
+ SectionHeader {
+ id: polygonSection
+ text: qsTr("Polygon Fences")
+ }
+
+ QGCLabel {
+ text: qsTr("None")
+ visible: polygonSection.checked && myGeoFenceController.polygons.count == 0
+ }
+
+ GridLayout {
+ anchors.left: parent.left
+ anchors.right: parent.right
+ columns: 3
+ flow: GridLayout.TopToBottom
+ visible: polygonSection.checked && myGeoFenceController.polygons.count > 0
+
+ QGCLabel {
+ text: qsTr("Inclusion")
+ Layout.column: 0
+ Layout.alignment: Qt.AlignHCenter
+ }
+
+ Repeater {
+ model: myGeoFenceController.polygons
+
+ QGCCheckBox {
+ checked: object.inclusion
+ onClicked: object.inclusion = checked
+ Layout.alignment: Qt.AlignHCenter
+ }
+ }
+
+ QGCLabel {
+ text: qsTr("Edit")
+ Layout.column: 1
+ Layout.alignment: Qt.AlignHCenter
+ }
+
+ Repeater {
+ model: myGeoFenceController.polygons
+
+ QGCRadioButton {
+ checked: _interactive
+ Layout.alignment: Qt.AlignHCenter
+
+ property bool _interactive: object.interactive
- QGCButton {
- text: qsTr("Remove fence polygon")
- visible: myGeoFenceController.polygonSupported && myGeoFenceController.mapPolygon.count > 0
- onClicked: myGeoFenceController.removePolygon()
+ on_InteractiveChanged: checked = _interactive
+
+ onClicked: {
+ myGeoFenceController.clearAllInteractive()
+ object.interactive = checked
+ }
+ }
+ }
+
+ QGCLabel {
+ text: qsTr("Delete")
+ Layout.column: 2
+ Layout.alignment: Qt.AlignHCenter
+ }
+
+ Repeater {
+ model: myGeoFenceController.polygons
+
+ QGCColoredImage {
+ width: ScreenTools.defaultFontPixelHeight
+ height: width
+ sourceSize.height: width
+ source: "/res/XDelete.svg"
+ fillMode: Image.PreserveAspectFit
+ color: qgcPal.text
+ Layout.alignment: Qt.AlignHCenter
+
+ property int _polygonIndex: index
+
+ QGCMouseArea {
+ fillItem: parent
+ onClicked: myGeoFenceController.deletePolygon(parent._polygonIndex)
+ }
+ }
+ }
+ } // GridLayout
+
+ SectionHeader {
+ id: circleSection
+ text: qsTr("Circular Fences")
+ }
+
+ QGCLabel {
+ text: qsTr("None")
+ visible: circleSection.checked && myGeoFenceController.circles.count == 0
+ }
+
+ GridLayout {
+ anchors.left: parent.left
+ anchors.right: parent.right
+ columns: 4
+ flow: GridLayout.TopToBottom
+ visible: polygonSection.checked && myGeoFenceController.circles.count > 0
+
+ QGCLabel {
+ text: qsTr("Inclusion")
+ Layout.column: 0
+ Layout.alignment: Qt.AlignHCenter
+ }
+
+ Repeater {
+ model: myGeoFenceController.circles
+
+ QGCCheckBox {
+ checked: object.inclusion
+ onClicked: object.inclusion = checked
+ Layout.alignment: Qt.AlignHCenter
+ }
+ }
+
+ QGCLabel {
+ text: qsTr("Edit")
+ Layout.column: 1
+ Layout.alignment: Qt.AlignHCenter
+ }
+
+ Repeater {
+ model: myGeoFenceController.circles
+
+ QGCRadioButton {
+ checked: _interactive
+ Layout.alignment: Qt.AlignHCenter
+
+ property bool _interactive: object.interactive
+
+ on_InteractiveChanged: checked = _interactive
+
+ onClicked: {
+ myGeoFenceController.clearAllInteractive()
+ object.interactive = checked
+ }
+ }
+ }
+
+ QGCLabel {
+ text: qsTr("Radius")
+ Layout.column: 2
+ Layout.alignment: Qt.AlignHCenter
+ }
+
+ Repeater {
+ model: myGeoFenceController.circles
+
+ FactTextField {
+ fact: object.radius
+ Layout.fillWidth: true
+ Layout.alignment: Qt.AlignHCenter
+ }
+ }
+
+ QGCLabel {
+ text: qsTr("Delete")
+ Layout.column: 3
+ Layout.alignment: Qt.AlignHCenter
+ }
+
+ Repeater {
+ model: myGeoFenceController.circles
+
+ QGCColoredImage {
+ width: ScreenTools.defaultFontPixelHeight
+ height: width
+ sourceSize.height: width
+ source: "/res/XDelete.svg"
+ fillMode: Image.PreserveAspectFit
+ color: qgcPal.text
+ Layout.alignment: Qt.AlignHCenter
+
+ property int _circleIndex: index
+
+ QGCMouseArea {
+ fillItem: parent
+ onClicked: myGeoFenceController.deleteCircle(parent._polygonIndex)
+ }
+ }
+ }
+ } // GridLayout
+ }
}
}
- }
+ } // Rectangle
}
}
diff --git a/src/PlanView/GeoFenceMapVisuals.qml b/src/PlanView/GeoFenceMapVisuals.qml
index e66cbd666233f14cca4f6d36a81d637e27c33ac1..9e41cc65f3555abe0fac680d1254836f2299eb05 100644
--- a/src/PlanView/GeoFenceMapVisuals.qml
+++ b/src/PlanView/GeoFenceMapVisuals.qml
@@ -30,34 +30,53 @@ Item {
property var _breachReturnPointComponent
property var _mouseAreaComponent
- property var _circleComponent
- property var _mapPolygon: myGeoFenceController.mapPolygon
- property bool _interactive: interactive
- property bool _circleSupported: myGeoFenceController.circleRadiusFact !== null
- property bool _circleEnabled: myGeoFenceController.circleEnabled
- property real _circleRadius: _circleSupported ? myGeoFenceController.circleRadiusFact.rawValue : 0
- property bool _polygonSupported: myGeoFenceController.polygonSupported
- property bool _polygonEnabled: myGeoFenceController.polygonEnabled
+ property var _polygons: myGeoFenceController.polygons
+ property var _circles: myGeoFenceController.circles
+
+ function addPolygon(inclusionPolygon) {
+ // Initial polygon is inset to take 2/3rds space
+ var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height)
+ rect.x += (rect.width * 0.25) / 2
+ rect.y += (rect.height * 0.25) / 2
+ rect.width *= 0.75
+ rect.height *= 0.75
+
+ var centerCoord = map.toCoordinate(Qt.point(rect.x + (rect.width / 2), rect.y + (rect.height / 2)), false /* clipToViewPort */)
+ var topLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */)
+ var topRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y), false /* clipToViewPort */)
+ var bottomLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y + rect.height), false /* clipToViewPort */)
+ var bottomRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */)
+
+ // Initial polygon has max width and height of 3000 meters
+ var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), 3000) / 2
+ var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), 3000) / 2
+ topLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0)
+ topRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0)
+ bottomLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180)
+ bottomRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180)
+
+ console.log(map.center)
+ console.log(topLeftCoord)
+ console.log(bottomRightCoord)
+
+ if (inclusionPolygon) {
+ myGeoFenceController.addInclusion(topLeftCoord, bottomRightCoord)
+ } else {
+ myGeoFenceController.addExclusion(topLeftCoord, bottomRightCoord)
+ }
+ }
Component.onCompleted: {
- _circleComponent = circleComponent.createObject(map)
- map.addMapItem(_circleComponent)
_breachReturnPointComponent = breachReturnPointComponent.createObject(map)
map.addMapItem(_breachReturnPointComponent)
_mouseAreaComponent = mouseAreaComponent.createObject(map)
}
Component.onDestruction: {
- _circleComponent.destroy()
_breachReturnPointComponent.destroy()
_mouseAreaComponent.destroy()
}
- Connections {
- target: myGeoFenceController
- onAddInitialFencePolygon: mapPolygonVisuals.addInitialPolygon()
- }
-
// Mouse area to capture breach return point coordinate
Component {
id: mouseAreaComponent
@@ -69,28 +88,29 @@ Item {
}
}
- QGCMapPolygonVisuals {
- id: mapPolygonVisuals
- mapControl: map
- mapPolygon: _mapPolygon
- interactive: _interactive
- borderWidth: 2
- borderColor: "orange"
- visible: _polygonSupported && (planView || _polygonEnabled)
+ Instantiator {
+ model: _polygons
+
+ delegate : QGCMapPolygonVisuals {
+ mapControl: map
+ mapPolygon: object
+ borderWidth: object.inclusion ? 2 : 0
+ borderColor: "orange"
+ interiorColor: object.inclusion ? "transparent" : "orange"
+ interiorOpacity: object.inclusion ? 1: 0.2
+ }
}
- // GeoFence circle
- Component {
- id: circleComponent
+ Instantiator {
+ model: _circles
- MapCircle {
- z: QGroundControl.zOrderMapItems
- border.width: 2
- border.color: "orange"
- color: "transparent"
- center: homePosition ? homePosition : QtPositioning.coordinate()
- radius: _circleRadius
- visible: _circleSupported && _circleRadius > 0 && (planView || _circleEnabled)
+ delegate : QGCMapCircleVisuals {
+ mapControl: map
+ mapCircle: object
+ borderWidth: object.inclusion ? 2 : 0
+ borderColor: "orange"
+ interiorColor: object.inclusion ? "transparent" : "orange"
+ interiorOpacity: object.inclusion ? 1: 0.2
}
}
diff --git a/src/PlanView/PlanView.qml b/src/PlanView/PlanView.qml
index 673d930cf67be5201400dd658c4655db9a74f3d5..f4672d2c97b33526360e7748aceb32ec32e7ab9e 100644
--- a/src/PlanView/PlanView.qml
+++ b/src/PlanView/PlanView.qml
@@ -335,7 +335,7 @@ QGCView {
}
break
case _layerRallyPoints:
- if (_rallyPointController.rallyPointsSupported) {
+ if (_rallyPointController.supported) {
_rallyPointController.addPoint(coordinate)
}
break
diff --git a/src/PlanView/RallyPointEditorHeader.qml b/src/PlanView/RallyPointEditorHeader.qml
index ba87f949fdde9f27239a488d9ac6cf65664a69ac..ff0a86277f00775ef7c4f7b44bcc446648543bf2 100644
--- a/src/PlanView/RallyPointEditorHeader.qml
+++ b/src/PlanView/RallyPointEditorHeader.qml
@@ -59,7 +59,7 @@ QGCFlickable {
anchors.right: parent.right
anchors.top: infoLabel.bottom
wrapMode: Text.WordWrap
- text: controller.rallyPointsSupported ?
+ text: controller.supported ?
qsTr("Click in the map to add new rally points.") :
qsTr("This vehicle does not support Rally Points.")
}
diff --git a/src/PlanView/RallyPointMapVisuals.qml b/src/PlanView/RallyPointMapVisuals.qml
index f303349ad0396d461f08b4066273be8ea747f63c..57c2f5a3d229cf678d6644f3f21094dd103f191d 100644
--- a/src/PlanView/RallyPointMapVisuals.qml
+++ b/src/PlanView/RallyPointMapVisuals.qml
@@ -29,7 +29,7 @@ Item {
property bool _interactive: interactive
property var _rallyPointsComponent
- property bool _rallyPointsSupported: myRallyPointController.rallyPointsSupported
+ property bool _rallyPointsSupported: myRallyPointController.supported
property var _rallyPoints: myRallyPointController.points
Component.onCompleted: {
diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir
index b16c14e3478ed1842e80666dac1510735b5eda31..6bff369b7595ca1779ca8ccc269c87a357d5bf2c 100644
--- a/src/QmlControls/QGroundControl.Controls.qmldir
+++ b/src/QmlControls/QGroundControl.Controls.qmldir
@@ -43,6 +43,7 @@ QGCGroupBox 1.0 QGCGroupBox.qml
QGCLabel 1.0 QGCLabel.qml
QGCListView 1.0 QGCListView.qml
QGCMapLabel 1.0 QGCMapLabel.qml
+QGCMapCircleVisuals 1.0 QGCMapCircleVisuals.qml
QGCMapPolygonVisuals 1.0 QGCMapPolygonVisuals.qml
QGCMouseArea 1.0 QGCMouseArea.qml
QGCMovableItem 1.0 QGCMovableItem.qml
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index e0e8a834533f48307359019123b7604a0a46502e..753faf6fa88daf51bf733181017f7c6ae2bc70be 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -116,7 +116,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _telemetryRNoise(0)
, _maxProtoVersion(0)
, _vehicleCapabilitiesKnown(false)
- , _supportsMissionItemInt(false)
+ , _capabilityBits(0)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
@@ -281,7 +281,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
, _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
, _vehicleCapabilitiesKnown(true)
- , _supportsMissionItemInt(false)
+ , _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
@@ -350,11 +350,11 @@ void Vehicle::_commonInit(void)
connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
// GeoFenceManager needs to access ParameterManager so make sure to create after
- _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
+ _geoFenceManager = new GeoFenceManager(this);
connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_geoFenceLoadComplete);
- _rallyPointManager = _firmwarePlugin->newRallyPointManager(this);
+ _rallyPointManager = new RallyPointManager(this);
connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
connect(_rallyPointManager, &RallyPointManager::loadComplete, this, &Vehicle::_rallyPointLoadComplete);
@@ -416,6 +416,12 @@ void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
_firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
emit firmwareTypeChanged();
+ if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+ _capabilityBits = 0;
+ } else {
+ _capabilityBits = MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
+ }
+ emit capabilityBitsChanged(_capabilityBits);
}
void Vehicle::_offlineVehicleTypeSettingChanged(QVariant value)
@@ -749,11 +755,10 @@ void Vehicle::_handleAltitude(mavlink_message_t& message)
void Vehicle::_setCapabilities(uint64_t capabilityBits)
{
- if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) {
- _supportsMissionItemInt = true;
- }
+ _capabilityBits = capabilityBits;
_vehicleCapabilitiesKnown = true;
emit capabilitiesKnownChanged(true);
+ emit capabilityBitsChanged(_capabilityBits);
// This should potentially be turned into a user-facing warning
// if the general experience after deployment is that users want MAVLink 2
@@ -762,7 +767,13 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits)
qCDebug(VehicleLog) << QString("Vehicle does support MAVLink 2 but the link does not allow for it.");
}
- qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_supportsMissionItemInt ? QStringLiteral("supports") : QStringLiteral("does not support"));
+ QString supports("supports");
+ QString doesNotSupport("does not support");
+
+ qCDebug(VehicleLog) << QString("Vehicle %1 Mavlink 2.0").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? supports : doesNotSupport);
+ qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT ? supports : doesNotSupport);
+ qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport);
+ qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport);
}
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
@@ -1908,8 +1919,13 @@ void Vehicle::_missionLoadComplete(void)
// After the initial mission request completes we ask for the geofence
if (!_geoFenceManagerInitialRequestSent) {
_geoFenceManagerInitialRequestSent = true;
- qCDebug(VehicleLog) << "_missionLoadComplete requesting geoFence";
- _geoFenceManager->loadFromVehicle();
+ if (_geoFenceManager->supported()) {
+ qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence";
+ _geoFenceManager->loadFromVehicle();
+ } else {
+ qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping";
+ _geoFenceLoadComplete();
+ }
}
}
@@ -1918,15 +1934,23 @@ void Vehicle::_geoFenceLoadComplete(void)
// After geofence request completes we ask for the rally points
if (!_rallyPointManagerInitialRequestSent) {
_rallyPointManagerInitialRequestSent = true;
- qCDebug(VehicleLog) << "_missionLoadComplete requesting rally points";
- _rallyPointManager->loadFromVehicle();
+ if (_rallyPointManager->supported()) {
+ qCDebug(VehicleLog) << "_missionLoadComplete requesting Rally Points";
+ _rallyPointManager->loadFromVehicle();
+ } else {
+ qCDebug(VehicleLog) << "_missionLoadComplete Rally Points not supported skipping";
+ _rallyPointLoadComplete();
+ }
}
}
void Vehicle::_rallyPointLoadComplete(void)
{
qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
- _initialPlanRequestComplete = true;
+ if (!_initialPlanRequestComplete) {
+ _initialPlanRequestComplete = true;
+ emit initialPlanRequestCompleted();
+ }
}
void Vehicle::_parametersReady(bool parametersReady)
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 733c4d9b09c162d8d97b5abb138aafad7ba43f84..249c3de3eb5649fd268deb45a6f7648f90e74331 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -7,12 +7,7 @@
*
****************************************************************************/
-
-/// @file
-/// @author Don Gagne <don@thegagnes.com>
-
-#ifndef Vehicle_H
-#define Vehicle_H
+#pragma once
#include <QObject>
#include <QGeoCoordinate>
@@ -687,8 +682,8 @@ public:
const QVariantList& toolBarIndicators ();
const QVariantList& cameraList (void) const;
- bool capabilitiesKnown(void) const { return _vehicleCapabilitiesKnown; }
- bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; }
+ bool capabilitiesKnown (void) const { return _vehicleCapabilitiesKnown; }
+ uint64_t capabilityBits (void) const { return _capabilityBits; } // Change signalled by capabilityBitsChanged
/// @true: When flying a mission the vehicle is always facing towards the next waypoint
bool vehicleYawsToNextWaypointInMission(void) const;
@@ -731,6 +726,8 @@ signals:
void firmwareTypeChanged(void);
void vehicleTypeChanged(void);
void capabilitiesKnownChanged(bool capabilitiesKnown);
+ void initialPlanRequestCompleted(void);
+ void capabilityBitsChanged(uint64_t capabilityBits);
void messagesReceivedChanged ();
void messagesSentChanged ();
@@ -932,7 +929,7 @@ private:
int _telemetryRNoise;
unsigned _maxProtoVersion;
bool _vehicleCapabilitiesKnown;
- bool _supportsMissionItemInt;
+ uint64_t _capabilityBits;
typedef struct {
int component;
@@ -1073,4 +1070,3 @@ private:
static const char* _joystickEnabledSettingsKey;
};
-#endif
diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index 1e0092aa66ac008cce8b50f5f567062cd9278372..1c36b72dc0a661e73ff1c2922bf42f4b9508f8fb 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -892,7 +892,7 @@ void MockLink::_respondWithAutopilotVersion(void)
_vehicleComponentId,
_mavlinkChannel,
&msg,
- 0, // capabilities,
+ MAV_PROTOCOL_CAPABILITY_MAVLINK2 | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY),
flightVersion, // flight_sw_version,
0, // middleware_sw_version,
0, // os_sw_version,
diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc
index 5fa572db1bac1dd46d27347865693cf8018da147..305e1050ebb1fdbf8733882c9f749e41bdfc3cf3 100644
--- a/src/comm/MockLinkMissionItemHandler.cc
+++ b/src/comm/MockLinkMissionItemHandler.cc
@@ -45,37 +45,37 @@ void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void)
bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
{
switch (msg.msgid) {
- case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
- _handleMissionRequestList(msg);
- break;
-
- case MAVLINK_MSG_ID_MISSION_REQUEST:
- _handleMissionRequest(msg);
- break;
-
- case MAVLINK_MSG_ID_MISSION_ITEM:
- _handleMissionItem(msg);
- break;
-
- case MAVLINK_MSG_ID_MISSION_COUNT:
- _handleMissionCount(msg);
- break;
-
- case MAVLINK_MSG_ID_MISSION_ACK:
- // Acks are received back for each MISSION_ITEM message
- break;
-
- case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
- // Sets the currently active mission item
- break;
-
- case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
- // Delete all mission items
- _missionItems.clear();
- break;
-
- default:
- return false;
+ case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+ _handleMissionRequestList(msg);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST:
+ _handleMissionRequest(msg);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ITEM:
+ _handleMissionItem(msg);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_COUNT:
+ _handleMissionCount(msg);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ACK:
+ // Acks are received back for each MISSION_ITEM message
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
+ // Sets the currently active mission item
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+ // Delete all mission items
+ _missionItems.clear();
+ break;
+
+ default:
+ return false;
}
return true;
@@ -100,9 +100,24 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
Q_ASSERT(request.target_system == _mockLink->vehicleId());
- int itemCount = _missionItems.count();
- if (itemCount == 0 && _sendHomePositionOnEmptyList) {
- itemCount = 1;
+ _requestType = (MAV_MISSION_TYPE)request.mission_type;
+
+ int itemCount;
+ switch (_requestType) {
+ case MAV_MISSION_TYPE_MISSION:
+ itemCount = _missionItems.count();
+ if (itemCount == 0 && _sendHomePositionOnEmptyList) {
+ itemCount = 1;
+ }
+ break;
+ case MAV_MISSION_TYPE_FENCE:
+ itemCount = _fenceItems.count();
+ break;
+ case MAV_MISSION_TYPE_RALLY:
+ itemCount = _rallyItems.count();
+ break;
+ default:
+ Q_ASSERT(false);
}
mavlink_message_t responseMsg;
@@ -114,7 +129,7 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
itemCount, // Number of mission items
- MAV_MISSION_TYPE_MISSION);
+ _requestType);
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
@@ -128,8 +143,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
mavlink_msg_mission_request_decode(&msg, &request);
Q_ASSERT(request.target_system == _mockLink->vehicleId());
- Q_ASSERT(request.seq < _missionItems.count());
-
+
if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) {
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse";
} else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) {
@@ -148,20 +162,32 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
}
if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) ||
- (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
+ (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) {
_sendAck(MAV_MISSION_ERROR);
} else {
- mavlink_message_t responseMsg;
-
- mavlink_mission_item_t item;
- if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
- item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
- item.command = MAV_CMD_NAV_WAYPOINT;
- item.current = false;
- item.autocontinue = true;
- item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
- } else {
- item = _missionItems[request.seq];
+ mavlink_mission_item_t item;
+ mavlink_message_t responseMsg;
+
+ switch (request.mission_type) {
+ case MAV_MISSION_TYPE_MISSION:
+ if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
+ item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+ item.command = MAV_CMD_NAV_WAYPOINT;
+ item.current = false;
+ item.autocontinue = true;
+ item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
+ } else {
+ item = _missionItems[request.seq];
+ }
+ break;
+ case MAV_MISSION_TYPE_FENCE:
+ item = _fenceItems[request.seq];
+ break;
+ case MAV_MISSION_TYPE_RALLY:
+ item = _rallyItems[request.seq];
+ break;
+ default:
+ Q_ASSERT(false);
}
mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(),
@@ -177,7 +203,7 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
item.autocontinue,
item.param1, item.param2, item.param3, item.param4,
item.x, item.y, item.z,
- MAV_MISSION_TYPE_MISSION);
+ _requestType);
_mockLink->respondWithMavlinkMessage(responseMsg);
}
}
@@ -190,12 +216,23 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms
mavlink_msg_mission_count_decode(&msg, &missionCount);
Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());
+ _requestType = (MAV_MISSION_TYPE)missionCount.mission_type;
_writeSequenceCount = missionCount.count;
Q_ASSERT(_writeSequenceCount >= 0);
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount;
- _missionItems.clear();
+ switch (missionCount.mission_type) {
+ case MAV_MISSION_TYPE_MISSION:
+ _missionItems.clear();
+ break;
+ case MAV_MISSION_TYPE_FENCE:
+ _fenceItems.clear();
+ break;
+ case MAV_MISSION_TYPE_RALLY:
+ _rallyItems.clear();
+ break;
+ }
if (_writeSequenceCount == 0) {
_sendAck(MAV_MISSION_ACCEPTED);
@@ -228,28 +265,25 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
}
if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) ||
- (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
+ (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) {
sequenceNumber ++;
}
if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) ||
- (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
+ (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) {
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode";
_sendAck(MAV_MISSION_ERROR);
} else {
- mavlink_message_t message;
- mavlink_mission_request_t missionRequest;
-
- memset(&missionRequest, 0, sizeof(missionRequest));
- missionRequest.target_system = _mavlinkProtocol->getSystemId();
- missionRequest.target_component = _mavlinkProtocol->getComponentId();
- missionRequest.seq = sequenceNumber;
-
- mavlink_msg_mission_request_encode_chan(_mockLink->vehicleId(),
- MAV_COMP_ID_MISSIONPLANNER,
- _mockLink->mavlinkChannel(),
- &message,
- &missionRequest);
+ mavlink_message_t message;
+
+ mavlink_msg_mission_request_pack_chan(_mockLink->vehicleId(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ _mockLink->mavlinkChannel(),
+ &message,
+ _mavlinkProtocol->getSystemId(),
+ _mavlinkProtocol->getComponentId(),
+ sequenceNumber,
+ _requestType);
_mockLink->respondWithMavlinkMessage(message);
// If response with Mission Item doesn't come before timer fires it's an error
@@ -262,19 +296,16 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType)
{
qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType;
- mavlink_message_t message;
- mavlink_mission_ack_t missionAck;
-
- memset(&missionAck, 0, sizeof(missionAck));
- missionAck.target_system = _mavlinkProtocol->getSystemId();
- missionAck.target_component = _mavlinkProtocol->getComponentId();
- missionAck.type = ackType;
+ mavlink_message_t message;
- mavlink_msg_mission_ack_encode_chan(_mockLink->vehicleId(),
- MAV_COMP_ID_MISSIONPLANNER,
- _mockLink->mavlinkChannel(),
- &message,
- &missionAck);
+ mavlink_msg_mission_ack_pack_chan(_mockLink->vehicleId(),
+ MAV_COMP_ID_MISSIONPLANNER,
+ _mockLink->mavlinkChannel(),
+ &message,
+ _mavlinkProtocol->getSystemId(),
+ _mavlinkProtocol->getComponentId(),
+ ackType,
+ _requestType);
_mockLink->respondWithMavlinkMessage(message);
}
@@ -290,8 +321,18 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg
Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
- _missionItems[missionItem.seq] = missionItem;
-
+ switch (missionItem.mission_type) {
+ case MAV_MISSION_TYPE_MISSION:
+ _missionItems[missionItem.seq] = missionItem;
+ break;
+ case MAV_MISSION_TYPE_FENCE:
+ _fenceItems[missionItem.seq] = missionItem;
+ break;
+ case MAV_MISSION_TYPE_RALLY:
+ _rallyItems[missionItem.seq] = missionItem;
+ break;
+ }
+
_writeSequenceIndex++;
if (_writeSequenceIndex < _writeSequenceCount) {
if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) {
diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h
index 765058fd62aa3fda8fedd38b06833af0ef5ac35d..eadba36edb04196fe73ae68c8a9edbe6dab0a747 100644
--- a/src/comm/MockLinkMissionItemHandler.h
+++ b/src/comm/MockLinkMissionItemHandler.h
@@ -98,9 +98,13 @@ private:
int _writeSequenceCount; ///< Numbers of items about to be written
int _writeSequenceIndex; ///< Current index being reqested
- typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
- MissionList_t _missionItems;
-
+ typedef QMap<uint16_t, mavlink_mission_item_t> MissionItemList_t;
+
+ MAV_MISSION_TYPE _requestType;
+ MissionItemList_t _missionItems;
+ MissionItemList_t _fenceItems;
+ MissionItemList_t _rallyItems;
+
QTimer* _missionItemResponseTimer;
FailureMode_t _failureMode;
bool _sendHomePositionOnEmptyList;
diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc
index 8384ee09d8f34c53aff6fcb247c4cd865a359f92..a970b5f1c9d681dfd49f6813bf294de384b04cb3 100644
--- a/src/qgcunittest/UnitTest.cc
+++ b/src/qgcunittest/UnitTest.cc
@@ -391,6 +391,12 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot)
QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable());
_vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
QVERIFY(_vehicle);
+
+ // Wait for plan request to complete
+ if (!_vehicle->initialPlanRequestComplete()) {
+ QSignalSpy spyPlan(_vehicle, SIGNAL(initialPlanRequestCompleted()));
+ QCOMPARE(spyPlan.wait(10000), true);
+ }
}
void UnitTest::_disconnectMockLink(void)