From 92e7bf389e6056c980f38c311ac4cf4859f2c6d8 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 1 Nov 2018 11:27:00 -0700 Subject: [PATCH] FW Landing Pattern: Support for stop photo/video. Plus item unit test. --- ChangeLog.md | 1 + qgroundcontrol.pro | 2 + src/MissionManager/CameraSection.cc | 145 +++++++---- src/MissionManager/CameraSection.h | 9 +- src/MissionManager/CameraSectionTest.cc | 52 ++-- src/MissionManager/CameraSectionTest.h | 6 +- .../FWLandingPattern.FactMetaData.json | 12 + src/MissionManager/FWLandingPatternTest.cc | 238 ++++++++++++++++++ src/MissionManager/FWLandingPatternTest.h | 54 ++++ .../FixedWingLandingComplexItem.cc | 188 +++++++++++--- .../FixedWingLandingComplexItem.h | 19 ++ src/MissionManager/SectionTest.cc | 15 -- src/MissionManager/SectionTest.h | 1 - src/PlanView/FWLandingPatternEditor.qml | 29 +++ src/qgcunittest/UnitTest.cc | 20 ++ src/qgcunittest/UnitTest.h | 6 + src/qgcunittest/UnitTestList.cc | 2 + 17 files changed, 667 insertions(+), 132 deletions(-) create mode 100644 src/MissionManager/FWLandingPatternTest.cc create mode 100644 src/MissionManager/FWLandingPatternTest.h diff --git a/ChangeLog.md b/ChangeLog.md index 9b0a6ebd0..d3875773a 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -12,6 +12,7 @@ Note: This file only contains high level features or important fixes. * Make Distance to GCS available for display from instrument panel. * Make Heading to Home available for display from instrument panel. * Edit Position dialog available on polygon vertices. +* Fixed Wing Landing Pattern: Add stop photo/video support. Defaults to on such that doing an RTL will stop camera. ## 3.4 diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 72d68bfef..cef995c9b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -434,6 +434,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/CameraCalcTest.h \ src/MissionManager/CameraSectionTest.h \ src/MissionManager/CorridorScanComplexItemTest.h \ + src/MissionManager/FWLandingPatternTest.h \ src/MissionManager/MissionCommandTreeTest.h \ src/MissionManager/MissionControllerManagerTest.h \ src/MissionManager/MissionControllerTest.h \ @@ -475,6 +476,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/CameraCalcTest.cc \ src/MissionManager/CameraSectionTest.cc \ src/MissionManager/CorridorScanComplexItemTest.cc \ + src/MissionManager/FWLandingPatternTest.cc \ src/MissionManager/MissionCommandTreeTest.cc \ src/MissionManager/MissionControllerManagerTest.cc \ src/MissionManager/MissionControllerTest.cc \ diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index 9114c8f7c..feccaf552 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -37,7 +37,7 @@ CameraSection::CameraSection(Vehicle* vehicle, QObject* parent) , _dirty(false) { if (_metaDataMap.isEmpty()) { - _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CameraSection.FactMetaData.json"), NULL /* metaDataParent */); + _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CameraSection.FactMetaData.json"), Q_NULLPTR /* metaDataParent */); } _gimbalPitchFact.setMetaData (_metaDataMap[_gimbalPitchName]); @@ -122,11 +122,11 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss MissionItem* item = new MissionItem(nextSequenceNumber++, MAV_CMD_SET_CAMERA_MODE, MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) + 0, // Reserved (Set to 0) _cameraModeFact.rawValue().toDouble(), - NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved - true, // autoContinue - false, // isCurrentItem + qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), // reserved + true, // autoContinue + false, // isCurrentItem missionItemParent); items.append(item); } @@ -157,7 +157,7 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss 0, // Reserved (Set to 0) _cameraPhotoIntervalTimeFact.rawValue().toInt(), // Interval 0, // Unlimited photo count - NAN, NAN, NAN, NAN, // param 4-7 reserved + qQNaN(), qQNaN(), qQNaN(), qQNaN(), // reserved true, // autoContinue false, // isCurrentItem missionItemParent); @@ -180,55 +180,32 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss item = new MissionItem(nextSequenceNumber++, MAV_CMD_VIDEO_START_CAPTURE, MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - VIDEO_CAPTURE_STATUS_INTERVAL, // CAMERA_CAPTURE_STATUS (default to every 5 seconds) - NAN, NAN, NAN, NAN, NAN, // param 3-7 reserved - true, // autoContinue - false, // isCurrentItem + 0, // Reserved (Set to 0) + VIDEO_CAPTURE_STATUS_INTERVAL, // CAMERA_CAPTURE_STATUS (default to every 5 seconds) + qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), // reserved + true, // autoContinue + false, // isCurrentItem missionItemParent); break; case StopTakingVideo: - item = new MissionItem(nextSequenceNumber++, - MAV_CMD_VIDEO_STOP_CAPTURE, - MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - NAN, NAN, NAN, NAN, NAN, NAN, // param 2-7 reserved - true, // autoContinue - false, // isCurrentItem - missionItemParent); + appendStopTakingVideo(items, nextSequenceNumber, missionItemParent); break; case StopTakingPhotos: - item = new MissionItem(nextSequenceNumber++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0, // Trigger distance = 0 means stop - 0, 0, 0, 0, 0, 0, // param 2-7 not used - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - item = new MissionItem(nextSequenceNumber++, - MAV_CMD_IMAGE_STOP_CAPTURE, - MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - NAN, NAN, NAN, NAN, NAN, NAN, // param 2-7 reserved - true, // autoContinue - false, // isCurrentItem - missionItemParent); + appendStopTakingPhotos(items, nextSequenceNumber, missionItemParent); break; case TakePhoto: item = new MissionItem(nextSequenceNumber++, MAV_CMD_IMAGE_START_CAPTURE, MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // Interval (none) - 1, // Take 1 photo - NAN, NAN, NAN, NAN, // param 4-7 reserved - true, // autoContinue - false, // isCurrentItem + 0, // Reserved (Set to 0) + 0, // Interval (none) + 1, // Take 1 photo + qQNaN(), qQNaN(), qQNaN(), qQNaN(), // reserved + true, // autoContinue + false, // isCurrentItem missionItemParent); break; } @@ -238,8 +215,46 @@ void CameraSection::appendSectionItems(QList& items, QObject* miss } } +void CameraSection::appendStopTakingPhotos(QList& items, int& seqNum, QObject* missionItemParent) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0, // Trigger distance = 0 means stop + 0, 0, 0, 0, 0, 0, // param 2-7 not used + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + item = new MissionItem(seqNum++, + MAV_CMD_IMAGE_STOP_CAPTURE, + MAV_FRAME_MISSION, + 0, // Reserved (Set to 0) + qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), // reserved + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void CameraSection::appendStopTakingVideo(QList& items, int& seqNum, QObject* missionItemParent) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_VIDEO_STOP_CAPTURE, + MAV_FRAME_MISSION, + 0, // Reserved (Set to 0) + qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), // reserved + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + bool CameraSection::_scanGimbal(QmlObjectListModel* visualItems, int scanIndex) { + if (scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -259,6 +274,9 @@ bool CameraSection::_scanGimbal(QmlObjectListModel* visualItems, int scanIndex) bool CameraSection::_scanTakePhoto(QmlObjectListModel* visualItems, int scanIndex) { + if (scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -276,6 +294,9 @@ bool CameraSection::_scanTakePhoto(QmlObjectListModel* visualItems, int scanInde bool CameraSection::_scanTakePhotosIntervalTime(QmlObjectListModel* visualItems, int scanIndex) { + if (scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -292,8 +313,11 @@ bool CameraSection::_scanTakePhotosIntervalTime(QmlObjectListModel* visualItems, return false; } -bool CameraSection::_scanStopTakingPhotos(QmlObjectListModel* visualItems, int scanIndex) +bool CameraSection::scanStopTakingPhotos(QmlObjectListModel* visualItems, int scanIndex, bool removeScannedItems) { + if (scanIndex < 0 || scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -304,9 +328,10 @@ bool CameraSection::_scanStopTakingPhotos(QmlObjectListModel* visualItems, int s if (nextItem) { MissionItem& nextMissionItem = nextItem->missionItem(); if (nextMissionItem.command() == MAV_CMD_IMAGE_STOP_CAPTURE && nextMissionItem.param1() == 0) { - cameraAction()->setRawValue(StopTakingPhotos); - visualItems->removeAt(scanIndex)->deleteLater(); - visualItems->removeAt(scanIndex)->deleteLater(); + if (removeScannedItems) { + visualItems->removeAt(scanIndex)->deleteLater(); + visualItems->removeAt(scanIndex)->deleteLater(); + } return true; } } @@ -320,6 +345,9 @@ bool CameraSection::_scanStopTakingPhotos(QmlObjectListModel* visualItems, int s bool CameraSection::_scanTriggerStartDistance(QmlObjectListModel* visualItems, int scanIndex) { + if (scanIndex < 0 || scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -338,6 +366,9 @@ bool CameraSection::_scanTriggerStartDistance(QmlObjectListModel* visualItems, i bool CameraSection::_scanTriggerStopDistance(QmlObjectListModel* visualItems, int scanIndex) { + if (scanIndex < 0 || scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -356,6 +387,9 @@ bool CameraSection::_scanTriggerStopDistance(QmlObjectListModel* visualItems, in bool CameraSection::_scanTakeVideo(QmlObjectListModel* visualItems, int scanIndex) { + if (scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -371,15 +405,19 @@ bool CameraSection::_scanTakeVideo(QmlObjectListModel* visualItems, int scanInde return false; } -bool CameraSection::_scanStopTakingVideo(QmlObjectListModel* visualItems, int scanIndex) +bool CameraSection::scanStopTakingVideo(QmlObjectListModel* visualItems, int scanIndex, bool removeScannedItems) { + if (scanIndex < 0 || scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); if ((MAV_CMD)item->command() == MAV_CMD_VIDEO_STOP_CAPTURE) { if (missionItem.param1() == 0) { - cameraAction()->setRawValue(StopTakingVideo); - visualItems->removeAt(scanIndex)->deleteLater(); + if (removeScannedItems) { + visualItems->removeAt(scanIndex)->deleteLater(); + } return true; } } @@ -390,6 +428,9 @@ bool CameraSection::_scanStopTakingVideo(QmlObjectListModel* visualItems, int sc bool CameraSection::_scanSetCameraMode(QmlObjectListModel* visualItems, int scanIndex) { + if (scanIndex < 0 || scanIndex > visualItems->count() -1) { + return false; + } SimpleMissionItem* item = visualItems->value(scanIndex); if (item) { MissionItem& missionItem = item->missionItem(); @@ -434,7 +475,8 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde foundCameraAction = true; continue; } - if (!foundCameraAction && _scanStopTakingPhotos(visualItems, scanIndex)) { + if (!foundCameraAction && scanStopTakingPhotos(visualItems, scanIndex, true /* removeScannedItems */)) { + cameraAction()->setRawValue(StopTakingPhotos); foundCameraAction = true; continue; } @@ -450,7 +492,8 @@ bool CameraSection::scanForSection(QmlObjectListModel* visualItems, int scanInde foundCameraAction = true; continue; } - if (!foundCameraAction && _scanStopTakingVideo(visualItems, scanIndex)) { + if (!foundCameraAction && scanStopTakingVideo(visualItems, scanIndex, true /* removeScannedItems */)) { + cameraAction()->setRawValue(StopTakingVideo); foundCameraAction = true; continue; } diff --git a/src/MissionManager/CameraSection.h b/src/MissionManager/CameraSection.h index c57b2a028..49f230896 100644 --- a/src/MissionManager/CameraSection.h +++ b/src/MissionManager/CameraSection.h @@ -67,6 +67,13 @@ public: ///< @return The gimbal pitch specified by this item, NaN if not specified double specifiedGimbalPitch(void) const; + static bool scanStopTakingPhotos(QmlObjectListModel* visualItems, int scanIndex, bool removeScannedItems); + static bool scanStopTakingVideo(QmlObjectListModel* visualItems, int scanIndex, bool removeScannedItems); + static void appendStopTakingPhotos(QList& items, int& seqNum, QObject* missionItemParent); + static void appendStopTakingVideo(QList& items, int& seqNum, QObject* missionItemParent); + static int stopTakingPhotosCommandCount(void) { return 2; } + static int stopTakingVideoCommandCount(void) { return 1; } + // Overrides from Section bool available (void) const override { return _available; } bool dirty (void) const override { return _dirty; } @@ -97,11 +104,9 @@ private: bool _scanGimbal(QmlObjectListModel* visualItems, int scanIndex); bool _scanTakePhoto(QmlObjectListModel* visualItems, int scanIndex); bool _scanTakePhotosIntervalTime(QmlObjectListModel* visualItems, int scanIndex); - bool _scanStopTakingPhotos(QmlObjectListModel* visualItems, int scanIndex); bool _scanTriggerStartDistance(QmlObjectListModel* visualItems, int scanIndex); bool _scanTriggerStopDistance(QmlObjectListModel* visualItems, int scanIndex); bool _scanTakeVideo(QmlObjectListModel* visualItems, int scanIndex); - bool _scanStopTakingVideo(QmlObjectListModel* visualItems, int scanIndex); bool _scanSetCameraMode(QmlObjectListModel* visualItems, int scanIndex); bool _available; diff --git a/src/MissionManager/CameraSectionTest.cc b/src/MissionManager/CameraSectionTest.cc index a575dff16..18eef23d2 100644 --- a/src/MissionManager/CameraSectionTest.cc +++ b/src/MissionManager/CameraSectionTest.cc @@ -20,9 +20,6 @@ CameraSectionTest::CameraSectionTest(void) , _validDistanceItem (NULL) , _validTimeItem (NULL) , _validStartVideoItem (NULL) - , _validStopVideoItem (NULL) - , _validStopDistanceItem (NULL) - , _validStopTimeItem (NULL) , _validCameraPhotoModeItem (NULL) , _validCameraVideoModeItem (NULL) , _validCameraSurveyPhotoModeItem (NULL) @@ -75,18 +72,6 @@ void CameraSectionTest::init(void) true, // autocontinue false), // isCurrentItem this); - _validStopVideoItem = new SimpleMissionItem(_offlineVehicle, - false, // flyView - MissionItem(0, MAV_CMD_VIDEO_STOP_CAPTURE, MAV_FRAME_MISSION, 0, NAN, NAN, NAN, NAN, NAN, NAN, true, false), - this); - _validStopDistanceItem = new SimpleMissionItem(_offlineVehicle, - false, // flyView - MissionItem(0, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, 0, 0, 0, 0, 0, 0, 0, true, false), - this); - _validStopTimeItem = new SimpleMissionItem(_offlineVehicle, - false, // flyView - MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, NAN, NAN, NAN, NAN, NAN, NAN, true, false), - this); _validCameraPhotoModeItem = new SimpleMissionItem(_offlineVehicle, false, // flyView MissionItem(0, // sequence number @@ -132,6 +117,10 @@ void CameraSectionTest::init(void) true, // autoContinue false), // isCurrentItem this); + + _validStopVideoItem = createValidStopVideoItem(_offlineVehicle, this); + _validStopDistanceItem = createValidStopDistanceItem(_offlineVehicle, this); + _validStopTimeItem = createValidStopTimeItem(_offlineVehicle, this); } void CameraSectionTest::cleanup(void) @@ -471,8 +460,6 @@ void CameraSectionTest::_testAppendSectionItems(void) QCOMPARE(seqNum, 0); rgMissionItems.clear(); - // Test specifyGimbal - _cameraSection->setSpecifyGimbal(true); _cameraSection->gimbalPitch()->setRawValue(_validGimbalItem->missionItem().param1()); _cameraSection->gimbalYaw()->setRawValue(_validGimbalItem->missionItem().param3()); @@ -484,8 +471,6 @@ void CameraSectionTest::_testAppendSectionItems(void) rgMissionItems.clear(); seqNum = 0; - // Test specifyCameraMode - _cameraSection->setSpecifyCameraMode(true); _cameraSection->cameraMode()->setRawValue(CAMERA_MODE_IMAGE); _cameraSection->appendSectionItems(rgMissionItems, this, seqNum); @@ -910,7 +895,7 @@ void CameraSectionTest::_testScanForStopVideoSection(void) /* MAV_CMD_VIDEO_STOP_CAPTURE Stop the current video capture (recording) Mission Param #1 Reserved (Set to 0) -*/ + */ SimpleMissionItem* newValidStopVideoItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, this); newValidStopVideoItem->missionItem() = _validStopVideoItem->missionItem(); @@ -941,7 +926,7 @@ void CameraSectionTest::_testScanForStopVideoSection(void) visualItems.clear(); } -void CameraSectionTest::_testScanForStopImageSection(void) +void CameraSectionTest::_testScanForStopPhotoSection(void) { QCOMPARE(_cameraSection->available(), true); @@ -1124,3 +1109,28 @@ void CameraSectionTest::_testSpecifiedGimbalValuesChanged(void) _cameraSection->gimbalPitch()->setRawValue(_cameraSection->gimbalPitch()->rawValue().toDouble() + 1); QVERIFY(_spyCamera->checkSignalByMask(specifiedGimbalPitchChangedMask)); } + +SimpleMissionItem* CameraSectionTest::createValidStopVideoItem(Vehicle* vehicle, QObject* parent) +{ + return new SimpleMissionItem(vehicle, + false, // flyView + MissionItem(0, MAV_CMD_VIDEO_STOP_CAPTURE, MAV_FRAME_MISSION, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), true, false), + parent); +} + + +SimpleMissionItem* CameraSectionTest::createValidStopDistanceItem(Vehicle* vehicle, QObject* parent) +{ + return new SimpleMissionItem(vehicle, + false, // flyView + MissionItem(0, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, 0, 0, 0, 0, 0, 0, 0, true, false), + parent); +} + +SimpleMissionItem* CameraSectionTest::createValidStopTimeItem(Vehicle* vehicle, QObject* parent) +{ + return new SimpleMissionItem(vehicle, + false, // flyView + MissionItem(1, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_FRAME_MISSION, 0, qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), qQNaN(), true, false), + parent); +} diff --git a/src/MissionManager/CameraSectionTest.h b/src/MissionManager/CameraSectionTest.h index 5e824a8fb..8f15caf92 100644 --- a/src/MissionManager/CameraSectionTest.h +++ b/src/MissionManager/CameraSectionTest.h @@ -23,6 +23,10 @@ public: void init(void) override; void cleanup(void) override; + static SimpleMissionItem* createValidStopVideoItem (Vehicle* vehicle, QObject* parent); + static SimpleMissionItem* createValidStopDistanceItem(Vehicle* vehicle, QObject* parent); + static SimpleMissionItem* createValidStopTimeItem (Vehicle* vehicle, QObject* parent); + private slots: void _testDirty (void); void _testSettingsAvailable (void); @@ -34,7 +38,7 @@ private slots: void _testScanForPhotoIntervalDistanceSection (void); void _testScanForStartVideoSection (void); void _testScanForStopVideoSection (void); - void _testScanForStopImageSection (void); + void _testScanForStopPhotoSection (void); void _testScanForCameraModeSection (void); void _testScanForTakePhotoSection (void); void _testScanForMultipleItems (void); diff --git a/src/MissionManager/FWLandingPattern.FactMetaData.json b/src/MissionManager/FWLandingPattern.FactMetaData.json index 6d1a9a16b..5605343e5 100644 --- a/src/MissionManager/FWLandingPattern.FactMetaData.json +++ b/src/MissionManager/FWLandingPattern.FactMetaData.json @@ -52,5 +52,17 @@ "max": 90, "decimalPlaces": 1, "defaultValue": 12.0 +}, +{ + "name": "StopTakingPhotos", + "shortDescription": "Stop taking photos", + "type": "bool", + "defaultValue": true +}, +{ + "name": "StopTakingVideo", + "shortDescription": "Stop taking video", + "type": "bool", + "defaultValue": true } ] diff --git a/src/MissionManager/FWLandingPatternTest.cc b/src/MissionManager/FWLandingPatternTest.cc new file mode 100644 index 000000000..5d8bec721 --- /dev/null +++ b/src/MissionManager/FWLandingPatternTest.cc @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "FWLandingPatternTest.h" +#include "QGCApplication.h" +#include "MissionCommandTree.h" +#include "MissionCommandUIInfo.h" +#include "CameraSectionTest.h" + +FWLandingPatternTest::FWLandingPatternTest(void) + : _offlineVehicle (Q_NULLPTR) + , _fwItem (Q_NULLPTR) + , _multiSpy (Q_NULLPTR) + , _validStopVideoItem (Q_NULLPTR) + , _validStopDistanceItem (Q_NULLPTR) + , _validStopTimeItem (Q_NULLPTR) +{ + +} + +void FWLandingPatternTest::init(void) +{ + UnitTest::init(); + + rgSignals[dirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); + + _offlineVehicle = new Vehicle(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, qgcApp()->toolbox()->firmwarePluginManager(), this); + _fwItem = new FixedWingLandingComplexItem(_offlineVehicle, false /* flyView */, this); + _multiSpy = new MultiSignalSpy(); + QCOMPARE(_multiSpy->init(_fwItem, rgSignals, cSignals), true); + + // Start in a clean state + QVERIFY(!_fwItem->dirty()); + _fwItem->setLandingCoordinate(QGeoCoordinate(47, -122)); + _fwItem->setDirty(false); + QVERIFY(!_fwItem->dirty()); + _multiSpy->clearAllSignals(); + + _validStopVideoItem = CameraSectionTest::createValidStopTimeItem(_offlineVehicle, this); + _validStopDistanceItem = CameraSectionTest::createValidStopTimeItem(_offlineVehicle, this); + _validStopTimeItem = CameraSectionTest::createValidStopTimeItem(_offlineVehicle, this); +} + +void FWLandingPatternTest::cleanup(void) +{ + delete _fwItem; + delete _offlineVehicle; + delete _multiSpy; + delete _validStopVideoItem; + delete _validStopDistanceItem; + delete _validStopTimeItem; + UnitTest::cleanup(); +} + + +void FWLandingPatternTest::_testDefaults(void) +{ + QCOMPARE(_fwItem->stopTakingPhotos()->rawValue().toBool(), true); + QCOMPARE(_fwItem->stopTakingVideo()->rawValue().toBool(), true); +} + +void FWLandingPatternTest::_testItemCount(void) +{ + QList items; + + _fwItem->stopTakingPhotos()->setRawValue(true); + _fwItem->stopTakingVideo()->setRawValue(true); + _fwItem->appendMissionItems(items, this); + QCOMPARE(items.count(), 3 + CameraSection::stopTakingPhotosCommandCount() + CameraSection::stopTakingVideoCommandCount()); + QCOMPARE(items.count() - 1, _fwItem->lastSequenceNumber()); + items.clear(); + + _fwItem->stopTakingPhotos()->setRawValue(false); + _fwItem->stopTakingVideo()->setRawValue(false); + _fwItem->appendMissionItems(items, this); + QCOMPARE(items.count(), 3); + QCOMPARE(items.count() - 1, _fwItem->lastSequenceNumber()); + items.clear(); +} + +void FWLandingPatternTest::_testAppendSectionItems(void) +{ + QList rgMissionItems; + + // Create the set of appended mission items + _fwItem->stopTakingPhotos()->setRawValue(true); + _fwItem->stopTakingVideo()->setRawValue(true); + _fwItem->appendMissionItems(rgMissionItems, this); + + // Convert to visual items + QmlObjectListModel* simpleItems = new QmlObjectListModel(this); + + for (MissionItem* item: rgMissionItems) { + SimpleMissionItem* simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, simpleItems); + simpleItem->missionItem() = *item; + simpleItems->append(simpleItem); + } + + // Scan the items back in to verify the same values come back + // Note that the compares does the best it can with doubles going to floats and back causing inaccuracies beyond a fuzzy compare. + QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _offlineVehicle)); + QCOMPARE(simpleItems->count(), 1); + _validateItem(simpleItems->value(0)); + + // Reset + simpleItems->deleteLater(); + rgMissionItems.clear(); + + // Check for no stop camera actions + _fwItem->stopTakingPhotos()->setRawValue(false); + _fwItem->stopTakingVideo()->setRawValue(false); + _fwItem->appendMissionItems(rgMissionItems, this); + simpleItems = new QmlObjectListModel(this); + for (MissionItem* item: rgMissionItems) { + SimpleMissionItem* simpleItem = new SimpleMissionItem(_offlineVehicle, false /* flyView */, simpleItems); + simpleItem->missionItem() = *item; + simpleItems->append(simpleItem); + } + QVERIFY(FixedWingLandingComplexItem::scanForItem(simpleItems, false /* flyView */, _offlineVehicle)); + QCOMPARE(simpleItems->count(), 1); + _validateItem(simpleItems->value(0)); + + // Reset + simpleItems->deleteLater(); + rgMissionItems.clear(); +} + +void FWLandingPatternTest::_testDirty(void) +{ + _fwItem->setDirty(true); + QVERIFY(_fwItem->dirty()); + QVERIFY(_multiSpy->checkOnlySignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _fwItem->setDirty(false); + _multiSpy->clearAllSignals(); + + // These facts should set dirty when changed + QList rgFacts; + rgFacts << _fwItem->loiterAltitude() + << _fwItem->landingHeading() + << _fwItem->loiterRadius() + << _fwItem->landingAltitude() + << _fwItem->landingDistance() + << _fwItem->glideSlope() + << _fwItem->stopTakingPhotos() + << _fwItem->stopTakingVideo(); + for(Fact* fact: rgFacts) { + qDebug() << fact->name(); + QVERIFY(!_fwItem->dirty()); + if (fact->typeIsBool()) { + fact->setRawValue(!fact->rawValue().toBool()); + } else { + fact->setRawValue(fact->rawValue().toDouble() + 1); + } + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _fwItem->setDirty(false); + _multiSpy->clearAllSignals(); + } + rgFacts.clear(); + + // These bool properties should set dirty when changed + QList rgBoolNames; + rgBoolNames << "valueSetIsDistance" + << "loiterClockwise" + << "altitudesAreRelative"; + const QMetaObject* metaObject = _fwItem->metaObject(); + for(const char* boolName: rgBoolNames) { + qDebug() << boolName; + QVERIFY(!_fwItem->dirty()); + QMetaProperty boolProp = metaObject->property(metaObject->indexOfProperty(boolName)); + QVERIFY(boolProp.write(_fwItem, !boolProp.read(_fwItem).toBool())); + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _fwItem->setDirty(false); + _multiSpy->clearAllSignals(); + } + rgFacts.clear(); + + // These coordinates should set dirty when changed + QVERIFY(!_fwItem->dirty()); + _fwItem->setLoiterCoordinate(_fwItem->loiterCoordinate().atDistanceAndAzimuth(1, 0)); + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _fwItem->setDirty(false); + _multiSpy->clearAllSignals(); + QVERIFY(!_fwItem->dirty()); + _fwItem->setLoiterCoordinate(_fwItem->landingCoordinate().atDistanceAndAzimuth(1, 0)); + QVERIFY(_multiSpy->checkSignalByMask(dirtyChangedMask)); + QVERIFY(_multiSpy->pullBoolFromSignalIndex(dirtyChangedIndex)); + _fwItem->setDirty(false); + _multiSpy->clearAllSignals(); +} + +void FWLandingPatternTest::_testSaveLoad(void) +{ + QJsonArray items; + _fwItem->save(items); + + QString errorString; + FixedWingLandingComplexItem* newItem = new FixedWingLandingComplexItem(_offlineVehicle, false /* flyView */, this /* parent */); + bool success =newItem->load(items[0].toObject(), 10, errorString); + if (!success) { + qDebug() << errorString; + } + QVERIFY(success); + QVERIFY(errorString.isEmpty()); + _validateItem(newItem); + newItem->deleteLater(); +} + +void FWLandingPatternTest::_validateItem(FixedWingLandingComplexItem* newItem) +{ + QVERIFY(newItem); + + QVERIFY(fuzzyCompareLatLon(newItem->loiterCoordinate(), _fwItem->loiterCoordinate())); + QVERIFY(fuzzyCompareLatLon(newItem->loiterTangentCoordinate(), _fwItem->loiterTangentCoordinate())); + QVERIFY(fuzzyCompareLatLon(newItem->landingCoordinate(), _fwItem->landingCoordinate())); + + QCOMPARE(newItem->stopTakingPhotos()->rawValue().toBool(), _fwItem->stopTakingPhotos()->rawValue().toBool()); + QCOMPARE(newItem->stopTakingVideo()->rawValue().toBool(), _fwItem->stopTakingVideo()->rawValue().toBool()); + QCOMPARE(newItem->loiterAltitude()->rawValue().toInt(), _fwItem->loiterAltitude()->rawValue().toInt()); + QCOMPARE(newItem->loiterRadius()->rawValue().toInt(), _fwItem->loiterRadius()->rawValue().toInt()); + QCOMPARE(newItem->landingAltitude()->rawValue().toInt(), _fwItem->landingAltitude()->rawValue().toInt()); + QCOMPARE(newItem->landingHeading()->rawValue().toInt(), _fwItem->landingHeading()->rawValue().toInt()); + QCOMPARE(newItem->landingDistance()->rawValue().toInt(), _fwItem->landingDistance()->rawValue().toInt()); + QCOMPARE(newItem->glideSlope()->rawValue().toInt(), _fwItem->glideSlope()->rawValue().toInt()); + QCOMPARE(newItem->_valueSetIsDistance, _fwItem->_valueSetIsDistance); + QCOMPARE(newItem->_loiterClockwise, _fwItem->_loiterClockwise); + QCOMPARE(newItem->_altitudesAreRelative, _fwItem->_altitudesAreRelative); + QCOMPARE(newItem->_landingCoordSet, _fwItem->_landingCoordSet); +} diff --git a/src/MissionManager/FWLandingPatternTest.h b/src/MissionManager/FWLandingPatternTest.h new file mode 100644 index 000000000..d1cd807af --- /dev/null +++ b/src/MissionManager/FWLandingPatternTest.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * 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 "FixedWingLandingComplexItem.h" +#include "MultiSignalSpy.h" + +class FWLandingPatternTest : public UnitTest +{ + Q_OBJECT + +public: + FWLandingPatternTest(void); + + void init(void) override; + void cleanup(void) override; + +private slots: + void _testDirty (void); + void _testItemCount (void); + void _testDefaults (void); + void _testAppendSectionItems (void); + void _testSaveLoad (void); + +private: + void _validateItem(FixedWingLandingComplexItem* newItem); + + enum { + dirtyChangedIndex = 0, + maxSignalIndex, + }; + + enum { + dirtyChangedMask = 1 << dirtyChangedIndex, + }; + + static const size_t cSignals = maxSignalIndex; + const char* rgSignals[cSignals]; + + Vehicle* _offlineVehicle; + FixedWingLandingComplexItem* _fwItem; + MultiSignalSpy* _multiSpy; + SimpleMissionItem* _validStopVideoItem; + SimpleMissionItem* _validStopDistanceItem; + SimpleMissionItem* _validStopTimeItem; +}; diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index 1d6de1a04..9e26762f0 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -27,6 +27,8 @@ const char* FixedWingLandingComplexItem::loiterAltitudeName = "LoiterAltit const char* FixedWingLandingComplexItem::loiterRadiusName = "LoiterRadius"; const char* FixedWingLandingComplexItem::landingAltitudeName = "LandingAltitude"; const char* FixedWingLandingComplexItem::glideSlopeName = "GlideSlope"; +const char* FixedWingLandingComplexItem::stopTakingPhotosName = "StopTakingPhotos"; +const char* FixedWingLandingComplexItem::stopTakingVideoName = "StopTakingVideo"; const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate"; const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius"; @@ -34,6 +36,8 @@ const char* FixedWingLandingComplexItem::_jsonLoiterClockwiseKey = "loi const char* FixedWingLandingComplexItem::_jsonLandingCoordinateKey = "landCoordinate"; const char* FixedWingLandingComplexItem::_jsonValueSetIsDistanceKey = "valueSetIsDistance"; const char* FixedWingLandingComplexItem::_jsonAltitudesAreRelativeKey = "altitudesAreRelative"; +const char* FixedWingLandingComplexItem::_jsonStopTakingPhotosKey = "stopTakingPhotos"; +const char* FixedWingLandingComplexItem::_jsonStopTakingVideoKey = "stopVideoPhotos"; // Usage deprecated const char* FixedWingLandingComplexItem::_jsonFallRateKey = "fallRate"; @@ -53,6 +57,8 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool , _landingHeadingFact (settingsGroup, _metaDataMap[landingHeadingName]) , _landingAltitudeFact (settingsGroup, _metaDataMap[landingAltitudeName]) , _glideSlopeFact (settingsGroup, _metaDataMap[glideSlopeName]) + , _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName]) + , _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName]) , _loiterClockwise (true) , _altitudesAreRelative (true) , _valueSetIsDistance (true) @@ -73,15 +79,21 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool connect(&_glideSlopeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged); + connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged); + connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged); + connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_landingDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_loiterRadiusFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); + connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); + connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::_setDirty); + connect(this, &FixedWingLandingComplexItem::valueSetIsDistanceChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged); connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged); @@ -89,8 +101,11 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool int FixedWingLandingComplexItem::lastSequenceNumber(void) const { - // land start, loiter, land - return _sequenceNumber + 2; + // Fixed items are: + // land start, loiter, land + // Optional items are: + // stop photos/video + return _sequenceNumber + 2 + (_stopTakingPhotosFact.rawValue().toBool() ? 2 : 0) + (_stopTakingVideoFact.rawValue().toBool() ? 1 : 0); } void FixedWingLandingComplexItem::setDirty(bool dirty) @@ -123,6 +138,8 @@ void FixedWingLandingComplexItem::save(QJsonArray& missionItems) saveObject[_jsonLandingCoordinateKey] = jsonCoordinate; saveObject[_jsonLoiterRadiusKey] = _loiterRadiusFact.rawValue().toDouble(); + saveObject[_jsonStopTakingPhotosKey] = _stopTakingPhotosFact.rawValue().toBool(); + saveObject[_jsonStopTakingVideoKey] = _stopTakingVideoFact.rawValue().toBool(); saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise; saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative; saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistance; @@ -149,6 +166,8 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq { _jsonLoiterRadiusKey, QJsonValue::Double, true }, { _jsonLoiterClockwiseKey, QJsonValue::Bool, true }, { _jsonLandingCoordinateKey, QJsonValue::Array, true }, + { _jsonStopTakingPhotosKey, QJsonValue::Bool, false }, + { _jsonStopTakingVideoKey, QJsonValue::Bool, false }, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; @@ -220,6 +239,17 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq _loiterRadiusFact.setRawValue(complexObject[_jsonLoiterRadiusKey].toDouble()); _loiterClockwise = complexObject[_jsonLoiterClockwiseKey].toBool(); + if (complexObject.contains(_jsonStopTakingPhotosKey)) { + _stopTakingPhotosFact.setRawValue(complexObject[_jsonStopTakingPhotosKey].toBool()); + } else { + _stopTakingPhotosFact.setRawValue(false); + } + if (complexObject.contains(_jsonStopTakingVideoKey)) { + _stopTakingVideoFact.setRawValue(complexObject[_jsonStopTakingVideoKey].toBool()); + } else { + _stopTakingVideoFact.setRawValue(false); + } + _calcGlideSlope(); _landingCoordSet = true; @@ -242,47 +272,75 @@ bool FixedWingLandingComplexItem::specifiesCoordinate(void) const return true; } -void FixedWingLandingComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +MissionItem* FixedWingLandingComplexItem::createDoLandStartItem(int seqNum, QObject* parent) { - int seqNum = _sequenceNumber; - - // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem - - MissionItem* item = new MissionItem(seqNum++, // sequence number - MAV_CMD_DO_LAND_START, // MAV_CMD - MAV_FRAME_MISSION, // MAV_FRAME - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7 - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + return new MissionItem(seqNum, // sequence number + MAV_CMD_DO_LAND_START, // MAV_CMD + MAV_FRAME_MISSION, // MAV_FRAME + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 1-7 + true, // autoContinue + false, // isCurrentItem + parent); +} - float loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0); - item = new MissionItem(seqNum++, +MissionItem* FixedWingLandingComplexItem::createLoiterToAltItem(int seqNum, bool altRel, double loiterRadius, double lat, double lon, double alt, QObject* parent) +{ + return new MissionItem(seqNum, MAV_CMD_NAV_LOITER_TO_ALT, - _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, 1.0, // Heading required = true loiterRadius, // Loiter radius 0.0, // param 3 - unused 1.0, // Exit crosstrack - tangent of loiter to land point - _loiterCoordinate.latitude(), - _loiterCoordinate.longitude(), - _loiterAltitudeFact.rawValue().toDouble(), + lat, lon, alt, true, // autoContinue false, // isCurrentItem - missionItemParent); - items.append(item); + parent); +} - item = new MissionItem(seqNum++, +MissionItem* FixedWingLandingComplexItem::createLandItem(int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent) +{ + return new MissionItem(seqNum, MAV_CMD_NAV_LAND, - _altitudesAreRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, - 0.0, 0.0, 0.0, 0.0, // param 1-4 - _landingCoordinate.latitude(), - _landingCoordinate.longitude(), - _landingAltitudeFact.rawValue().toDouble(), + altRel ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0.0, 0.0, 0.0, 0.0, + lat, lon, alt, true, // autoContinue false, // isCurrentItem - missionItemParent); + parent); + +} + +void FixedWingLandingComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + int seqNum = _sequenceNumber; + + // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem + + MissionItem* item = createDoLandStartItem(seqNum++, missionItemParent); + items.append(item); + + + if (_stopTakingPhotosFact.rawValue().toBool()) { + CameraSection::appendStopTakingPhotos(items, seqNum, missionItemParent); + } + + if (_stopTakingVideoFact.rawValue().toBool()) { + CameraSection::appendStopTakingVideo(items, seqNum, missionItemParent); + } + + double loiterRadius = _loiterRadiusFact.rawValue().toDouble() * (_loiterClockwise ? 1.0 : -1.0); + item = createLoiterToAltItem(seqNum++, + _altitudesAreRelative, + loiterRadius, + _loiterCoordinate.latitude(), _loiterCoordinate.longitude(), _loiterAltitudeFact.rawValue().toDouble(), + missionItemParent); + items.append(item); + + item = createLandItem(seqNum++, + _altitudesAreRelative, + _landingCoordinate.latitude(), _landingCoordinate.longitude(), _landingAltitudeFact.rawValue().toDouble(), + missionItemParent); items.append(item); } @@ -290,13 +348,25 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, b { qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count(); - if (visualItems->count() < 4) { + if (visualItems->count() < 3) { return false; } - int lastItem = visualItems->count() - 1; + // A valid fixed wing landing pattern is comprised of the follow commands in this order at the end of the item list: + // MAV_CMD_DO_LAND_START - required + // Stop taking photos sequence - optional + // Stop taking video sequence - optional + // MAV_CMD_NAV_LOITER_TO_ALT - required + // MAV_CMD_NAV_LAND - required - SimpleMissionItem* item = visualItems->value(lastItem--); + // Start looking for the commands in reverse order from the end of the list + + int scanIndex = visualItems->count() - 1; + + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + SimpleMissionItem* item = visualItems->value(scanIndex--); if (!item) { return false; } @@ -308,7 +378,10 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, b } MAV_FRAME landPointFrame = missionItemLand.frame(); - item = visualItems->value(lastItem--); + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + item = visualItems->value(scanIndex); if (!item) { return false; } @@ -319,7 +392,23 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, b return false; } - item = visualItems->value(lastItem--); + scanIndex -= CameraSection::stopTakingVideoCommandCount(); + bool stopTakingVideo = CameraSection::scanStopTakingVideo(visualItems, scanIndex, false /* removeScannedItems */); + if (!stopTakingVideo) { + scanIndex += CameraSection::stopTakingVideoCommandCount(); + } + + scanIndex -= CameraSection::stopTakingPhotosCommandCount(); + bool stopTakingPhotos = CameraSection::scanStopTakingPhotos(visualItems, scanIndex, false /* removeScannedItems */); + if (!stopTakingPhotos) { + scanIndex += CameraSection::stopTakingPhotosCommandCount(); + } + + scanIndex--; + if (scanIndex < 0 || scanIndex > visualItems->count() - 1) { + return false; + } + item = visualItems->value(scanIndex); if (!item) { return false; } @@ -329,7 +418,21 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, b return false; } - // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission + // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission. + // Since we have scanned it we need to remove the items for it fromt the list + int deleteCount = 3; + if (stopTakingPhotos) { + deleteCount += CameraSection::stopTakingPhotosCommandCount(); + } + if (stopTakingVideo) { + deleteCount += CameraSection::stopTakingVideoCommandCount(); + } + int firstItem = visualItems->count() - deleteCount; + while (deleteCount--) { + visualItems->removeAt(firstItem)->deleteLater(); + } + + // Now stuff all the scanned information into the item FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(vehicle, flyView, visualItems); @@ -345,6 +448,9 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, b complexItem->_landingCoordinate.setLongitude(missionItemLand.param6()); complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7()); + complexItem->_stopTakingPhotosFact.setRawValue(stopTakingPhotos); + complexItem->_stopTakingVideoFact.setRawValue(stopTakingVideo); + complexItem->_calcGlideSlope(); complexItem->_landingCoordSet = true; @@ -353,11 +459,6 @@ bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, b complexItem->_recalcFromCoordinateChange(); complexItem->setDirty(false); - lastItem = visualItems->count() - 1; - visualItems->removeAt(lastItem--)->deleteLater(); - visualItems->removeAt(lastItem--)->deleteLater(); - visualItems->removeAt(lastItem--)->deleteLater(); - visualItems->append(complexItem); return true; @@ -586,3 +687,8 @@ void FixedWingLandingComplexItem::_calcGlideSlope(void) _glideSlopeFact.setRawValue(qRadiansToDegrees(qAtan(landingAltDifference / landingDistance))); } + +void FixedWingLandingComplexItem::_signalLastSequenceNumberChanged(void) +{ + emit lastSequenceNumberChanged(lastSequenceNumber()); +} diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index c456c15bc..0b0af9d80 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -17,6 +17,8 @@ Q_DECLARE_LOGGING_CATEGORY(FixedWingLandingComplexItemLog) +class FWLandingPatternTest; + class FixedWingLandingComplexItem : public ComplexMissionItem { Q_OBJECT @@ -33,6 +35,8 @@ public: Q_PROPERTY(Fact* glideSlope READ glideSlope CONSTANT) Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) Q_PROPERTY(bool altitudesAreRelative MEMBER _altitudesAreRelative NOTIFY altitudesAreRelativeChanged) + Q_PROPERTY(Fact* stopTakingPhotos READ stopTakingPhotos CONSTANT) + Q_PROPERTY(Fact* stopTakingVideo READ stopTakingVideo CONSTANT) Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged) Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged) Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged) @@ -44,6 +48,8 @@ public: Fact* landingDistance (void) { return &_landingDistanceFact; } Fact* landingHeading (void) { return &_landingHeadingFact; } Fact* glideSlope (void) { return &_glideSlopeFact; } + Fact* stopTakingPhotos (void) { return &_stopTakingPhotosFact; } + Fact* stopTakingVideo (void) { return &_stopTakingVideoFact; } QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; } QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; } QGeoCoordinate loiterTangentCoordinate (void) const { return _loiterTangentCoordinate; } @@ -54,6 +60,10 @@ public: /// Scans the loaded items for a landing pattern complex item static bool scanForItem(QmlObjectListModel* visualItems, bool flyView, Vehicle* vehicle); + static MissionItem* createDoLandStartItem (int seqNum, QObject* parent); + static MissionItem* createLoiterToAltItem (int seqNum, bool altRel, double loiterRaidus, double lat, double lon, double alt, QObject* parent); + static MissionItem* createLandItem (int seqNum, bool altRel, double lat, double lon, double alt, QObject* parent); + // Overrides from ComplexMissionItem double complexDistance (void) const final; @@ -98,6 +108,8 @@ public: static const char* landingHeadingName; static const char* landingAltitudeName; static const char* glideSlopeName; + static const char* stopTakingPhotosName; + static const char* stopTakingVideoName; signals: void loiterCoordinateChanged (QGeoCoordinate coordinate); @@ -118,6 +130,7 @@ private slots: double _headingToMathematicAngle (double heading); void _setDirty (void); void _glideSlopeChanged (void); + void _signalLastSequenceNumberChanged (void); private: QPointF _rotatePoint (const QPointF& point, const QPointF& origin, double angle); @@ -139,6 +152,8 @@ private: Fact _landingHeadingFact; Fact _landingAltitudeFact; Fact _glideSlopeFact; + Fact _stopTakingPhotosFact; + Fact _stopTakingVideoFact; bool _loiterClockwise; bool _altitudesAreRelative; @@ -151,11 +166,15 @@ private: static const char* _jsonLandingCoordinateKey; static const char* _jsonValueSetIsDistanceKey; static const char* _jsonAltitudesAreRelativeKey; + static const char* _jsonStopTakingPhotosKey; + static const char* _jsonStopTakingVideoKey; // Only in older file formats static const char* _jsonLandingAltitudeRelativeKey; static const char* _jsonLoiterAltitudeRelativeKey; static const char* _jsonFallRateKey; + + friend FWLandingPatternTest; }; #endif diff --git a/src/MissionManager/SectionTest.cc b/src/MissionManager/SectionTest.cc index 4d8ffc3fb..a959712a2 100644 --- a/src/MissionManager/SectionTest.cc +++ b/src/MissionManager/SectionTest.cc @@ -54,21 +54,6 @@ void SectionTest::_createSpy(Section* section, MultiSignalSpy** sectionSpy) *sectionSpy = spy; } -void SectionTest::_missionItemsEqual(MissionItem& actual, MissionItem& expected) -{ - QCOMPARE(actual.command(), expected.command()); - QCOMPARE(actual.frame(), expected.frame()); - QCOMPARE(actual.autoContinue(), expected.autoContinue()); - - QVERIFY(UnitTest::doubleNaNCompare(actual.param1(), expected.param1())); - QVERIFY(UnitTest::doubleNaNCompare(actual.param2(), expected.param2())); - QVERIFY(UnitTest::doubleNaNCompare(actual.param3(), expected.param3())); - QVERIFY(UnitTest::doubleNaNCompare(actual.param4(), expected.param4())); - QVERIFY(UnitTest::doubleNaNCompare(actual.param5(), expected.param5())); - QVERIFY(UnitTest::doubleNaNCompare(actual.param6(), expected.param6())); - QVERIFY(UnitTest::doubleNaNCompare(actual.param7(), expected.param7())); -} - void SectionTest::_commonScanTest(Section* section) { QCOMPARE(section->available(), true); diff --git a/src/MissionManager/SectionTest.h b/src/MissionManager/SectionTest.h index 9cb5c30c8..78122b2d7 100644 --- a/src/MissionManager/SectionTest.h +++ b/src/MissionManager/SectionTest.h @@ -25,7 +25,6 @@ public: protected: void _createSpy(Section* section, MultiSignalSpy** sectionSpy); - void _missionItemsEqual(MissionItem& actual, MissionItem& expected); void _commonScanTest(Section* section); enum { diff --git a/src/PlanView/FWLandingPatternEditor.qml b/src/PlanView/FWLandingPatternEditor.qml index 7632fd959..fcd56385d 100644 --- a/src/PlanView/FWLandingPatternEditor.qml +++ b/src/PlanView/FWLandingPatternEditor.qml @@ -16,6 +16,7 @@ import QGroundControl 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Vehicle 1.0 import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 import QGroundControl.FactControls 1.0 import QGroundControl.Palette 1.0 @@ -163,6 +164,34 @@ Rectangle { visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || !missionItem.altitudesAreRelative onClicked: missionItem.altitudesAreRelative = checked } + + SectionHeader { + id: cameraSection + text: qsTr("Camera") + } + + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: cameraSection.checked + + Item { width: 1; height: _spacer } + + FactCheckBox { + text: _stopTakingPhotos.shortDescription + fact: _stopTakingPhotos + + property Fact _stopTakingPhotos: missionItem.stopTakingPhotos + } + + FactCheckBox { + text: _stopTakingVideo.shortDescription + fact: _stopTakingVideo + + property Fact _stopTakingVideo: missionItem.stopTakingVideo + } + } } Column { diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc index 545d26536..fbcb52598 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/src/qgcunittest/UnitTest.cc @@ -535,3 +535,23 @@ void UnitTest::changeFactValue(Fact* fact,double increment) fact->setRawValue(fact->rawValue().toDouble() + increment); } } + +void UnitTest::_missionItemsEqual(MissionItem& actual, MissionItem& expected) +{ + QCOMPARE(static_cast(actual.command()), static_cast(expected.command())); + QCOMPARE(static_cast(actual.frame()), static_cast(expected.frame())); + QCOMPARE(actual.autoContinue(), expected.autoContinue()); + + QVERIFY(UnitTest::doubleNaNCompare(actual.param1(), expected.param1())); + QVERIFY(UnitTest::doubleNaNCompare(actual.param2(), expected.param2())); + QVERIFY(UnitTest::doubleNaNCompare(actual.param3(), expected.param3())); + QVERIFY(UnitTest::doubleNaNCompare(actual.param4(), expected.param4())); + QVERIFY(UnitTest::doubleNaNCompare(actual.param5(), expected.param5())); + QVERIFY(UnitTest::doubleNaNCompare(actual.param6(), expected.param6())); + QVERIFY(UnitTest::doubleNaNCompare(actual.param7(), expected.param7())); +} + +bool UnitTest::fuzzyCompareLatLon(const QGeoCoordinate& coord1, const QGeoCoordinate& coord2) +{ + return coord1.distanceTo(coord2) < 1.0; +} diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index 34b18484e..79d30cd2c 100644 --- a/src/qgcunittest/UnitTest.h +++ b/src/qgcunittest/UnitTest.h @@ -25,6 +25,7 @@ #include "QGCMAVLink.h" #include "LinkInterface.h" #include "Fact.h" +#include "MissionItem.h" #define UT_REGISTER_TEST(className) static UnitTestWrapper className(#className); @@ -101,6 +102,10 @@ public: /// @param increment 0 use standard increment, other increment by specified amount if double value void changeFactValue(Fact* fact, double increment = 0); + /// Returns true is the position of the two coordinates is less then a meter from each other. + /// Does not check altitude. + static bool fuzzyCompareLatLon(const QGeoCoordinate& coord1, const QGeoCoordinate& coord2); + protected slots: // These are all pure virtuals to force the derived class to implement each one and in turn @@ -119,6 +124,7 @@ protected: void _disconnectMockLink(void); void _createMainWindow(void); void _closeMainWindow(bool cancelExpected = false); + void _missionItemsEqual(MissionItem& actual, MissionItem& expected); LinkManager* _linkManager; MockLink* _mockLink; diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index 834e7b1c5..e27ed409e 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -44,6 +44,7 @@ #include "CorridorScanComplexItemTest.h" #include "TransectStyleComplexItemTest.h" #include "CameraCalcTest.h" +#include "FWLandingPatternTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -75,6 +76,7 @@ UT_REGISTER_TEST(CorridorScanComplexItemTest) UT_REGISTER_TEST(TransectStyleComplexItemTest) UT_REGISTER_TEST(QGCMapPolylineTest) UT_REGISTER_TEST(CameraCalcTest) +UT_REGISTER_TEST(FWLandingPatternTest) // List of unit test which are currently disabled. // If disabling a new test, include reason in comment. -- 2.22.0