diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index 9bc6f7d5ecb8c081b554cbd934a20290a5efa529..2faa6d49834aa203e2a3eeeeed588728fb14c431 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 9bc6f7d5ecb8c081b554cbd934a20290a5efa529 +Subproject commit 2faa6d49834aa203e2a3eeeeed588728fb14c431 diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index b04dcc3315ffa6a3250f97c286c238a1ddb10f51..24ddd4942d5276d135b052a63c983b8e3574e2db 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -268,6 +268,7 @@ QList 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; diff --git a/src/MissionManager/CameraSection.FactMetaData.json b/src/MissionManager/CameraSection.FactMetaData.json index 6856fc1bf0183e4a4049611f459e898737ba8549..cd95721f51c4849fd2b7edcf9ac3a5fc75692396 100644 --- a/src/MissionManager/CameraSection.FactMetaData.json +++ b/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 } ] diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index fc35be418e9cc1ed216dac2f31a545ce890b5e1f..80cfc569f714164a17c5b7fcea05a4cd4458544c 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -12,11 +12,12 @@ QGC_LOGGING_CATEGORY(CameraSectionLog, "CameraSectionLog") -const char* CameraSection::_gimbalPitchName = "GimbalPitch"; -const char* CameraSection::_gimbalYawName = "GimbalYaw"; -const char* CameraSection::_cameraActionName = "CameraAction"; -const char* CameraSection::_cameraPhotoIntervalDistanceName = "CameraPhotoIntervalDistance"; -const char* CameraSection::_cameraPhotoIntervalTimeName = "CameraPhotoIntervalTime"; +const char* CameraSection::_gimbalPitchName = "GimbalPitch"; +const char* CameraSection::_gimbalYawName = "GimbalYaw"; +const char* CameraSection::_cameraActionName = "CameraAction"; +const char* CameraSection::_cameraPhotoIntervalDistanceName = "CameraPhotoIntervalDistance"; +const char* CameraSection::_cameraPhotoIntervalTimeName = "CameraPhotoIntervalTime"; +const char* CameraSection::_cameraModeName = "CameraMode"; QMap CameraSection::_metaDataMap; @@ -25,11 +26,13 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) , _available(false) , _settingsSpecified(false) , _specifyGimbal(false) + , _specifyCameraMode(false) , _gimbalYawFact (0, _gimbalYawName, FactMetaData::valueTypeDouble) , _gimbalPitchFact (0, _gimbalPitchName, FactMetaData::valueTypeDouble) , _cameraActionFact (0, _cameraActionName, FactMetaData::valueTypeDouble) , _cameraPhotoIntervalDistanceFact (0, _cameraPhotoIntervalDistanceName, FactMetaData::valueTypeDouble) , _cameraPhotoIntervalTimeFact (0, _cameraPhotoIntervalTimeName, FactMetaData::valueTypeUint32) + , _cameraModeFact (0, _cameraModeName, FactMetaData::valueTypeUint32) , _dirty(false) { if (_metaDataMap.isEmpty()) { @@ -41,23 +44,29 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) _cameraActionFact.setMetaData (_metaDataMap[_cameraActionName]); _cameraPhotoIntervalDistanceFact.setMetaData (_metaDataMap[_cameraPhotoIntervalDistanceName]); _cameraPhotoIntervalTimeFact.setMetaData (_metaDataMap[_cameraPhotoIntervalTimeName]); + _cameraModeFact.setMetaData (_metaDataMap[_cameraModeName]); _gimbalPitchFact.setRawValue (_gimbalPitchFact.rawDefaultValue()); _gimbalYawFact.setRawValue (_gimbalYawFact.rawDefaultValue()); _cameraActionFact.setRawValue (_cameraActionFact.rawDefaultValue()); _cameraPhotoIntervalDistanceFact.setRawValue (_cameraPhotoIntervalDistanceFact.rawDefaultValue()); _cameraPhotoIntervalTimeFact.setRawValue (_cameraPhotoIntervalTimeFact.rawDefaultValue()); + _cameraModeFact.setRawValue (_cameraModeFact.rawDefaultValue()); - connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_specifyGimbalChanged); - connect(&_cameraActionFact, &Fact::valueChanged, this, &CameraSection::_cameraActionChanged); + connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_specifyChanged); + connect(this, &CameraSection::specifyCameraModeChanged, this, &CameraSection::_specifyChanged); - connect(&_gimbalPitchFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(&_cameraPhotoIntervalDistanceFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(&_cameraPhotoIntervalTimeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); - connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_setDirty); + connect(&_cameraActionFact, &Fact::valueChanged, this, &CameraSection::_cameraActionChanged); - connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); + connect(&_gimbalPitchFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_cameraPhotoIntervalDistanceFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_cameraPhotoIntervalTimeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(&_cameraModeFact, &Fact::valueChanged, this, &CameraSection::_setDirty); + connect(this, &CameraSection::specifyGimbalChanged, this, &CameraSection::_setDirty); + connect(this, &CameraSection::specifyCameraModeChanged, this, &CameraSection::_setDirty); + + connect(&_gimbalYawFact, &Fact::valueChanged, this, &CameraSection::_updateSpecifiedGimbalYaw); } void CameraSection::setSpecifyGimbal(bool specifyGimbal) @@ -68,6 +77,14 @@ void CameraSection::setSpecifyGimbal(bool specifyGimbal) } } +void CameraSection::setSpecifyCameraMode(bool specifyCameraMode) +{ + if (specifyCameraMode != _specifyCameraMode) { + _specifyCameraMode = specifyCameraMode; + emit specifyCameraModeChanged(specifyCameraMode); + } +} + int CameraSection::itemCount(void) const { int itemCount = 0; @@ -75,6 +92,9 @@ int CameraSection::itemCount(void) const if (_specifyGimbal) { itemCount++; } + if (_specifyCameraMode) { + itemCount++; + } if (_cameraActionFact.rawValue().toInt() != CameraActionNone) { itemCount++; } @@ -121,7 +141,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss 0, // Unlimited photo count -1, // Max resolution 0, 0, // param 4-5 not used - 0, // Camera ID + 0, // Camera ID, all cameras 0, // param 7 not used true, // autoContinue false, // isCurrentItem @@ -143,7 +163,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_VIDEO_START_CAPTURE, MAV_FRAME_MISSION, - 0, // Camera ID + 0, // Camera ID, all cameras -1, // Max fps -1, // Max resolution 0, 0, 0, 0, // param 5-7 not used @@ -156,7 +176,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_VIDEO_STOP_CAPTURE, MAV_FRAME_MISSION, - 0, // Camera ID + 0, // Camera ID, all cameras 0, 0, 0, 0, 0, 0, // param 2-7 not used true, // autoContinue false, // isCurrentItem @@ -176,7 +196,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, - 0, // camera id + 0, // camera id, all cameras 0, 0, 0, 0, 0, 0, // param 2-7 not used true, // autoContinue false, // isCurrentItem @@ -187,12 +207,26 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss items.append(item); } } + + if (_specifyCameraMode) { + MissionItem* item = new MissionItem(nextSequenceNumber++, + MAV_CMD_SET_CAMERA_MODE, + MAV_FRAME_MISSION, + 0, // camera id, all cameras + _cameraModeFact.rawValue().toDouble(), + NAN, NAN, NAN, NAN, NAN, // param 3-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } } bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanIndex) { bool foundGimbal = false; bool foundCameraAction = false; + bool foundCameraMode = false; bool stopLooking = false; qCDebug(CameraSectionLog) << "CameraSection::scanForCameraSection" << visualItems->count() << scanIndex; @@ -234,8 +268,9 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde cameraAction()->setRawValue(TakePhotosIntervalTime); cameraPhotoIntervalTime()->setRawValue(missionItem.param1()); visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; } - stopLooking = true; break; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: @@ -277,8 +312,9 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde foundCameraAction = true; cameraAction()->setRawValue(TakeVideo); visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; } - stopLooking = true; break; case MAV_CMD_VIDEO_STOP_CAPTURE: @@ -286,8 +322,21 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde foundCameraAction = true; cameraAction()->setRawValue(StopTakingVideo); visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; + } + break; + + case 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 (!foundCameraMode && missionItem.param1() == 0 && (missionItem.param2() == 0 || missionItem.param2() == 1) && qIsNaN(missionItem.param3())) { + foundCameraMode = true; + setSpecifyCameraMode(true); + cameraMode()->setRawValue(missionItem.param2()); + visualItems->removeAt(scanIndex)->deleteLater(); + } else { + stopLooking = true; } - stopLooking = true; break; default: @@ -296,12 +345,12 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde } } - qCDebug(CameraSectionLog) << foundGimbal << foundCameraAction; + qCDebug(CameraSectionLog) << foundGimbal << foundCameraAction << foundCameraMode; - _settingsSpecified = foundGimbal || foundCameraAction; + _settingsSpecified = foundGimbal || foundCameraAction || foundCameraMode; emit settingsSpecifiedChanged(_settingsSpecified); - return foundGimbal || foundCameraAction; + return _settingsSpecified; } void CameraSection::_setDirty(void) @@ -335,16 +384,15 @@ void CameraSection::_updateSpecifiedGimbalYaw(void) void CameraSection::_updateSettingsSpecified(void) { - bool newSettingsSpecified = _specifyGimbal || _cameraActionFact.rawValue().toInt() != CameraActionNone; + bool newSettingsSpecified = _specifyGimbal || _specifyCameraMode || _cameraActionFact.rawValue().toInt() != CameraActionNone; if (newSettingsSpecified != _settingsSpecified) { _settingsSpecified = newSettingsSpecified; emit settingsSpecifiedChanged(newSettingsSpecified); } } -void CameraSection::_specifyGimbalChanged(bool specifyGimbal) +void CameraSection::_specifyChanged(void) { - Q_UNUSED(specifyGimbal); _setDirtyAndUpdateItemCount(); _updateSettingsSpecified(); } diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index 9437955221c1eb4c7dd1d18595549d404d0e55f3..59e2d77ace6b0373bac4fc953ced65e34c62fb0a 100644 --- a/src/MissionManager/CameraSection.h +++ b/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 _metaDataMap; @@ -91,4 +99,5 @@ private: static const char* _cameraActionName; static const char* _cameraPhotoIntervalDistanceName; static const char* _cameraPhotoIntervalTimeName; + static const char* _cameraModeName; }; diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index 1660e5599dc0a8c123f49ac7601a45fa0cfe4b8e..4b1f9c66df5441d67dabe4e2d146ed17ca65eff3 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -30,6 +30,7 @@ void CameraSectionTest::init(void) rgCameraSignals[specifyGimbalChangedIndex] = SIGNAL(specifyGimbalChanged(bool)); rgCameraSignals[specifiedGimbalYawChangedIndex] = SIGNAL(specifiedGimbalYawChanged(double)); + rgCameraSignals[specifyCameraModeChangedIndex] = SIGNAL(specifyCameraModeChanged(bool)); _cameraSection = _simpleItem->cameraSection(); _createSpy(_cameraSection, &_spyCamera); @@ -58,6 +59,26 @@ void CameraSectionTest::init(void) _validStopTimeItem = new SimpleMissionItem(_offlineVehicle, MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, 0, 0, 0, 0, 0, 0, true, false), this); + _validCameraPhotoModeItem = new SimpleMissionItem(_offlineVehicle, + MissionItem(0, // sequence number + MAV_CMD_SET_CAMERA_MODE, + MAV_FRAME_MISSION, + 0, // camera id = 0, all cameras + 0, // photo mode + NAN, NAN, NAN, NAN, NAN, // param 3-7 unused + true, // autocontinue + false), // isCurrentItem + this); + _validCameraVideoModeItem = new SimpleMissionItem(_offlineVehicle, + MissionItem(0, // sequence number + MAV_CMD_SET_CAMERA_MODE, + MAV_FRAME_MISSION, + 0, // camera id = 0, all cameras + 1, // video mode + NAN, NAN, NAN, NAN, NAN, // param 3-7 unused + true, // autocontinue + false), // isCurrentItem + this); } void CameraSectionTest::cleanup(void) @@ -85,74 +106,104 @@ void CameraSectionTest::_createSpy(CameraSection* cameraSection, MultiSignalSpy* void CameraSectionTest::_testDirty(void) { // Check for dirty not signalled if same value + _cameraSection->setSpecifyGimbal(_cameraSection->specifyGimbal()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + + _cameraSection->setSpecifyCameraMode(_cameraSection->specifyCameraMode()); + QVERIFY(_spySection->checkNoSignals()); + QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalPitch()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->cameraAction()->setRawValue(_cameraSection->cameraAction()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->cameraPhotoIntervalTime()->setRawValue(_cameraSection->cameraPhotoIntervalTime()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); + _cameraSection->cameraPhotoIntervalDistance()->setRawValue(_cameraSection->cameraPhotoIntervalDistance()->rawValue()); QVERIFY(_spySection->checkNoSignals()); QCOMPARE(_cameraSection->dirty(), false); // Check for no duplicate dirty signalling on change + _cameraSection->setSpecifyGimbal(!_cameraSection->specifyGimbal()); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _spySection->clearAllSignals(); + _cameraSection->setSpecifyGimbal(!_cameraSection->specifyGimbal()); QVERIFY(_spySection->checkNoSignalByMask(dirtyChangedMask)); QCOMPARE(_cameraSection->dirty(), true); + _cameraSection->setDirty(false); _spySection->clearAllSignals(); - // Check that the dirty bit can be cleared - _cameraSection->setDirty(false); + _cameraSection->setSpecifyCameraMode(!_cameraSection->specifyCameraMode()); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); - QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), false); - QCOMPARE(_cameraSection->dirty(), false); + QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); + QCOMPARE(_cameraSection->dirty(), true); + _spySection->clearAllSignals(); + + _cameraSection->setSpecifyCameraMode(!_cameraSection->specifyCameraMode()); + QVERIFY(_spySection->checkNoSignalByMask(dirtyChangedMask)); + QCOMPARE(_cameraSection->dirty(), true); + _cameraSection->setDirty(false); _spySection->clearAllSignals(); // Check the remaining items that should set dirty bit + _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->gimbalYaw()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->cameraAction()->setRawValue(_cameraSection->cameraAction()->rawValue().toInt() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->cameraPhotoIntervalTime()->setRawValue(_cameraSection->cameraPhotoIntervalTime()->rawValue().toInt() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + _cameraSection->cameraPhotoIntervalDistance()->setRawValue(_cameraSection->cameraPhotoIntervalDistance()->rawValue().toDouble() + 1); QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); QCOMPARE(_cameraSection->dirty(), true); _cameraSection->setDirty(false); _spySection->clearAllSignals(); + + _cameraSection->cameraMode()->setRawValue(_cameraSection->cameraMode()->rawValue().toInt() == 0 ? 1 : 0); + QVERIFY(_spySection->checkSignalByMask(dirtyChangedMask)); + QCOMPARE(_spySection->pullBoolFromSignalIndex(dirtyChangedIndex), true); + QCOMPARE(_cameraSection->dirty(), true); + _cameraSection->setDirty(false); + _spySection->clearAllSignals(); } void CameraSectionTest::_testSettingsAvailable(void) @@ -162,7 +213,7 @@ void CameraSectionTest::_testSettingsAvailable(void) QCOMPARE(_cameraSection->specifyGimbal(), false); QCOMPARE(_cameraSection->settingsSpecified(), false); - // Check correct reaction to specifyGimbal on/off + // Check correct reaction to specify methods on/off _cameraSection->setSpecifyGimbal(true); QCOMPARE(_cameraSection->specifyGimbal(), true); @@ -184,6 +235,26 @@ void CameraSectionTest::_testSettingsAvailable(void) _spySection->clearAllSignals(); _spyCamera->clearAllSignals(); + _cameraSection->setSpecifyCameraMode(true); + QCOMPARE(_cameraSection->specifyCameraMode(), true); + QCOMPARE(_cameraSection->settingsSpecified(), true); + QVERIFY(_spyCamera->checkSignalByMask(specifyCameraModeChangedMask)); + QCOMPARE(_spyCamera->pullBoolFromSignalIndex(specifyCameraModeChangedIndex), true); + QVERIFY(_spySection->checkSignalByMask(settingsSpecifiedChangedMask)); + QCOMPARE(_spySection->pullBoolFromSignalIndex(settingsSpecifiedChangedIndex), true); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyCameraMode(false); + QCOMPARE(_cameraSection->specifyCameraMode(), false); + QCOMPARE(_cameraSection->settingsSpecified(), false); + QVERIFY(_spyCamera->checkSignalByMask(specifyCameraModeChangedMask)); + QCOMPARE(_spyCamera->pullBoolFromSignalIndex(specifyCameraModeChangedIndex), false); + QVERIFY(_spySection->checkSignalByMask(settingsSpecifiedChangedMask)); + QCOMPARE(_spySection->pullBoolFromSignalIndex(settingsSpecifiedChangedIndex), false); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + // Check correct reaction to cameraAction on/off _cameraSection->cameraAction()->setRawValue(CameraSection::TakePhotosIntervalTime); @@ -235,7 +306,7 @@ void CameraSectionTest::_testItemCount(void) // No settings specified to start QCOMPARE(_cameraSection->itemCount(), 0); - // Check specifyGimbal + // Check specify methods _cameraSection->setSpecifyGimbal(true); QCOMPARE(_cameraSection->itemCount(), 1); @@ -251,6 +322,34 @@ void CameraSectionTest::_testItemCount(void) _spySection->clearAllSignals(); _spyCamera->clearAllSignals(); + _cameraSection->setSpecifyCameraMode(true); + QCOMPARE(_cameraSection->itemCount(), 1); + QVERIFY(_spySection->checkSignalByMask(itemCountChangedMask)); + QCOMPARE(_spySection->pullIntFromSignalIndex(itemCountChangedIndex), 1); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyCameraMode(false); + QCOMPARE(_cameraSection->itemCount(), 0); + QVERIFY(_spySection->checkSignalByMask(itemCountChangedMask)); + QCOMPARE(_spySection->pullIntFromSignalIndex(itemCountChangedIndex), 0); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyGimbal(true); + _cameraSection->setSpecifyCameraMode(true); + QCOMPARE(_cameraSection->itemCount(), 2); + QVERIFY(_spySection->checkSignalsByMask(itemCountChangedMask)); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + + _cameraSection->setSpecifyGimbal(false); + _cameraSection->setSpecifyCameraMode(false); + QCOMPARE(_cameraSection->itemCount(), 0); + QVERIFY(_spySection->checkSignalsByMask(itemCountChangedMask)); + _spySection->clearAllSignals(); + _spyCamera->clearAllSignals(); + // Check camera actions QList rgCameraActions; @@ -318,6 +417,28 @@ void CameraSectionTest::_testAppendSectionItems(void) rgMissionItems.clear(); seqNum = 0; + // Test specifyCameraMode + + _cameraSection->setSpecifyCameraMode(true); + _cameraSection->cameraMode()->setRawValue(0); + _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); + QCOMPARE(rgMissionItems.count(), 1); + QCOMPARE(seqNum, 1); + _missionItemsEqual(*rgMissionItems[0], _validCameraPhotoModeItem->missionItem()); + _cameraSection->setSpecifyGimbal(false); + rgMissionItems.clear(); + seqNum = 0; + + _cameraSection->setSpecifyCameraMode(true); + _cameraSection->cameraMode()->setRawValue(1); + _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); + QCOMPARE(rgMissionItems.count(), 1); + QCOMPARE(seqNum, 1); + _missionItemsEqual(*rgMissionItems[0], _validCameraVideoModeItem->missionItem()); + _cameraSection->setSpecifyCameraMode(false); + rgMissionItems.clear(); + seqNum = 0; + // Test camera actions _cameraSection->cameraAction()->setRawValue(CameraSection::TakePhotosIntervalTime); @@ -468,6 +589,62 @@ void CameraSectionTest::_testScanForGimbalSection(void) visualItems.clear(); } +void CameraSectionTest::_testScanForCameraModeSection(void) +{ + QCOMPARE(_cameraSection->available(), true); + + int scanIndex = 0; + QmlObjectListModel visualItems; + + _commonScanTest(_cameraSection); + + // Check for a scan success + + SimpleMissionItem* newValidCameraModeItem = new SimpleMissionItem(_offlineVehicle, this); + newValidCameraModeItem->missionItem() = _validCameraPhotoModeItem->missionItem(); + visualItems.append(newValidCameraModeItem); + scanIndex = 0; + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); + QCOMPARE(visualItems.count(), 0); + QCOMPARE(_cameraSection->settingsSpecified(), true); + QCOMPARE(_cameraSection->specifyCameraMode(), true); + QCOMPARE(_cameraSection->cameraMode()->rawValue().toDouble(), _validCameraPhotoModeItem->missionItem().param2()); + _cameraSection->setSpecifyCameraMode(false); + visualItems.clear(); + scanIndex = 0; + + newValidCameraModeItem->missionItem() = _validCameraVideoModeItem->missionItem(); + visualItems.append(newValidCameraModeItem); + scanIndex = 0; + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), true); + QCOMPARE(visualItems.count(), 0); + QCOMPARE(_cameraSection->settingsSpecified(), true); + QCOMPARE(_cameraSection->specifyCameraMode(), true); + QCOMPARE(_cameraSection->cameraMode()->rawValue().toDouble(), _validCameraVideoModeItem->missionItem().param2()); + _cameraSection->setSpecifyCameraMode(false); + visualItems.clear(); + scanIndex = 0; + +#if 0 + MAV_CMD_SET_CAMERA_MODE + Mission Param #1 Camera ID (0 for all cameras, 1 for first, 2 for second, etc.) + Mission Param #2 Camera mode (0: photo mode, 1: video mode) + Mission Param #3 Audio recording enabled (0: off 1: on) + Mission Param #4 Reserved (all remaining params) +#endif + + // Mode command but incorrect settings + + SimpleMissionItem invalidSimpleItem(_offlineVehicle, _validCameraPhotoModeItem->missionItem()); + invalidSimpleItem.missionItem().setParam3(0); // Audio is not supported + visualItems.append(&invalidSimpleItem); + QCOMPARE(_cameraSection->scanForSection(&visualItems, scanIndex), false); + QCOMPARE(visualItems.count(), 1); + QCOMPARE(_cameraSection->specifyCameraMode(), false); + QCOMPARE(_cameraSection->settingsSpecified(), false); + visualItems.clear(); +} + void CameraSectionTest::_testScanForPhotoIntervalTimeSection(void) { QCOMPARE(_cameraSection->available(), true); diff --git a/src/MissionManager/CameraSectionTest.h b/src/MissionManager/CameraSectionTest.h index cf288964324ade0932667070e43c92d351f4d4b4..16cfbb96aaad35edcb6bca03087da54b53a50c74 100644 --- a/src/MissionManager/CameraSectionTest.h +++ b/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; }; diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 1d6cc1f38b5ebdf607adecf767fd8c0414c7c6ee..3534f354b3f5fed842542ff9e12ae4c770a80443 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/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", diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 1e637a4c08b75b781c79ff6ef8c6b667a6df78f4..6839585c8bd90627862f0ebe1837e4154f21a7ca 100644 --- a/src/MissionManager/MissionController.cc +++ b/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(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(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(0); + CameraSection* cameraSection = settingsItem->cameraSection(); if (_controllerVehicle->firmwarePlugin()->hasGimbal(_controllerVehicle, rollSupported, pitchSupported, yawSupported) && pitchSupported) { - MissionSettingsItem* settingsItem = _visualItems->value(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); + } } } diff --git a/src/MissionManager/SectionTest.cc b/src/MissionManager/SectionTest.cc index cd95f96c0183f854b8aa9598b231f48ccae96455..ef7b4acc963fa650b5b8ea9f58131f55ef69fea3 100644 --- a/src/MissionManager/SectionTest.cc +++ b/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) diff --git a/src/PlanView/CameraSection.qml b/src/PlanView/CameraSection.qml index 602f57718884c1a947fc48fe202dcdce3d38f6dc..64d8fee3b77dfdfb9a0c41a240468e022a49d51d 100644 --- a/src/PlanView/CameraSection.qml +++ b/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 + } + } } } diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc index 161a02dfd903f5388ba16e39da98103b21fd6fa8..1352b175cf4c715ff0cf9c9c7b7f0ff3692d57af 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/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); + } +} diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index 8e2b9cd4ab536778ca0926ea25ae50659d9f6eb0..fa1d164f175ebeda594d3c909d5a4326686634ef 100644 --- a/src/qgcunittest/UnitTest.h +++ b/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