From 9e5f64a1ec63ee220548acac51ec58825fec55a0 Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <don@thegagnes.com>
Date: Tue, 12 Dec 2017 11:05:07 -0800
Subject: [PATCH] Support custom gimbal angles in Structure Scan

---
 qgroundcontrol.pro                                 |   2 +
 .../StructureScan.SettingsGroup.json               |  20 +++
 src/MissionManager/StructureScanComplexItem.cc     |  63 +++++++--
 src/MissionManager/StructureScanComplexItem.h      |  58 +++++---
 src/MissionManager/StructureScanComplexItemTest.cc | 154 +++++++++++++++++++++
 src/MissionManager/StructureScanComplexItemTest.h  |  52 +++++++
 src/PlanView/StructureScanEditor.qml               |  41 ++++--
 src/PlanView/StructureScanMapVisual.qml            |   7 +-
 src/qgcunittest/UnitTestList.cc                    |   2 +
 9 files changed, 356 insertions(+), 43 deletions(-)
 create mode 100644 src/MissionManager/StructureScanComplexItemTest.cc
 create mode 100644 src/MissionManager/StructureScanComplexItemTest.h

diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index b6d383f..74e6e2d 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -426,6 +426,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory  { APMFirmwarePlugin {
         src/MissionManager/SectionTest.h \
         src/MissionManager/SimpleMissionItemTest.h \
         src/MissionManager/SpeedSectionTest.h \
+        src/MissionManager/StructureScanComplexItemTest.h \
         src/MissionManager/SurveyMissionItemTest.h \
         src/MissionManager/VisualMissionItemTest.h \
         src/qgcunittest/FileDialogTest.h \
@@ -462,6 +463,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory  { APMFirmwarePlugin {
         src/MissionManager/SectionTest.cc \
         src/MissionManager/SimpleMissionItemTest.cc \
         src/MissionManager/SpeedSectionTest.cc \
+        src/MissionManager/StructureScanComplexItemTest.cc \
         src/MissionManager/SurveyMissionItemTest.cc \
         src/MissionManager/VisualMissionItemTest.cc \
         src/qgcunittest/FileDialogTest.cc \
diff --git a/src/MissionManager/StructureScan.SettingsGroup.json b/src/MissionManager/StructureScan.SettingsGroup.json
index 8439fa6..9c3beeb 100644
--- a/src/MissionManager/StructureScan.SettingsGroup.json
+++ b/src/MissionManager/StructureScan.SettingsGroup.json
@@ -1,5 +1,25 @@
 [
 {
+    "name":             "GimbalPitch",
+    "shortDescription": "Gimbal pitch rotation.",
+    "type":             "double",
+    "units":            "gimbal-degrees",
+    "min":              -90,
+    "max":              0,
+    "decimalPlaces":    0,
+    "defaultValue":     0
+},
+{
+    "name":             "GimbalYaw",
+    "shortDescription": "Gimbal yaw rotation.",
+    "type":             "double",
+    "units":            "deg",
+    "min":              -180.0,
+    "max":              180.0,
+    "decimalPlaces":    0,
+    "defaultValue":     90
+},
+{
     "name":             "Altitude",
     "shortDescription": "Altitude for the bottom layer of the structure scan.",
     "type":             "double",
diff --git a/src/MissionManager/StructureScanComplexItem.cc b/src/MissionManager/StructureScanComplexItem.cc
index c03379b..b5e9c8b 100644
--- a/src/MissionManager/StructureScanComplexItem.cc
+++ b/src/MissionManager/StructureScanComplexItem.cc
@@ -21,9 +21,12 @@
 
 QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog")
 
-const char* StructureScanComplexItem::jsonComplexItemTypeValue =        "StructureScan";
 const char* StructureScanComplexItem::_altitudeFactName =               "Altitude";
 const char* StructureScanComplexItem::_layersFactName =                 "Layers";
+const char* StructureScanComplexItem::_gimbalPitchFactName =            "GimbalPitch";
+const char* StructureScanComplexItem::_gimbalYawFactName =              "GimbalYaw";
+
+const char* StructureScanComplexItem::jsonComplexItemTypeValue =        "StructureScan";
 const char* StructureScanComplexItem::_jsonCameraCalcKey =              "CameraCalc";
 const char* StructureScanComplexItem::_jsonAltitudeRelativeKey =        "altitudeRelative";
 const char* StructureScanComplexItem::_jsonYawVehicleToStructureKey =   "yawVehicleToStructure";
@@ -44,6 +47,8 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa
     , _yawVehicleToStructure    (false)
     , _altitudeFact             (0, _altitudeFactName,              FactMetaData::valueTypeDouble)
     , _layersFact               (0, _layersFactName,                FactMetaData::valueTypeUint32)
+    , _gimbalPitchFact          (0, _gimbalPitchFactName,                   FactMetaData::valueTypeDouble)
+    , _gimbalYawFact            (0, _gimbalYawFactName,                     FactMetaData::valueTypeDouble)
 {
     _editorQml = "qrc:/qml/StructureScanEditor.qml";
 
@@ -53,18 +58,25 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa
 
     _altitudeFact.setMetaData   (_metaDataMap[_altitudeFactName]);
     _layersFact.setMetaData     (_metaDataMap[_layersFactName]);
+    _gimbalPitchFact.setMetaData(_metaDataMap[_gimbalPitchFactName]);
+    _gimbalYawFact.setMetaData  (_metaDataMap[_gimbalYawFactName]);
 
     _altitudeFact.setRawValue   (_altitudeFact.rawDefaultValue());
     _layersFact.setRawValue     (_layersFact.rawDefaultValue());
+    _gimbalPitchFact.setRawValue(_gimbalPitchFact.rawDefaultValue());
+    _gimbalYawFact.setRawValue  (_gimbalYawFact.rawDefaultValue());
 
     _altitudeFact.setRawValue(qgcApp()->toolbox()->settingsManager()->appSettings()->defaultMissionItemAltitude()->rawValue());
 
-    connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
-    connect(&_layersFact,   &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
+    connect(&_altitudeFact,     &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
+    connect(&_layersFact,       &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
+    connect(&_gimbalPitchFact,  &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
+    connect(&_gimbalYawFact,    &Fact::valueChanged, this, &StructureScanComplexItem::_setDirty);
 
-    connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::_setDirty);
-    connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::coordinateHasRelativeAltitudeChanged);
-    connect(this, &StructureScanComplexItem::altitudeRelativeChanged, this, &StructureScanComplexItem::exitCoordinateHasRelativeAltitudeChanged);
+    connect(this, &StructureScanComplexItem::altitudeRelativeChanged,       this, &StructureScanComplexItem::_setDirty);
+    connect(this, &StructureScanComplexItem::altitudeRelativeChanged,       this, &StructureScanComplexItem::coordinateHasRelativeAltitudeChanged);
+    connect(this, &StructureScanComplexItem::altitudeRelativeChanged,       this, &StructureScanComplexItem::exitCoordinateHasRelativeAltitudeChanged);
+    connect(this, &StructureScanComplexItem::yawVehicleToStructureChanged,  this, &StructureScanComplexItem::_setDirty);
 
     connect(&_altitudeFact, &Fact::valueChanged, this, &StructureScanComplexItem::_updateCoordinateAltitudes);
 
@@ -74,7 +86,8 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa
 
     connect(&_flightPolygon,    &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_flightPathChanged);
 
-    connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon);
+    connect(_cameraCalc.distanceToSurface(),    &Fact::valueChanged,                this, &StructureScanComplexItem::_rebuildFlightPolygon);
+    connect(&_cameraCalc,                       &CameraCalc::cameraSpecTypeChanged, this, &StructureScanComplexItem::_cameraSpecTypeChanged);
 
     connect(&_flightPolygon,                        &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_recalcCameraShots);
     connect(_cameraCalc.adjustedFootprintSide(),    &Fact::valueChanged,            this, &StructureScanComplexItem::_recalcCameraShots);
@@ -115,7 +128,7 @@ int StructureScanComplexItem::lastSequenceNumber(void) const
 {
     return _sequenceNumber +
             ((_flightPolygon.count() + 1) * _layersFact.rawValue().toInt()) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex
-            1;  // Gimbal yaw command
+            1;  // Gimbal control command
 }
 
 void StructureScanComplexItem::setDirty(bool dirty)
@@ -135,6 +148,8 @@ void StructureScanComplexItem::save(QJsonArray&  missionItems)
     saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
     saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
 
+    saveObject[_gimbalPitchFactName] =          _gimbalPitchFact.rawValue().toDouble();
+    saveObject[_gimbalYawFactName] =            _gimbalYawFact.rawValue().toDouble();
     saveObject[_altitudeFactName] =             _altitudeFact.rawValue().toDouble();
     saveObject[_jsonAltitudeRelativeKey] =      _altitudeRelative;
     saveObject[_layersFactName] =               _layersFact.rawValue().toDouble();
@@ -165,6 +180,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
         { VisualMissionItem::jsonTypeKey,               QJsonValue::String, true },
         { ComplexMissionItem::jsonComplexItemTypeKey,   QJsonValue::String, true },
         { QGCMapPolygon::jsonPolygonKey,                QJsonValue::Array,  true },
+        { _gimbalPitchFactName,                         QJsonValue::Double, true },
+        { _gimbalYawFactName,                           QJsonValue::Double, true },
         { _altitudeFactName,                            QJsonValue::Double, true },
         { _jsonAltitudeRelativeKey,                     QJsonValue::Bool,   false },
         { _layersFactName,                              QJsonValue::Double, true },
@@ -192,6 +209,8 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
 
     setSequenceNumber(sequenceNumber);
 
+    _gimbalPitchFact.setRawValue(complexObject[_gimbalPitchFactName].toDouble());
+    _gimbalYawFact.setRawValue  (complexObject[_gimbalYawFactName].toDouble());
     _altitudeFact.setRawValue   (complexObject[_altitudeFactName].toDouble());
     _layersFact.setRawValue     (complexObject[_layersFactName].toDouble());
     _altitudeRelative =         complexObject[_jsonAltitudeRelativeKey].toBool(true);
@@ -258,9 +277,9 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO
         MissionItem* item = new MissionItem(seqNum++,
                                             MAV_CMD_DO_MOUNT_CONTROL,
                                             MAV_FRAME_MISSION,
-                                            0,                                  // Gimbal pitch
+                                            _gimbalPitchFact.rawValue().toDouble(),
                                             0,                                  // Gimbal roll
-                                            90,                                 // Gimbal yaw
+                                            _gimbalYawFact.rawValue().toDouble(),
                                             0, 0, 0,                            // param 4-6 not used
                                             MAV_MOUNT_MODE_MAVLINK_TARGETING,
                                             true,                               // autoContinue
@@ -433,3 +452,27 @@ void StructureScanComplexItem::_recalcCameraShots(void)
     int cameraShots = distance / _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
     _setCameraShots(cameraShots * _layersFact.rawValue().toInt());
 }
+
+void StructureScanComplexItem::_cameraSpecTypeChanged(CameraCalc::CameraSpecType cameraSpecType)
+{
+    Q_UNUSED(cameraSpecType);
+
+    _gimbalPitchFact.setCookedValue(0);
+    _gimbalYawFact.setCookedValue(90);
+}
+
+void StructureScanComplexItem::setAltitudeRelative(bool altitudeRelative)
+{
+    if (altitudeRelative != _altitudeRelative) {
+        _altitudeRelative = altitudeRelative;
+        emit altitudeRelativeChanged(altitudeRelative);
+    }
+}
+
+void StructureScanComplexItem::setYawVehicleToStructure(bool yawVehicleToStructure)
+{
+    if (yawVehicleToStructure != _yawVehicleToStructure) {
+        _yawVehicleToStructure = yawVehicleToStructure;
+        emit yawVehicleToStructureChanged(yawVehicleToStructure);
+    }
+}
diff --git a/src/MissionManager/StructureScanComplexItem.h b/src/MissionManager/StructureScanComplexItem.h
index 1b3374d..1843edb 100644
--- a/src/MissionManager/StructureScanComplexItem.h
+++ b/src/MissionManager/StructureScanComplexItem.h
@@ -27,25 +27,34 @@ class StructureScanComplexItem : public ComplexMissionItem
 public:
     StructureScanComplexItem(Vehicle* vehicle, QObject* parent = NULL);
 
-    Q_PROPERTY(CameraCalc*      cameraCalc                  READ cameraCalc                     CONSTANT)
-    Q_PROPERTY(Fact*            altitude                    READ altitude                       CONSTANT)
-    Q_PROPERTY(Fact*            layers                      READ layers                         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*   structurePolygon            READ structurePolygon               CONSTANT)
-    Q_PROPERTY(QGCMapPolygon*   flightPolygon               READ flightPolygon                  CONSTANT)
-    Q_PROPERTY(bool             yawVehicleToStructure       MEMBER _yawVehicleToStructure       NOTIFY yawVehicleToStructureChanged)    ///< true: vehicle yaws to point to structure, false: gimbal yaws to point to structure
+    Q_PROPERTY(CameraCalc*      cameraCalc                  READ cameraCalc                                                 CONSTANT)
+    Q_PROPERTY(Fact*            gimbalPitch                 READ gimbalPitch                                                CONSTANT)
+    Q_PROPERTY(Fact*            gimbalYaw                   READ gimbalYaw                                                  CONSTANT)
+    Q_PROPERTY(Fact*            altitude                    READ altitude                                                   CONSTANT)
+    Q_PROPERTY(Fact*            layers                      READ layers                                                     CONSTANT)
+    Q_PROPERTY(bool             altitudeRelative            READ altitudeRelative           WRITE setAltitudeRelative       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*   structurePolygon            READ structurePolygon                                           CONSTANT)
+    Q_PROPERTY(QGCMapPolygon*   flightPolygon               READ flightPolygon                                              CONSTANT)
+    Q_PROPERTY(bool             yawVehicleToStructure       READ yawVehicleToStructure      WRITE setYawVehicleToStructure  NOTIFY yawVehicleToStructureChanged)    ///< true: vehicle yaws to point to structure, false: gimbal yaws to point to structure
 
     CameraCalc* cameraCalc  (void) { return &_cameraCalc; }
     Fact* altitude          (void) { return &_altitudeFact; }
     Fact* layers            (void) { return &_layersFact; }
 
-    int             cameraShots     (void) const;
-    double          timeBetweenShots(void);
-    QGCMapPolygon*  structurePolygon(void) { return &_structurePolygon; }
-    QGCMapPolygon*  flightPolygon   (void) { return &_flightPolygon; }
+    bool            altitudeRelative        (void) const { return _altitudeRelative; }
+    int             cameraShots             (void) const;
+    Fact*           gimbalPitch             (void) { return &_gimbalPitchFact; }
+    Fact*           gimbalYaw               (void) { return &_gimbalYawFact; }
+    double          timeBetweenShots        (void);
+    QGCMapPolygon*  structurePolygon        (void) { return &_structurePolygon; }
+    QGCMapPolygon*  flightPolygon           (void) { return &_flightPolygon; }
+    bool            yawVehicleToStructure   (void) const { return _yawVehicleToStructure; }
+
+    void setAltitudeRelative        (bool altitudeRelative);
+    void setYawVehicleToStructure   (bool yawVehicleToStructure);
 
     Q_INVOKABLE void rotateEntryPoint(void);
 
@@ -96,13 +105,14 @@ signals:
 
 private slots:
     void _setDirty(void);
-    void _polygonDirtyChanged(bool dirty);
-    void _polygonCountChanged(int count);
-    void _flightPathChanged(void);
-    void _clearInternal(void);
-    void _updateCoordinateAltitudes(void);
-    void _rebuildFlightPolygon(void);
-    void _recalcCameraShots(void);
+    void _polygonDirtyChanged       (bool dirty);
+    void _polygonCountChanged       (int count);
+    void _flightPathChanged         (void);
+    void _clearInternal             (void);
+    void _updateCoordinateAltitudes (void);
+    void _rebuildFlightPolygon      (void);
+    void _recalcCameraShots         (void);
+    void _cameraSpecTypeChanged     (CameraCalc::CameraSpecType cameraSpecType);
 
 private:
     void _setExitCoordinate(const QGeoCoordinate& coordinate);
@@ -130,13 +140,19 @@ private:
 
     Fact    _altitudeFact;
     Fact    _layersFact;
+    Fact    _gimbalPitchFact;
+    Fact    _gimbalYawFact;
 
     static const char* _altitudeFactName;
     static const char* _layersFactName;
+    static const char* _gimbalPitchFactName;
+    static const char* _gimbalYawFactName;
 
     static const char* _jsonCameraCalcKey;
     static const char* _jsonAltitudeRelativeKey;
     static const char* _jsonYawVehicleToStructureKey;
+
+    friend class StructureScanComplexItemTest;
 };
 
 #endif
diff --git a/src/MissionManager/StructureScanComplexItemTest.cc b/src/MissionManager/StructureScanComplexItemTest.cc
new file mode 100644
index 0000000..f894e5f
--- /dev/null
+++ b/src/MissionManager/StructureScanComplexItemTest.cc
@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   (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 "StructureScanComplexItemTest.h"
+#include "QGCApplication.h"
+
+StructureScanComplexItemTest::StructureScanComplexItemTest(void)
+    : _offlineVehicle(NULL)
+{
+    _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) <<
+                   QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124);
+}
+
+void StructureScanComplexItemTest::init(void)
+{
+    UnitTest::init();
+
+    _rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool));
+
+    _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this);
+    _structureScanItem = new StructureScanComplexItem(_offlineVehicle, this);
+    _structureScanItem->setDirty(false);
+
+    _multiSpy = new MultiSignalSpy();
+    Q_CHECK_PTR(_multiSpy);
+    QCOMPARE(_multiSpy->init(_structureScanItem, _rgSignals, _cSignals), true);
+}
+
+void StructureScanComplexItemTest::cleanup(void)
+{
+    delete _structureScanItem;
+    delete _offlineVehicle;
+}
+
+void StructureScanComplexItemTest::_testDirty(void)
+{
+    QVERIFY(!_structureScanItem->dirty());
+    _structureScanItem->setDirty(false);
+    QVERIFY(!_structureScanItem->dirty());
+    QVERIFY(_multiSpy->checkNoSignals());
+
+    _structureScanItem->setDirty(true);
+    QVERIFY(_structureScanItem->dirty());
+    QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask));
+    QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
+    _multiSpy->clearAllSignals();
+
+    _structureScanItem->setDirty(false);
+    QVERIFY(!_structureScanItem->dirty());
+    QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask));
+    QVERIFY(!_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
+    _multiSpy->clearAllSignals();
+
+    // These facts should set dirty when changed
+    QList<Fact*> rgFacts;
+    rgFacts << _structureScanItem->gimbalPitch() << _structureScanItem->gimbalYaw() << _structureScanItem->altitude() << _structureScanItem->layers();
+    foreach(Fact* fact, rgFacts) {
+        qDebug() << fact->name();
+        QVERIFY(!_structureScanItem->dirty());
+        if (fact->typeIsBool()) {
+            fact->setRawValue(!fact->rawValue().toBool());
+        } else {
+            fact->setRawValue(fact->rawValue().toDouble() + 1);
+        }
+        QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask));
+        QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
+        _structureScanItem->setDirty(false);
+        _multiSpy->clearAllSignals();
+    }
+    rgFacts.clear();
+
+    QVERIFY(!_structureScanItem->dirty());
+    _structureScanItem->setAltitudeRelative(!_structureScanItem->altitudeRelative());
+    QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask));
+    QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
+    _structureScanItem->setDirty(false);
+    _multiSpy->clearAllSignals();
+
+    QVERIFY(!_structureScanItem->dirty());
+    _structureScanItem->setYawVehicleToStructure(!_structureScanItem->yawVehicleToStructure());
+    QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask));
+    QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex));
+    _structureScanItem->setDirty(false);
+    _multiSpy->clearAllSignals();
+}
+
+void StructureScanComplexItemTest::_initItem(void)
+{
+    QGCMapPolygon* mapPolygon = _structureScanItem->structurePolygon();
+
+    for (int i=0; i<_polyPoints.count(); i++) {
+        QGeoCoordinate& vertex = _polyPoints[i];
+        mapPolygon->appendVertex(vertex);
+    }
+
+    _structureScanItem->cameraCalc()->setCameraSpecType(CameraCalc::CameraSpecNone);
+    _structureScanItem->gimbalPitch()->setCookedValue(45);
+    _structureScanItem->gimbalYaw()->setCookedValue(45);
+    _structureScanItem->layers()->setCookedValue(2);
+    _structureScanItem->setDirty(false);
+
+    _validateItem(_structureScanItem);
+}
+
+void StructureScanComplexItemTest::_validateItem(StructureScanComplexItem* item)
+{
+    QGCMapPolygon* mapPolygon = item->structurePolygon();
+
+    for (int i=0; i<_polyPoints.count(); i++) {
+        QGeoCoordinate& expectedVertex = _polyPoints[i];
+        QGeoCoordinate actualVertex = mapPolygon->vertexCoordinate(i);
+        QCOMPARE(expectedVertex, actualVertex);
+    }
+
+    QCOMPARE((int)item->cameraCalc()->cameraSpecType(), (int)CameraCalc::CameraSpecNone);
+    QCOMPARE(item->gimbalPitch()->cookedValue().toDouble(), 45.0);
+    QCOMPARE(item->gimbalYaw()->cookedValue().toDouble(), 45.0);
+    QCOMPARE(item->layers()->cookedValue().toInt(), 2);
+
+    int seqNum = item->sequenceNumber();
+    QCOMPARE(item->lastSequenceNumber(), seqNum + (5 /* 5 waypoints per layer */ * item->layers()->cookedValue().toInt()) + 1 /* gimbal command */);
+}
+
+void StructureScanComplexItemTest::_testSaveLoad(void)
+{
+    _initItem();
+
+    QJsonArray  items;
+    _structureScanItem->save(items);
+
+    QString errorString;
+    StructureScanComplexItem* newItem = new StructureScanComplexItem(_offlineVehicle, this);
+    QVERIFY(newItem->load(items[0].toObject(), 10, errorString));
+    QVERIFY(errorString.isEmpty());
+    _validateItem(newItem);
+    newItem->deleteLater();
+}
+
+void StructureScanComplexItemTest::_testGimbalAngleUpdate(void)
+{
+    // This sets the item to CameraCalc::CameraSpecNone and non-standard gimbal angles
+    _initItem();
+
+    // Switching to a camera specific setup should set gimbal angles to defaults surface scan
+    _structureScanItem->cameraCalc()->setCameraSpecType(CameraCalc::CameraSpecCustom);
+    QCOMPARE(_structureScanItem->gimbalPitch()->cookedValue().toDouble(), 0.0);
+    QCOMPARE(_structureScanItem->gimbalYaw()->cookedValue().toDouble(), 90.0);
+}
diff --git a/src/MissionManager/StructureScanComplexItemTest.h b/src/MissionManager/StructureScanComplexItemTest.h
new file mode 100644
index 0000000..5358424
--- /dev/null
+++ b/src/MissionManager/StructureScanComplexItemTest.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   (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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "UnitTest.h"
+#include "MultiSignalSpy.h"
+#include "StructureScanComplexItem.h"
+
+class StructureScanComplexItemTest : public UnitTest
+{
+    Q_OBJECT
+    
+public:
+    StructureScanComplexItemTest(void);
+
+protected:
+    void init(void) final;
+    void cleanup(void) final;
+    
+private slots:
+    void _testDirty(void);
+    void _testSaveLoad(void);
+    void _testGimbalAngleUpdate(void);
+
+private:
+    void _initItem(void);
+    void _validateItem(StructureScanComplexItem* item);
+
+    enum {
+        dirtyChangedIndex,
+        maxSignalIndex
+    };
+
+    enum {
+        dirtyChangedMask = 1 << dirtyChangedIndex
+    };
+
+    static const size_t _cSignals = maxSignalIndex;
+    const char*         _rgSignals[_cSignals];
+
+    Vehicle*                    _offlineVehicle;
+    MultiSignalSpy*             _multiSpy;
+    StructureScanComplexItem*   _structureScanItem;
+    QList<QGeoCoordinate>       _polyPoints;
+};
diff --git a/src/PlanView/StructureScanEditor.qml b/src/PlanView/StructureScanEditor.qml
index 0aeeae7..52fb2c9 100644
--- a/src/PlanView/StructureScanEditor.qml
+++ b/src/PlanView/StructureScanEditor.qml
@@ -29,7 +29,6 @@ Rectangle {
     property real   _fieldWidth:    ScreenTools.defaultFontPixelWidth * 10.5
     property var    _vehicle:       QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle
 
-
     function polygonCaptureStarted() {
         missionItem.clearPolygon()
     }
@@ -65,14 +64,6 @@ Rectangle {
         QGCLabel {
             anchors.left:   parent.left
             anchors.right:  parent.right
-            text:           qsTr("WORK IN PROGRESS. CAREFUL!")
-            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
@@ -95,6 +86,33 @@ Rectangle {
             sideDistanceLabel:      qsTr("Trigger Distance")
         }
 
+        GridLayout {
+            anchors.left:   parent.left
+            anchors.right:  parent.right
+            columnSpacing:  ScreenTools.defaultFontPixelWidth / 2
+            rowSpacing:     0
+            columns:        3
+            enabled:        missionItem.cameraCalc.cameraSpecType === CameraCalc.CameraSpecNone
+
+            Item { width: 1; height: 1 }
+            QGCLabel { text: qsTr("Pitch") }
+            QGCLabel { text: qsTr("Yaw") }
+
+            QGCLabel {
+                text:               qsTr("Gimbal")
+                Layout.fillWidth:   true
+            }
+            FactTextField {
+                fact:           missionItem.gimbalPitch
+                implicitWidth:  ScreenTools.defaultFontPixelWidth * 9
+            }
+
+            FactTextField {
+                fact:           missionItem.gimbalYaw
+                implicitWidth:  ScreenTools.defaultFontPixelWidth * 9
+            }
+        }
+
         SectionHeader {
             id:         scanHeader
             text:       qsTr("Scan")
@@ -155,6 +173,11 @@ Rectangle {
                 }
             }
 
+            Item {
+                height: ScreenTools.defaultFontPixelHeight / 2
+                width:  1
+            }
+
             QGCButton {
                 text:       qsTr("Rotate entry point")
                 onClicked:  missionItem.rotateEntryPoint()
diff --git a/src/PlanView/StructureScanMapVisual.qml b/src/PlanView/StructureScanMapVisual.qml
index 21f97ce..333c53b 100644
--- a/src/PlanView/StructureScanMapVisual.qml
+++ b/src/PlanView/StructureScanMapVisual.qml
@@ -61,9 +61,10 @@ Item {
             var bottomLeftCoord =   map.toCoordinate(Qt.point(rect.x, rect.y + rect.height),                            false /* clipToViewPort */)
             var bottomRightCoord =  map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height),               false /* clipToViewPort */)
 
-            // Initial polygon has max width and height of 3000 meters
-            var halfWidthMeters =   Math.min(topLeftCoord.distanceTo(topRightCoord), 3000) / 2
-            var halfHeightMeters =  Math.min(topLeftCoord.distanceTo(bottomLeftCoord), 3000) / 2
+            // Adjust polygon to max size
+            var maxSize = 100
+            var halfWidthMeters =   Math.min(topLeftCoord.distanceTo(topRightCoord), maxSize) / 2
+            var halfHeightMeters =  Math.min(topLeftCoord.distanceTo(bottomLeftCoord), maxSize) / 2
             topLeftCoord =      centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0)
             topRightCoord =     centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0)
             bottomLeftCoord =   centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180)
diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc
index 0fb23b5..fe67513 100644
--- a/src/qgcunittest/UnitTestList.cc
+++ b/src/qgcunittest/UnitTestList.cc
@@ -39,6 +39,7 @@
 #include "MissionSettingsTest.h"
 #include "QGCMapPolygonTest.h"
 #include "AudioOutputTest.h"
+#include "StructureScanComplexItemTest.h"
 
 UT_REGISTER_TEST(FactSystemTestGeneric)
 UT_REGISTER_TEST(FactSystemTestPX4)
@@ -65,6 +66,7 @@ UT_REGISTER_TEST(PlanMasterControllerTest)
 UT_REGISTER_TEST(MissionSettingsTest)
 UT_REGISTER_TEST(QGCMapPolygonTest)
 UT_REGISTER_TEST(AudioOutputTest)
+UT_REGISTER_TEST(StructureScanComplexItemTest)
 
 // List of unit test which are currently disabled.
 // If disabling a new test, include reason in comment.