Merge pull request #4050 from DonLakeFlyer/BreachValidate

GeoFence: Breach validate

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -540,14 +540,40 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes
 
 void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
 {
-    // Streams are not started automatically on APM stack
-    vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,        2);
-    vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS,    2);
-    vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,        2);
-    vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,           3);
-    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,             10);
-    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,             10);
-    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,             3);
+    if (vehicle->isOfflineEditingVehicle()) {
+        switch (vehicle->vehicleType()) {
+        case MAV_TYPE_QUADROTOR:
+        case MAV_TYPE_HEXAROTOR:
+        case MAV_TYPE_OCTOROTOR:
+        case MAV_TYPE_TRICOPTER:
+        case MAV_TYPE_COAXIAL:
+        case MAV_TYPE_HELICOPTER:
+            vehicle->setFirmwareVersion(3, 4, 0);
+            break;
+        case MAV_TYPE_FIXED_WING:
+            vehicle->setFirmwareVersion(3, 5, 0);
+            break;
+        case MAV_TYPE_GROUND_ROVER:
+        case MAV_TYPE_SURFACE_BOAT:
+            vehicle->setFirmwareVersion(3, 0, 0);
+            break;
+        case MAV_TYPE_SUBMARINE:
+            vehicle->setFirmwareVersion(3, 4, 0);
+            break;
+        default:
+            // No version set
+            break;
+        }
+    } else {
+        // Streams are not started automatically on APM stack
+        vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,     2);
+        vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2);
+        vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,     2);
+        vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,        3);
+        vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,          10);
+        vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,          10);
+        vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,          3);
+    }
 }
 
 void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)

src/FirmwarePlugin/APM/CopterGeoFenceEditor.qml

@@ -78,5 +78,7 @@ Column {
         polygon:        geoFenceController.polygon
         sectionLabel:   qsTr("Fence Polygon:")
         visible:        geoFenceController.polygonSupported
+
+        onPolygonEditCompleted: geoFenceController.validateBreachReturn()
     }
 }

src/FirmwarePlugin/APM/PlaneGeoFenceEditor.qml

@@ -202,5 +202,7 @@ Column {
         flightMap:      editorMap
         polygon:        geoFenceController.polygon
         sectionLabel:   qsTr("Fence Polygon:")
+
+        onPolygonEditCompleted: geoFenceController.validateBreachReturn()
     }
 }

src/FirmwarePlugin/FirmwarePlugin.h

@@ -60,7 +60,7 @@ public:
     ///     value:  remapParamNameMinorVersionRemapMap_t entry
     typedef QMap<int, remapParamNameMinorVersionRemapMap_t> remapParamNameMajorVersionMap_t;
 
-    /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware.
+    /// Called when Vehicle is first created to perform any firmware specific setup.
     virtual void initializeVehicle(Vehicle* vehicle);
 
     /// @return true: Firmware supports all specified capabilites

src/FirmwarePlugin/PX4/PX4GeoFenceManager.cc

@@ -18,6 +18,10 @@ PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
     , _circleRadiusFact(NULL)
 {
     connect(_vehicle->getParameterManager(), &ParameterManager::parametersReady,  this, &PX4GeoFenceManager::_parametersReady);
+
+    if (_vehicle->getParameterManager()->parametersAreReady()) {
+        _parametersReady();
+    }
 }
 
 PX4GeoFenceManager::~PX4GeoFenceManager()

src/MissionEditor/MissionEditor.qml

@@ -154,6 +154,12 @@ QGCView {
 
         Component.onCompleted: start(true /* editMode */)
 
+        onFenceSupportedChanged: {
+            if (!fenceSupported && _editingLayer == _layerGeoFence) {
+                _editingLayer = _layerMission
+            }
+        }
+
         function saveToSelectedFile() {
             if (ScreenTools.isMobile) {
                 qgcView.showDialog(mobileFileSaver, qsTr("Save Fence File"), qgcView.showDialogDefaultWidth, StandardButton.Save | StandardButton.Cancel)
@@ -170,9 +176,14 @@ QGCView {
             }
         }
 
-        onFenceSupportedChanged: {
-            if (!fenceSupported && _editingLayer == _layerGeoFence) {
-                _editingLayer = _layerMission
+        function validateBreachReturn() {
+            if (geoFenceController.polygon.path.length > 0) {
+                if (!geoFenceController.polygon.containsCoordinate(geoFenceController.breachReturnPoint)) {
+                    geoFenceController.breachReturnPoint = geoFenceController.polygon.center()
+                }
+                if (!geoFenceController.polygon.containsCoordinate(geoFenceController.breachReturnPoint)) {
+                    geoFenceController.breachReturnPoint = geoFenceController.polygon.path[0]
+                }
             }
         }
     }
@@ -316,6 +327,7 @@ QGCView {
                             break
                         case _layerGeoFence:
                             geoFenceController.breachReturnPoint = coordinate
+                            geoFenceController.validateBreachReturn()
                             break
                         }
                     }
@@ -373,7 +385,7 @@ QGCView {
 
                 // Add the complex mission item polygon to the map
                 MapItemView {
-                    model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined
+                    model: missionController.complexVisualItems
 
                     delegate: MapPolygon {
                         color:      'green'
@@ -384,7 +396,7 @@ QGCView {
 
                 // Add the complex mission item grid to the map
                 MapItemView {
-                    model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined
+                    model: missionController.complexVisualItems
 
                     delegate: MapPolyline {
                         line.color: "white"
@@ -395,7 +407,7 @@ QGCView {
 
                 // Add the complex mission item exit coordinates
                 MapItemView {
-                    model: _editingLayer == _layerMission ? missionController.complexVisualItems : undefined
+                    model: missionController.complexVisualItems
                     delegate:   exitCoordinateComponent
                 }
 
@@ -413,7 +425,7 @@ QGCView {
 
                 // Add the simple mission items to the map
                 MapItemView {
-                    model:      _editingLayer == _layerMission ? missionController.visualItems : undefined
+                    model:      missionController.visualItems
                     delegate:   missionItemComponent
                 }
 

src/MissionManager/QGCMapPolygon.cc

@@ -13,7 +13,6 @@
 
 #include <QGeoRectangle>
 #include <QDebug>
-#include <QPolygon>
 #include <QJsonArray>
 
 const char* QGCMapPolygon::_jsonPolygonKey = "polygon";
@@ -67,24 +66,61 @@ void QGCMapPolygon::setDirty(bool dirty)
     }
 }
 
-QGeoCoordinate QGCMapPolygon::center(void) const
+QGeoCoordinate QGCMapPolygon::_coordFromPointF(const QPointF& point) const
 {
-    QPolygonF polygon;
+    QGeoCoordinate coord;
+
+    if (_polygonPath.count() > 0) {
+        QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
+        convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord);
+    }
 
-    QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
+    return coord;
+}
 
-    foreach(const QVariant& coordVar, _polygonPath) {
+QPointF QGCMapPolygon::_pointFFromCoord(const QGeoCoordinate& coordinate) const
+{
+    if (_polygonPath.count() > 0) {
         double y, x, down;
+        QGeoCoordinate tangentOrigin = _polygonPath[0].value<QGeoCoordinate>();
 
-        convertGeoToNed(coordVar.value<QGeoCoordinate>(), tangentOrigin, &y, &x, &down);
-        polygon << QPointF(x, -y);
+        convertGeoToNed(coordinate, tangentOrigin, &y, &x, &down);
+        return QPointF(x, -y);
     }
 
-    QGeoCoordinate centerCoord;
-    QPointF centerPoint = polygon.boundingRect().center();
-    convertNedToGeo(-centerPoint.y(), centerPoint.x(), 0, tangentOrigin, &centerCoord);
+    return QPointF();
+}
+
+QPolygonF QGCMapPolygon::_toPolygonF(void) const
+{
+    QPolygonF polygon;
 
-    return centerCoord;
+    if (_polygonPath.count() > 2) {
+        for (int i=0; i<_polygonPath.count(); i++) {
+            polygon.append(_pointFFromCoord(_polygonPath[i].value<QGeoCoordinate>()));
+        }
+    }
+
+    return polygon;
+}
+
+bool QGCMapPolygon::containsCoordinate(const QGeoCoordinate& coordinate) const
+{
+    if (_polygonPath.count() > 2) {
+        return _toPolygonF().containsPoint(_pointFFromCoord(coordinate), Qt::OddEvenFill);
+    } else {
+        return false;
+    }
+}
+
+QGeoCoordinate QGCMapPolygon::center(void) const
+{
+    if (_polygonPath.count() > 2) {
+        QPointF centerPoint = _toPolygonF().boundingRect().center();
+        return _coordFromPointF(centerPoint);
+    } else {
+        return QGeoCoordinate();
+    }
 }
 
 void QGCMapPolygon::setPath(const QList<QGeoCoordinate>& path)

src/MissionManager/QGCMapPolygon.h

@@ -13,7 +13,11 @@
 #include <QObject>
 #include <QGeoCoordinate>
 #include <QVariantList>
+#include <QPolygon>
 
+/// The QGCMapPolygon class provides a polygon which can be displayed on a map using a MapPolygon control.
+/// It works in conjunction with the QGCMapPolygonControls control which provides the UI for drawing and
+/// editing map polygons.
 class QGCMapPolygon : public QObject
 {
     Q_OBJECT
@@ -23,24 +27,33 @@ public:
 
     const QGCMapPolygon& operator=(const QGCMapPolygon& other);
 
-    Q_PROPERTY(QVariantList path    READ path   WRITE setPath   NOTIFY pathChanged)
-    Q_PROPERTY(bool         dirty   READ dirty  WRITE setDirty  NOTIFY dirtyChanged)
+    QGeoCoordinate operator[](int index) const { return _polygonPath[index].value<QGeoCoordinate>(); }
+
+    /// The polygon path to be bound to the MapPolygon.path property
+    Q_PROPERTY(QVariantList path READ path WRITE setPath NOTIFY pathChanged)
 
+    /// true: Polygon has changed since last time dirty was false
+    Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged)
+
+    /// Remove all points from polygon
     Q_INVOKABLE void clear(void);
+
+    /// Adjust the value for the specified coordinate
+    ///     @param vertexIndex Polygon point index to modify (0-based)
+    ///     @param coordinate New coordinate for point
     Q_INVOKABLE void adjustCoordinate(int vertexIndex, const QGeoCoordinate coordinate);
-    Q_INVOKABLE QGeoCoordinate center(void) const;
-    Q_INVOKABLE int count(void) const { return _polygonPath.count(); }
 
-    QVariantList path(void) const { return _polygonPath; }
-    void setPath(const QList<QGeoCoordinate>& path);
-    void setPath(const QVariantList& path);
+    /// Returns the center point coordinate for the polygon
+    Q_INVOKABLE QGeoCoordinate center(void) const;
 
-    QList<QGeoCoordinate> coordinateList(void) const;
+    /// Returns true if the specified coordinate is within the polygon
+    Q_INVOKABLE bool containsCoordinate(const QGeoCoordinate& coordinate) const;
 
-    QGeoCoordinate operator[](int index) const { return _polygonPath[index].value<QGeoCoordinate>(); }
+    /// Returns the number of points in the polygon
+    Q_INVOKABLE int count(void) const { return _polygonPath.count(); }
 
-    bool dirty(void) const { return _dirty; }
-    void setDirty(bool dirty);
+    /// Returns the path in a list of QGeoCoordinate's format
+    QList<QGeoCoordinate> coordinateList(void) const;
 
     /// Saves the polygon to the json object.
     ///     @param json Json object to save to
@@ -53,11 +66,24 @@ public:
     /// @return true: success, false: failure (errorString set)
     bool loadFromJson(const QJsonObject& json, bool required, QString& errorString);
 
+    // Property methods
+
+    bool dirty(void) const { return _dirty; }
+    void setDirty(bool dirty);
+
+    QVariantList path(void) const { return _polygonPath; }
+    void setPath(const QList<QGeoCoordinate>& path);
+    void setPath(const QVariantList& path);
+
 signals:
     void pathChanged(void);
     void dirtyChanged(bool dirty);
 
 private:
+    QPolygonF _toPolygonF(void) const;
+    QGeoCoordinate _coordFromPointF(const QPointF& point) const;
+    QPointF _pointFFromCoord(const QGeoCoordinate& coordinate) const;
+
     QVariantList    _polygonPath;
     bool            _dirty;
 

src/Vehicle/Vehicle.cc

@@ -328,6 +328,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _vibrationFactGroup(this)
 {
     _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
+    _firmwarePlugin->initializeVehicle(this);
 
     _missionManager = new MissionManager(this);
     connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError);