Merge pull request #5751 from DonLakeFlyer/StructureScan

Structure scan: Support for scan distance from structure polygon

src/MissionManager/PlanManager.cc

@@ -911,7 +911,7 @@ void PlanManager::removeAll(void)
         return;
     }
 
-    qCDebug(PlanManagerLog) << QStringLiteral("removeAll").arg(_planTypeString());
+    qCDebug(PlanManagerLog) << QStringLiteral("removeAll %1").arg(_planTypeString());
 
     _clearAndDeleteMissionItems();
 

src/MissionManager/QGCMapPolygon.cc

@@ -15,6 +15,7 @@
 #include <QGeoRectangle>
 #include <QDebug>
 #include <QJsonArray>
+#include <QLineF>
 
 const char* QGCMapPolygon::jsonPolygonKey = "polygon";
 
@@ -290,7 +291,7 @@ void QGCMapPolygon::_updateCenter(void)
     if (!_ignoreCenterUpdates) {
         QGeoCoordinate center;
 
-        if (_polygonPath.count() > 2) {            
+        if (_polygonPath.count() > 2) {
             QPointF centroid(0, 0);
             QPolygonF polygonF = _toPolygonF();
             for (int i=0; i<polygonF.count(); i++) {
@@ -356,3 +357,80 @@ QGeoCoordinate QGCMapPolygon::vertexCoordinate(int vertex) const
         return QGeoCoordinate();
     }
 }
+
+QList<QPointF> QGCMapPolygon::nedPolygon(void)
+{
+    QList<QPointF>  nedPolygon;
+
+    if (count() > 0) {
+        QGeoCoordinate  tangentOrigin = vertexCoordinate(0);
+
+        for (int i=0; i<_polygonModel.count(); i++) {
+            double y, x, down;
+            QGeoCoordinate vertex = vertexCoordinate(i);
+            if (i == 0) {
+                // This avoids a nan calculation that comes out of convertGeoToNed
+                x = y = 0;
+            } else {
+                convertGeoToNed(vertex, tangentOrigin, &y, &x, &down);
+            }
+            nedPolygon += QPointF(x, y);
+        }
+    }
+
+    return nedPolygon;
+}
+
+
+void QGCMapPolygon::offset(double distance)
+{
+    QList<QGeoCoordinate> rgNewPolygon;
+
+    // I'm sure there is some beautiful famous algorithm to do this, but here is a brute force method
+
+    if (count() > 2) {
+        // Convert the polygon to NED
+        QList<QPointF> rgNedVertices = nedPolygon();
+
+        // Walk the edges, offsetting by the specified distance
+        QList<QLineF> rgOffsetEdges;
+        for (int i=0; i<rgNedVertices.count(); i++) {
+            int     lastIndex = i == rgNedVertices.count() - 1 ? 0 : i + 1;
+            QLineF  offsetEdge;
+            QLineF  originalEdge(rgNedVertices[i], rgNedVertices[lastIndex]);
+
+            QLineF workerLine = originalEdge;
+            workerLine.setLength(distance);
+            workerLine.setAngle(workerLine.angle() - 90.0);
+            offsetEdge.setP1(workerLine.p2());
+
+            workerLine.setPoints(originalEdge.p2(), originalEdge.p1());
+            workerLine.setLength(distance);
+            workerLine.setAngle(workerLine.angle() + 90.0);
+            offsetEdge.setP2(workerLine.p2());
+
+            rgOffsetEdges.append(offsetEdge);
+        }
+
+        // Intersect the offset edges to generate new vertices
+        QPointF         newVertex;
+        QGeoCoordinate  tangentOrigin = vertexCoordinate(0);
+        for (int i=0; i<rgOffsetEdges.count(); i++) {
+            int prevIndex = i == 0 ? rgOffsetEdges.count() - 1 : i - 1;
+            if (rgOffsetEdges[prevIndex].intersect(rgOffsetEdges[i], &newVertex) == QLineF::NoIntersection) {
+                // FIXME: Better error handling?
+                qWarning("Intersection failed");
+                return;
+            }
+            QGeoCoordinate coord;
+            convertNedToGeo(newVertex.y(), newVertex.x(), 0, tangentOrigin, &coord);
+            rgNewPolygon.append(coord);
+        }
+    }
+
+    // Update internals
+    clear();
+    for (int i=0; i<rgNewPolygon.count(); i++) {
+        appendVertex(rgNewPolygon[i]);
+    }
+}

src/MissionManager/QGCMapPolygon.h

@@ -52,6 +52,9 @@ public:
     /// Returns true if the specified coordinate is within the polygon
     Q_INVOKABLE bool containsCoordinate(const QGeoCoordinate& coordinate) const;
 
+    /// Offsets the current polygon edges by the specified distance in meters
+    Q_INVOKABLE void offset(double distance);
+
     /// Returns the path in a list of QGeoCoordinate's format
     QList<QGeoCoordinate> coordinateList(void) const;
 
@@ -69,6 +72,9 @@ public:
     /// @return true: success, false: failure (errorString set)
     bool loadFromJson(const QJsonObject& json, bool required, QString& errorString);
 
+    /// Convert polygon to NED and return (D is ignored)
+    QList<QPointF> nedPolygon(void);
+
     // Property methods
 
     int             count       (void) const { return _polygonPath.count(); }

src/MissionManager/StructureScan.SettingsGroup.json

@@ -23,6 +23,15 @@
     "units":            "m",
     "defaultValue":     25
 },
+{
+    "name":             "Scan distance",
+    "shortDescription": "Scan distance away from structure.",
+    "type":             "double",
+    "decimalPlaces":    2,
+    "min":              0,
+    "units":            "m",
+    "defaultValue":     25
+},
 {
     "name":             "Trigger distance",
     "shortDescription": "Distance between each triggering of the camera. 0 specifies not camera trigger.",

src/MissionManager/StructureScanComplexItem.h

@@ -30,20 +30,24 @@ public:
     Q_PROPERTY(Fact*            layers                      READ layers                         CONSTANT)
     Q_PROPERTY(Fact*            layerDistance               READ layerDistance                  CONSTANT)
     Q_PROPERTY(Fact*            cameraTriggerDistance       READ cameraTriggerDistance          CONSTANT)
+    Q_PROPERTY(Fact*            scanDistance                READ scanDistance                   CONSTANT)
     Q_PROPERTY(bool             altitudeRelative            MEMBER _altitudeRelative            NOTIFY altitudeRelativeChanged)
     Q_PROPERTY(int              cameraShots                 READ cameraShots                    NOTIFY cameraShotsChanged)
     Q_PROPERTY(double           timeBetweenShots            READ timeBetweenShots               NOTIFY timeBetweenShotsChanged)
     Q_PROPERTY(double           cameraMinTriggerInterval    MEMBER _cameraMinTriggerInterval    NOTIFY cameraMinTriggerIntervalChanged)
-    Q_PROPERTY(QGCMapPolygon*   mapPolygon                  READ mapPolygon                     CONSTANT)
+    Q_PROPERTY(QGCMapPolygon*   structurePolygon            READ structurePolygon               CONSTANT)
+    Q_PROPERTY(QGCMapPolygon*   flightPolygon               READ flightPolygon                  CONSTANT)
 
     Fact* altitude              (void) { return &_altitudeFact; }
     Fact* layers                (void) { return &_layersFact; }
     Fact* layerDistance         (void) { return &_layerDistanceFact; }
     Fact* cameraTriggerDistance (void) { return &_cameraTriggerDistanceFact; }
+    Fact* scanDistance          (void) { return &_scanDistanceFact; }
 
     int             cameraShots     (void) const;
     double          timeBetweenShots(void) const;
-    QGCMapPolygon*  mapPolygon      (void) { return &_mapPolygon; }
+    QGCMapPolygon*  structurePolygon(void) { return &_structurePolygon; }
+    QGCMapPolygon*  flightPolygon   (void) { return &_flightPolygon; }
 
     Q_INVOKABLE void rotateEntryPoint(void);
 
@@ -95,9 +99,10 @@ private slots:
     void _setDirty(void);
     void _polygonDirtyChanged(bool dirty);
     void _polygonCountChanged(int count);
-    void _polygonPathChanged(void);
+    void _flightPathChanged(void);
     void _clearInternal(void);
     void _updateCoordinateAltitudes(void);
+    void _rebuildFlightPolygon(void);
 
 private:
     void _setExitCoordinate(const QGeoCoordinate& coordinate);
@@ -107,7 +112,8 @@ private:
 
     int             _sequenceNumber;
     bool            _dirty;
-    QGCMapPolygon   _mapPolygon;
+    QGCMapPolygon   _structurePolygon;
+    QGCMapPolygon   _flightPolygon;
     bool            _altitudeRelative;
     int             _entryVertex;       // Polygon vertext which is used as the mission entry point
 
@@ -124,39 +130,13 @@ private:
     Fact    _layersFact;
     Fact    _layerDistanceFact;
     Fact    _cameraTriggerDistanceFact;
+    Fact    _scanDistanceFact;
 
     static const char* _altitudeFactName;
     static const char* _layersFactName;
     static const char* _layerDistanceFactName;
     static const char* _cameraTriggerDistanceFactName;
-
-    static const char* _jsonGridObjectKey;
-    static const char* _jsonGridAltitudeKey;
-    static const char* _jsonGridAltitudeRelativeKey;
-    static const char* _jsonGridAngleKey;
-    static const char* _jsonGridSpacingKey;
-    static const char* _jsonGridEntryLocationKey;
-    static const char* _jsonTurnaroundDistKey;
-    static const char* _jsonCameraTriggerDistanceKey;
-    static const char* _jsonCameraTriggerInTurnaroundKey;
-    static const char* _jsonHoverAndCaptureKey;
-    static const char* _jsonGroundResolutionKey;
-    static const char* _jsonFrontalOverlapKey;
-    static const char* _jsonSideOverlapKey;
-    static const char* _jsonCameraSensorWidthKey;
-    static const char* _jsonCameraSensorHeightKey;
-    static const char* _jsonCameraResolutionWidthKey;
-    static const char* _jsonCameraResolutionHeightKey;
-    static const char* _jsonCameraFocalLengthKey;
-    static const char* _jsonCameraMinTriggerIntervalKey;
-    static const char* _jsonManualGridKey;
-    static const char* _jsonCameraObjectKey;
-    static const char* _jsonCameraNameKey;
-    static const char* _jsonCameraOrientationLandscapeKey;
-    static const char* _jsonFixedValueIsAltitudeKey;
-    static const char* _jsonRefly90DegreesKey;
-
-    static const int _hoverAndCaptureDelaySeconds = 1;
+    static const char* _scanDistanceFactName;
 };
 
 #endif

src/PlanView/StructureScanEditor.qml

@@ -60,11 +60,19 @@ Rectangle {
         QGCLabel {
             anchors.left:   parent.left
             anchors.right:  parent.right
-            text:           qsTr("WARNING: WORK IN PROGRESS. USE AT YOUR OWN RISK.")
+            text:           qsTr("WARNING: WORK IN PROGRESS. USE AT YOUR OWN RISK. MEANT FOR DISCUSSION ONLY. DO NOT REPORT BUGS.")
             wrapMode:       Text.WordWrap
             color:          qgcPal.warningText
         }
 
+        QGCLabel {
+            anchors.left:   parent.left
+            anchors.right:  parent.right
+            text:           qsTr("Note: Polygon respresents structure surface not vehicle flight path.")
+            wrapMode:       Text.WordWrap
+            font.pointSize: ScreenTools.smallFontPointSize
+        }
+
         QGCLabel {
             anchors.left:   parent.left
             anchors.right:  parent.right
@@ -99,6 +107,12 @@ Rectangle {
                 Layout.fillWidth:   true
             }
 
+            QGCLabel { text: qsTr("Scan distance") }
+            FactTextField {
+                fact:               missionItem.scanDistance
+                Layout.fillWidth:   true
+            }
+
             QGCLabel { text: qsTr("Trigger Distance") }
             FactTextField {
                 fact:               missionItem.cameraTriggerDistance

src/PlanView/StructureScanMapVisual.qml

@@ -25,31 +25,28 @@ Item {
     property var map    ///< Map control to place item in
 
     property var _missionItem:      object
-    property var _mapPolygon:       object.mapPolygon
-    property var _gridComponent
+    property var _structurePolygon: object.structurePolygon
+    property var _flightPolygon:    object.flightPolygon
     property var _entryCoordinate
     property var _exitCoordinate
 
     signal clicked(int sequenceNumber)
 
     function _addVisualElements() {
-        _gridComponent = gridComponent.createObject(map)
         _entryCoordinate = entryPointComponent.createObject(map)
         _exitCoordinate = exitPointComponent.createObject(map)
-        map.addMapItem(_gridComponent)
         map.addMapItem(_entryCoordinate)
         map.addMapItem(_exitCoordinate)
     }
 
     function _destroyVisualElements() {
-        _gridComponent.destroy()
         _entryCoordinate.destroy()
         _exitCoordinate.destroy()
     }
 
     /// Add an initial 4 sided polygon if there is none
     function _addInitialPolygon() {
-        if (_mapPolygon.count < 3) {
+        if (_structurePolygon.count < 3) {
             // Initial polygon is inset to take 2/3rds space
             var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height)
             rect.x += (rect.width * 0.25) / 2
@@ -71,10 +68,10 @@ Item {
             bottomLeftCoord =   centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180)
             bottomRightCoord =  centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180)
 
-            _mapPolygon.appendVertex(topLeftCoord)
-            _mapPolygon.appendVertex(topRightCoord)
-            _mapPolygon.appendVertex(bottomRightCoord)
-            _mapPolygon.appendVertex(bottomLeftCoord)
+            _structurePolygon.appendVertex(topLeftCoord)
+            _structurePolygon.appendVertex(topRightCoord)
+            _structurePolygon.appendVertex(bottomRightCoord)
+            _structurePolygon.appendVertex(bottomLeftCoord)
         }
     }
 
@@ -88,9 +85,8 @@ Item {
     }
 
     QGCMapPolygonVisuals {
-        id:                 mapPolygonVisuals
         mapControl:         map
-        mapPolygon:         _mapPolygon
+        mapPolygon:         _structurePolygon
         interactive:        _missionItem.isCurrentItem
         borderWidth:        1
         borderColor:        "black"
@@ -98,15 +94,12 @@ Item {
         interiorOpacity:    0.5
     }
 
-    // Survey grid lines
-    Component {
-        id: gridComponent
-
-        MapPolyline {
-            line.color: "white"
-            line.width: 2
-            path:       _missionItem.gridPoints
-        }
+    QGCMapPolygonVisuals {
+        mapControl:         map
+        mapPolygon:         _flightPolygon
+        interactive:        false
+        borderWidth:        2
+        borderColor:        "white"
     }
 
     // Entry point

src/comm/MockLinkMissionItemHandler.cc

@@ -70,11 +70,7 @@ bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
         break;
 
     case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
-        // Delete all plan items
-        _missionItems.clear();
-        _fenceItems.clear();
-        _rallyItems.clear();
-        _sendAck(MAV_MISSION_ACCEPTED);
+        _handleMissionClearAll(msg);
         break;
 
     default:
@@ -84,6 +80,40 @@ bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
     return true;
 }
 
+void MockLinkMissionItemHandler::_handleMissionClearAll(const mavlink_message_t& msg)
+{
+
+    mavlink_mission_clear_all_t clearAll;
+
+    mavlink_msg_mission_clear_all_decode(&msg, &clearAll);
+
+    Q_ASSERT(clearAll.target_system == _mockLink->vehicleId());
+
+    _requestType = (MAV_MISSION_TYPE)clearAll.mission_type;
+    qCDebug(MockLinkMissionItemHandlerLog) << QStringLiteral("_handleMissionClearAll %1").arg(_requestType);
+
+    switch (_requestType) {
+    case MAV_MISSION_TYPE_MISSION:
+        _missionItems.clear();
+        break;
+    case MAV_MISSION_TYPE_FENCE:
+        _fenceItems.clear();
+        break;
+    case MAV_MISSION_TYPE_RALLY:
+        _rallyItems.clear();
+        break;
+    case MAV_MISSION_TYPE_ALL:
+        _missionItems.clear();
+        _fenceItems.clear();
+        _rallyItems.clear();
+        break;
+    default:
+        Q_ASSERT(false);
+    }
+
+    _sendAck(MAV_MISSION_ACCEPTED);
+}
+
 void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
 {
     qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";

src/comm/MockLinkMissionItemHandler.h

@@ -88,6 +88,7 @@ private:
     void _handleMissionRequest(const mavlink_message_t& msg);
     void _handleMissionItem(const mavlink_message_t& msg);
     void _handleMissionCount(const mavlink_message_t& msg);
+    void _handleMissionClearAll(const mavlink_message_t& msg);
     void _requestNextMissionItem(int sequenceNumber);
     void _sendAck(MAV_MISSION_RESULT ackType);
     void _startMissionItemResponseTimer(void);