Support for vehicle yaw to structure

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -277,7 +277,8 @@ QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
          << MAV_CMD_DO_MOUNT_CONTROL
          << MAV_CMD_SET_CAMERA_MODE
          << MAV_CMD_IMAGE_START_CAPTURE << MAV_CMD_IMAGE_STOP_CAPTURE << MAV_CMD_VIDEO_START_CAPTURE << MAV_CMD_VIDEO_STOP_CAPTURE
-         << MAV_CMD_NAV_DELAY;
+         << MAV_CMD_NAV_DELAY
+         << MAV_CMD_CONDITION_YAW;
 
     return list;
 }

src/MissionManager/CameraCalc.FactMetaData.json

@@ -12,7 +12,7 @@
     "min":              0.1,
     "units":            "m",
     "decimalPlaces":    2,
-    "defaultValue":     100.0
+    "defaultValue":     10.0
 },
 {
     "name":             "ImageDensity",

src/MissionManager/StructureScanComplexItem.cc

@@ -21,11 +21,12 @@
 
 QGC_LOGGING_CATEGORY(StructureScanComplexItemLog, "StructureScanComplexItemLog")
 
-const char* StructureScanComplexItem::jsonComplexItemTypeValue =    "StructureScan";
+const char* StructureScanComplexItem::jsonComplexItemTypeValue =        "StructureScan";
-const char* StructureScanComplexItem::_altitudeFactName =           "Altitude";
+const char* StructureScanComplexItem::_altitudeFactName =               "Altitude";
-const char* StructureScanComplexItem::_layersFactName =             "Layers";
+const char* StructureScanComplexItem::_layersFactName =                 "Layers";
-const char* StructureScanComplexItem::_jsonCameraCalcKey =          "CameraCalc";
+const char* StructureScanComplexItem::_jsonCameraCalcKey =              "CameraCalc";
-const char* StructureScanComplexItem::_jsonAltitudeRelativeKey =    "altitudeRelative";
+const char* StructureScanComplexItem::_jsonAltitudeRelativeKey =        "altitudeRelative";
+const char* StructureScanComplexItem::_jsonYawVehicleToStructureKey =   "yawVehicleToStructure";
 
 QMap<QString, FactMetaData*> StructureScanComplexItem::_metaDataMap;
 
@@ -40,6 +41,7 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa
     , _cameraShots              (0)
     , _cameraMinTriggerInterval (0)
     , _cameraCalc               (vehicle)
+    , _yawVehicleToStructure    (true)
     , _altitudeFact             (0, _altitudeFactName,              FactMetaData::valueTypeDouble)
     , _layersFact               (0, _layersFactName,                FactMetaData::valueTypeUint32)
 {
@@ -73,6 +75,10 @@ StructureScanComplexItem::StructureScanComplexItem(Vehicle* vehicle, QObject* pa
     connect(&_flightPolygon,    &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_flightPathChanged);
 
     connect(_cameraCalc.distanceToSurface(), &Fact::valueChanged, this, &StructureScanComplexItem::_rebuildFlightPolygon);
+
+    connect(&_flightPolygon,                        &QGCMapPolygon::pathChanged,    this, &StructureScanComplexItem::_recalcCameraShots);
+    connect(_cameraCalc.adjustedFootprintSide(),    &Fact::valueChanged,            this, &StructureScanComplexItem::_recalcCameraShots);
+    connect(&_layersFact,                           &Fact::valueChanged,            this, &StructureScanComplexItem::_recalcCameraShots);
 }
 
 void StructureScanComplexItem::_setScanDistance(double scanDistance)
@@ -129,9 +135,10 @@ void StructureScanComplexItem::save(QJsonArray&  missionItems)
     saveObject[VisualMissionItem::jsonTypeKey] =                VisualMissionItem::jsonTypeComplexItemValue;
     saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =    jsonComplexItemTypeValue;
 
-    saveObject[_altitudeFactName] =         _altitudeFact.rawValue().toDouble();
+    saveObject[_altitudeFactName] =             _altitudeFact.rawValue().toDouble();
-    saveObject[_jsonAltitudeRelativeKey] =  _altitudeRelative;
+    saveObject[_jsonAltitudeRelativeKey] =      _altitudeRelative;
-    saveObject[_layersFactName] =           _layersFact.rawValue().toDouble();
+    saveObject[_layersFactName] =               _layersFact.rawValue().toDouble();
+    saveObject[_jsonYawVehicleToStructureKey] = _yawVehicleToStructure;
 
     QJsonObject cameraCalcObject;
     _cameraCalc.save(cameraCalcObject);
@@ -162,6 +169,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
         { _jsonAltitudeRelativeKey,                     QJsonValue::Bool,   false },
         { _layersFactName,                              QJsonValue::Double, true },
         { _jsonCameraCalcKey,                           QJsonValue::Object, true },
+        { _jsonYawVehicleToStructureKey,                QJsonValue::Bool, true },
     };
     if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
         return false;
@@ -187,6 +195,7 @@ bool StructureScanComplexItem::load(const QJsonObject& complexObject, int sequen
     _altitudeFact.setRawValue   (complexObject[_altitudeFactName].toDouble());
     _layersFact.setRawValue     (complexObject[_layersFactName].toDouble());
     _altitudeRelative =         complexObject[_jsonAltitudeRelativeKey].toBool(true);
+    _yawVehicleToStructure =    complexObject[_jsonYawVehicleToStructureKey].toBool(true);
 
     if (!_cameraCalc.load(complexObject[_jsonCameraCalcKey].toObject(), errorString)) {
         return false;
@@ -231,19 +240,35 @@ void StructureScanComplexItem::appendMissionItems(QList<MissionItem*>& items, QO
 {
     int seqNum = _sequenceNumber;
     double baseAltitude = _altitudeFact.rawValue().toDouble();
+    MissionItem* item;
 
-    MissionItem* item = new MissionItem(seqNum++,
+    if (_yawVehicleToStructure) {
-                                        MAV_CMD_DO_MOUNT_CONTROL,
+        MissionItem* item = new MissionItem(seqNum++,
-                                        MAV_FRAME_MISSION,
+                                            MAV_CMD_CONDITION_YAW,
-                                        0,                                  // Gimbal pitch
+                                            MAV_FRAME_MISSION,
-                                        0,                                  // Gimbal roll
+                                            90.0,                               // Target angle
-                                        90,                                 // Gimbal yaw
+                                            0,                                  // Use default turn rate
-                                        0, 0, 0,                            // param 4-6 not used
+                                            1,                                  // Clockwise turn
-                                        MAV_MOUNT_MODE_MAVLINK_TARGETING,
+                                            0,                                  // Absolute angle specified
-                                        true,                               // autoContinue
+                                            0, 0, 0,                            // param 5-7 not used
-                                        false,                              // isCurrentItem
+                                            true,                               // autoContinue
-                                        missionItemParent);
+                                            false,                              // isCurrentItem
-    items.append(item);
+                                            missionItemParent);
+        items.append(item);
+    } else {
+        MissionItem* item = new MissionItem(seqNum++,
+                                            MAV_CMD_DO_MOUNT_CONTROL,
+                                            MAV_FRAME_MISSION,
+                                            0,                                  // Gimbal pitch
+                                            0,                                  // Gimbal roll
+                                            90,                                 // Gimbal yaw
+                                            0, 0, 0,                            // param 4-6 not used
+                                            MAV_MOUNT_MODE_MAVLINK_TARGETING,
+                                            true,                               // autoContinue
+                                            false,                              // isCurrentItem
+                                            missionItemParent);
+        items.append(item);
+    }
 
     for (int layer=0; layer<_layersFact.rawValue().toInt(); layer++) {
         bool addTriggerStart = true;
@@ -386,3 +411,26 @@ void StructureScanComplexItem::_rebuildFlightPolygon(void)
     _flightPolygon = _structurePolygon;
     _flightPolygon.offset(_cameraCalc.distanceToSurface()->rawValue().toDouble());
 }
+
+void StructureScanComplexItem::_recalcCameraShots(void)
+{
+    if (_flightPolygon.count() < 3) {
+        _setCameraShots(0);
+        return;
+    }
+
+    // Determine the distance for each polygon traverse
+    double distance = 0;
+    for (int i=0; i<_flightPolygon.count(); i++) {
+        QGeoCoordinate coord1 = _flightPolygon.vertexCoordinate(i);
+        QGeoCoordinate coord2 = _flightPolygon.vertexCoordinate(i + 1 == _flightPolygon.count() ? 0 : i + 1);
+        distance += coord1.distanceTo(coord2);
+    }
+    if (distance == 0.0) {
+        _setCameraShots(0);
+        return;
+    }
+
+    int cameraShots = distance / _cameraCalc.adjustedFootprintSide()->rawValue().toDouble();
+    _setCameraShots(cameraShots * _layersFact.rawValue().toInt());
+}

src/MissionManager/StructureScanComplexItem.h

@@ -36,6 +36,7 @@ public:
     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
 
     CameraCalc* cameraCalc  (void) { return &_cameraCalc; }
     Fact* altitude          (void) { return &_altitudeFact; }
@@ -91,6 +92,7 @@ signals:
     void timeBetweenShotsChanged        (void);
     void cameraMinTriggerIntervalChanged(double cameraMinTriggerInterval);
     void altitudeRelativeChanged        (bool altitudeRelative);
+    void yawVehicleToStructureChanged   (bool yawVehicleToStructure);
 
 private slots:
     void _setDirty(void);
@@ -100,6 +102,7 @@ private slots:
     void _clearInternal(void);
     void _updateCoordinateAltitudes(void);
     void _rebuildFlightPolygon(void);
+    void _recalcCameraShots(void);
 
 private:
     void _setExitCoordinate(const QGeoCoordinate& coordinate);
@@ -121,6 +124,7 @@ private:
     double          _cameraMinTriggerInterval;
     double          _cruiseSpeed;
     CameraCalc      _cameraCalc;
+    bool            _yawVehicleToStructure;
 
     static QMap<QString, FactMetaData*> _metaDataMap;
 
@@ -132,6 +136,7 @@ private:
 
     static const char* _jsonCameraCalcKey;
     static const char* _jsonAltitudeRelativeKey;
+    static const char* _jsonYawVehicleToStructureKey;
 };
 
 #endif

src/PlanView/StructureScanEditor.qml

@@ -49,6 +49,11 @@ Rectangle {
 
     QGCPalette { id: qgcPal; colorGroupEnabled: true }
 
+    ExclusiveGroup {
+        id: yawRadiosGroup
+        onCurrentChanged: missionItem.yawVehicleToStructure = yawVehicleRadio.checked
+    }
+
     Column {
         id:                 editorColumn
         anchors.margins:    _margin
@@ -129,8 +134,24 @@ Rectangle {
             }
 
             QGCLabel { text: qsTr("Point camera to structure using:") }
-            QGCRadioButton { text: qsTr("Vehicle yaw"); enabled: false }
+
-            QGCRadioButton { text: qsTr("Gimbal yaw"); checked: true; enabled: false }
+            RowLayout {
+                spacing: _margin
+
+                QGCRadioButton {
+                    id:             yawVehicleRadio
+                    text:           qsTr("Vehicle yaw")
+                    exclusiveGroup: yawRadiosGroup
+                    checked:        !!missionItem.yawVehicleToStructure
+                }
+
+                QGCRadioButton
+                {
+                    text:           qsTr("Gimbal yaw")
+                    exclusiveGroup: yawRadiosGroup
+                    checked:        !missionItem.yawVehicleToStructure
+                }
+            }
 
             QGCButton {
                 text:       qsTr("Rotate entry point")