diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index ca4c790b4c918c0e1430d878b74eed7d99a6b677..231729529864b172835c35e55fa0a1bbea3a8281 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -814,7 +814,7 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, MissionItem* item; int seqNum = _sequenceNumber; bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); - bool addTriggerAtBeginning = imagesEverywhere; + bool addTriggerAtBeginning = !hoverAndCaptureEnabled() && imagesEverywhere; bool firstOverallPoint = true; MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; @@ -826,17 +826,31 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, 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 + 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 + 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 + NAN, NAN, NAN, NAN, // param 4-7 reserved + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + } if (firstOverallPoint && addTriggerAtBeginning) { // Start triggering @@ -844,29 +858,29 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, 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 + 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); } firstOverallPoint = false; - if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && !imagesEverywhere) { + if (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge && triggerCamera() && !hoverAndCaptureEnabled() && !imagesEverywhere) { if (entryPoint) { // Start of transect, start triggering 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 + 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); } else { @@ -888,7 +902,7 @@ void SurveyComplexItem::_buildAndAppendMissionItems(QList& items, } } - if (imagesEverywhere) { + if (triggerCamera() && !hoverAndCaptureEnabled() && imagesEverywhere) { // Stop triggering MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, @@ -1230,6 +1244,20 @@ void SurveyComplexItem::_rebuildTransectsPhase1Worker(bool refly) coordInfo = { transect[1], CoordTypeSurveyEdge }; coordInfoTransect.append(coordInfo); + // For hover and capture we need points for each camera location within the transect + if (triggerCamera() && hoverAndCaptureEnabled()) { + double transectLength = transect[0].distanceTo(transect[1]); + double transectAzimuth = transect[0].azimuthTo(transect[1]); + if (triggerDistance() < transectLength) { + int cInnerHoverPoints = floor(transectLength / triggerDistance()); + qCDebug(SurveyComplexItemLog) << "cInnerHoverPoints" << cInnerHoverPoints; + for (int i=0; i().distanceTo(_visualTransectPoints[i+1].value()); } - double triggerDistance = _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(); if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { - _cameraShots = qCeil(_complexDistance / triggerDistance); + _cameraShots = qCeil(_complexDistance / triggerDistance()); } else { _cameraShots = 0; foreach (const QList& transect, _transects) { @@ -1275,7 +1302,7 @@ void SurveyComplexItem::_rebuildTransectsPhase2(void) firstCameraCoord = transect.first().coord; lastCameraCoord = transect.last().coord; } - _cameraShots += qCeil(firstCameraCoord.distanceTo(lastCameraCoord) / triggerDistance); + _cameraShots += qCeil(firstCameraCoord.distanceTo(lastCameraCoord) / triggerDistance()); } } @@ -1340,6 +1367,5 @@ void SurveyComplexItem::rotateEntryPoint(void) double SurveyComplexItem::timeBetweenShots(void) { - return _cruiseSpeed == 0 ? 0 : _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble() / _cruiseSpeed; + return _cruiseSpeed == 0 ? 0 : triggerDistance() / _cruiseSpeed; } - diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 258b599685cbcf0dc4cd6c3cacc448547f7018af..a15dbf71dd1b2a67d3daba0adf8a1cef4a579d7c 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -682,15 +682,18 @@ int TransectStyleComplexItem::lastSequenceNumber(void) const int itemCount = 0; foreach (const QList& rgCoordInfo, _transects) { - itemCount += rgCoordInfo.count(); + itemCount += rgCoordInfo.count() * (hoverAndCaptureEnabled() ? 2 : 1); } - if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { - // Only one camera start and on camera stop - itemCount += 2; - } else { - // Each transect will have a camera start and stop in it - itemCount += _transects.count() * 2; + + if (!hoverAndCaptureEnabled()) { + if (_cameraTriggerInTurnAroundFact.rawValue().toBool()) { + // Only one camera start and on camera stop + itemCount += 2; + } else { + // Each transect will have a camera start and stop in it + itemCount += _transects.count() * 2; + } } return _sequenceNumber + itemCount - 1; diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index 7d35574959bb7a5006883beb71a9c997f931f585..6049d20cd1f50ff018150428410b2b5e5f4062fa 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -57,6 +57,8 @@ public: Fact* terrainAdjustMaxDescentRate (void) { return &_terrainAdjustMaxClimbRateFact; } Fact* terrainAdjustMaxClimbRate (void) { return &_terrainAdjustMaxDescentRateFact; } + const Fact* hoverAndCapture (void) const { return &_hoverAndCaptureFact; } + int cameraShots (void) const { return _cameraShots; } double coveredArea (void) const; bool hoverAndCaptureAllowed (void) const; @@ -66,6 +68,10 @@ public: void setFollowTerrain(bool followTerrain); + double triggerDistance (void) const { return _cameraCalc.adjustedFootprintFrontal()->rawValue().toDouble(); } + bool hoverAndCaptureEnabled (void) const { return hoverAndCapture()->rawValue().toBool(); } + bool triggerCamera (void) const { return triggerDistance() != 0; } + // Overrides from ComplexMissionItem int lastSequenceNumber (void) const final; @@ -148,10 +154,11 @@ protected: QGCMapPolygon _surveyAreaPolygon; enum CoordType { - CoordTypeInterior, - CoordTypeInteriorTerrainAdded, - CoordTypeSurveyEdge, - CoordTypeTurnaround + 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 }; typedef struct { diff --git a/src/PlanView/SurveyItemEditor.qml b/src/PlanView/SurveyItemEditor.qml index 8f0c3503b653d2a3b92b45ab3313a104729c17a0..5e01474e4282a1ad3fb656f8b14a97550e1421cc 100644 --- a/src/PlanView/SurveyItemEditor.qml +++ b/src/PlanView/SurveyItemEditor.qml @@ -112,7 +112,7 @@ Rectangle { Layout.columnSpan: 2 onClicked: { if (checked) { - missionItem.cameraTriggerInTurnaround.rawValue = false + missionItem.cameraTriggerInTurnAround.rawValue = false } } }