diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index c5cffe94656544fb704a4e8e2a3101047a5ef6b4..419561db40dcc03399b1f72854777a5d0f5cf027 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -23,6 +23,8 @@ #include "RadioComponentController.h" #include "QGCCameraManager.h" #include "QGCFileDownload.h" +#include "SettingsManager.h" +#include "PlanViewSettings.h" #include @@ -279,7 +281,7 @@ void PX4FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataF QList PX4FirmwarePlugin::supportedMissionCommands(void) { - return { + QList cmds = { MAV_CMD_NAV_WAYPOINT, MAV_CMD_NAV_LOITER_UNLIM, MAV_CMD_NAV_LOITER_TIME, MAV_CMD_NAV_LOITER_TO_ALT, MAV_CMD_NAV_LAND, MAV_CMD_NAV_TAKEOFF, MAV_CMD_NAV_RETURN_TO_LAUNCH, @@ -298,6 +300,12 @@ QList PX4FirmwarePlugin::supportedMissionCommands(void) MAV_CMD_NAV_DELAY, MAV_CMD_CONDITION_YAW, }; + + if (qgcApp()->toolbox()->settingsManager()->planViewSettings()->useConditionGate()->rawValue().toBool()) { + cmds.append(MAV_CMD_CONDITION_GATE); + } + + return cmds; } QString PX4FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const diff --git a/src/MissionManager/CorridorScanComplexItem.cc b/src/MissionManager/CorridorScanComplexItem.cc index 4ad06458552afc2b3a55591995780bcc3abc51d5..091e2b2fad379abf7cdaf4ae394721088bb543a9 100644 --- a/src/MissionManager/CorridorScanComplexItem.cc +++ b/src/MissionManager/CorridorScanComplexItem.cc @@ -217,7 +217,7 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList& i firstOverallPoint = false; // Possibly add trigger start/stop to survey area entrance/exit - if (triggerCamera() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) { + 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. @@ -370,9 +370,9 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void) TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords[j], CoordTypeInterior }; transect.append(coordInfo); } - TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEdge }; + TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEntry }; transect.prepend(coordInfo); - coordInfo = { transectCoords.last(), CoordTypeSurveyEdge }; + coordInfo = { transectCoords.last(), CoordTypeSurveyExit }; transect.append(coordInfo); // Extend the transect ends for turnaround diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 8ada5854f4e59566837ec249d83d4a72e716d618..c84b524c3775ae05a2776c5862e268daab1be311 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -858,6 +858,20 @@ "default": 25, "units": "m", "decimalPlaces": 2 + }, + "param2": { + "label": "Shutter", + "default": 0, + "units": "msecs", + "decimalPlaces": 0 + }, + "param3": { + "label": "Trigger", + "default": 25, + "enumStrings": "No Trigger,Once Immediately", + "enumValues": "0,1", + "default": 0, + "decimalPlaces": 0 } }, { @@ -1063,6 +1077,22 @@ "enumValues": "3,4" } }, + { + "id": 4501, + "rawName": "MAV_CMD_CONDITION_GATE", + "friendlyName": "Condition Gate", + "description": "Delay mission state machine until gate has been reached.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "category": "Conditionals", + "param2": { + "label": "Ignore Alt", + "enumStrings": "False,True", + "enumValues": "0,1", + "default": 1, + "decimalPlaces": 0 + } + }, { "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "Payload prepare deploy" }, { "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "Payload control deploy" } ] diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index 82815ce9668d34fdd524b4337d61768222cc0b4c..79510f5a1fe6d241d4cc1db6e3651c76a4a25d7e 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -648,119 +648,152 @@ 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 - MissionItem* item; int seqNum = _sequenceNumber; - bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - bool addTriggerAtBeginning = !hoverAndCaptureEnabled() && imagesEverywhere; - bool firstOverallPoint = true; + 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 transectEntry = true; - + bool entryTurnaround = true; for (const CoordInfo_t& transectCoordInfo: transect) { - item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - mavFrame, - hoverAndCaptureEnabled() ? - _hoverAndCaptureDelaySeconds : 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) - 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 (hoverAndCaptureEnabled()) { - 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); - } - - if (firstOverallPoint && addTriggerAtBeginning) { - // Start triggering - addTriggerAtBeginning = false; - item = new MissionItem(seqNum++, - MAV_CMD_DO_SET_CAM_TRIGG_DIST, - MAV_FRAME_MISSION, - triggerDistance(), // 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); + 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; } - firstOverallPoint = false; - - // Possibly add trigger start/stop to survey area entrance/exit - if (triggerCamera() && !hoverAndCaptureEnabled() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) { - 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, - triggerDistance(), // 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); + 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; } } - } - - if (triggerCamera() && !hoverAndCaptureEnabled() && 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); + transectIndex++; } } @@ -971,9 +1004,9 @@ void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(bool refly) QList coordInfoTransect; TransectStyleComplexItem::CoordInfo_t coordInfo; - coordInfo = { transect[0], CoordTypeSurveyEdge }; + coordInfo = { transect[0], CoordTypeSurveyEntry }; coordInfoTransect.append(coordInfo); - coordInfo = { transect[1], CoordTypeSurveyEdge }; + coordInfo = { transect[1], CoordTypeSurveyExit }; coordInfoTransect.append(coordInfo); // For hover and capture we need points for each camera location within the transect @@ -1380,9 +1413,9 @@ void SurveyComplexItem::_rebuildTransectsFromPolygon(bool refly, const QPolygonF QList coordInfoTransect; TransectStyleComplexItem::CoordInfo_t coordInfo; - coordInfo = { transect[0], CoordTypeSurveyEdge }; + coordInfo = { transect[0], CoordTypeSurveyEntry }; coordInfoTransect.append(coordInfo); - coordInfo = { transect[1], CoordTypeSurveyEdge }; + coordInfo = { transect[1], CoordTypeSurveyExit }; coordInfoTransect.append(coordInfo); // For hover and capture we need points for each camera location within the transect diff --git a/src/MissionManager/SurveyComplexItem.h b/src/MissionManager/SurveyComplexItem.h index b2d59aece64e40a36c10815b8ead9ab831d3b17f..7b152fdfd34d72b3b4f905753d26c5cf8267c3fb 100644 --- a/src/MissionManager/SurveyComplexItem.h +++ b/src/MissionManager/SurveyComplexItem.h @@ -98,7 +98,6 @@ private: void _intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines); void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); void _adjustLineDirection(const QList& lineList, QList& resultLines); - int _appendWaypointToMission(QList& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent); bool _nextTransectCoord(const QList& transectPoints, int pointIndex, QGeoCoordinate& coord); bool _appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly); void _optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList>& transects); @@ -130,6 +129,11 @@ 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; diff --git a/src/MissionManager/SurveyComplexItemTest.cc b/src/MissionManager/SurveyComplexItemTest.cc index f812d7bcb5689d8f4584f76f66da2eba419db998..91dc49ddc399e3f247e0f9ee92dad11de5928c34 100644 --- a/src/MissionManager/SurveyComplexItemTest.cc +++ b/src/MissionManager/SurveyComplexItemTest.cc @@ -9,11 +9,16 @@ #include "SurveyComplexItemTest.h" #include "QGCApplication.h" +#include "JsonHelper.h" SurveyComplexItemTest::SurveyComplexItemTest(void) { - _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) << - QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124); + // We use a 100m by 100m square test polygon + const double edgeDistance = 100; + _polyVertices.append(QGeoCoordinate(47.633550640000003, -122.08982199)); + _polyVertices.append(_polyVertices[0].atDistanceAndAzimuth(edgeDistance, 90)); + _polyVertices.append(_polyVertices[1].atDistanceAndAzimuth(edgeDistance, 180)); + _polyVertices.append(_polyVertices[2].atDistanceAndAzimuth(edgeDistance, -90.0)); } void SurveyComplexItemTest::init(void) @@ -27,12 +32,26 @@ 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 */); - _surveyItem->turnAroundDistance()->setRawValue(0); // Unit test written for no turnaround distance - _surveyItem->setDirty(false); _mapPolygon = _surveyItem->surveyAreaPolygon(); + _mapPolygon->appendVertices(_polyVertices); + + QVERIFY(_surveyItem->cameraCalc()->isManualCamera()); + + // Set grid spacing to match expected transect count + double polyWidthDistance = _polyVertices[0].distanceTo(_polyVertices[1]); + double polyHeightDistance = _polyVertices[0].distanceTo(_polyVertices[3]); + _surveyItem->cameraCalc()->adjustedFootprintSide()->setRawValue((polyWidthDistance * 0.5) - 1.0); + _surveyItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(polyHeightDistance * 0.25); + + _surveyItem->gridAngle()->setRawValue(0); + int expectedTransectCount = _expectedTransectCount; + QCOMPARE(_surveyItem->_transectCount(), expectedTransectCount); + + _surveyItem->setDirty(false); // It's important to check that the right signals are emitted at the right time since that drives ui change. // It's also important to check that things are not being over-signalled when they should not be, since that can lead @@ -107,10 +126,7 @@ double SurveyComplexItemTest::_clampGridAngle180(double gridAngle) void SurveyComplexItemTest::_setPolygon(void) { - for (int i=0; i<_polyPoints.count(); i++) { - QGeoCoordinate& vertex = _polyPoints[i]; - _mapPolygon->appendVertex(vertex); - } + _mapPolygon->appendVertices(_polyVertices); } void SurveyComplexItemTest::_testGridAngle(void) @@ -184,3 +200,142 @@ void SurveyComplexItemTest::_testItemCount(void) QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber()); items.clear(); } + +QList SurveyComplexItemTest::_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_CONDITION_GATE, // Survey area exit edge + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_CMD_NAV_WAYPOINT, + }; + + QList singleTransect = singleFullTransect; + QList expectedCommands; + + if (!useConditionGate) { + for (MAV_CMD& cmd : singleTransect) { + cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd; + } + } + + if (!hasTurnaround) { + singleTransect.takeFirst(); + singleTransect.takeLast(); + } + + for (int i=0; i<_expectedTransectCount; i++) { + expectedCommands.append(singleTransect); + } + + return expectedCommands; +} + +void SurveyComplexItemTest::_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); + + _surveyItem->turnAroundDistance()->setRawValue(hasTurnaround ? 50 : 0); + _surveyItem->cameraTriggerInTurnAround()->setRawValue(imagesInTurnaround); + _planViewSettings->useConditionGate()->setRawValue(useConditionGate); + + QList items; + _surveyItem->appendMissionItems(items, this); +#if 0 + // Handy for debugging failures + for (const MissionItem* item : items) { + qDebug() << "Cmd" << item->command(); + } +#endif + QCOMPARE(items.count(), expectedCommands.count()); + for (int i=0; icommand(); + int expectedCommand = expectedCommands[i]; +#if 0 + // Handy for debugging failures + qDebug() << "Index" << i; +#endif + QCOMPARE(actualCommand, expectedCommand); + } +} + +void SurveyComplexItemTest::_testItemGeneration(void) +{ + // 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)); + } + + // Test cameraTriggerInTurnAround = true cases + + QList imagesInTurnaroundCommands = { + // Transect 1 + MAV_CMD_CONDITION_GATE, // First turaround + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_CMD_CONDITION_GATE, // Survey entry + MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Survey entry also has trigger start + MAV_CMD_NAV_WAYPOINT, // Survey exit + MAV_CMD_NAV_WAYPOINT, // Turnaround + // Transect 2 + MAV_CMD_NAV_WAYPOINT, // Turnaround + MAV_CMD_CONDITION_GATE, // Survey entry + MAV_CMD_DO_SET_CAM_TRIGG_DIST, // Survey entry also has trigger start + MAV_CMD_NAV_WAYPOINT, // Survey exit + MAV_CMD_CONDITION_GATE, // Final turnaround + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + }; + + _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundCommands); + + // Switch to non CONDITION_GATE usage + for (MAV_CMD& cmd : imagesInTurnaroundCommands) { + cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd; + } + _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundCommands); +} + +void SurveyComplexItemTest::_testHoverCaptureItemGeneration(void) +{ + static const QList singleFullTransect = { + MAV_CMD_NAV_WAYPOINT, // Turnaround + MAV_CMD_NAV_WAYPOINT, // Survey area entry edge + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Interior trigger + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Interior trigger + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Survey area exit edge + MAV_CMD_IMAGE_START_CAPTURE, + MAV_CMD_NAV_WAYPOINT, // Turnaround + }; + + QList expectedCommands; + for (int i=0; i<_expectedTransectCount; i++) { + expectedCommands.append(singleFullTransect); + } + + // Set trigger distance to generates two interior capture points + double polyHeightDistance = _polyVertices[0].distanceTo(_polyVertices[3]); + 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 b2a3728e041ac0b8c98262b7ca8309ed07a77f3a..6204d30cbee8bd50fa520ca12280b3fac45bebc1 100644 --- a/src/MissionManager/SurveyComplexItemTest.h +++ b/src/MissionManager/SurveyComplexItemTest.h @@ -14,6 +14,7 @@ #include "MultiSignalSpy.h" #include "SurveyComplexItem.h" #include "PlanMasterController.h" +#include "PlanViewSettings.h" #include @@ -30,15 +31,18 @@ protected: void cleanup(void) final; private slots: + void _testHoverCaptureItemGeneration(void); + +private: void _testDirty(void); void _testGridAngle(void); void _testEntryLocation(void); void _testItemCount(void); - -private: - + 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); // SurveyComplexItem signals @@ -69,5 +73,8 @@ private: MultiSignalSpy* _multiSpy = nullptr; SurveyComplexItem* _surveyItem = nullptr; QGCMapPolygon* _mapPolygon = nullptr; - QList _polyPoints; + PlanViewSettings* _planViewSettings = nullptr; + QList _polyVertices; + + static const int _expectedTransectCount = 2; }; diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index f1d3c26bbdd1b6354259db9aab580a8365b6a5ff..66f46eb044b00bfed1938d573c054cfbba1415de 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -74,6 +74,9 @@ public: bool hoverAndCaptureEnabled (void) const { return hoverAndCapture()->rawValue().toBool(); } bool triggerCamera (void) const { return triggerDistance() != 0; } + // Used internally only by unit tests + int _transectCount(void) { return _transects.count(); } + // Overrides from ComplexMissionItem int lastSequenceNumber (void) const final; @@ -159,8 +162,9 @@ protected: CoordTypeInterior, ///< Interior waypoint for flight path only CoordTypeInteriorHoverTrigger, ///< Interior waypoint for hover and capture trigger CoordTypeInteriorTerrainAdded, ///< Interior waypoint added for terrain - CoordTypeSurveyEdge, ///< Waypoint at edge of survey polygon - CoordTypeTurnaround ///< Waypoint outside of survey polygon for turnaround + CoordTypeSurveyEntry, ///< Waypoint at entry edge of survey polygon + CoordTypeSurveyExit, ///< Waypoint at exit edge of survey polygon + CoordTypeTurnaround, ///< First turnaround waypoint }; typedef struct { diff --git a/src/Settings/PlanView.SettingsGroup.json b/src/Settings/PlanView.SettingsGroup.json index eb1921d9d94c202513305533517669f02b0250eb..38a63492c720542939922789cc398fc57ce1fbd0 100644 --- a/src/Settings/PlanView.SettingsGroup.json +++ b/src/Settings/PlanView.SettingsGroup.json @@ -16,5 +16,11 @@ "shortDescription": "Show/Hide the mission item status display", "type": "bool", "defaultValue": false +}, +{ + "name": "useConditionGate", + "shortDescription": "Use MAV_CMD_CONDITION_GATE for pattern generation", + "type": "bool", + "defaultValue": false } ] diff --git a/src/Settings/PlanViewSettings.cc b/src/Settings/PlanViewSettings.cc index 8f82dc044f3aedd9453277162ef7aeab58e8e128..b6ea71fac50fe05607063b82d4fc7437af9b8bfd 100644 --- a/src/Settings/PlanViewSettings.cc +++ b/src/Settings/PlanViewSettings.cc @@ -20,3 +20,4 @@ DECLARE_SETTINGGROUP(PlanView, "PlanView") DECLARE_SETTINGSFACT(PlanViewSettings, displayPresetsTabFirst) DECLARE_SETTINGSFACT(PlanViewSettings, aboveTerrainWarning) DECLARE_SETTINGSFACT(PlanViewSettings, showMissionItemStatus) +DECLARE_SETTINGSFACT(PlanViewSettings, useConditionGate) diff --git a/src/Settings/PlanViewSettings.h b/src/Settings/PlanViewSettings.h index 2fbc3bdc03fa0d37be8f96f4d2f8e6ed915ddb2a..c1a6d0c04daf8c8d95d2ce4d3d8f6daeb933a8cb 100644 --- a/src/Settings/PlanViewSettings.h +++ b/src/Settings/PlanViewSettings.h @@ -23,4 +23,5 @@ public: DEFINE_SETTINGFACT(displayPresetsTabFirst) DEFINE_SETTINGFACT(aboveTerrainWarning) DEFINE_SETTINGFACT(showMissionItemStatus) + DEFINE_SETTINGFACT(useConditionGate) }; diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index 3897dc61a7196323bb6f69588240b01b5463ce31..c698d44af44b1e811df4f994d099107984a3fab8 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -621,6 +621,11 @@ Rectangle { fact: QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude } } + + FactCheckBox { + text: qsTr("Use MAV_CMD_CONDITION_GATE for pattern generation") + fact: QGroundControl.settingsManager.planViewSettings.useConditionGate + } } }