From a75f4b6daf21023a06957f4008437b79128c755c Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Wed, 31 Aug 2016 19:27:50 -0700
Subject: [PATCH] More work on GeoFence support
- GeoFence support comes from plugin
- Circle fence support
- Better param support
---
qgroundcontrol.pro | 2 +
src/AutoPilotPlugins/AutoPilotPlugin.cc | 2 +-
src/AutoPilotPlugins/AutoPilotPlugin.h | 2 +-
src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 38 +--
src/FirmwarePlugin/APM/APMGeoFenceManager.cc | 316 +++++++++++++++++++++
src/FirmwarePlugin/APM/APMGeoFenceManager.h | 73 +++++
.../APM/APMParameterFactMetaData.Copter.3.4.xml | 4 +
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 14 +-
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h | 7 +-
src/FirmwarePlugin/FirmwarePlugin.h | 13 +-
src/FlightDisplay/FlightDisplayViewMap.qml | 23 +-
src/MissionEditor/GeoFenceEditor.qml | 19 +-
src/MissionEditor/MissionEditor.qml | 59 ++--
src/MissionManager/GeoFenceController.cc | 195 ++++++++-----
src/MissionManager/GeoFenceController.h | 67 +++--
src/MissionManager/GeoFenceManager.cc | 227 ++-------------
src/MissionManager/GeoFenceManager.h | 84 +++---
src/MissionManager/MissionController.cc | 46 ++-
src/MissionManager/MissionController.h | 9 +-
src/MissionManager/MissionManager.cc | 2 +-
src/MissionManager/PlanElementController.cc | 4 +-
src/MissionManager/PlanElementController.h | 4 +-
src/Vehicle/Vehicle.cc | 11 +-
src/Vehicle/Vehicle.h | 2 +-
24 files changed, 780 insertions(+), 443 deletions(-)
create mode 100644 src/FirmwarePlugin/APM/APMGeoFenceManager.cc
create mode 100644 src/FirmwarePlugin/APM/APMGeoFenceManager.h
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 3a78fc0..0f8f6ab 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -692,6 +692,7 @@ HEADERS+= \
src/FirmwarePlugin/FirmwarePluginManager.h \
src/FirmwarePlugin/FirmwarePlugin.h \
src/FirmwarePlugin/APM/APMFirmwarePlugin.h \
+ src/FirmwarePlugin/APM/APMGeoFenceManager.h \
src/FirmwarePlugin/APM/APMParameterMetaData.h \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
@@ -751,6 +752,7 @@ SOURCES += \
src/AutoPilotPlugins/PX4/SensorsComponentController.cc \
src/AutoPilotPlugins/PX4/PX4TuningComponent.cc \
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \
+ src/FirmwarePlugin/APM/APMGeoFenceManager.cc \
src/FirmwarePlugin/APM/APMParameterMetaData.cc \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc \
diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc
index 8100333..529b1e7 100644
--- a/src/AutoPilotPlugins/AutoPilotPlugin.cc
+++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc
@@ -117,7 +117,7 @@ void AutoPilotPlugin::refreshParametersPrefix(int componentId, const QString& na
_vehicle->getParameterLoader()->refreshParametersPrefix(componentId, namePrefix);
}
-bool AutoPilotPlugin::parameterExists(int componentId, const QString& name)
+bool AutoPilotPlugin::parameterExists(int componentId, const QString& name) const
{
return _vehicle->getParameterLoader()->parameterExists(componentId, name);
}
diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h
index 5cc8c3c..b0910c2 100644
--- a/src/AutoPilotPlugins/AutoPilotPlugin.h
+++ b/src/AutoPilotPlugins/AutoPilotPlugin.h
@@ -67,7 +67,7 @@
Q_INVOKABLE void refreshParametersPrefix(int componentId, const QString& namePrefix);
/// Returns true if the specifed parameter exists from the default component
- Q_INVOKABLE bool parameterExists(int componentId, const QString& name);
+ Q_INVOKABLE bool parameterExists(int componentId, const QString& name) const;
/// Returns all parameter names
QStringList parameterNames(int componentId);
diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
index 4240cda..fd6824d 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h
@@ -17,6 +17,7 @@
#include "FirmwarePlugin.h"
#include "QGCLoggingCategory.h"
#include "APMParameterMetaData.h"
+#include "APMGeoFenceManager.h"
#include <QAbstractSocket>
@@ -73,24 +74,25 @@ public:
QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
QList<MAV_CMD> supportedMissionCommands(void) final;
- bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities);
- QStringList flightModes (Vehicle* vehicle) final;
- QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final;
- bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
- bool isGuidedMode (const Vehicle* vehicle) const final;
- void pauseVehicle (Vehicle* vehicle);
- int manualControlReservedButtonCount(void);
- bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final;
- void adjustOutgoingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final;
- void initializeVehicle (Vehicle* vehicle) final;
- bool sendHomePositionToVehicle (void) final;
- void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final;
- QString getDefaultComponentIdParam (void) const final { return QString("SYSID_SW_TYPE"); }
- QString missionCommandOverrides (MAV_TYPE vehicleType) const;
- QString getVersionParam (void) final { return QStringLiteral("SYSID_SW_MREV"); }
- QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/APM/APMParameterFactMetaData.xml"); }
- void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
- QObject* loadParameterMetaData (const QString& metaDataFile);
+ bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities);
+ QStringList flightModes (Vehicle* vehicle) final;
+ QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final;
+ bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
+ bool isGuidedMode (const Vehicle* vehicle) const final;
+ void pauseVehicle (Vehicle* vehicle);
+ int manualControlReservedButtonCount(void);
+ bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final;
+ void adjustOutgoingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final;
+ void initializeVehicle (Vehicle* vehicle) final;
+ bool sendHomePositionToVehicle (void) final;
+ void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final;
+ QString getDefaultComponentIdParam (void) const final { return QString("SYSID_SW_TYPE"); }
+ QString missionCommandOverrides (MAV_TYPE vehicleType) const;
+ QString getVersionParam (void) final { return QStringLiteral("SYSID_SW_MREV"); }
+ QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/APM/APMParameterFactMetaData.xml"); }
+ void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { APMParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
+ QObject* loadParameterMetaData (const QString& metaDataFile);
+ GeoFenceManager* newGeoFenceManager (Vehicle* vehicle) { return new APMGeoFenceManager(vehicle); }
QString getParameterMetaDataFile(Vehicle* vehicle);
diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc
new file mode 100644
index 0000000..61eea5e
--- /dev/null
+++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc
@@ -0,0 +1,316 @@
+/****************************************************************************
+ *
+ * (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 "ParameterLoader.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)
+ , _breachReturnSupported(vehicle->fixedWing())
+ , _firstParamLoadComplete(false)
+ , _circleRadiusFact(NULL)
+ , _readTransactionInProgress(false)
+ , _writeTransactionInProgress(false)
+ , _fenceEnableFact(NULL)
+ , _fenceTypeFact(NULL)
+{
+ connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &APMGeoFenceManager::_mavlinkMessageReceived);
+ connect(_vehicle->getParameterLoader(), &ParameterLoader::parametersReady, this, &APMGeoFenceManager::_parametersReady);
+}
+
+APMGeoFenceManager::~APMGeoFenceManager()
+{
+
+}
+
+void APMGeoFenceManager::sendToVehicle(void)
+{
+ if (_readTransactionInProgress) {
+ _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress."));
+ return;
+ }
+
+ if (!_geoFenceSupported()) {
+ return;
+ }
+
+ // Validate
+ if (polygonSupported()) {
+ if (_polygon.count() < 3) {
+ _sendError(TooFewPoints, QStringLiteral("Geo-Fence polygon must contain at least 3 points."));
+ return;
+ }
+ if (_polygon.count() > std::numeric_limits<uint8_t>::max()) {
+ _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(_polygon.count()));
+ return;
+ }
+ }
+
+ // First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air.
+ // Unfortunately the param to do this with differs between plane and copter.
+ const char* enableParam = _vehicle->fixedWing() ? _fenceActionParam : _fenceEnableParam;
+ Fact* fenceEnableFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, enableParam);
+ QVariant savedEnableState = fenceEnableFact->rawValue();
+ fenceEnableFact->setRawValue(0);
+
+ // Total point count, +1 polygon close in last index, +1 for breach in index 0
+ _cWriteFencePoints = _polygon.count() + 1 + 1 ;
+ _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_cWriteFencePoints);
+
+ // FIXME: No validation of correct fence received
+ for (uint8_t index=0; index<_cWriteFencePoints; index++) {
+ _sendFencePoint(index);
+ }
+
+ fenceEnableFact->setRawValue(savedEnableState);
+}
+
+void APMGeoFenceManager::loadFromVehicle(void)
+{
+ _polygon.clear();
+
+ if (!_geoFenceSupported()) {
+ return;
+ }
+
+ // Point 0: Breach return point (ArduPlane only)
+ // Point [1,N]: Polygon points
+ // Point N+1: Close polygon point (same as point 1)
+ int cFencePoints = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt();
+ int minFencePoints = 6;
+ qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::loadFromVehicle" << cFencePoints;
+ if (cFencePoints == 0) {
+ // No fence, no more work to do, fence data has already been cleared
+ 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 out 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
+ qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint;
+ return;
+ }
+
+ if (fencePoint.idx == 0) {
+ setBreachReturnPoint(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
+ _readTransactionInProgress = false;
+ emit polygonChanged(_polygon);
+ }
+ }
+ }
+}
+
+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(mavlink->getSystemId(),
+ mavlink->getComponentId(),
+ &msg,
+ _vehicle->id(),
+ _vehicle->defaultComponentId(),
+ pointIndex);
+ _vehicle->sendMessageOnPriorityLink(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(mavlink->getSystemId(),
+ mavlink->getComponentId(),
+ &msg,
+ _vehicle->id(),
+ _vehicle->defaultComponentId(),
+ pointIndex, // Index of point to set
+ totalPointCount,
+ fenceCoord.latitude(),
+ fenceCoord.longitude());
+ _vehicle->sendMessageOnPriorityLink(msg);
+}
+
+bool APMGeoFenceManager::inProgress(void) const
+{
+ return _readTransactionInProgress || _writeTransactionInProgress;
+}
+
+bool APMGeoFenceManager::_geoFenceSupported(void)
+{
+ // FIXME: MockLink doesn't support geo fence yet
+ if (qgcApp()->runningUnitTests()) {
+ return false;
+ }
+
+ if (!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) ||
+ !_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) {
+ return false;
+ } else {
+ return true;
+ }
+}
+
+void APMGeoFenceManager::_updateSupportedFlags(void)
+{
+ emit circleSupportedChanged(circleSupported());
+ emit polygonSupportedChanged(polygonSupported());
+}
+
+void APMGeoFenceManager::_parametersReady(void)
+{
+ if (!_firstParamLoadComplete) {
+ _firstParamLoadComplete = true;
+
+ _fenceSupported = _vehicle->parameterExists(FactSystem::defaultComponentId, QStringLiteral("FENCE_ACTION"));
+
+ if (_fenceSupported) {
+ QStringList paramNames;
+ QStringList paramLabels;
+
+ if (_vehicle->multiRotor()) {
+ _fenceEnableFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_ENABLE"));
+ _fenceTypeFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_TYPE"));
+
+ connect(_fenceEnableFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags);
+ connect(_fenceTypeFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_updateSupportedFlags);
+
+ _circleRadiusFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("FENCE_RADIUS"));
+ connect(_circleRadiusFact, &Fact::rawValueChanged, this, &APMGeoFenceManager::_circleRadiusRawValueChanged);
+ emit circleRadiusChanged(circleRadius());
+
+ 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()) {
+ 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->parameterExists(FactSystem::defaultComponentId, paramName)) {
+ Fact* paramFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, paramName);
+ _params << QVariant::fromValue(paramFact);
+ _paramLabels << paramLabels[i];
+ }
+ }
+ emit paramsChanged(_params);
+ emit paramLabelsChanged(_paramLabels);
+
+ emit fenceSupportedChanged(_fenceSupported);
+ emit circleSupportedChanged(circleSupported());
+ emit polygonSupportedChanged(polygonSupported());
+ }
+
+ qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" <<
+ _fenceSupported << circleSupported() << polygonSupported() << _breachReturnSupported;
+ }
+}
+
+float APMGeoFenceManager::circleRadius(void) const
+{
+ if (_circleRadiusFact) {
+ return _circleRadiusFact->rawValue().toFloat();
+ } else {
+ return 0.0;
+ }
+}
+
+void APMGeoFenceManager::_circleRadiusRawValueChanged(QVariant value)
+{
+ emit circleRadiusChanged(value.toFloat());
+}
+
+bool APMGeoFenceManager::circleSupported(void) const
+{
+ if (_fenceSupported && _vehicle->multiRotor() && _fenceEnableFact && _fenceTypeFact) {
+ return _fenceEnableFact->rawValue().toBool() && (_fenceTypeFact->rawValue().toInt() & 2);
+ }
+
+ return false;
+}
+
+bool APMGeoFenceManager::polygonSupported(void) const
+{
+ if (_fenceSupported) {
+ if (_vehicle->multiRotor()) {
+ if (_fenceEnableFact && _fenceTypeFact) {
+ return _fenceEnableFact->rawValue().toBool() && (_fenceTypeFact->rawValue().toInt() & 4);
+ }
+ } else if (_vehicle->fixedWing()) {
+ return true;
+ }
+ }
+
+ return false;
+}
diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.h b/src/FirmwarePlugin/APM/APMGeoFenceManager.h
new file mode 100644
index 0000000..6141c5a
--- /dev/null
+++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.h
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ * (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 APMGeoFenceManager : public GeoFenceManager
+{
+ Q_OBJECT
+
+public:
+ APMGeoFenceManager(Vehicle* vehicle);
+ ~APMGeoFenceManager();
+
+ // Overrides from GeoFenceManager
+ bool inProgress (void) const final;
+ void loadFromVehicle (void) final;
+ void sendToVehicle (void) final;
+ bool fenceSupported (void) const final { return _fenceSupported; }
+ bool circleSupported (void) const final;
+ bool polygonSupported (void) const final;
+ bool breachReturnSupported (void) const final { return _breachReturnSupported; }
+ float circleRadius (void) const final;
+ QVariantList params (void) const final { return _params; }
+ QStringList paramLabels (void) const final { return _paramLabels; }
+
+private slots:
+ void _mavlinkMessageReceived(const mavlink_message_t& message);
+ void _updateSupportedFlags(void);
+ void _circleRadiusRawValueChanged(QVariant value);
+ void _parametersReady(void);
+
+private:
+ void _requestFencePoint(uint8_t pointIndex);
+ void _sendFencePoint(uint8_t pointIndex);
+ bool _geoFenceSupported(void);
+
+private:
+ bool _fenceSupported;
+ bool _breachReturnSupported;
+ bool _firstParamLoadComplete;
+
+ QVariantList _params;
+ QStringList _paramLabels;
+
+ Fact* _circleRadiusFact;
+
+ bool _readTransactionInProgress;
+ bool _writeTransactionInProgress;
+
+ uint8_t _cReadFencePoints;
+ uint8_t _cWriteFencePoints;
+ uint8_t _currentFencePoint;
+
+ Fact* _fenceEnableFact;
+ Fact* _fenceTypeFact;
+
+ static const char* _fenceTotalParam;
+ static const char* _fenceActionParam;
+ static const char* _fenceEnableParam;
+};
+
+#endif
diff --git a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml
index 18afd10..e943e00 100644
--- a/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml
+++ b/src/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml
@@ -6512,6 +6512,10 @@
<value code="1">Altitude</value>
<value code="2">Circle</value>
<value code="3">Altitude and Circle</value>
+<value code="4">Polygon</value>
+<value code="5">Altitude and Polygon</value>
+<value code="6">Circle and Polygon</value>
+<value code="7">All</value>
</values>
</param>
<param humanName="Fence Action" name="FENCE_ACTION" documentation="What action should be taken when fence is breached" user="Standard">
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
index 6aa3e05..a1cf9da 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
@@ -98,9 +98,13 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
return majorVersionNumber == 3 ? 4: Vehicle::versionNotSetValue;
}
-bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
+bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
{
- return (capabilities & (SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability)) == capabilities;
+ Q_UNUSED(vehicle);
+
+ uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;
+
+ return (capabilities & vehicleCapabilities) == capabilities;
}
void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
@@ -208,3 +212,9 @@ bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
{
return vehicle->autopilotPlugin()->getParameterFact(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0;
}
+
+QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle)
+{
+ Q_UNUSED(vehicle);
+ return QStringLiteral("FENCE_RADIUS");
+}
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
index 394dd10..06cd34e 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
@@ -63,9 +63,10 @@ public:
void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
- virtual int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
- virtual bool multiRotorCoaxialMotors(Vehicle* vehicle) final;
- virtual bool multiRotorXConfig(Vehicle* vehicle) final;
+ int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
+ bool multiRotorCoaxialMotors(Vehicle* vehicle) final;
+ bool multiRotorXConfig(Vehicle* vehicle) final;
+ QString geoFenceRadiusParam(Vehicle* vehicle) final;
private:
static bool _remapParamNameIntialized;
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index 0c76250..17ea75c 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -17,6 +17,7 @@
#include "QGCMAVLink.h"
#include "VehicleComponent.h"
#include "AutoPilotPlugin.h"
+#include "GeoFenceManager.h"
#include <QList>
#include <QString>
@@ -40,8 +41,8 @@ public:
SetFlightModeCapability = 1 << 0, ///< FirmwarePlugin::setFlightMode method is supported
MavCmdPreflightStorageCapability = 1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported
PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location
- GuidedModeCapability = 1 << 3, ///< Vehicle Supports guided mode commands
- OrbitModeCapability = 1 << 4, ///< Vehicle Supports orbit mode
+ GuidedModeCapability = 1 << 3, ///< Vehicle supports guided mode commands
+ OrbitModeCapability = 1 << 4, ///< Vehicle supports orbit mode
} FirmwareCapabilities;
/// Maps from on parameter name to another
@@ -207,6 +208,14 @@ public:
/// @return true: X confiuration, false: Plus configuration
virtual bool multiRotorXConfig(Vehicle* vehicle) { Q_UNUSED(vehicle); return false; }
+
+ /// Returns a newly create geofence manager for this vehicle. Returns NULL if this vehicle
+ /// does not support geofence. You should make sense to check vehicle capabilites for geofence
+ /// support.
+ 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(); }
};
#endif
diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml
index 0f0eaba..bcaea77 100644
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/src/FlightDisplay/FlightDisplayViewMap.qml
@@ -27,7 +27,7 @@ FlightMap {
anchors.fill: parent
mapName: _mapName
- property alias missionController: _missionController
+ property alias missionController: missionController
property var flightWidgets
property bool _followVehicle: true
@@ -54,12 +54,12 @@ FlightMap {
QGCPalette { id: qgcPal; colorGroupEnabled: true }
MissionController {
- id: _missionController
+ id: missionController
Component.onCompleted: start(false /* editMode */)
}
GeoFenceController {
- id: _geoFenceController
+ id: geoFenceController
Component.onCompleted: start(false /* editMode */)
}
@@ -93,25 +93,34 @@ FlightMap {
// Add the mission items to the map
MissionItemView {
- model: _mainIsMap ? _missionController.visualItems : 0
+ model: _mainIsMap ? missionController.visualItems : 0
}
// Add lines between waypoints
MissionLineView {
- model: _mainIsMap ? _missionController.waypointLines : 0
+ model: _mainIsMap ? missionController.waypointLines : 0
}
// GeoFence polygon
MapPolygon {
border.color: "#80FF0000"
border.width: 3
- path: _geoFenceController.polygon.path
+ path: geoFenceController.polygonSupported ? geoFenceController.polygon.path : undefined
+ }
+
+ // GeoFence circle
+ MapCircle {
+ border.color: "#80FF0000"
+ border.width: 3
+ center: missionController.plannedHomePosition
+ radius: geoFenceController.circleSupported ? geoFenceController.circleRadius : 0
}
// GeoFence breach return point
MapQuickItem {
anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2)
- coordinate: _geoFenceController.breachReturnPoint
+ coordinate: geoFenceController.breachReturnPoint
+ visible: geoFenceController.breachReturnSupported
sourceItem: MissionItemIndexLabel { label: "F" }
}
diff --git a/src/MissionEditor/GeoFenceEditor.qml b/src/MissionEditor/GeoFenceEditor.qml
index 58a556e..f5fc9ee 100644
--- a/src/MissionEditor/GeoFenceEditor.qml
+++ b/src/MissionEditor/GeoFenceEditor.qml
@@ -7,6 +7,7 @@ import QGroundControl.Controls 1.0
import QGroundControl.FactControls 1.0
import QGroundControl.Palette 1.0
import QGroundControl.FlightMap 1.0
+import QGroundControl.FactSystem 1.0
QGCFlickable {
id: root
@@ -15,7 +16,7 @@ QGCFlickable {
contentHeight: geoFenceEditorRect.height
clip: true
- readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 12)
+ readonly property real _editFieldWidth: Math.min(width - _margin * 2, ScreenTools.defaultFontPixelWidth * 15)
readonly property real _margin: ScreenTools.defaultFontPixelWidth / 2
readonly property real _radius: ScreenTools.defaultFontPixelWidth / 2
@@ -70,6 +71,7 @@ QGCFlickable {
anchors.right: parent.right
wrapMode: Text.WordWrap
text: qsTr("Click in map to set breach return point.")
+ visible: geoFenceController.breachReturnSupported
}
QGCLabel { text: qsTr("Fence Settings:") }
@@ -96,7 +98,7 @@ QGCFlickable {
QGCLabel {
id: textFieldLabel
anchors.baseline: textField.baseline
- text: modelData.name
+ text: geoFenceController.paramLabels[index]
}
FactTextField {
@@ -105,6 +107,18 @@ QGCFlickable {
width: _editFieldWidth
showUnits: true
fact: modelData
+ visible: !comboField.visible
+ }
+
+ FactComboBox {
+ id: comboField
+ anchors.right: parent.right
+ width: _editFieldWidth
+ indexModel: false
+ fact: visible ? modelData : _nullFact
+ visible: modelData.enumStrings.length
+
+ property var _nullFact: Fact { }
}
}
}
@@ -115,6 +129,7 @@ QGCFlickable {
flightMap: editorMap
polygon: root.polygon
sectionLabel: qsTr("Fence Polygon:")
+ visible: geoFenceController.polygonSupported
}
}
}
diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml
index cb6cd17..5989968 100644
--- a/src/MissionEditor/MissionEditor.qml
+++ b/src/MissionEditor/MissionEditor.qml
@@ -154,6 +154,12 @@ QGCView {
id: geoFenceController
Component.onCompleted: start(true /* editMode */)
+
+ onFenceSupportedChanged: {
+ if (!fenceSupported && _editingLayer == _layerGeoFence) {
+ _editingLayer = _layerMission
+ }
+ }
}
QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
@@ -515,7 +521,7 @@ QGCView {
setCurrentItem(sequenceNumber)
}
- onMoveHomeToMapCenter: missionController.visualItems.get(0).coordinate = editorMap.center
+ onMoveHomeToMapCenter: _visualItems.get(0).coordinate = editorMap.center
}
} // ListView
} // Item - Mission Item editor
@@ -537,16 +543,31 @@ QGCView {
MapPolygon {
border.color: "#80FF0000"
border.width: 3
- path: geoFenceController.polygon.path
+ path: geoFenceController.polygonSupported ? geoFenceController.polygon.path : undefined
+ }
+
+ // GeoFence circle
+ MapCircle {
+ border.color: "#80FF0000"
+ border.width: 3
+ center: missionController.plannedHomePosition
+ radius: geoFenceController.circleSupported ? geoFenceController.circleRadius : 0
+ }
+
+ // GeoFence circle
+ MapCircle {
+ border.color: "#80FF0000"
+ border.width: 3
+ center: missionController.plannedHomePosition
+ radius: geoFenceController.circleSupported ? geoFenceController.circleRadius : 0
}
// GeoFence breach return point
MapQuickItem {
anchorPoint: Qt.point(sourceItem.width / 2, sourceItem.height / 2)
coordinate: geoFenceController.breachReturnPoint
- sourceItem: MissionItemIndexLabel {
- label: "F"
- }
+ visible: geoFenceController.breachReturnSupported
+ sourceItem: MissionItemIndexLabel { label: "F" }
}
//-- Dismiss Drop Down (if any)
@@ -583,10 +604,10 @@ QGCView {
DropButton {
id: layerButton
dropDirection: dropRight
- //buttonImage: "/qmlimages/MapCenter.svg"
viewportMargins: ScreenTools.defaultFontPixelWidth / 2
exclusiveGroup: _dropButtonsExclusiveGroup
lightBorders: _lightWidgetBorders
+ visible: geoFenceController.fenceSupported
dropDownComponent: Component {
Column {
@@ -775,19 +796,19 @@ QGCView {
}
MissionItemStatus {
- id: waypointValuesDisplay
- anchors.margins: ScreenTools.defaultFontPixelWidth
- anchors.left: parent.left
- anchors.bottom: parent.bottom
- z: QGroundControl.zOrderTopMost
- currentMissionItem: _currentMissionItem
- missionItems: missionController.visualItems
- expandedWidth: missionItemEditor.x - (ScreenTools.defaultFontPixelWidth * 2)
- missionDistance: missionController.missionDistance
- missionMaxTelemetry: missionController.missionMaxTelemetry
- cruiseDistance: missionController.cruiseDistance
- hoverDistance: missionController.hoverDistance
- visible: _editingLayer == _layerMission && !ScreenTools.isShortScreen
+ id: waypointValuesDisplay
+ anchors.margins: ScreenTools.defaultFontPixelWidth
+ anchors.left: parent.left
+ anchors.bottom: parent.bottom
+ z: QGroundControl.zOrderTopMost
+ currentMissionItem: _currentMissionItem
+ missionItems: missionController.visualItems
+ expandedWidth: missionItemEditor.x - (ScreenTools.defaultFontPixelWidth * 2)
+ missionDistance: missionController.missionDistance
+ missionMaxTelemetry: missionController.missionMaxTelemetry
+ cruiseDistance: missionController.cruiseDistance
+ hoverDistance: missionController.hoverDistance
+ visible: _editingLayer == _layerMission && !ScreenTools.isShortScreen
}
} // FlightMap
} // Item - split view container
diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc
index 7f0398b..71bc2b4 100644
--- a/src/MissionManager/GeoFenceController.cc
+++ b/src/MissionManager/GeoFenceController.cc
@@ -24,7 +24,7 @@ GeoFenceController::GeoFenceController(QObject* parent)
: PlanElementController(parent)
, _dirty(false)
{
- _clearGeoFence();
+
}
GeoFenceController::~GeoFenceController()
@@ -38,83 +38,61 @@ void GeoFenceController::start(bool editMode)
PlanElementController::start(editMode);
- connect(_multiVehicleMgr, &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &GeoFenceController::_parameterReadyVehicleAvailableChanged);
- connect(&_geoFence.polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
-}
-
-void GeoFenceController::setFenceType(GeoFenceTypeEnum fenceType)
-{
- if (_geoFence.fenceType != (GeoFenceManager::GeoFenceType_t)fenceType) {
- _geoFence.fenceType = (GeoFenceManager::GeoFenceType_t)fenceType;
- emit fenceTypeChanged(fenceType);
- }
-}
-
-void GeoFenceController::setCircleRadius(float circleRadius)
-{
- if (qFuzzyCompare(_geoFence.circleRadius, circleRadius)) {
- _geoFence.circleRadius = circleRadius;
- emit circleRadiusChanged(circleRadius);
- }
+ connect(&_polygon, &QGCMapPolygon::dirtyChanged, this, &GeoFenceController::_polygonDirtyChanged);
}
void GeoFenceController::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint)
{
- if (_geoFence.breachReturnPoint != breachReturnPoint) {
- _geoFence.breachReturnPoint = breachReturnPoint;
+ if (_breachReturnPoint != breachReturnPoint) {
+ _breachReturnPoint = breachReturnPoint;
+ setDirty(true);
emit breachReturnPointChanged(breachReturnPoint);
}
}
-void GeoFenceController::_setParams(void)
+void GeoFenceController::_signalAll(void)
{
- if (_params.count() == 0 && _activeVehicle && _multiVehicleMgr->parameterReadyVehicleAvailable()) {
- QStringList skipList;
- skipList << QStringLiteral("FENCE_TOTAL") << QStringLiteral("FENCE_ENABLE");
-
- QStringList allNames = _activeVehicle->autopilotPlugin()->parameterNames(-1);
- foreach (const QString& paramName, allNames) {
- if (paramName.startsWith(QStringLiteral("FENCE_")) && !skipList.contains(paramName)) {
- _params << QVariant::fromValue(_activeVehicle->autopilotPlugin()->getParameterFact(-1, paramName));
- }
- }
- emit paramsChanged();
- }
+ emit fenceSupportedChanged(fenceSupported());
+ emit circleSupportedChanged(circleSupported());
+ emit polygonSupportedChanged(polygonSupported());
+ emit breachReturnSupportedChanged(breachReturnSupported());
+ emit circleRadiusChanged(circleRadius());
+ emit paramsChanged(params());
+ emit paramLabelsChanged(paramLabels());
}
-void GeoFenceController::_activeVehicleBeingRemoved(void)
+void GeoFenceController::_activeVehicleBeingRemoved(Vehicle* vehicle)
{
_clearGeoFence();
- _params.clear();
- emit paramsChanged();
- _activeVehicle->geoFenceManager()->disconnect(this);
+ _signalAll();
+ vehicle->geoFenceManager()->disconnect(this);
}
void GeoFenceController::_activeVehicleSet(void)
{
- connect(_activeVehicle->geoFenceManager(), &GeoFenceManager::newGeoFenceAvailable, this, &GeoFenceController::_newGeoFenceAvailable);
-
- _setParams();
-
- if (_activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) {
- // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle.
- // We don't request mission items for new vehicles since that will happen autamatically.
- loadFromVehicle();
+ GeoFenceManager* geoFenceManager = _activeVehicle->geoFenceManager();
+ connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::_setDirty);
+ connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::_setDirty);
+ connect(geoFenceManager, &GeoFenceManager::fenceSupportedChanged, this, &GeoFenceController::fenceSupportedChanged);
+ connect(geoFenceManager, &GeoFenceManager::circleSupportedChanged, this, &GeoFenceController::circleSupportedChanged);
+ connect(geoFenceManager, &GeoFenceManager::polygonSupportedChanged, this, &GeoFenceController::polygonSupportedChanged);
+ connect(geoFenceManager, &GeoFenceManager::breachReturnSupportedChanged, this, &GeoFenceController::breachReturnSupportedChanged);
+ connect(geoFenceManager, &GeoFenceManager::circleRadiusChanged, this, &GeoFenceController::circleRadiusChanged);
+ connect(geoFenceManager, &GeoFenceManager::polygonChanged, this, &GeoFenceController::_setPolygon);
+ connect(geoFenceManager, &GeoFenceManager::breachReturnPointChanged, this, &GeoFenceController::setBreachReturnPoint);
+ connect(geoFenceManager, &GeoFenceManager::paramsChanged, this, &GeoFenceController::paramsChanged);
+ connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged, this, &GeoFenceController::paramLabelsChanged);
+
+ if (_activeVehicle->getParameterLoader()->parametersAreReady()) {
+ _signalAll();
+ if (!syncInProgress()) {
+ // We are switching between two previously existing vehicles. We have to manually ask for the items from the Vehicle.
+ // We don't request mission items for new vehicles since that will happen autamatically.
+ loadFromVehicle();
+ }
}
}
-void GeoFenceController::_parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable)
-{
- Q_UNUSED(parameterReadyVehicleAvailable);
- _setParams();
-}
-
-void GeoFenceController::_newGeoFenceAvailable(void)
-{
- _setGeoFence(_activeVehicle->geoFenceManager()->geoFence());
- setDirty(false);
-}
-
void GeoFenceController::loadFromFilePicker(void)
{
@@ -143,7 +121,7 @@ void GeoFenceController::removeAll(void)
void GeoFenceController::loadFromVehicle(void)
{
if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) {
- _activeVehicle->geoFenceManager()->requestGeoFence();
+ _activeVehicle->geoFenceManager()->loadFromVehicle();
} else {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress();
}
@@ -152,11 +130,11 @@ void GeoFenceController::loadFromVehicle(void)
void GeoFenceController::sendToVehicle(void)
{
if (_activeVehicle && _activeVehicle->getParameterLoader()->parametersAreReady() && !syncInProgress()) {
- // FIXME: Hack
- setFenceType(GeoFencePolygon);
+ _activeVehicle->geoFenceManager()->setPolygon(polygon());
+ _activeVehicle->geoFenceManager()->setBreachReturnPoint(breachReturnPoint());
setDirty(false);
- _geoFence.polygon.setDirty(false);
- _activeVehicle->geoFenceManager()->setGeoFence(_geoFence);
+ _polygon.setDirty(false);
+ _activeVehicle->geoFenceManager()->sendToVehicle();
} else {
qCWarning(GeoFenceControllerLog) << "GeoFenceController::loadFromVehicle call at wrong time" << _activeVehicle << _activeVehicle->getParameterLoader()->parametersAreReady() << syncInProgress();
}
@@ -164,19 +142,8 @@ void GeoFenceController::sendToVehicle(void)
void GeoFenceController::_clearGeoFence(void)
{
- setFenceType(GeoFenceNone);
- setCircleRadius(0.0);
setBreachReturnPoint(QGeoCoordinate());
- _geoFence.polygon.clear();
-}
-
-void GeoFenceController::_setGeoFence(const GeoFenceManager::GeoFence_t& geoFence)
-{
- _clearGeoFence();
- setFenceType(static_cast<GeoFenceTypeEnum>(geoFence.fenceType));
- setCircleRadius(geoFence.circleRadius);
- setBreachReturnPoint(geoFence.breachReturnPoint);
- _geoFence.polygon = geoFence.polygon;
+ _polygon.clear();
}
bool GeoFenceController::syncInProgress(void) const
@@ -190,7 +157,7 @@ bool GeoFenceController::syncInProgress(void) const
bool GeoFenceController::dirty(void) const
{
- return _dirty | _geoFence.polygon.dirty();
+ return _dirty | _polygon.dirty();
}
@@ -199,7 +166,7 @@ void GeoFenceController::setDirty(bool dirty)
if (dirty != _dirty) {
_dirty = dirty;
if (!dirty) {
- _geoFence.polygon.setDirty(dirty);
+ _polygon.setDirty(dirty);
}
emit dirtyChanged(dirty);
}
@@ -211,3 +178,77 @@ void GeoFenceController::_polygonDirtyChanged(bool dirty)
setDirty(true);
}
}
+
+bool GeoFenceController::fenceSupported(void) const
+{
+ if (_activeVehicle) {
+ return _activeVehicle->geoFenceManager()->fenceSupported();
+ } else {
+ return true;
+ }
+}
+
+bool GeoFenceController::circleSupported(void) const
+{
+ if (_activeVehicle) {
+ return _activeVehicle->geoFenceManager()->circleSupported();
+ } else {
+ return true;
+ }
+}
+
+bool GeoFenceController::polygonSupported(void) const
+{
+ if (_activeVehicle) {
+ return _activeVehicle->geoFenceManager()->polygonSupported();
+ } else {
+ return true;
+ }
+}
+
+bool GeoFenceController::breachReturnSupported(void) const
+{
+ if (_activeVehicle) {
+ return _activeVehicle->geoFenceManager()->breachReturnSupported();
+ } else {
+ return true;
+ }
+}
+
+void GeoFenceController::_setDirty(void)
+{
+ setDirty(true);
+}
+
+void GeoFenceController::_setPolygon(const QList<QGeoCoordinate>& polygon)
+{
+ _polygon.setPath(polygon);
+ setDirty(true);
+}
+
+float GeoFenceController::circleRadius(void) const
+{
+ if (_activeVehicle) {
+ return _activeVehicle->geoFenceManager()->circleRadius();
+ } else {
+ return 0.0;
+ }
+}
+
+QVariantList GeoFenceController::params(void) const
+{
+ if (_activeVehicle) {
+ return _activeVehicle->geoFenceManager()->params();
+ } else {
+ return QVariantList();
+ }
+}
+
+QStringList GeoFenceController::paramLabels(void) const
+{
+ if (_activeVehicle) {
+ return _activeVehicle->geoFenceManager()->paramLabels();
+ } else {
+ return QStringList();
+ }
+}
diff --git a/src/MissionManager/GeoFenceController.h b/src/MissionManager/GeoFenceController.h
index a5a6d88..52cb99e 100644
--- a/src/MissionManager/GeoFenceController.h
+++ b/src/MissionManager/GeoFenceController.h
@@ -27,17 +27,15 @@ public:
GeoFenceController(QObject* parent = NULL);
~GeoFenceController();
- enum GeoFenceTypeEnum {
- GeoFenceNone = GeoFenceManager::GeoFenceNone,
- GeoFenceCircle = GeoFenceManager::GeoFenceCircle,
- GeoFencePolygon = GeoFenceManager::GeoFencePolygon,
- };
-
- Q_PROPERTY(GeoFenceTypeEnum fenceType READ fenceType WRITE setFenceType NOTIFY fenceTypeChanged)
- Q_PROPERTY(float circleRadius READ circleRadius WRITE setCircleRadius NOTIFY circleRadiusChanged)
- Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT)
- Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
- Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged)
+ Q_PROPERTY(bool fenceSupported READ fenceSupported NOTIFY fenceSupportedChanged)
+ Q_PROPERTY(bool circleSupported READ circleSupported NOTIFY circleSupportedChanged)
+ Q_PROPERTY(bool polygonSupported READ polygonSupported NOTIFY polygonSupportedChanged)
+ Q_PROPERTY(bool breachReturnSupported READ breachReturnSupported NOTIFY breachReturnSupportedChanged)
+ Q_PROPERTY(float circleRadius READ circleRadius NOTIFY circleRadiusChanged)
+ Q_PROPERTY(QGCMapPolygon* polygon READ polygon CONSTANT)
+ Q_PROPERTY(QGeoCoordinate breachReturnPoint READ breachReturnPoint WRITE setBreachReturnPoint NOTIFY breachReturnPointChanged)
+ Q_PROPERTY(QVariantList params READ params NOTIFY paramsChanged)
+ Q_PROPERTY(QStringList paramLabels READ paramLabels NOTIFY paramLabelsChanged)
void start (bool editMode) final;
void loadFromVehicle (void) final;
@@ -51,39 +49,46 @@ public:
bool dirty (void) const final;
void setDirty (bool dirty) final;
- GeoFenceTypeEnum fenceType (void) const { return (GeoFenceTypeEnum)_geoFence.fenceType; }
- float circleRadius (void) const { return _geoFence.circleRadius; }
- QGCMapPolygon* polygon (void) { return &_geoFence.polygon; }
- QGeoCoordinate breachReturnPoint (void) const { return _geoFence.breachReturnPoint; }
- QVariantList params (void) { return _params; }
+ bool fenceSupported (void) const;
+ bool circleSupported (void) const;
+ bool polygonSupported (void) const;
+ bool breachReturnSupported (void) const;
+ float circleRadius (void) const;
+ QGCMapPolygon* polygon (void) { return &_polygon; }
+ QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
+ QVariantList params (void) const;
+ QStringList paramLabels (void) const;
- void setFenceType(GeoFenceTypeEnum fenceType);
- void setCircleRadius(float circleRadius);
+public slots:
void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
signals:
- void fenceTypeChanged (GeoFenceTypeEnum fenceType);
- void circleRadiusChanged (float circleRadius);
- void polygonPathChanged (const QVariantList& polygonPath);
- void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
- void paramsChanged (void);
+ void fenceSupportedChanged (bool fenceSupported);
+ void circleSupportedChanged (bool circleSupported);
+ void polygonSupportedChanged (bool polygonSupported);
+ void breachReturnSupportedChanged (bool breachReturnSupported);
+ void circleRadiusChanged (float circleRadius);
+ void polygonPathChanged (const QVariantList& polygonPath);
+ void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
+ void paramsChanged (QVariantList params);
+ void paramLabelsChanged (QStringList paramLabels);
private slots:
- void _parameterReadyVehicleAvailableChanged(bool parameterReadyVehicleAvailable);
- void _newGeoFenceAvailable(void);
void _polygonDirtyChanged(bool dirty);
+ void _setDirty(void);
+ void _setPolygon(const QList<QGeoCoordinate>& polygon);
private:
- void _setParams(void);
void _clearGeoFence(void);
- void _setGeoFence(const GeoFenceManager::GeoFence_t& geoFence);
+ void _signalAll(void);
- void _activeVehicleBeingRemoved(void) final;
+ void _activeVehicleBeingRemoved(Vehicle* vehicle) final;
void _activeVehicleSet(void) final;
- bool _dirty;
- GeoFenceManager::GeoFence_t _geoFence;
- QVariantList _params;
+ bool _dirty;
+ QGCMapPolygon _polygon;
+ QGeoCoordinate _breachReturnPoint;
+ QVariantList _params;
};
#endif
diff --git a/src/MissionManager/GeoFenceManager.cc b/src/MissionManager/GeoFenceManager.cc
index 9bc9ace..e34ea2a 100644
--- a/src/MissionManager/GeoFenceManager.cc
+++ b/src/MissionManager/GeoFenceManager.cc
@@ -7,27 +7,16 @@
*
****************************************************************************/
-
-/// @file
-/// @author Don Gagne <don@thegagnes.com>
-
#include "GeoFenceManager.h"
#include "Vehicle.h"
-#include "FirmwarePlugin.h"
-#include "MAVLinkProtocol.h"
-#include "QGCApplication.h"
QGC_LOGGING_CATEGORY(GeoFenceManagerLog, "GeoFenceManagerLog")
-const char* GeoFenceManager::_fenceTotalParam = "FENCE_TOTAL";
-const char* GeoFenceManager::_fenceActionParam = "FENCE_ACTION";
-
GeoFenceManager::GeoFenceManager(Vehicle* vehicle)
- : _vehicle(vehicle)
- , _readTransactionInProgress(false)
- , _writeTransactionInProgress(false)
+ : QObject(vehicle)
+ , _vehicle(vehicle)
{
- connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &GeoFenceManager::_mavlinkMessageReceived);
+
}
GeoFenceManager::~GeoFenceManager()
@@ -42,213 +31,29 @@ void GeoFenceManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
emit error(errorCode, errorMsg);
}
-void GeoFenceManager::setGeoFence(const GeoFence_t& geoFence)
+void GeoFenceManager::loadFromVehicle(void)
{
- if (_readTransactionInProgress) {
- _sendError(InternalError, QStringLiteral("Geo-Fence write attempted while read in progress."));
- return;
- }
-
- if (!_geoFenceSupported()) {
- return;
- }
-
- // Validate
- switch (geoFence.fenceType) {
- case GeoFencePolygon:
- if (geoFence.polygon.count() < 3) {
- _sendError(TooFewPoints, QStringLiteral("Geo-Fence polygon must contain at least 3 points."));
- return;
- }
- if (geoFence.polygon.count() > std::numeric_limits<uint8_t>::max()) {
- _sendError(TooManyPoints, QStringLiteral("Geo-Fence polygon has too many points: %1.").arg(geoFence.polygon.count()));
- return;
- }
- break;
- case GeoFenceCircle:
- if (geoFence.circleRadius <= 0.0) {
- _sendError(InvalidCircleRadius, QStringLiteral("Geo-Fence circle radius must be greater than 0."));
- return;
- }
- default:
- break;
- }
-
- _geoFence.fenceType = geoFence.fenceType;
- _geoFence.circleRadius = geoFence.circleRadius;
- _geoFence.polygon = geoFence.polygon;
- _geoFence.breachReturnPoint = geoFence.breachReturnPoint;
-
- if (_geoFence.fenceType != GeoFencePolygon) {
- // Circle is just params, so no more work to do
- return;
- }
-
- emit newGeoFenceAvailable();
-
- // First thing is to turn off geo fence while we are updating. This prevents the vehicle from going haywire it is in the air
- Fact* fenceActionFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceActionParam);
- _savedWriteFenceAction = fenceActionFact->rawValue();
- fenceActionFact->setRawValue(0);
-
- // Fence total param includes:
- // index 0: breach return
- // last index: polygon close (same as first polygon point)
- _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->setRawValue(_geoFence.polygon.count() + 2);
-
- // FIXME: No validation of correct fence received
- // Send points:
- // breach return
- // polygon fence points
- // polygon close
- // Put back previous fence action to start geofence again
- _sendFencePoint(0);
- for (uint8_t index=0; index<_geoFence.polygon.count(); index++) {
- _sendFencePoint(index + 1);
- }
- _sendFencePoint(_geoFence.polygon.count() + 1);
- fenceActionFact->setRawValue(_savedWriteFenceAction);
+ // No geofence support in unknown vehicle
}
-void GeoFenceManager::requestGeoFence(void)
+void GeoFenceManager::sendToVehicle(void)
{
- _clearGeoFence();
-
- if (!_geoFenceSupported()) {
- return;
- }
-
- // Point 0: Breach return point
- // Point [1,N]: Polygon points
- // Point N+1: Close polygon point (same as point 1)
- int cFencePoints = _vehicle->getParameterFact(FactSystem::defaultComponentId, _fenceTotalParam)->rawValue().toInt();
- qCDebug(GeoFenceManagerLog) << "GeoFenceManager::requestGeoFence" << cFencePoints;
- if (cFencePoints == 0) {
- // No fence, no more work to do, fence data has already been cleared
- return;
- }
- if (cFencePoints < 0 || (cFencePoints > 0 && cFencePoints < 6)) {
- _sendError(TooFewPoints, QStringLiteral("Geo-Fence information from Vehicle has too few points: %1").arg(cFencePoints));
- return;
- }
- if (cFencePoints > std::numeric_limits<uint8_t>::max()) {
- _sendError(TooManyPoints, QStringLiteral("Geo-Fence information from Vehicle has too many points: %1").arg(cFencePoints));
- return;
- }
-
- _readTransactionInProgress = true;
- _cReadFencePoints = cFencePoints;
- _currentFencePoint = 0;
-
- _requestFencePoint(_currentFencePoint);
-}
-
-/// Called when a new mavlink message for out vehicle is received
-void GeoFenceManager::_mavlinkMessageReceived(const mavlink_message_t& message)
-{
- if (message.msgid == MAVLINK_MSG_ID_FENCE_POINT) {
- mavlink_fence_point_t fencePoint;
-
- mavlink_msg_fence_point_decode(&message, &fencePoint);
- if (fencePoint.idx != _currentFencePoint) {
- // FIXME: Protocol out of whack
- qCWarning(GeoFenceManagerLog) << "Indices out of sync" << fencePoint.idx << _currentFencePoint;
- return;
- }
-
- if (fencePoint.idx == 0) {
- _geoFence.breachReturnPoint = QGeoCoordinate(fencePoint.lat, fencePoint.lng);
- qCDebug(GeoFenceManagerLog) << "From vehicle: breach return point" << _geoFence.breachReturnPoint;
- _requestFencePoint(++_currentFencePoint);
- } else if (fencePoint.idx < _cReadFencePoints - 1) {
- QGeoCoordinate polyCoord(fencePoint.lat, fencePoint.lng);
- _geoFence.polygon.addCoordinate(polyCoord);
- qCDebug(GeoFenceManagerLog) << "From vehicle: polygon point" << fencePoint.idx << polyCoord;
- if (fencePoint.idx < _cReadFencePoints - 2) {
- // Still more points to request
- _requestFencePoint(++_currentFencePoint);
- } else {
- // We've finished collecting fence points
- _readTransactionInProgress = false;
- emit newGeoFenceAvailable();
- }
- }
- }
-}
-
-void GeoFenceManager::_clearGeoFence(void)
-{
- _geoFence.fenceType = GeoFenceNone;
- _geoFence.circleRadius = 0.0;
- _geoFence.polygon.clear();
- _geoFence.breachReturnPoint = QGeoCoordinate();
- emit newGeoFenceAvailable();
+ // No geofence support in unknown vehicle
}
-void GeoFenceManager::_requestFencePoint(uint8_t pointIndex)
+void GeoFenceManager::setPolygon(QGCMapPolygon* polygon)
{
- mavlink_message_t msg;
- MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-
- qCDebug(GeoFenceManagerLog) << "GeoFenceManager::_requestFencePoint" << pointIndex;
- mavlink_msg_fence_fetch_point_pack(mavlink->getSystemId(),
- mavlink->getComponentId(),
- &msg,
- _vehicle->id(),
- _vehicle->defaultComponentId(),
- pointIndex);
- _vehicle->sendMessageOnPriorityLink(msg);
-}
-
-void GeoFenceManager::_sendFencePoint(uint8_t pointIndex)
-{
- mavlink_message_t msg;
- MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
-
- QGeoCoordinate fenceCoord;
- if (pointIndex == 0) {
- fenceCoord = _geoFence.breachReturnPoint;
- } else if (pointIndex - 1 < _geoFence.polygon.count()) {
- fenceCoord = _geoFence.polygon[pointIndex - 1];
- } else {
- // Polygon close point
- fenceCoord = _geoFence.polygon[0];
+ _polygon.clear();
+ for (int index=0; index<polygon->count(); index++) {
+ _polygon.append((*polygon)[index]);
}
-
- mavlink_msg_fence_point_pack(mavlink->getSystemId(),
- mavlink->getComponentId(),
- &msg,
- _vehicle->id(),
- _vehicle->defaultComponentId(),
- pointIndex, // Index of point to set
- _geoFence.polygon.count() + 2, // Total point count, +1 for breach in index 0, +1 polygon close in last index
- fenceCoord.latitude(),
- fenceCoord.longitude());
- _vehicle->sendMessageOnPriorityLink(msg);
+ emit polygonChanged(_polygon);
}
-bool GeoFenceManager::inProgress(void) const
+void GeoFenceManager::setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint)
{
- return _readTransactionInProgress || _writeTransactionInProgress;
-}
-
-bool GeoFenceManager::_geoFenceSupported(void)
-{
- // FIXME: MockLink doesn't support geo fence yet
- if (qgcApp()->runningUnitTests()) {
- return false;
- }
-
- // FIXME: Hack to get around lack of plugin-ized version of code
- if (_vehicle->apmFirmware()) {
- if (!_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceTotalParam) ||
- !_vehicle->parameterExists(FactSystem::defaultComponentId, _fenceActionParam)) {
- _sendError(InternalError, QStringLiteral("Vehicle does not support Geo-Fence implementation."));
- return false;
- } else {
- return true;
- }
- } else {
- return false;
+ if (breachReturnPoint != _breachReturnPoint) {
+ _breachReturnPoint = breachReturnPoint;
+ emit breachReturnPointChanged(breachReturnPoint);
}
}
diff --git a/src/MissionManager/GeoFenceManager.h b/src/MissionManager/GeoFenceManager.h
index bfbbd60..4a0f4e6 100644
--- a/src/MissionManager/GeoFenceManager.h
+++ b/src/MissionManager/GeoFenceManager.h
@@ -11,19 +11,17 @@
#define GeoFenceManager_H
#include <QObject>
-#include <QTimer>
#include <QGeoCoordinate>
-#include "MissionItem.h"
-#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
-#include "LinkInterface.h"
#include "QGCMapPolygon.h"
class Vehicle;
Q_DECLARE_LOGGING_CATEGORY(GeoFenceManagerLog)
+/// This is the base class for firmware specific geofence managers. A geofence manager is responsible
+/// for communicating with the vehicle to set/get geofence settings.
class GeoFenceManager : public QObject
{
Q_OBJECT
@@ -32,29 +30,30 @@ public:
GeoFenceManager(Vehicle* vehicle);
~GeoFenceManager();
- typedef enum {
- GeoFenceNone,
- GeoFenceCircle,
- GeoFencePolygon,
- } GeoFenceType_t;
+ /// Returns true if the manager is currently communicating with the vehicle
+ virtual bool inProgress(void) const { return false; }
+
+ /// Load the current settings from teh vehicle
+ virtual void loadFromVehicle(void);
- typedef struct {
- GeoFenceType_t fenceType;
- float circleRadius;
- QGCMapPolygon polygon;
- QGeoCoordinate breachReturnPoint;
- } GeoFence_t;
+ /// Send the current settings to the vehicle
+ virtual void sendToVehicle(void);
- bool inProgress(void) const;
+ // Support flags
+ virtual bool fenceSupported (void) const { return false; }
+ virtual bool circleSupported (void) const { return false; }
+ virtual bool polygonSupported (void) const { return false; }
+ virtual bool breachReturnSupported (void) const { return false; }
- /// Request the geo fence from the vehicle
- void requestGeoFence(void);
+ virtual float circleRadius (void) const { return 0.0; }
+ QList<QGeoCoordinate> polygon (void) const { return _polygon; }
+ QGeoCoordinate breachReturnPoint (void) const { return _breachReturnPoint; }
- /// Returns the current geofence settings
- const GeoFence_t& geoFence(void) const { return _geoFence; }
+ virtual QVariantList params (void) const { return QVariantList(); }
+ virtual QStringList paramLabels (void) const { return QStringList(); }
- /// Set and send the specified geo fence to the vehicle
- void setGeoFence(const GeoFence_t& geoFence);
+ void setPolygon(QGCMapPolygon* polygon);
+ void setBreachReturnPoint(const QGeoCoordinate& breachReturnPoint);
/// Error codes returned in error signal
typedef enum {
@@ -65,35 +64,24 @@ public:
} ErrorCode_t;
signals:
- void newGeoFenceAvailable(void);
- void inProgressChanged(bool inProgress);
- void error(int errorCode, const QString& errorMsg);
+ void fenceSupportedChanged (bool fenceSupported);
+ void circleSupportedChanged (bool circleSupported);
+ void polygonSupportedChanged (bool polygonSupported);
+ void breachReturnSupportedChanged (bool fenceSupported);
+ void circleRadiusChanged (float circleRadius);
+ void polygonChanged (QList<QGeoCoordinate> polygon);
+ void breachReturnPointChanged (QGeoCoordinate breachReturnPoint);
+ void inProgressChanged (bool inProgress);
+ void error (int errorCode, const QString& errorMsg);
+ void paramsChanged (QVariantList params);
+ void paramLabelsChanged (QStringList paramLabels);
-private slots:
- void _mavlinkMessageReceived(const mavlink_message_t& message);
- //void _ackTimeout(void);
-
-private:
+protected:
void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
- void _clearGeoFence(void);
- void _requestFencePoint(uint8_t pointIndex);
- void _sendFencePoint(uint8_t pointIndex);
- bool _geoFenceSupported(void);
-
-private:
- Vehicle* _vehicle;
-
- bool _readTransactionInProgress;
- bool _writeTransactionInProgress;
-
- uint8_t _cReadFencePoints;
- uint8_t _currentFencePoint;
- QVariant _savedWriteFenceAction;
-
- GeoFence_t _geoFence;
- static const char* _fenceTotalParam;
- static const char* _fenceActionParam;
+ Vehicle* _vehicle;
+ QList<QGeoCoordinate> _polygon;
+ QGeoCoordinate _breachReturnPoint;
};
#endif
diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc
index 63d3ac0..d73604a 100644
--- a/src/MissionManager/MissionController.cc
+++ b/src/MissionManager/MissionController.cc
@@ -623,8 +623,6 @@ void MissionController::_recalcWaypointLines(void)
if (!homeItem) {
qWarning() << "Home item is not SimpleMissionItem";
- } else {
- connect(homeItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcAltitudeRangeBearing);
}
bool showHomePosition = homeItem->showHomePosition();
@@ -944,6 +942,10 @@ void MissionController::_initAllVisualItems(void)
homeItem->setShowHomePosition(true);
}
+ emit plannedHomePositionChanged(plannedHomePosition());
+
+ connect(homeItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_homeCoordinateChanged);
+
QmlObjectListModel* newComplexItems = new QmlObjectListModel(this);
for (int i=0; i<_visualItems->count(); i++) {
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
@@ -1021,17 +1023,17 @@ void MissionController::_itemCommandChanged(void)
_recalcWaypointLines();
}
-void MissionController::_activeVehicleBeingRemoved(void)
+void MissionController::_activeVehicleBeingRemoved(Vehicle* vehicle)
{
qCDebug(MissionControllerLog) << "_activeVehicleSet _activeVehicleBeingRemoved";
- MissionManager* missionManager = _activeVehicle->missionManager();
+ MissionManager* missionManager = vehicle->missionManager();
disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailableFromVehicle);
disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged);
disconnect(missionManager, &MissionManager::currentItemChanged, this, &MissionController::_currentMissionItemChanged);
- disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged);
- disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged);
+ disconnect(vehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged);
+ disconnect(vehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged);
// We always remove all items on vehicle change. This leaves a user model hole:
// If the user has unsaved changes in the Plan view they will lose them
@@ -1069,6 +1071,7 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi
if (homeItem) {
homeItem->setShowHomePosition(homePositionAvailable);
+ emit plannedHomePositionChanged(plannedHomePosition());
_recalcWaypointLines();
} else {
qWarning() << "Unabled to cast home item to SimpleMissionItem";
@@ -1079,8 +1082,17 @@ void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePosi
void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
{
if (!_editMode && _visualItems) {
- qobject_cast<VisualMissionItem*>(_visualItems->get(0))->setCoordinate(homePosition);
- _recalcWaypointLines();
+ VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(0));
+ if (item) {
+ if (item->coordinate() != homePosition) {
+ item->setCoordinate(homePosition);
+ qCDebug(MissionControllerLog) << "Home position update" << homePosition;
+ emit plannedHomePositionChanged(plannedHomePosition());
+ _recalcWaypointLines();
+ }
+ } else {
+ qWarning() << "Unabled to cast home item to VisualMissionItem";
+ }
}
}
@@ -1256,3 +1268,21 @@ void MissionController::setDirty(bool dirty)
_visualItems->setDirty(dirty);
}
}
+
+QGeoCoordinate MissionController::plannedHomePosition(void)
+{
+ if (_visualItems && _visualItems->count() > 0) {
+ SimpleMissionItem* item = qobject_cast<SimpleMissionItem*>(_visualItems->get(0));
+ if (item && item->showHomePosition()) {
+ return item->coordinate();
+ }
+ }
+
+ return QGeoCoordinate();
+}
+
+void MissionController::_homeCoordinateChanged(void)
+{
+ emit plannedHomePositionChanged(plannedHomePosition());
+ _recalcAltitudeRangeBearing();
+}
diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h
index ff5f7b2..a662c7e 100644
--- a/src/MissionManager/MissionController.h
+++ b/src/MissionManager/MissionController.h
@@ -34,6 +34,7 @@ public:
MissionController(QObject* parent = NULL);
~MissionController();
+ Q_PROPERTY(QGeoCoordinate plannedHomePosition READ plannedHomePosition NOTIFY plannedHomePositionChanged)
Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged)
Q_PROPERTY(QmlObjectListModel* complexVisualItems READ complexVisualItems NOTIFY complexVisualItemsChanged)
Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged)
@@ -70,6 +71,7 @@ public:
// Property accessors
+ QGeoCoordinate plannedHomePosition (void);
QmlObjectListModel* visualItems (void) { return _visualItems; }
QmlObjectListModel* complexVisualItems (void) { return _complexItems; }
QmlObjectListModel* waypointLines (void) { return &_waypointLines; }
@@ -79,7 +81,6 @@ public:
double cruiseDistance (void) const { return _cruiseDistance; }
double hoverDistance (void) const { return _hoverDistance; }
-
void setMissionDistance (double missionDistance );
void setMissionMaxTelemetry (double missionMaxTelemetry);
void setCruiseDistance (double cruiseDistance );
@@ -88,6 +89,7 @@ public:
static const char* jsonSimpleItemsKey; ///< Key for simple items in a json file
signals:
+ void plannedHomePositionChanged(QGeoCoordinate plannedHomePosition);
void visualItemsChanged(void);
void complexVisualItemsChanged(void);
void waypointLinesChanged(void);
@@ -105,7 +107,8 @@ private slots:
void _inProgressChanged(bool inProgress);
void _currentMissionItemChanged(int sequenceNumber);
void _recalcWaypointLines(void);
- void _recalcAltitudeRangeBearing();
+ void _recalcAltitudeRangeBearing(void);
+ void _homeCoordinateChanged(void);
private:
void _recalcSequence(void);
@@ -128,7 +131,7 @@ private:
int _nextSequenceNumber(void);
// Overrides from PlanElementController
- void _activeVehicleBeingRemoved(void) final;
+ void _activeVehicleBeingRemoved(Vehicle* vehicle) final;
void _activeVehicleSet(void) final;
private:
diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc
index 05ebd3e..981b5e7 100644
--- a/src/MissionManager/MissionManager.cc
+++ b/src/MissionManager/MissionManager.cc
@@ -574,8 +574,8 @@ void MissionManager::_handleMissionCurrent(const mavlink_message_t& message)
mavlink_msg_mission_current_decode(&message, &missionCurrent);
- qCDebug(MissionManagerLog) << "_handleMissionCurrent seq:" << missionCurrent.seq;
if (missionCurrent.seq != _currentMissionItem) {
+ qCDebug(MissionManagerLog) << "_handleMissionCurrent seq:" << missionCurrent.seq;
_currentMissionItem = missionCurrent.seq;
emit currentItemChanged(_currentMissionItem);
}
diff --git a/src/MissionManager/PlanElementController.cc b/src/MissionManager/PlanElementController.cc
index 11c2acc..e875590 100644
--- a/src/MissionManager/PlanElementController.cc
+++ b/src/MissionManager/PlanElementController.cc
@@ -34,7 +34,9 @@ void PlanElementController::start(bool editMode)
void PlanElementController::_activeVehicleChanged(Vehicle* activeVehicle)
{
if (_activeVehicle) {
- _activeVehicleBeingRemoved();
+ Vehicle* vehicleSave = _activeVehicle;
+ _activeVehicle = NULL;
+ _activeVehicleBeingRemoved(vehicleSave);
}
_activeVehicle = activeVehicle;
diff --git a/src/MissionManager/PlanElementController.h b/src/MissionManager/PlanElementController.h
index b7d26c1..63b8ec7 100644
--- a/src/MissionManager/PlanElementController.h
+++ b/src/MissionManager/PlanElementController.h
@@ -57,8 +57,8 @@ protected:
bool _editMode;
/// Called when the current active vehicle has been removed. Derived classes should override
- /// to implement custom behavior.
- virtual void _activeVehicleBeingRemoved(void) = 0;
+ /// to implement custom behavior. When this is called _activeVehicle has already been cleared.
+ virtual void _activeVehicleBeingRemoved(Vehicle* removedVehicle) = 0;
/// Called when a new active vehicle has been set. Derived classes should override
/// to implement custom behavior.
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 8947836..0be5314 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -199,13 +199,14 @@ Vehicle::Vehicle(LinkInterface* link,
connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError);
connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_newMissionItemsAvailable);
- _geoFenceManager = new GeoFenceManager(this);
- connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
-
_parameterLoader = new ParameterLoader(this);
connect(_parameterLoader, &ParameterLoader::parametersReady, _autopilotPlugin, &AutoPilotPlugin::_parametersReadyPreChecks);
connect(_parameterLoader, &ParameterLoader::parameterListProgress, _autopilotPlugin, &AutoPilotPlugin::parameterListProgress);
+ // GeoFenceManager needs to access ParameterLoader so make sure to create afters
+ _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
+ connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
+
// Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet.
mavlink_message_t versionMsg;
@@ -1826,7 +1827,7 @@ void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
#endif
/// Returns true if the specifed parameter exists from the default component
-bool Vehicle::parameterExists(int componentId, const QString& name)
+bool Vehicle::parameterExists(int componentId, const QString& name) const
{
return _autopilotPlugin->parameterExists(componentId, name);
}
@@ -1844,7 +1845,7 @@ void Vehicle::_newMissionItemsAvailable(void)
// After the initial mission request complets we ask for the geofence
if (!_geoFenceManagerInitialRequestComplete) {
_geoFenceManagerInitialRequestComplete = true;
- _geoFenceManager->requestGeoFence();
+ _geoFenceManager->loadFromVehicle();
}
}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index a2cfe95..2a6def0 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -570,7 +570,7 @@ public:
bool xConfigMotors(void);
/// Returns true if the specifed parameter exists from the default component
- bool parameterExists(int componentId, const QString& name);
+ bool parameterExists(int componentId, const QString& name) const;
/// Returns the specified parameter Fact from the default component
/// WARNING: Returns a default Fact if parameter does not exists. If that possibility exists, check for existence first with