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/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;
             }