Merge pull request #5111 from DonLakeFlyer/SetCameraMode

Plan: Add support for SET_CAMERA_MODE command

v2.0 @ 2faa6d49

-Subproject commit 9bc6f7d5ecb8c081b554cbd934a20290a5efa529
+Subproject commit 2faa6d49834aa203e2a3eeeeed588728fb14c431

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -268,6 +268,7 @@ QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
          << MAV_CMD_DO_LAND_START
          << MAV_CMD_DO_MOUNT_CONFIGURE
          << 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;
 

src/MissionManager/CameraSection.FactMetaData.json

@@ -44,5 +44,13 @@
     "max":              180.0,
     "decimalPlaces":    0,
     "defaultValue":     0
+},
+{
+    "name":             "CameraMode",
+    "shortDescription": "Specify whether the camera should switch to photo or video mode",
+    "type":             "uint32",
+    "enumStrings":      "Take photos,Record video",
+    "enumValues":       "0,1",
+    "defaultValue":     0
 }
 ]

src/MissionManager/CameraSection.h

@@ -38,6 +38,8 @@ public:
     Q_PROPERTY(Fact*    cameraAction                    READ cameraAction                                                       CONSTANT)
     Q_PROPERTY(Fact*    cameraPhotoIntervalTime         READ cameraPhotoIntervalTime                                            CONSTANT)
     Q_PROPERTY(Fact*    cameraPhotoIntervalDistance     READ cameraPhotoIntervalDistance                                        CONSTANT)
+    Q_PROPERTY(bool     specifyCameraMode               READ specifyCameraMode              WRITE setSpecifyCameraMode          NOTIFY specifyCameraModeChanged)
+    Q_PROPERTY(Fact*    cameraMode                      READ cameraMode                                                         CONSTANT)
 
     bool    specifyGimbal               (void) const { return _specifyGimbal; }
     Fact*   gimbalYaw                   (void) { return &_gimbalYawFact; }
@@ -45,8 +47,11 @@ public:
     Fact*   cameraAction                (void) { return &_cameraActionFact; }
     Fact*   cameraPhotoIntervalTime     (void) { return &_cameraPhotoIntervalTimeFact; }
     Fact*   cameraPhotoIntervalDistance (void) { return &_cameraPhotoIntervalDistanceFact; }
+    bool    specifyCameraMode           (void) const { return _specifyCameraMode; }
+    Fact*   cameraMode                  (void) { return &_cameraModeFact; }
 
-    void setSpecifyGimbal   (bool specifyGimbal);
+    void setSpecifyGimbal       (bool specifyGimbal);
+    void setSpecifyCameraMode   (bool specifyCameraMode);
 
     ///< @return The gimbal yaw specified by this item, NaN if not specified
     double specifiedGimbalYaw(void) const;
@@ -63,13 +68,14 @@ public:
 
 signals:
     bool specifyGimbalChanged       (bool specifyGimbal);
+    bool specifyCameraModeChanged   (bool specifyCameraMode);
     void specifiedGimbalYawChanged  (double gimbalYaw);
 
 private slots:
     void _setDirty(void);
     void _setDirtyAndUpdateItemCount(void);
     void _updateSpecifiedGimbalYaw(void);
-    void _specifyGimbalChanged(bool specifyGimbal);
+    void _specifyChanged(void);
     void _updateSettingsSpecified(void);
     void _cameraActionChanged(void);
 
@@ -77,11 +83,13 @@ private:
     bool    _available;
     bool    _settingsSpecified;
     bool    _specifyGimbal;
+    bool    _specifyCameraMode;
     Fact    _gimbalYawFact;
     Fact    _gimbalPitchFact;
     Fact    _cameraActionFact;
     Fact    _cameraPhotoIntervalDistanceFact;
     Fact    _cameraPhotoIntervalTimeFact;
+    Fact    _cameraModeFact;
     bool    _dirty;
 
     static QMap<QString, FactMetaData*> _metaDataMap;
@@ -91,4 +99,5 @@ private:
     static const char* _cameraActionName;
     static const char* _cameraPhotoIntervalDistanceName;
     static const char* _cameraPhotoIntervalTimeName;
+    static const char* _cameraModeName;
 };

src/MissionManager/CameraSectionTest.h

@@ -35,6 +35,7 @@ private slots:
     void _testScanForStartVideoSection(void);
     void _testScanForStopVideoSection(void);
     void _testScanForStopImageSection(void);
+    void _testScanForCameraModeSection(void);
     void _testScanForFullSection(void);
 
 private:
@@ -43,12 +44,14 @@ private:
     enum {
         specifyGimbalChangedIndex = 0,
         specifiedGimbalYawChangedIndex,
+        specifyCameraModeChangedIndex,
         maxSignalIndex,
     };
 
     enum {
         specifyGimbalChangedMask =      1 << specifyGimbalChangedIndex,
-        specifiedGimbalYawChangedMask = 1 << specifiedGimbalYawChangedIndex
+        specifiedGimbalYawChangedMask = 1 << specifiedGimbalYawChangedIndex,
+        specifyCameraModeChangedMask =  1 << specifyCameraModeChangedIndex,
     };
 
     static const size_t cCameraSignals = maxSignalIndex;
@@ -64,4 +67,6 @@ private:
     SimpleMissionItem*  _validStopVideoItem;
     SimpleMissionItem*  _validStopDistanceItem;
     SimpleMissionItem*  _validStopTimeItem;
+    SimpleMissionItem*  _validCameraPhotoModeItem;
+    SimpleMissionItem*  _validCameraVideoModeItem;
 };

src/MissionManager/MavCmdInfoCommon.json

@@ -967,6 +967,30 @@
         { "id": 510, "rawName": "MAV_CMD_GET_MESSAGE_INTERVAL", "friendlyName": "Get message interval" },
         { "id": 511, "rawName": "MAV_CMD_SET_MESSAGE_INTERVAL", "friendlyName": "Set message interval" },
         { "id": 520, "rawName": "MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES", "friendlyName": "Get capabilities" },
+        {
+            "id":           530,
+            "rawName":      "MAV_CMD_SET_CAMERA_MODE",
+            "friendlyName": "Set camera modes" ,
+            "description":  "Set camera photo, video, audio modes.",
+            "category":     "Camera",
+            "param1": {
+                "label":            "Camera id",
+                "default":          0,
+                "decimalPlaces":    0
+            },
+            "param2": {
+                "label":            "Mode",
+                "enumStrings":      "Take photos,Record video",
+                "enumValues":       "0,1",
+                "default":          0
+            },
+            "param3": {
+                "label":            "Audio",
+                "enumStrings":      "Recording disabled,Recording enabled",
+                "enumValues":       "0,1",
+                "default":          0
+            }
+        },
         {
             "id":           2000,
             "rawName":      "MAV_CMD_IMAGE_START_CAPTURE",

src/MissionManager/MissionController.cc

@@ -316,10 +316,16 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
         bool rollSupported = false;
         bool pitchSupported = false;
         bool yawSupported = false;
+        MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
+        CameraSection* cameraSection = settingsItem->cameraSection();
+        // Set camera to photo mode (leave alone if user already specified)
+        if (!cameraSection->specifyCameraMode()) {
+            cameraSection->setSpecifyCameraMode(true);
+            cameraSection->cameraMode()->setRawValue(0);
+        }
+        // Point gimbal straight down
         if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
-            MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
             // If the user already specified a gimbal angle leave it alone
-            CameraSection* cameraSection = settingsItem->cameraSection();
             if (!cameraSection->specifyGimbal()) {
                 cameraSection->setSpecifyGimbal(true);
                 cameraSection->gimbalPitch()->setRawValue(-90.0);
@@ -364,18 +370,21 @@ void MissionController::removeMissionItem(int index)
             }
         }
 
-        // If there is no longer a survey item in the mission remove the gimbal pitch command
+        // If there is no longer a survey item in the mission remove added commands
         if (!foundSurvey) {
             bool rollSupported = false;
             bool pitchSupported = false;
             bool yawSupported = false;
+            MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
+            CameraSection* cameraSection = settingsItem->cameraSection();
             if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) {
-                MissionSettingsItem* settingsItem = _visualItems->value<MissionSettingsItem*>(0);
-                CameraSection* cameraSection = settingsItem->cameraSection();
                 if (cameraSection->specifyGimbal() && cameraSection->gimbalPitch()->rawValue().toDouble() == -90.0 && cameraSection->gimbalYaw()->rawValue().toDouble() == 0.0) {
                     cameraSection->setSpecifyGimbal(false);
                 }
             }
+            if (cameraSection->specifyCameraMode() && cameraSection->cameraMode()->rawValue().toInt() == 0) {
+                cameraSection->setSpecifyCameraMode(false);
+            }
         }
     }
 

src/MissionManager/SectionTest.cc

@@ -56,16 +56,17 @@ void SectionTest::_createSpy(Section* section, MultiSignalSpy** sectionSpy)
 
 void SectionTest::_missionItemsEqual(MissionItem& item1, MissionItem& item2)
 {
-    QCOMPARE(item1.command(),           item2.command());
-    QCOMPARE(item1.frame(),             item2.frame());
-    QCOMPARE(item1.autoContinue(),      item2.autoContinue());
-    QCOMPARE(item1.param1(),            item2.param1());
-    QCOMPARE(item1.param2(),            item2.param2());
-    QCOMPARE(item1.param3(),            item2.param3());
-    QCOMPARE(item1.param4(),            item2.param4());
-    QCOMPARE(item1.param5(),            item2.param5());
-    QCOMPARE(item1.param6(),            item2.param6());
-    QCOMPARE(item1.param7(),            item2.param7());
+    QCOMPARE(item1.command(),       item2.command());
+    QCOMPARE(item1.frame(),         item2.frame());
+    QCOMPARE(item1.autoContinue(),  item2.autoContinue());
+
+    QVERIFY(UnitTest::doubleNaNCompare(item1.param1(), item2.param1()));
+    QVERIFY(UnitTest::doubleNaNCompare(item1.param2(), item2.param2()));
+    QVERIFY(UnitTest::doubleNaNCompare(item1.param3(), item2.param3()));
+    QVERIFY(UnitTest::doubleNaNCompare(item1.param4(), item2.param4()));
+    QVERIFY(UnitTest::doubleNaNCompare(item1.param5(), item2.param5()));
+    QVERIFY(UnitTest::doubleNaNCompare(item1.param6(), item2.param6()));
+    QVERIFY(UnitTest::doubleNaNCompare(item1.param7(), item2.param7()));
 }
 
 void SectionTest::_commonScanTest(Section* section)

src/PlanView/CameraSection.qml

@@ -104,5 +104,24 @@ Column {
                 enabled:        gimbalCheckBox.checked
             }
         }
+
+        RowLayout {
+            anchors.left:   parent.left
+            anchors.right:  parent.right
+            spacing:        ScreenTools.defaultFontPixelWidth
+
+            QGCCheckBox {
+                id:                 modeCheckBox
+                text:               qsTr("Mode")
+                checked:            _camera.specifyCameraMode
+                onClicked:          _camera.specifyCameraMode = checked
+            }
+            FactComboBox {
+                fact:               _camera.cameraMode
+                indexModel:         false
+                enabled:            modeCheckBox.checked
+                Layout.fillWidth:   true
+            }
+        }
     }
 }

src/qgcunittest/UnitTest.cc

@@ -504,3 +504,12 @@ bool UnitTest::fileCompare(const QString& file1, const QString& file2)
 
     return true;
 }
+
+bool UnitTest::doubleNaNCompare(double value1, double value2)
+{
+    if (qIsNaN(value1) && qIsNaN(value2)) {
+        return true;
+    } else {
+        return qFuzzyCompare(value1, value2);
+    }
+}

src/qgcunittest/UnitTest.h

@@ -92,6 +92,10 @@ public:
     /// @return true: files are alike, false: files differ
     static bool fileCompare(const QString& file1, const QString& file2);
 
+    /// Fuzzy compare on two doubles, where NaN is a possible value
+    /// @return true: equal
+    static bool doubleNaNCompare(double value1, double value2);
+
 protected slots:
     
     // These are all pure virtuals to force the derived class to implement each one and in turn