From 842ba9ebfb115569a0279e3efa12995efa2949ec Mon Sep 17 00:00:00 2001 From: DoinLakeFlyer Date: Sun, 15 Mar 2020 11:02:22 -0700 Subject: [PATCH] Survey And Corridor now share same transect generation code. Better unit tests for both with more coverage. --- qgroundcontrol.pro | 2 + src/MissionManager/CorridorScanComplexItem.cc | 142 +----------- src/MissionManager/CorridorScanComplexItem.h | 7 +- .../CorridorScanComplexItemTest.cc | 175 +++++++++------ .../CorridorScanComplexItemTest.h | 40 ++-- src/MissionManager/SurveyComplexItem.cc | 176 +-------------- src/MissionManager/SurveyComplexItem.h | 11 - src/MissionManager/SurveyComplexItemTest.cc | 116 +++++----- src/MissionManager/SurveyComplexItemTest.h | 31 ++- .../TransectStyleComplexItem.cc | 205 +++++++++++++++++- src/MissionManager/TransectStyleComplexItem.h | 16 +- .../TransectStyleComplexItemTest.cc | 5 +- .../TransectStyleComplexItemTest.h | 9 +- .../TransectStyleComplexItemTestBase.cc | 42 ++++ .../TransectStyleComplexItemTestBase.h | 34 +++ 15 files changed, 516 insertions(+), 495 deletions(-) create mode 100644 src/MissionManager/TransectStyleComplexItemTestBase.cc create mode 100644 src/MissionManager/TransectStyleComplexItemTestBase.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 8bbccce11..c6c424382 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -491,6 +491,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/StructureScanComplexItemTest.h \ src/MissionManager/SurveyComplexItemTest.h \ src/MissionManager/TransectStyleComplexItemTest.h \ + src/MissionManager/TransectStyleComplexItemTestBase.h \ src/MissionManager/VisualMissionItemTest.h \ src/qgcunittest/GeoTest.h \ src/qgcunittest/LinkManagerTest.h \ @@ -533,6 +534,7 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { src/MissionManager/StructureScanComplexItemTest.cc \ src/MissionManager/SurveyComplexItemTest.cc \ src/MissionManager/TransectStyleComplexItemTest.cc \ + src/MissionManager/TransectStyleComplexItemTestBase.cc \ src/MissionManager/VisualMissionItemTest.cc \ src/qgcunittest/GeoTest.cc \ src/qgcunittest/LinkManagerTest.cc \ diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index 091e2b2fa..aa003ebc6 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -143,140 +143,10 @@ bool CorridorScanComplexItem::specifiesCoordinate(void) const return _corridorPolyline.count() > 1; } -int CorridorScanComplexItem::_transectCount(void) const +int CorridorScanComplexItem::_calcTransectCount(void) const { double fullWidth = _corridorWidthFact.rawValue().toDouble(); - return fullWidth > 0.0 ? qCeil(fullWidth / _transectSpacing()) : 1; -} - -void CorridorScanComplexItem::_appendLoadedMissionItems(QList& items, QObject* missionItemParent) -{ - qCDebug(CorridorScanComplexItemLog) << "_appendLoadedMissionItems"; - - int seqNum = _sequenceNumber; - - for (const MissionItem* loadedMissionItem: _loadedMissionItems) { - MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent); - item->setSequenceNumber(seqNum++); - items.append(item); - } -} - -void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) -{ - qCDebug(CorridorScanComplexItemLog) << "_buildAndAppendMissionItems"; - - // Now build the mission items from the transect points - - MissionItem* item; - int seqNum = _sequenceNumber; - bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - bool addTriggerAtBeginning = imagesEverywhere; - bool firstOverallPoint = true; - - MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; - - //qDebug() << "_buildAndAppendMissionItems"; - for (const QList& transect: _transects) { - bool transectEntry = true; - - //qDebug() << "start transect"; - for (const CoordInfo_t& transectCoordInfo: transect) { - //qDebug() << transectCoordInfo.coordType; - - item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - mavFrame, - 0, // No hold time - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - transectCoordInfo.coord.latitude(), - transectCoordInfo.coord.longitude(), - transectCoordInfo.coord.altitude(), - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - - if (triggerCamera() && firstOverallPoint && addTriggerAtBeginning) { - // Start triggering - addTriggerAtBeginning = false; - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance - 0, // shutter integration (ignore) - 1, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - firstOverallPoint = false; - - // Possibly add trigger start/stop to survey area entrance/exit - if (triggerCamera() && (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEntry || transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyExit)) { - if (transectEntry) { - // Start of transect, always start triggering. We do this even if we are taking images everywhere. - // This allows a restart of the mission in mid-air without losing images from the entire mission. - // At most you may lose part of a transect. - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(), // trigger distance - 0, // shutter integration (ignore) - 1, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - transectEntry = false; - } else if (!imagesEverywhere && !transectEntry){ - // End of transect, stop triggering - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0, // stop triggering - 0, // shutter integration (ignore) - 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - } - } - } - - if (imagesEverywhere) { - // Stop triggering - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - 0, // stop triggering - 0, // shutter integration (ignore) - 0, // trigger immediately when starting - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } -} - -void CorridorScanComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) -{ - if (_loadedMissionItems.count()) { - // We have mission items from the loaded plan, use those - _appendLoadedMissionItems(items, missionItemParent); - } else { - // Build the mission items on the fly - _buildAndAppendMissionItems(items, missionItemParent); - } + return fullWidth > 0.0 ? qCeil(fullWidth / _calcTransectSpacing()) : 1; } void CorridorScanComplexItem::applyNewAltitude(double newAltitude) @@ -343,10 +213,10 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) _transects.clear(); _transectsPathHeightInfo.clear(); - double transectSpacing = _transectSpacing(); + double transectSpacing = _calcTransectSpacing(); double fullWidth = _corridorWidthFact.rawValue().toDouble(); double halfWidth = fullWidth / 2.0; - int transectCount = _transectCount(); + int transectCount = _calcTransectCount(); double normalizedTransectPosition = transectSpacing / 2.0; if (_corridorPolyline.count() >= 2) { @@ -486,7 +356,7 @@ void CorridorScanComplexItem::_recalcCameraShots(void) _cameraShots = qCeil(_complexDistance / triggerDistance); } else { int singleTransectImageCount = qCeil(_corridorPolyline.length() / triggerDistance); - _cameraShots = singleTransectImageCount * _transectCount(); + _cameraShots = singleTransectImageCount * _calcTransectCount(); } } emit cameraShotsChanged(); @@ -502,7 +372,7 @@ double CorridorScanComplexItem::timeBetweenShots(void) return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / _cruiseSpeed; } -double CorridorScanComplexItem::_transectSpacing(void) const +double CorridorScanComplexItem::_calcTransectSpacing(void) const { double transectSpacing = _cameraCalc.adjustedFootprintSide()->rawValue().toDouble(); if (transectSpacing < 0.5) { diff --git a/src/MissionManager/CorridorScanComplexItem.h b/src/MissionManager/CorridorScanComplexItem.h index ab3704cd2..00080ac5b 100644 --- a/src/MissionManager/CorridorScanComplexItem.h +++ b/src/MissionManager/CorridorScanComplexItem.h @@ -38,7 +38,6 @@ public: // Overrides from TransectStyleComplexItem void save (QJsonArray& planItems) final; bool specifiesCoordinate (void) const final; - void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final; double timeBetweenShots (void) final; @@ -68,10 +67,8 @@ private slots: void _recalcCameraShots (void) final; private: - double _transectSpacing (void) const; - int _transectCount (void) const; - void _buildAndAppendMissionItems (QList& items, QObject* missionItemParent); - void _appendLoadedMissionItems (QList& items, QObject* missionItemParent); + double _calcTransectSpacing (void) const; + int _calcTransectCount (void) const; QGCMapPolyline _corridorPolyline; QList> _transectSegments; ///< Internal transect segments including grid exit, turnaround and internal camera points diff --git a/src/MissionManager/CorridorScanComplexItemTest.cc b/src/MissionManager/CorridorScanComplexItemTest.cc index 691bf5c7b..de1c7d189 100644 --- a/src/MissionManager/CorridorScanComplexItemTest.cc +++ b/src/MissionManager/CorridorScanComplexItemTest.cc @@ -12,25 +12,31 @@ CorridorScanComplexItemTest::CorridorScanComplexItemTest(void) { - _linePoints << QGeoCoordinate(47.633550640000003, -122.08982199) - << QGeoCoordinate(47.634129020000003, -122.08887249) - << QGeoCoordinate(47.633619320000001, -122.08811074); + _polyLineVertices.append(QGeoCoordinate(47.633550640000003, -122.08982199)); + _polyLineVertices.append(_polyLineVertices[0].atDistanceAndAzimuth(_corridorLineSegmentDistance, 0)); + _polyLineVertices.append(_polyLineVertices[1].atDistanceAndAzimuth(_corridorLineSegmentDistance, 20)); } void CorridorScanComplexItemTest::init(void) { - UnitTest::init(); + TransectStyleComplexItemTestBase::init(); - _masterController = new PlanMasterController(this); - _controllerVehicle = _masterController->controllerVehicle(); _corridorItem = new CorridorScanComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */); + _corridorItem->corridorPolyline()->appendVertices(_polyLineVertices); + + // Setup for expected transect count + _corridorItem->corridorWidth()->setRawValue(_corridorWidth); + _corridorItem->cameraCalc()->adjustedFootprintSide()->setRawValue((_corridorWidth * 0.5) + 1.0); + _corridorItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(_corridorLineSegmentDistance * 0.25); + + int expectedTransectCount = _expectedTransectCount; + QCOMPARE(_corridorItem->_transectCount(), expectedTransectCount); // vehicleSpeed need for terrain calcs MissionController::MissionFlightStatus_t missionFlightStatus; missionFlightStatus.vehicleSpeed = 5; _corridorItem->setMissionFlightStatus(missionFlightStatus); - _setPolyline(); _corridorItem->setDirty(false); _rgCorridorPolygonSignals[corridorPolygonPathChangedIndex] = SIGNAL(pathChanged()); @@ -42,6 +48,7 @@ void CorridorScanComplexItemTest::init(void) void CorridorScanComplexItemTest::cleanup(void) { delete _corridorItem; + TransectStyleComplexItemTestBase::cleanup(); } void CorridorScanComplexItemTest::_testDirty(void) @@ -98,14 +105,6 @@ void CorridorScanComplexItemTest::_testCameraTrigger(void) #endif } -void CorridorScanComplexItemTest::_setPolyline(void) -{ - for (int i=0; i<_linePoints.count(); i++) { - QGeoCoordinate& vertex = _linePoints[i]; - _corridorItem->corridorPolyline()->appendVertex(vertex); - } -} - #if 0 void CorridorScanComplexItemTest::_testEntryLocation(void) { @@ -148,71 +147,111 @@ void CorridorScanComplexItemTest::_waitForReadyForSave(void) void CorridorScanComplexItemTest::_testItemCount(void) { + typedef struct { + bool triggerInTurnAround; + bool hasTurnaround; + } TestCase_t; + + static const TestCase_t rgTestCases[] = { + { false, false }, + { false, false }, + { false, true }, + { false, true }, + }; + QList items; + for (const TestCase_t& testCase : rgTestCases) { + qDebug() << "triggerInTurnAround:hasTurnaround" << testCase.triggerInTurnAround << testCase.hasTurnaround; + _corridorItem->cameraTriggerInTurnAround()->setRawValue(testCase.triggerInTurnAround); + _corridorItem->turnAroundDistance()->setRawValue(testCase.hasTurnaround ? 50 : 0); + _corridorItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); + items.clear(); + } +} - _corridorItem->turnAroundDistance()->setRawValue(0); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); - _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); +void CorridorScanComplexItemTest::_testPathChanges(void) +{ + QGeoCoordinate vertex = _corridorItem->corridorPolyline()->vertexCoordinate(1); + vertex.setLatitude(vertex.latitude() + 0.01); + _corridorItem->corridorPolyline()->adjustVertex(1, vertex); - _corridorItem->turnAroundDistance()->setRawValue(0); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); - _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); + QVERIFY(_multiSpyCorridorPolygon->checkSignalsByMask(corridorPolygonPathChangedMask)); +} - _corridorItem->turnAroundDistance()->setRawValue(20); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); - _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); +QList CorridorScanComplexItemTest::_createExpectedCommands(bool hasTurnaround, bool useConditionGate) +{ + static const QList singleFullTransect = { + MAV_CMD_NAV_WAYPOINT, // Turnaround + MAV_CMD_CONDITION_GATE, // Survey area entry edge + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_CMD_NAV_WAYPOINT, // Polyline turn + MAV_CMD_CONDITION_GATE, // Survey area exit edge + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_CMD_NAV_WAYPOINT, // Turnaround + }; + + QList singleTransect = singleFullTransect; + QList expectedCommands; + + if (!useConditionGate) { + for (MAV_CMD& cmd : singleTransect) { + cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd; + } + } - _corridorItem->turnAroundDistance()->setRawValue(20); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); - _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); + if (!hasTurnaround) { + singleTransect.takeFirst(); + singleTransect.takeLast(); + } -#if 0 - // Terrain queries seem to take random amount of time so these don't work 100% - _corridorItem->setFollowTerrain(true); + for (int i=0; i<_expectedTransectCount; i++) { + expectedCommands.append(singleTransect); + } - _corridorItem->turnAroundDistance()->setRawValue(0); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); - _waitForReadyForSave(); - _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); + return expectedCommands; +} - _corridorItem->turnAroundDistance()->setRawValue(0); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); - _waitForReadyForSave(); - _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); - _corridorItem->turnAroundDistance()->setRawValue(20); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(true); - _waitForReadyForSave(); - _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); - _corridorItem->turnAroundDistance()->setRawValue(20); - _corridorItem->cameraTriggerInTurnAround()->setRawValue(false); - _waitForReadyForSave(); +void CorridorScanComplexItemTest::_testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList& expectedCommands) +{ + qDebug() << QStringLiteral("_testItemGenerationWorker imagesInTuraround:%1 turnaround:%2 gate:%3").arg(imagesInTurnaround).arg(hasTurnaround).arg(useConditionGate); + + _corridorItem->turnAroundDistance()->setRawValue(hasTurnaround ? 50 : 0); + _corridorItem->cameraTriggerInTurnAround()->setRawValue(imagesInTurnaround); + _planViewSettings->useConditionGate()->setRawValue(useConditionGate); + + QList items; _corridorItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _corridorItem->lastSequenceNumber()); - items.clear(); -#endif + //_printItemCommands(items); + QCOMPARE(items.count(), expectedCommands.count()); + for (int i=0; icommand(); + int expectedCommand = expectedCommands[i]; + //qDebug() << "Index" << i; + QCOMPARE(actualCommand, expectedCommand); + } } -void CorridorScanComplexItemTest::_testPathChanges(void) +void CorridorScanComplexItemTest::_testItemGeneration(void) { - QGeoCoordinate vertex = _corridorItem->corridorPolyline()->vertexCoordinate(1); - vertex.setLatitude(vertex.latitude() + 0.01); - _corridorItem->corridorPolyline()->adjustVertex(1, vertex); - - QVERIFY(_multiSpyCorridorPolygon->checkSignalsByMask(corridorPolygonPathChangedMask)); + // Test all the combinations of: cameraTriggerInTurnAround: false, hasTurnAround: *, useConditionGate: * + + typedef struct { + bool hasTurnaround; + bool useConditionGate; + } TestCase_t; + + static const TestCase_t rgTestCases[] = { + { false, false }, + { false, true }, + { true, false }, + { true, true }, + }; + + for (const TestCase_t& testCase : rgTestCases) { + _testItemGenerationWorker(false /* imagesInTurnaround */, testCase.hasTurnaround, testCase.useConditionGate, _createExpectedCommands(testCase.hasTurnaround, testCase.useConditionGate)); + } } + diff --git a/src/MissionManager/CorridorScanComplexItemTest.h b/src/MissionManager/CorridorScanComplexItemTest.h index 4e1faac33..d3194d535 100644 --- a/src/MissionManager/CorridorScanComplexItemTest.h +++ b/src/MissionManager/CorridorScanComplexItemTest.h @@ -9,7 +9,7 @@ #pragma once -#include "UnitTest.h" +#include "TransectStyleComplexItemTestBase.h" #include "TCPLink.h" #include "MultiSignalSpy.h" #include "CorridorScanComplexItem.h" @@ -17,7 +17,7 @@ #include -class CorridorScanComplexItemTest : public UnitTest +class CorridorScanComplexItemTest : public TransectStyleComplexItemTestBase { Q_OBJECT @@ -25,19 +25,31 @@ public: CorridorScanComplexItemTest(void); protected: - void init(void) final; + void init (void) final; void cleanup(void) final; +#if 1 private slots: - void _testDirty(void); - void _testCameraTrigger(void); -// void _testEntryLocation(void); - void _testItemCount(void); - void _testPathChanges(void); + void _testDirty (void); + void _testCameraTrigger (void); + void _testPathChanges (void); + void _testItemGeneration(void); + void _testItemCount (void); +#else + // Used to debug a single test +private slots: + void _testItemGeneration(void); +private: + void _testDirty (void); + void _testCameraTrigger (void); + void _testPathChanges (void); + void _testItemCount (void); +#endif private: - void _setPolyline(void); - void _waitForReadyForSave(void); + void _waitForReadyForSave(void); + QList _createExpectedCommands(bool hasTurnaround, bool useConditionGate); + void _testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList& expectedCommands); enum { corridorPolygonPathChangedIndex = 0, @@ -51,9 +63,11 @@ private: static const size_t _cCorridorPolygonSignals = maxCorridorPolygonSignalIndex; const char* _rgCorridorPolygonSignals[_cCorridorPolygonSignals]; - PlanMasterController* _masterController = nullptr; - Vehicle* _controllerVehicle = nullptr; MultiSignalSpy* _multiSpyCorridorPolygon = nullptr; CorridorScanComplexItem* _corridorItem = nullptr; - QList _linePoints; + QList _polyLineVertices; + + static constexpr int _expectedTransectCount = 2; + static constexpr double _corridorLineSegmentDistance = 200.0; + static constexpr double _corridorWidth = 50.0; }; diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index 3ee340f13..60a2dd0cf 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -653,159 +653,9 @@ bool SurveyComplexItem::_nextTransectCoord(const QList& transect return true; } -void SurveyComplexItem::_appendWaypoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate) -{ - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - mavFrame, - holdTime, - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - coordinate.latitude(), - coordinate.longitude(), - coordinate.altitude(), - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -} - -void SurveyComplexItem::_appendSinglePhotoCapture(QList& items, QObject* missionItemParent, int& seqNum) -{ - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_IMAGE_START_CAPTURE, - MAV_FRAME_MISSION, - 0, // Reserved (Set to 0) - 0, // Interval (none) - 1, // Take 1 photo - qQNaN(), qQNaN(), qQNaN(), qQNaN(), // param 4-7 reserved - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -} - -void SurveyComplexItem::_appendConditionGate(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate) -{ - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_CONDITION_GATE, - mavFrame, - 0, // Gate is orthogonal to path - 0, // Ignore altitude - 0, 0, // Param 3-4 ignored - coordinate.latitude(), - coordinate.longitude(), - 0, // No altitude - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -} - -void SurveyComplexItem::_appendCameraTriggerDistance(QList& items, QObject* missionItemParent, int& seqNum, float triggerDistance) -{ - MissionItem* item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - triggerDistance, - 0, // shutter integration (ignore) - 1, // 1 - trigger one image immediately, both and entry and exit to get full coverage - 0, 0, 0, 0, // param 4-7 unused - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); -} - -void SurveyComplexItem::_appendCameraTriggerDistanceUpdatePoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance) -{ - if (useConditionGate) { - _appendConditionGate(items, missionItemParent, seqNum, mavFrame, coordinate); - } else { - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, coordinate); - } - _appendCameraTriggerDistance(items, missionItemParent, seqNum, triggerDistance); -} - -void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) -{ - qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems"; - - // Now build the mission items from the transect points - - int seqNum = _sequenceNumber; - bool imagesInTurnaround = _cameraTriggerInTurnAroundFact.rawValue().toBool() && turnAroundDistance()->rawValue().toDouble() != 0; - bool addTriggerAtBeginningEnd = !hoverAndCaptureEnabled() && imagesInTurnaround && triggerCamera(); - bool useConditionGate = _controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_CONDITION_GATE) && - triggerCamera() && - !hoverAndCaptureEnabled(); - - MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; - - // Note: The code below is written to be understable as oppose to being compact and/or remove duplicate code - int transectIndex = 0; - for (const QList& transect: _transects) { - bool entryTurnaround = true; - for (const CoordInfo_t& transectCoordInfo: transect) { - switch (transectCoordInfo.coordType) { - case CoordTypeTurnaround: - { - bool firstEntryTurnaround = transectIndex == 0 && entryTurnaround; - bool lastExitTurnaround = transectIndex == _transects.count() - 1 && !entryTurnaround; - if (addTriggerAtBeginningEnd && (firstEntryTurnaround || lastExitTurnaround)) { - _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, firstEntryTurnaround ? triggerDistance() : 0); - } else { - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); - } - entryTurnaround = false; - } - break; - case CoordTypeInterior: - case CoordTypeInteriorTerrainAdded: - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); - break; - case CoordTypeInteriorHoverTrigger: - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); - _appendSinglePhotoCapture(items, missionItemParent, seqNum); - break; - case CoordTypeSurveyEntry: - if (triggerCamera()) { - if (hoverAndCaptureEnabled()) { - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); - _appendSinglePhotoCapture(items, missionItemParent, seqNum); - } else { - // We always add a trigger start to survey entry. Even for imagesInTurnaround = true. This allows you to resume a mission and refly a transect - _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, triggerDistance()); - } - } else { - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); - } - break; - case CoordTypeSurveyExit: - if (triggerCamera()) { - if (hoverAndCaptureEnabled()) { - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); - _appendSinglePhotoCapture(items, missionItemParent, seqNum); - } else if (imagesInTurnaround) { - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); - } else { - _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, 0 /* triggerDistance */); - } - } else { - _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); - } - break; - } - } - transectIndex++; - } -} - - bool SurveyComplexItem::_hasTurnaround(void) const { - return _turnaroundDistance() > 0; + return _turnAroundDistance() > 0; } double SurveyComplexItem::_turnaroundDistance(void) const @@ -1548,30 +1398,6 @@ SurveyComplexItem::ReadyForSaveState SurveyComplexItem::readyForSaveState(void) return TransectStyleComplexItem::readyForSaveState(); } -void SurveyComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) -{ - if (_loadedMissionItems.count()) { - // We have mission items from the loaded plan, use those - _appendLoadedMissionItems(items, missionItemParent); - } else { - // Build the mission items on the fly - _buildAndAppendMissionItems(items, missionItemParent); - } -} - -void SurveyComplexItem::_appendLoadedMissionItems(QList& items, QObject* missionItemParent) -{ - qCDebug(SurveyComplexItemLog) << "_appendLoadedMissionItems"; - - int seqNum = _sequenceNumber; - - for (const MissionItem* loadedMissionItem: _loadedMissionItems) { - MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent); - item->setSequenceNumber(seqNum++); - items.append(item); - } -} - void SurveyComplexItem::rotateEntryPoint(void) { if (_entryPoint == EntryLocationLast) { diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index 7b152fdfd..916ff600d 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/src/MissionManager/SurveyComplexItem.h @@ -47,7 +47,6 @@ public: // Overrides from TransectStyleComplexItem void save (QJsonArray& planItems) final; bool specifiesCoordinate (void) const final { return true; } - void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final; double timeBetweenShots (void) final; @@ -109,8 +108,6 @@ private: void _adjustTransectsToEntryPointLocation(QList>& transects); bool _gridAngleIsNorthSouthTransects(); double _clampGridAngle90(double gridAngle); - void _buildAndAppendMissionItems(QList& items, QObject* missionItemParent); - void _appendLoadedMissionItems (QList& items, QObject* missionItemParent); bool _imagesEverywhere(void) const; bool _triggerCamera(void) const; bool _hasTurnaround(void) const; @@ -129,11 +126,6 @@ private: // return true if vertex a can see vertex b bool _VertexCanSeeOther(const QPolygonF& polygon, const QPointF* vertexA, const QPointF* vertexB); bool _VertexIsReflex(const QPolygonF& polygon, const QPointF* vertex); - void _appendWaypoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate); - void _appendSinglePhotoCapture(QList& items, QObject* missionItemParent, int& seqNum); - void _appendConditionGate(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate); - void _appendCameraTriggerDistance(QList& items, QObject* missionItemParent, int& seqNum, float triggerDistance); - void _appendCameraTriggerDistanceUpdatePoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance); QMap _metaDataMap; @@ -172,7 +164,4 @@ private: static const char* _jsonV3CameraOrientationLandscapeKey; static const char* _jsonV3FixedValueIsAltitudeKey; static const char* _jsonV3Refly90DegreesKey; - - - static const int _hoverAndCaptureDelaySeconds = 4; }; diff --git a/src/MissionManager/SurveyComplexItemTest.cc b/src/MissionManager/SurveyComplexItemTest.cc index 91dc49ddc..76582fbf4 100644 --- a/src/MissionManager/SurveyComplexItemTest.cc +++ b/src/MissionManager/SurveyComplexItemTest.cc @@ -23,7 +23,7 @@ SurveyComplexItemTest::SurveyComplexItemTest(void) void SurveyComplexItemTest::init(void) { - UnitTest::init(); + TransectStyleComplexItemTestBase::init(); _rgSurveySignals[surveyVisualTransectPointsChangedIndex] = SIGNAL(visualTransectPointsChanged()); _rgSurveySignals[surveyCameraShotsChangedIndex] = SIGNAL(cameraShotsChanged()); @@ -32,9 +32,6 @@ void SurveyComplexItemTest::init(void) _rgSurveySignals[surveyRefly90DegreesChangedIndex] = SIGNAL(refly90DegreesChanged(bool)); _rgSurveySignals[surveyDirtyChangedIndex] = SIGNAL(dirtyChanged(bool)); - _planViewSettings = qgcApp()->toolbox()->settingsManager()->planViewSettings(); - _masterController = new PlanMasterController(this); - _controllerVehicle = _masterController->controllerVehicle(); _surveyItem = new SurveyComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */); _mapPolygon = _surveyItem->surveyAreaPolygon(); _mapPolygon->appendVertices(_polyVertices); @@ -66,6 +63,8 @@ void SurveyComplexItemTest::cleanup(void) { delete _surveyItem; delete _multiSpy; + + TransectStyleComplexItemTestBase::cleanup(); } void SurveyComplexItemTest::_testDirty(void) @@ -124,15 +123,8 @@ double SurveyComplexItemTest::_clampGridAngle180(double gridAngle) return gridAngle; } -void SurveyComplexItemTest::_setPolygon(void) -{ - _mapPolygon->appendVertices(_polyVertices); -} - void SurveyComplexItemTest::_testGridAngle(void) { - _setPolygon(); - for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) { _surveyItem->gridAngle()->setRawValue(gridAngle); @@ -141,14 +133,14 @@ void SurveyComplexItemTest::_testGridAngle(void) QGeoCoordinate firstTransectExit = gridPoints[1].value(); double azimuth = firstTransectEntry.azimuthTo(firstTransectExit); //qDebug() << gridAngle << azimuth << _clampGridAngle180(gridAngle) << _clampGridAngle180(azimuth); - QCOMPARE((int)_clampGridAngle180(gridAngle), (int)_clampGridAngle180(azimuth)); + int clampGridAngle = qRound(_clampGridAngle180(gridAngle)); + int clampAzimuth = qRound(_clampGridAngle180(azimuth)); + QCOMPARE(clampGridAngle, clampAzimuth); } } void SurveyComplexItemTest::_testEntryLocation(void) { - _setPolygon(); - for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) { _surveyItem->gridAngle()->setRawValue(gridAngle); @@ -165,40 +157,43 @@ void SurveyComplexItemTest::_testEntryLocation(void) } } - void SurveyComplexItemTest::_testItemCount(void) { - QList items; - - _setPolygon(); - - _surveyItem->hoverAndCapture()->setRawValue(false); - _surveyItem->cameraTriggerInTurnAround()->setRawValue(false); - _surveyItem->refly90Degrees()->setRawValue(false); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); - items.clear(); - - _surveyItem->hoverAndCapture()->setRawValue(false); - _surveyItem->cameraTriggerInTurnAround()->setRawValue(true); - _surveyItem->refly90Degrees()->setRawValue(false); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); - items.clear(); + typedef struct { + bool hoverAndCapture; + bool triggerInTurnAround; + bool refly90; + bool hasTurnaround; + } TestCase_t; - _surveyItem->hoverAndCapture()->setRawValue(true); - _surveyItem->cameraTriggerInTurnAround()->setRawValue(false); - _surveyItem->refly90Degrees()->setRawValue(false); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); - items.clear(); + QList rgTestCases; + + for (int i=0; i<2; i++) { + for (int j=0; i<2; i++) { + for (int k=0; i<2; i++) { + for (int l=0; i<2; i++) { + TestCase_t testCase; + testCase.hoverAndCapture = i; + testCase.triggerInTurnAround = j; + testCase.refly90 = k; + testCase.hasTurnaround = l; + rgTestCases.append(testCase); + } + } + } + } - _surveyItem->hoverAndCapture()->setRawValue(true); - _surveyItem->cameraTriggerInTurnAround()->setRawValue(false); - _surveyItem->refly90Degrees()->setRawValue(true); - _surveyItem->appendMissionItems(items, this); - QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); - items.clear(); + QList items; + for (const TestCase_t& testCase : rgTestCases) { + qDebug() << "hoverAndCapture:triggerInTurnAround:refly90:hasTuraround" << testCase.hoverAndCapture << testCase.triggerInTurnAround << testCase.refly90 << testCase.hasTurnaround; + _surveyItem->hoverAndCapture()->setRawValue(testCase.hoverAndCapture); + _surveyItem->cameraTriggerInTurnAround()->setRawValue(testCase.triggerInTurnAround); + _surveyItem->refly90Degrees()->setRawValue(testCase.refly90); + _surveyItem->turnAroundDistance()->setRawValue(testCase.hasTurnaround ? 50 : 0); + _surveyItem->appendMissionItems(items, this); + QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); + items.clear(); + } } QList SurveyComplexItemTest::_createExpectedCommands(bool hasTurnaround, bool useConditionGate) @@ -245,9 +240,7 @@ void SurveyComplexItemTest::_testItemGenerationWorker(bool imagesInTurnaround, b _surveyItem->appendMissionItems(items, this); #if 0 // Handy for debugging failures - for (const MissionItem* item : items) { - qDebug() << "Cmd" << item->command(); - } + _printItemCommands(items); #endif QCOMPARE(items.count(), expectedCommands.count()); for (int i=0; i imagesInTurnaroundCommands = { + QList imagesInTurnaroundWithTurnaroundDistanceCommands = { // Transect 1 MAV_CMD_CONDITION_GATE, // First turaround MAV_CMD_DO_SET_CAM_TRIGG_DIST, @@ -300,13 +293,33 @@ void SurveyComplexItemTest::_testItemGeneration(void) MAV_CMD_DO_SET_CAM_TRIGG_DIST, }; - _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundCommands); + _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundWithTurnaroundDistanceCommands); + + // Switch to non CONDITION_GATE usage + for (MAV_CMD& cmd : imagesInTurnaroundWithTurnaroundDistanceCommands) { + cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd; + } + _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundWithTurnaroundDistanceCommands); + + QList imagesInTurnaroundWithoutTurnaroundDistanceCommands = { + // Transect 1 + MAV_CMD_CONDITION_GATE, // Survey entry + MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Camera trigger start for entire survey + MAV_CMD_NAV_WAYPOINT, // Survey exit + // Transect 2 + MAV_CMD_CONDITION_GATE, // Survey entry + MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Survey entry also has trigger start + MAV_CMD_CONDITION_GATE, // Survey exit + MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Camera trigger stop for entire survey + }; + + _testItemGenerationWorker(true /* imagesInTurnaround */, false /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundWithoutTurnaroundDistanceCommands); // Switch to non CONDITION_GATE usage - for (MAV_CMD& cmd : imagesInTurnaroundCommands) { + for (MAV_CMD& cmd : imagesInTurnaroundWithoutTurnaroundDistanceCommands) { cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd; } - _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundCommands); + _testItemGenerationWorker(true /* imagesInTurnaround */, false /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundWithoutTurnaroundDistanceCommands); } void SurveyComplexItemTest::_testHoverCaptureItemGeneration(void) @@ -334,7 +347,6 @@ void SurveyComplexItemTest::_testHoverCaptureItemGeneration(void) double triggerDistance = (polyHeightDistance / 3.0) + 1.0; _surveyItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(triggerDistance); - qDebug() << "_testHoverCaptureItemGeneration"; _surveyItem->hoverAndCapture()->setRawValue(true); _testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, expectedCommands); _testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, expectedCommands); diff --git a/src/MissionManager/SurveyComplexItemTest.h b/src/MissionManager/SurveyComplexItemTest.h index 6204d30cb..fcc95d3f0 100644 --- a/src/MissionManager/SurveyComplexItemTest.h +++ b/src/MissionManager/SurveyComplexItemTest.h @@ -9,8 +9,7 @@ #pragma once -#include "UnitTest.h" -#include "TCPLink.h" +#include "TransectStyleComplexItemTestBase.h" #include "MultiSignalSpy.h" #include "SurveyComplexItem.h" #include "PlanMasterController.h" @@ -19,7 +18,7 @@ #include /// Unit test for SurveyComplexItem -class SurveyComplexItemTest : public UnitTest +class SurveyComplexItemTest : public TransectStyleComplexItemTestBase { Q_OBJECT @@ -30,19 +29,30 @@ protected: void init(void) final; void cleanup(void) final; +#if 1 private slots: + void _testDirty(void); + void _testGridAngle(void); + void _testEntryLocation(void); + void _testItemGeneration(void); + void _testItemCount(void); void _testHoverCaptureItemGeneration(void); - +#else + // Handy mechanism to to a single test +private slots: + void _testItemCount(void); private: void _testDirty(void); void _testGridAngle(void); void _testEntryLocation(void); - void _testItemCount(void); void _testItemGeneration(void); - double _clampGridAngle180(double gridAngle); - void _setPolygon(void); - QList _createExpectedCommands(bool hasTurnaround, bool useConditionGate); - void _testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList& expectedCommands); + void _testHoverCaptureItemGeneration(void); +#endif + +private: + double _clampGridAngle180(double gridAngle); + QList _createExpectedCommands(bool hasTurnaround, bool useConditionGate); + void _testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList& expectedCommands); // SurveyComplexItem signals @@ -68,12 +78,9 @@ private: static const size_t _cSurveySignals = surveyMaxSignalIndex; const char* _rgSurveySignals[_cSurveySignals]; - PlanMasterController* _masterController = nullptr; - Vehicle* _controllerVehicle = nullptr; MultiSignalSpy* _multiSpy = nullptr; SurveyComplexItem* _surveyItem = nullptr; QGCMapPolygon* _mapPolygon = nullptr; - PlanViewSettings* _planViewSettings = nullptr; QList _polyVertices; static const int _expectedTransectCount = 2; diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 89c0a768e..e796d9bb1 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -37,8 +37,6 @@ const char* TransectStyleComplexItem::_jsonItemsKey = "Ite const char* TransectStyleComplexItem::_jsonFollowTerrainKey = "FollowTerrain"; const char* TransectStyleComplexItem::_jsonCameraShotsKey = "CameraShots"; -const int TransectStyleComplexItem::_terrainQueryTimeoutMsecs = 1000; - TransectStyleComplexItem::TransectStyleComplexItem(PlanMasterController* masterController, bool flyView, QString settingsGroup, QObject* parent) : ComplexMissionItem (masterController, flyView, parent) , _sequenceNumber (0) @@ -337,10 +335,10 @@ double TransectStyleComplexItem::coveredArea(void) const bool TransectStyleComplexItem::_hasTurnaround(void) const { - return _turnaroundDistance() > 0; + return _turnAroundDistance() > 0; } -double TransectStyleComplexItem::_turnaroundDistance(void) const +double TransectStyleComplexItem::_turnAroundDistance(void) const { return _turnAroundDistanceFact.rawValue().toDouble(); } @@ -755,16 +753,26 @@ int TransectStyleComplexItem::lastSequenceNumber(void) const int itemCount = 0; for (const QList& rgCoordInfo: _transects) { - itemCount += rgCoordInfo.count() * (hoverAndCaptureEnabled() ? 2 : 1); + int commandsPerCoord = 1; // Waypoint command + if (hoverAndCaptureEnabled()) { + commandsPerCoord++; // Camera trigger + } + itemCount += rgCoordInfo.count() * commandsPerCoord; + if (hoverAndCaptureEnabled() && _turnAroundDistance() != 0) { + // The turnaround points do not have camera triggers on them + itemCount -= 2; + } } if (!hoverAndCaptureEnabled() && triggerCamera()) { if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { - // One camera start/stop for beginning/end of entire survey - itemCount += 2; - // One camera start for each transect - itemCount += _transects.count(); + itemCount += _transects.count(); // One camera start for each transect entry + itemCount++; // Single camera stop and the very end + if (_turnAroundDistance() != 0) { + // If there are turnarounds then there is an additional camera start on the first turnaround + itemCount++; + } } else { // Each transect will have a camera start and stop in it itemCount += _transects.count() * 2; @@ -797,7 +805,184 @@ void TransectStyleComplexItem::_followTerrainChanged(bool followTerrain) void TransectStyleComplexItem::_handleHoverAndCaptureEnabled(QVariant enabled) { if (enabled.toBool() && _cameraTriggerInTurnAroundFact.rawValue().toBool()) { - qDebug() << "_handleHoverAndCaptureEnabled"; _cameraTriggerInTurnAroundFact.setRawValue(false); } } + +void TransectStyleComplexItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + if (_loadedMissionItems.count()) { + // We have mission items from the loaded plan, use those + _appendLoadedMissionItems(items, missionItemParent); + } else { + // Build the mission items on the fly + _buildAndAppendMissionItems(items, missionItemParent); + } +} + +void TransectStyleComplexItem::_appendWaypoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + mavFrame, + holdTime, + 0.0, // No acceptance radius specified + 0.0, // Pass through waypoint + std::numeric_limits::quiet_NaN(), // Yaw unchanged + coordinate.latitude(), + coordinate.longitude(), + coordinate.altitude(), + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void TransectStyleComplexItem::_appendSinglePhotoCapture(QList& items, QObject* missionItemParent, int& seqNum) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_IMAGE_START_CAPTURE, + MAV_FRAME_MISSION, + 0, // Reserved (Set to 0) + 0, // Interval (none) + 1, // Take 1 photo + qQNaN(), qQNaN(), qQNaN(), qQNaN(), // param 4-7 reserved + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void TransectStyleComplexItem::_appendConditionGate(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_CONDITION_GATE, + mavFrame, + 0, // Gate is orthogonal to path + 0, // Ignore altitude + 0, 0, // Param 3-4 ignored + coordinate.latitude(), + coordinate.longitude(), + 0, // No altitude + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void TransectStyleComplexItem::_appendCameraTriggerDistance(QList& items, QObject* missionItemParent, int& seqNum, float triggerDistance) +{ + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + triggerDistance, + 0, // shutter integration (ignore) + 1, // 1 - trigger one image immediately, both and entry and exit to get full coverage + 0, 0, 0, 0, // param 4-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); +} + +void TransectStyleComplexItem::_appendCameraTriggerDistanceUpdatePoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance) +{ + if (useConditionGate) { + _appendConditionGate(items, missionItemParent, seqNum, mavFrame, coordinate); + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, coordinate); + } + _appendCameraTriggerDistance(items, missionItemParent, seqNum, triggerDistance); +} + +void TransectStyleComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) +{ + qCDebug(TransectStyleComplexItemLog) << "_buildAndAppendMissionItems"; + + // Now build the mission items from the transect points + + int seqNum = _sequenceNumber; + bool imagesInTurnaround = _cameraTriggerInTurnAroundFact.rawValue().toBool(); + bool hasTurnarounds = _turnAroundDistance() != 0; + bool addTriggerAtBeginningEnd = !hoverAndCaptureEnabled() && imagesInTurnaround && triggerCamera(); + bool useConditionGate = _controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(MAV_CMD_CONDITION_GATE) && + triggerCamera() && + !hoverAndCaptureEnabled(); + + MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; + + // Note: The code below is written to be understable as oppose to being compact and/or remove duplicate code + int transectIndex = 0; + for (const QList& transect: _transects) { + bool entryTurnaround = true; + for (const CoordInfo_t& transectCoordInfo: transect) { + switch (transectCoordInfo.coordType) { + case CoordTypeTurnaround: + { + bool firstEntryTurnaround = transectIndex == 0 && entryTurnaround; + bool lastExitTurnaround = transectIndex == _transects.count() - 1 && !entryTurnaround; + if (addTriggerAtBeginningEnd && (firstEntryTurnaround || lastExitTurnaround)) { + _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, firstEntryTurnaround ? triggerDistance() : 0); + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + } + entryTurnaround = false; + } + break; + case CoordTypeInterior: + case CoordTypeInteriorTerrainAdded: + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + break; + case CoordTypeInteriorHoverTrigger: + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); + _appendSinglePhotoCapture(items, missionItemParent, seqNum); + break; + case CoordTypeSurveyEntry: + if (triggerCamera()) { + if (hoverAndCaptureEnabled()) { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); + _appendSinglePhotoCapture(items, missionItemParent, seqNum); + } else { + // We always add a trigger start to survey entry. Even for imagesInTurnaround = true. This allows you to resume a mission and refly a transect + _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, triggerDistance()); + } + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + } + break; + case CoordTypeSurveyExit: + bool lastSurveyExit = transectIndex == _transects.count() - 1; + if (triggerCamera()) { + if (hoverAndCaptureEnabled()) { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, _hoverAndCaptureDelaySeconds, transectCoordInfo.coord); + _appendSinglePhotoCapture(items, missionItemParent, seqNum); + } else if (addTriggerAtBeginningEnd && !hasTurnarounds && lastSurveyExit) { + _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, 0 /* triggerDistance */); + } else if (imagesInTurnaround) { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + } else { + // If we get this far it means the camera is triggering start/stop for each transect + _appendCameraTriggerDistanceUpdatePoint(items, missionItemParent, seqNum, mavFrame, transectCoordInfo.coord, useConditionGate, 0 /* triggerDistance */); + } + } else { + _appendWaypoint(items, missionItemParent, seqNum, mavFrame, 0 /* holdTime */, transectCoordInfo.coord); + } + break; + } + } + transectIndex++; + } +} + +void TransectStyleComplexItem::_appendLoadedMissionItems(QList& items, QObject* missionItemParent) +{ + qCDebug(TransectStyleComplexItemLog) << "_appendLoadedMissionItems"; + + int seqNum = _sequenceNumber; + + for (const MissionItem* loadedMissionItem: _loadedMissionItems) { + MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent); + item->setSequenceNumber(seqNum++); + items.append(item); + } +} diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index 66f46eb04..bcc368846 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -75,7 +75,7 @@ public: bool triggerCamera (void) const { return triggerDistance() != 0; } // Used internally only by unit tests - int _transectCount(void) { return _transects.count(); } + int _transectCount(void) const { return _transects.count(); } // Overrides from ComplexMissionItem @@ -90,7 +90,7 @@ public: void save (QJsonArray& planItems) override = 0; bool specifiesCoordinate (void) const override = 0; - void appendMissionItems (QList& items, QObject* missionItemParent) override = 0; + void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) override = 0; bool dirty (void) const final { return _dirty; } @@ -151,7 +151,14 @@ protected: void _setCameraShots (int cameraShots); double _triggerDistance (void) const; bool _hasTurnaround (void) const; - double _turnaroundDistance (void) const; + double _turnAroundDistance (void) const; + void _appendWaypoint (QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate); + void _appendSinglePhotoCapture (QList& items, QObject* missionItemParent, int& seqNum); + void _appendConditionGate (QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate); + void _appendCameraTriggerDistance (QList& items, QObject* missionItemParent, int& seqNum, float triggerDistance); + void _appendCameraTriggerDistanceUpdatePoint(QList& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance); + void _buildAndAppendMissionItems (QList& items, QObject* missionItemParent); + void _appendLoadedMissionItems (QList& items, QObject* missionItemParent); int _sequenceNumber; QGeoCoordinate _coordinate; @@ -206,7 +213,8 @@ protected: static const char* _jsonFollowTerrainKey; static const char* _jsonCameraShotsKey; - static const int _terrainQueryTimeoutMsecs; + static const int _terrainQueryTimeoutMsecs= 1000; + static const int _hoverAndCaptureDelaySeconds = 4; private slots: void _reallyQueryTransectsPathHeightInfo(void); diff --git a/src/MissionManager/TransectStyleComplexItemTest.cc b/src/MissionManager/TransectStyleComplexItemTest.cc index a2b75eb99..5d78535cf 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.cc +++ b/src/MissionManager/TransectStyleComplexItemTest.cc @@ -20,10 +20,8 @@ TransectStyleComplexItemTest::TransectStyleComplexItemTest(void) void TransectStyleComplexItemTest::init(void) { - UnitTest::init(); + TransectStyleComplexItemTestBase::init(); - _masterController = new PlanMasterController(this); - _controllerVehicle = _masterController->controllerVehicle(); _transectStyleItem = new TransectStyleItem(_masterController, this); _transectStyleItem->cameraTriggerInTurnAround()->setRawValue(false); _transectStyleItem->cameraCalc()->cameraName()->setRawValue(_transectStyleItem->cameraCalc()->customCameraName()); @@ -50,6 +48,7 @@ void TransectStyleComplexItemTest::cleanup(void) { delete _transectStyleItem; delete _multiSpy; + TransectStyleComplexItemTestBase::cleanup(); } void TransectStyleComplexItemTest::_testDirty(void) diff --git a/src/MissionManager/TransectStyleComplexItemTest.h b/src/MissionManager/TransectStyleComplexItemTest.h index 52356e216..88f7b61e2 100644 --- a/src/MissionManager/TransectStyleComplexItemTest.h +++ b/src/MissionManager/TransectStyleComplexItemTest.h @@ -9,7 +9,7 @@ #pragma once -#include "UnitTest.h" +#include "TransectStyleComplexItemTestBase.h" #include "MultiSignalSpy.h" #include "CorridorScanComplexItem.h" #include "PlanMasterController.h" @@ -18,7 +18,7 @@ class TransectStyleItem; -class TransectStyleComplexItemTest : public UnitTest +class TransectStyleComplexItemTest : public TransectStyleComplexItemTestBase { Q_OBJECT @@ -28,7 +28,7 @@ public: protected: void init(void) final; void cleanup(void) final; - + private slots: void _testDirty (void); void _testRebuildTransects (void); @@ -73,8 +73,6 @@ private: static const size_t _cSignals = maxSignalIndex; const char* _rgSignals[_cSignals]; - PlanMasterController* _masterController = nullptr; - Vehicle* _controllerVehicle = nullptr; MultiSignalSpy* _multiSpy = nullptr; QList _polygonVertices; TransectStyleItem* _transectStyleItem = nullptr; @@ -94,7 +92,6 @@ public: // Overrides from VisualMissionItem void save (QJsonArray& missionItems) final { Q_UNUSED(missionItems); } bool specifiesCoordinate (void) const final { return true; } - void appendMissionItems (QList& items, QObject* missionItemParent) final { Q_UNUSED(items); Q_UNUSED(missionItemParent); } void applyNewAltitude (double newAltitude) final { Q_UNUSED(newAltitude); } double additionalTimeDelay (void) const final { return 0; } diff --git a/src/MissionManager/TransectStyleComplexItemTestBase.cc b/src/MissionManager/TransectStyleComplexItemTestBase.cc new file mode 100644 index 000000000..dc41c17c9 --- /dev/null +++ b/src/MissionManager/TransectStyleComplexItemTestBase.cc @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "TransectStyleComplexItemTestBase.h" +#include "QGCApplication.h" + +TransectStyleComplexItemTestBase::TransectStyleComplexItemTestBase(void) +{ +} + +void TransectStyleComplexItemTestBase::init(void) +{ + UnitTest::init(); + + _planViewSettings = qgcApp()->toolbox()->settingsManager()->planViewSettings(); + _masterController = new PlanMasterController(this); + _controllerVehicle = _masterController->controllerVehicle(); +} + +void TransectStyleComplexItemTestBase::cleanup(void) +{ + delete _masterController; + _planViewSettings = nullptr; + _masterController = nullptr; + _controllerVehicle = nullptr; + UnitTest::cleanup(); +} + +void TransectStyleComplexItemTestBase::_printItemCommands(QList items) +{ + // Handy for debugging failures + for (int i=0; icommand(); + } +} diff --git a/src/MissionManager/TransectStyleComplexItemTestBase.h b/src/MissionManager/TransectStyleComplexItemTestBase.h new file mode 100644 index 000000000..da79436eb --- /dev/null +++ b/src/MissionManager/TransectStyleComplexItemTestBase.h @@ -0,0 +1,34 @@ +/**************************************************************************** + * + * (c) 2009-2020 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 "CorridorScanComplexItem.h" +#include "PlanMasterController.h" +#include "PlanViewSettings.h" + +/// Base class for all TransectStyleComplexItem unit tests +class TransectStyleComplexItemTestBase : public UnitTest +{ + Q_OBJECT + +public: + TransectStyleComplexItemTestBase(void); + +protected: + void init (void) override; + void cleanup(void) override; + + void _printItemCommands(QList items); + + PlanMasterController* _masterController = nullptr; + Vehicle* _controllerVehicle = nullptr; + PlanViewSettings* _planViewSettings = nullptr; +}; -- 2.22.0