From e1fb25c6aa71f5909aae031c0f7d457749eedcd0 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Wed, 16 Aug 2017 09:59:33 -0700 Subject: [PATCH] CAMERA_MODE_IMAGE_SURVEY support --- .../CameraSection.FactMetaData.json | 4 +- src/MissionManager/CameraSection.cc | 2 +- src/MissionManager/CameraSection.h | 6 -- src/MissionManager/CameraSectionTest.cc | 58 +++++++++++++------ src/MissionManager/CameraSectionTest.h | 1 + src/MissionManager/MavCmdInfoCommon.json | 11 +--- src/MissionManager/MissionController.cc | 2 +- 7 files changed, 48 insertions(+), 36 deletions(-) diff --git a/src/MissionManager/CameraSection.FactMetaData.json b/src/MissionManager/CameraSection.FactMetaData.json index b35c37609..5177b1250 100644 --- a/src/MissionManager/CameraSection.FactMetaData.json +++ b/src/MissionManager/CameraSection.FactMetaData.json @@ -49,8 +49,8 @@ "name": "CameraMode", "shortDescription": "Specify whether the camera should switch to photo or video mode", "type": "uint32", - "enumStrings": "Take photos,Record video", - "enumValues": "0,1", + "enumStrings": "Take photos,Record video,Survey photo mode", + "enumValues": "0,1,2", "defaultValue": 0 } ] diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index bf28f9353..275b29633 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -392,7 +392,7 @@ bool CameraSection::_scanSetCameraMode(QmlObjectListModel* visualItems, int scan MissionItem& missionItem = item->missionItem(); if ((MAV_CMD)item->command() == MAV_CMD_SET_CAMERA_MODE) { // We specifically don't test param 5/6/7 since we don't have NaN persistence for those fields - if (missionItem.param1() == 0 && (missionItem.param2() == 0 || missionItem.param2() == 1) && qIsNaN(missionItem.param3())) { + if (missionItem.param1() == 0 && (missionItem.param2() == CAMERA_MODE_IMAGE || missionItem.param2() == CAMERA_MODE_VIDEO || missionItem.param2() == CAMERA_MODE_IMAGE_SURVEY) && qIsNaN(missionItem.param3())) { setSpecifyCameraMode(true); cameraMode()->setRawValue(missionItem.param2()); visualItems->removeAt(scanIndex)->deleteLater(); diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index e0538473a..c1fba5110 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -34,12 +34,6 @@ public: }; Q_ENUMS(CameraAction) - enum CameraMode { - CameraModePhoto, - CameraModeVideo - }; - Q_ENUMS(CameraMode) - Q_PROPERTY(bool specifyGimbal READ specifyGimbal WRITE setSpecifyGimbal NOTIFY specifyGimbalChanged) Q_PROPERTY(Fact* gimbalPitch READ gimbalPitch CONSTANT) Q_PROPERTY(Fact* gimbalYaw READ gimbalYaw CONSTANT) diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index 8edef76e1..3408a2378 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -13,18 +13,19 @@ #include "MissionCommandUIInfo.h" CameraSectionTest::CameraSectionTest(void) - : _spyCamera(NULL) - , _spySection(NULL) - , _cameraSection(NULL) - , _validGimbalItem(NULL) - , _validDistanceItem(NULL) - , _validTimeItem(NULL) - , _validStartVideoItem(NULL) - , _validStopVideoItem(NULL) - , _validStopDistanceItem(NULL) - , _validStopTimeItem(NULL) - , _validCameraPhotoModeItem(NULL) - , _validCameraVideoModeItem(NULL) + : _spyCamera (NULL) + , _spySection (NULL) + , _cameraSection (NULL) + , _validGimbalItem (NULL) + , _validDistanceItem (NULL) + , _validTimeItem (NULL) + , _validStartVideoItem (NULL) + , _validStopVideoItem (NULL) + , _validStopDistanceItem (NULL) + , _validStopTimeItem (NULL) + , _validCameraPhotoModeItem (NULL) + , _validCameraVideoModeItem (NULL) + , _validCameraSurveyPhotoModeItem (NULL) { } @@ -83,7 +84,7 @@ void CameraSectionTest::init(void) MAV_CMD_SET_CAMERA_MODE, MAV_FRAME_MISSION, 0, // Reserved (Set to 0) - CameraSection::CameraModePhoto, + CAMERA_MODE_IMAGE, NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved true, // autocontinue false), // isCurrentItem @@ -93,11 +94,21 @@ void CameraSectionTest::init(void) MAV_CMD_SET_CAMERA_MODE, MAV_FRAME_MISSION, 0, // Reserved (Set to 0) - CameraSection::CameraModeVideo, + CAMERA_MODE_VIDEO, NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved true, // autocontinue false), // isCurrentItem this); + _validCameraSurveyPhotoModeItem = new SimpleMissionItem(_offlineVehicle, + MissionItem(0, // sequence number + MAV_CMD_SET_CAMERA_MODE, + MAV_FRAME_MISSION, + 0, // Reserved (Set to 0) + CAMERA_MODE_IMAGE_SURVEY, + NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved + true, // autocontinue + false), // isCurrentItem + this); _validTakePhotoItem = new SimpleMissionItem(_offlineVehicle, MissionItem(0, MAV_CMD_IMAGE_START_CAPTURE, @@ -125,6 +136,7 @@ void CameraSectionTest::cleanup(void) delete _validTakePhotoItem; delete _validCameraPhotoModeItem; delete _validCameraVideoModeItem; + delete _validCameraSurveyPhotoModeItem; SectionTest::cleanup(); } @@ -453,7 +465,7 @@ void CameraSectionTest::_testAppendSectionItems(void) // Test specifyCameraMode _cameraSection->setSpecifyCameraMode(true); - _cameraSection->cameraMode()->setRawValue(CameraSection::CameraModePhoto); + _cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE); _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); QCOMPARE(rgMissionItems.count(), 1); QCOMPARE(seqNum, 1); @@ -463,7 +475,7 @@ void CameraSectionTest::_testAppendSectionItems(void) seqNum = 0; _cameraSection->setSpecifyCameraMode(true); - _cameraSection->cameraMode()->setRawValue(CameraSection::CameraModeVideo); + _cameraSection->cameraMode()->setRawValue(CAMERA_MODE_VIDEO); _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); QCOMPARE(rgMissionItems.count(), 1); QCOMPARE(seqNum, 1); @@ -472,6 +484,16 @@ void CameraSectionTest::_testAppendSectionItems(void) rgMissionItems.clear(); seqNum = 0; + _cameraSection->setSpecifyCameraMode(true); + _cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE_SURVEY); + _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); + QCOMPARE(rgMissionItems.count(), 1); + QCOMPARE(seqNum, 1); + _missionItemsEqual(*rgMissionItems[0], _validCameraSurveyPhotoModeItem->missionItem()); + _cameraSection->setSpecifyCameraMode(false); + rgMissionItems.clear(); + seqNum = 0; + // Test camera actions _cameraSection->cameraAction()->setRawValue(CameraSection::TakePhoto); @@ -539,7 +561,7 @@ void CameraSectionTest::_testAppendSectionItems(void) _cameraSection->cameraAction()->setRawValue(CameraSection::TakePhotosIntervalTime); _cameraSection->cameraPhotoIntervalTime()->setRawValue(_validTimeItem->missionItem().param2()); _cameraSection->setSpecifyCameraMode(true); - _cameraSection->cameraMode()->setRawValue(CameraSection::CameraModePhoto); + _cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE); _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); QCOMPARE(rgMissionItems.count(), 3); QCOMPARE(seqNum, 3); @@ -995,7 +1017,7 @@ void CameraSectionTest::_resetSection(void) _cameraSection->cameraPhotoIntervalTime()->setRawValue(0); _cameraSection->cameraPhotoIntervalDistance()->setRawValue(0); _cameraSection->cameraAction()->setRawValue(CameraSection::CameraActionNone); - _cameraSection->cameraMode()->setRawValue(CameraSection::CameraModePhoto); + _cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE); _cameraSection->setSpecifyCameraMode(false); } diff --git a/src/MissionManager/CameraSectionTest.h b/src/MissionManager/CameraSectionTest.h index 6c5ba39bb..0ca4ffdd0 100644 --- a/src/MissionManager/CameraSectionTest.h +++ b/src/MissionManager/CameraSectionTest.h @@ -72,5 +72,6 @@ private: SimpleMissionItem* _validStopTimeItem; SimpleMissionItem* _validCameraPhotoModeItem; SimpleMissionItem* _validCameraVideoModeItem; + SimpleMissionItem* _validCameraSurveyPhotoModeItem; SimpleMissionItem* _validTakePhotoItem; }; diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 239b79cfe..ff0a85c31 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -971,17 +971,12 @@ "id": 530, "rawName": "MAV_CMD_SET_CAMERA_MODE", "friendlyName": "Set camera modes" , - "description": "Set camera photo, video, audio modes.", + "description": "Set camera photo, video modes.", "category": "Camera", - "param1": { - "label": "Reserved", - "default": 0, - "decimalPlaces": 0 - }, "param2": { "label": "Mode", - "enumStrings": "Take photos,Record video", - "enumValues": "0,1", + "enumStrings": "Take photos,Record video,Survey photo mode", + "enumValues": "0,1,2", "default": 0 } }, diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 42c491519..5a6810c72 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -327,7 +327,7 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate // Set camera to photo mode (leave alone if user already specified) if (cameraSection->cameraModeSupported() && !cameraSection->specifyCameraMode()) { cameraSection->setSpecifyCameraMode(true); - cameraSection->cameraMode()->setRawValue(0); + cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE_SURVEY); } // Point gimbal straight down if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) { -- 2.22.0