PX4 GeoFence plugin

Also change Plan Element selection ui

qgroundcontrol.pro

@@ -700,6 +700,7 @@ HEADERS+= \
     src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h \
     src/FirmwarePlugin/PX4/px4_custom_mode.h \
     src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \
+    src/FirmwarePlugin/PX4/PX4GeoFenceManager.h \
     src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \
     src/Vehicle/MultiVehicleManager.h \
     src/Vehicle/Vehicle.h \
@@ -761,6 +762,7 @@ SOURCES += \
     src/FirmwarePlugin/FirmwarePlugin.cc \
     src/FirmwarePlugin/FirmwarePluginManager.cc \
     src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \
+    src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc \
     src/FirmwarePlugin/PX4/PX4ParameterMetaData.cc \
     src/Vehicle/MultiVehicleManager.cc \
     src/Vehicle/Vehicle.cc \

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -17,6 +17,7 @@
 #include "FirmwarePlugin.h"
 #include "ParameterLoader.h"
 #include "PX4ParameterMetaData.h"
+#include "PX4GeoFenceManager.h"
 
 class PX4FirmwarePlugin : public FirmwarePlugin
 {
@@ -30,31 +31,32 @@ public:
     QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
     QList<MAV_CMD> supportedMissionCommands(void) final;
 
-    bool        isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
-    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;
-    void        setGuidedMode                   (Vehicle* vehicle, bool guidedMode) final;
-    void        pauseVehicle                    (Vehicle* vehicle) final;
-    void        guidedModeRTL                   (Vehicle* vehicle) final;
-    void        guidedModeLand                  (Vehicle* vehicle) final;
-    void        guidedModeTakeoff               (Vehicle* vehicle, double altitudeRel) final;
-    void        guidedModeOrbit                 (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) final;
-    void        guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
-    void        guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel) final;
-    bool        isGuidedMode                    (const Vehicle* vehicle) const;
-    int         manualControlReservedButtonCount(void) final;
-    bool        supportsManualControl           (void) 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("SYS_AUTOSTART"); }
-    QString     missionCommandOverrides         (MAV_TYPE vehicleType) const final;
-    QString     getVersionParam                 (void) final { return QString("SYS_PARAM_VER"); }
-    QString     internalParameterMetaDataFile   (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); }
-    void        getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
-    QObject*    loadParameterMetaData           (const QString& metaDataFile);
-    bool        adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message);
+    bool                isCapable                       (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
+    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;
+    void                setGuidedMode                   (Vehicle* vehicle, bool guidedMode) final;
+    void                pauseVehicle                    (Vehicle* vehicle) final;
+    void                guidedModeRTL                   (Vehicle* vehicle) final;
+    void                guidedModeLand                  (Vehicle* vehicle) final;
+    void                guidedModeTakeoff               (Vehicle* vehicle, double altitudeRel) final;
+    void                guidedModeOrbit                 (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) final;
+    void                guidedModeGotoLocation          (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
+    void                guidedModeChangeAltitude        (Vehicle* vehicle, double altitudeRel) final;
+    bool                isGuidedMode                    (const Vehicle* vehicle) const;
+    int                 manualControlReservedButtonCount(void) final;
+    bool                supportsManualControl           (void) 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("SYS_AUTOSTART"); }
+    QString             missionCommandOverrides         (MAV_TYPE vehicleType) const final;
+    QString             getVersionParam                 (void) final { return QString("SYS_PARAM_VER"); }
+    QString             internalParameterMetaDataFile   (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); }
+    void                getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
+    QObject*            loadParameterMetaData           (const QString& metaDataFile);
+    bool                adjustIncomingMavlinkMessage    (Vehicle* vehicle, mavlink_message_t* message);
+    GeoFenceManager*    newGeoFenceManager              (Vehicle* vehicle) { return new PX4GeoFenceManager(vehicle); }
 
     // Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the
     // names may change.

src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc

+/****************************************************************************
+ *
+ *   (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 "ParameterLoader.h"
+
+PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
+    : GeoFenceManager(vehicle)
+    , _firstParamLoadComplete(false)
+    , _circleRadiusFact(NULL)
+{
+    connect(_vehicle->getParameterLoader(), &ParameterLoader::parametersReady,  this, &PX4GeoFenceManager::_parametersReady);
+}
+
+PX4GeoFenceManager::~PX4GeoFenceManager()
+{
+
+}
+
+void PX4GeoFenceManager::_parametersReady(void)
+{
+    if (!_firstParamLoadComplete) {
+        _firstParamLoadComplete = true;
+
+        _circleRadiusFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("GF_MAX_HOR_DIST"));
+        connect(_circleRadiusFact, &Fact::rawValueChanged, this, &PX4GeoFenceManager::_circleRadiusRawValueChanged);
+        emit circleRadiusChanged(circleRadius());
+
+        QStringList paramNames;
+        QStringList paramLabels;
+
+        paramNames << QStringLiteral("GF_ACTION") << QStringLiteral("GF_MAX_HOR_DIST") << QStringLiteral("GF_MAX_VER_DIST");
+        paramLabels << QStringLiteral("Action:") << QStringLiteral("Radius:") << QStringLiteral("Max Altitude:");
+
+        _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 circleSupportedChanged(circleSupported());
+
+        qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" <<
+                                       fenceSupported() << circleSupported() << polygonSupported() << breachReturnSupported();
+    }
+}
+
+float PX4GeoFenceManager::circleRadius(void) const
+{
+    if (_circleRadiusFact) {
+        return _circleRadiusFact->rawValue().toFloat();
+    } else {
+        return 0.0;
+    }
+}
+
+void PX4GeoFenceManager::_circleRadiusRawValueChanged(QVariant value)
+{
+    emit circleRadiusChanged(value.toFloat());
+    emit circleSupportedChanged(circleSupported());
+}
+
+bool PX4GeoFenceManager::circleSupported(void) const
+{
+    if (_circleRadiusFact) {
+        return _circleRadiusFact->rawValue().toFloat() >= 0.0;
+    }
+
+    return false;
+}

src/FirmwarePlugin/PX4/PX4GeoFenceManager.h

+/****************************************************************************
+ *
+ *   (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            fenceSupported          (void) const final { return true; }
+    bool            circleSupported         (void) const final;
+    float           circleRadius            (void) const final;
+    QVariantList    params                  (void) const final { return _params; }
+    QStringList     paramLabels             (void) const final { return _paramLabels; }
+
+private slots:
+    void _circleRadiusRawValueChanged(QVariant value);
+    void _parametersReady(void);
+    
+private:
+    bool _firstParamLoadComplete;
+
+    QVariantList    _params;
+    QStringList     _paramLabels;
+
+    Fact* _circleRadiusFact;
+};
+
+#endif

src/MissionEditor/GeoFenceEditor.qml

@@ -44,7 +44,7 @@ QGCFlickable {
             anchors.margins:    _margin
             anchors.left:       parent.left
             anchors.top:        parent.top
-            text:               qsTr("Geo-Fence (WIP careful!)")
+            text:               qsTr("GeoFence (WIP careful!)")
             color:              "black"
         }
 

src/MissionEditor/MissionEditor.qml

@@ -471,16 +471,58 @@ QGCView {
                         }
                 }
 
+                // Mission/GeoFence selector
+                Item {
+                    id:                 planElementSelector
+                    anchors.topMargin:  parent.height - ScreenTools.availableHeight + _margin
+                    anchors.top:        parent.top
+                    anchors.leftMargin: parent.width - _rightPanelWidth
+                    anchors.left:       parent.left
+                    width:              planElementSelectorRow.width
+                    height:             geoFenceController.fenceSupported ? planElementSelectorRow.height : 0
+                    visible:            geoFenceController.fenceSupported
+
+                    ExclusiveGroup {
+                        id: planElementSelectorGroup
+                        onCurrentChanged: {
+                            var layerIsMission = current == planElementMission
+                            _editingLayer = layerIsMission ? _layerMission : _layerGeoFence
+                            _syncDropDownController = layerIsMission ? missionController : geoFenceController
+                        }
+                    }
+
+                    Row {
+                        id:     planElementSelectorRow
+                        spacing: _horizontalMargin
+
+                        QGCRadioButton {
+                            id:             planElementMission
+                            text:           qsTr("Mission")
+                            checked:        true
+                            exclusiveGroup: planElementSelectorGroup
+                            color:          mapPal.text
+                        }
+
+                        QGCRadioButton {
+                            id:             planElementGeoFence
+                            text:           qsTr("GeoFence")
+                            exclusiveGroup: planElementSelectorGroup
+                            color:          mapPal.text
+                        }
+                    }
+                }
+
                 // Mission Item Editor
                 Item {
-                    id:             missionItemEditor
-                    height:         ScreenTools.availableHeight
-                    anchors.bottom: parent.bottom
-                    anchors.right:  parent.right
-                    width:          _rightPanelWidth
-                    opacity:        _rightPanelOpacity
-                    z:              QGroundControl.zOrderTopMost
-                    visible:        _editingLayer == _layerMission
+                    id:                 missionItemEditor
+                    anchors.topMargin:  _margin
+                    anchors.top:        planElementSelector.bottom
+                    anchors.bottom:     parent.bottom
+                    anchors.right:      parent.right
+                    width:              _rightPanelWidth
+                    opacity:            _rightPanelOpacity
+                    z:                  QGroundControl.zOrderTopMost
+                    visible:            _editingLayer == _layerMission
 
                     MouseArea {
                          // This MouseArea prevents the Map below it from getting Mouse events. Without this
@@ -528,8 +570,8 @@ QGCView {
 
                 // GeoFence Editor
                 Loader {
-                    anchors.topMargin:  parent.height - ScreenTools.availableHeight
-                    anchors.top:        parent.top
+                    anchors.topMargin:  _margin
+                    anchors.top:        planElementSelector.bottom
                     anchors.right:      parent.right
                     opacity:            _rightPanelOpacity
                     z:                  QGroundControl.zOrderTopMost
@@ -601,47 +643,6 @@ QGCView {
                     spacing:            ScreenTools.defaultFontPixelHeight
                     z:                  QGroundControl.zOrderWidgets
 
-                    DropButton {
-                        id:                 layerButton
-                        dropDirection:      dropRight
-                        viewportMargins:    ScreenTools.defaultFontPixelWidth / 2
-                        exclusiveGroup:     _dropButtonsExclusiveGroup
-                        lightBorders:       _lightWidgetBorders
-                        visible:            geoFenceController.fenceSupported
-
-                        dropDownComponent: Component {
-                            Column {
-                                spacing: ScreenTools.defaultFontPixelWidth * 0.5
-
-                                QGCLabel { text: qsTr("Editing Layer:") }
-
-                                Row {
-                                    spacing: ScreenTools.defaultFontPixelWidth
-
-                                    QGCButton {
-                                        text: qsTr("Mission")
-
-                                        onClicked: {
-                                            layerButton.hideDropDown()
-                                            _editingLayer = _layerMission
-                                            _syncDropDownController = missionController
-                                        }
-                                    }
-
-                                    QGCButton {
-                                        text: qsTr("GeoFence")
-
-                                        onClicked: {
-                                            layerButton.hideDropDown()
-                                            _editingLayer = _layerGeoFence
-                                            _syncDropDownController = geoFenceController
-                                        }
-                                    }
-                                }
-                            }
-                        }
-                    }
-
                     RoundButton {
                         id:             addMissionItemsButton
                         buttonImage:    "/qmlimages/MapAddMission.svg"
@@ -818,10 +819,10 @@ QGCView {
         id: syncLoadFromVehicleOverwrite
         QGCViewMessage {
             id:         syncLoadFromVehicleCheck
-            message:   qsTr("You have unsaved/unsent mission changes. Loading the mission from the Vehicle will lose these changes. Are you sure you want to load the mission from the Vehicle?")
+            message:   qsTr("You have unsaved/unsent changes. Loading from the Vehicle will lose these changes. Are you sure you want to load from the Vehicle?")
             function accept() {
                 hideDialog()
-                loadFromVehicle()
+                _syncDropDownController.loadFromVehicle()
             }
         }
     }
@@ -830,10 +831,10 @@ QGCView {
         id: syncLoadFromFileOverwrite
         QGCViewMessage {
             id:         syncLoadFromVehicleCheck
-            message:   qsTr("You have unsaved/unsent mission changes. Loading a mission from a file will lose these changes. Are you sure you want to load a mission from a file?")
+            message:   qsTr("You have unsaved/unsent changes. Loading a from a file will lose these changes. Are you sure you want to load from a file?")
             function accept() {
                 hideDialog()
-                loadFromFile()
+                _syncDropDownController.loadFromSelectedFile()
             }
         }
     }
@@ -841,10 +842,10 @@ QGCView {
     Component {
         id: removeAllPromptDialog
         QGCViewMessage {
-            message: qsTr("Are you sure you want to delete all mission items?")
+            message: qsTr("Are you sure you want to remove all items?")
             function accept() {
                 itemDragger.clearItem()
-                missionController.removeAll()
+                _syncDropDownController.removeAll()
                 hideDialog()
             }
         }
@@ -857,11 +858,13 @@ QGCView {
             id:         columnHolder
             spacing:    _margin
 
+            property string _overwriteText: (_editingLayer == _layerMission) ? qsTr("Mission overwrite") : qsTr("GeoFence overwrite")
+
             QGCLabel {
                 width:      sendSaveGrid.width
                 wrapMode:   Text.WordWrap
                 text:       _syncDropDownController.dirty ?
-                                qsTr("You have unsaved changes to your mission. You should send to your vehicle, or save to a file:") :
+                                qsTr("You have unsaved changes. You should send to your vehicle, or save to a file:") :
                                 qsTr("Sync:")
             }
 
@@ -889,7 +892,7 @@ QGCView {
                     onClicked: {
                         syncButton.hideDropDown()
                         if (_syncDropDownController.dirty) {
-                            _root.showDialog(syncLoadFromVehicleOverwrite, qsTr("Mission overwrite"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel)
+                            _root.showDialog(syncLoadFromVehicleOverwrite, columnHolder._overwriteText, _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel)
                         } else {
                             _syncDropDownController.loadFromVehicle()
                         }
@@ -913,7 +916,7 @@ QGCView {
                     onClicked: {
                         syncButton.hideDropDown()
                         if (_syncDropDownController.dirty) {
-                            _root.showDialog(syncLoadFromFileOverwrite, qsTr("Mission overwrite"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel)
+                            _root.showDialog(syncLoadFromFileOverwrite, columnHolder._overwriteText, _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.Cancel)
                         } else {
                             _syncDropDownController.loadFromSelectedFile()
                         }
@@ -926,7 +929,7 @@ QGCView {
                     onClicked:  {
                         syncButton.hideDropDown()
                         _syncDropDownController.removeAll()
-                        _root.showDialog(removeAllPromptDialog, qsTr("Delete all"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No)
+                        _root.showDialog(removeAllPromptDialog, qsTr("Remove all"), _root.showDialogDefaultWidth, StandardButton.Yes | StandardButton.No)
                     }
                 }
             }

src/MissionManager/GeoFenceController.cc

@@ -83,8 +83,9 @@ void GeoFenceController::_activeVehicleSet(void)
     connect(geoFenceManager, &GeoFenceManager::paramsChanged,                   this, &GeoFenceController::paramsChanged);
     connect(geoFenceManager, &GeoFenceManager::paramLabelsChanged,              this, &GeoFenceController::paramLabelsChanged);
 
+    _signalAll();
+
     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.
@@ -223,7 +224,8 @@ void GeoFenceController::_setDirty(void)
 void GeoFenceController::_setPolygon(const QList<QGeoCoordinate>& polygon)
 {
     _polygon.setPath(polygon);
-    setDirty(true);
+    // This is coming from a GeoFenceManager::loadFromVehicle call
+    setDirty(false);
 }
 
 float GeoFenceController::circleRadius(void) const

src/comm/LinkManager.cc

@@ -188,7 +188,7 @@ void LinkManager::_addLink(LinkInterface* link)
             if (!(_mavlinkChannelsUsedBitMask & 1 << i)) {
                 mavlink_reset_channel_status(i);
                 link->_setMavlinkChannel(i);
-                _mavlinkChannelsUsedBitMask |= i << i;
+                _mavlinkChannelsUsedBitMask |= 1 << i;
                 channelSet = true;
                 break;
             }