diff --git a/src/MissionManager/CameraSection.FactMetaData.json b/src/MissionManager/CameraSection.FactMetaData.json index 3895f1ac02b1f06f1878de9f0e6fb2e7689b9ff4..563e585cdf8681a6ea171bf3a4a2927a3588c85f 100644 --- a/src/MissionManager/CameraSection.FactMetaData.json +++ b/src/MissionManager/CameraSection.FactMetaData.json @@ -12,7 +12,7 @@ "shortDescription": "Specify the distance between each photo", "type": "double", "units": "m", - "min": 0, + "min": 0.1, "decimalPlaces": 1, "defaultValue": 1 }, diff --git a/src/MissionManager/CameraSection.cc b/src/MissionManager/CameraSection.cc index 247487e91bcbaedf6da1b038449387f9d8e8060b..3a2b349b226efda82ab3c4df0db3cc12797cbf2b 100644 --- a/src/MissionManager/CameraSection.cc +++ b/src/MissionManager/CameraSection.cc @@ -229,36 +229,40 @@ bool CameraSection::scanForCameraSection(QmlObjectListModel* visualItems, int sc cameraAction()->setRawValue(TakePhotosIntervalTime); cameraPhotoIntervalTime()->setRawValue(missionItem.param1()); visualItems->removeAt(scanIndex)->deleteLater(); - } else { - stopLooking = true; } + stopLooking = true; break; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: if (!foundCameraAction && missionItem.param1() >= 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0 && missionItem.param5() == 0 && missionItem.param6() == 0 && missionItem.param7() == 0) { - // At this point we don't know if we have a stop taking photos pair, or a single distance trigger where the user specified 0 - // We need to look at the next item to check for the stop taking photos pari + // At this point we don't know if we have a stop taking photos pair, or just a distance trigger if (missionItem.param1() == 0 && scanIndex < visualItems->count() - 1) { + // Possible stop taking photos pair SimpleMissionItem* nextItem = visualItems->value(scanIndex + 1); if (nextItem) { - missionItem = nextItem->missionItem(); - if ((MAV_CMD)item->command() == MAV_CMD_IMAGE_STOP_CAPTURE && missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0 && missionItem.param5() == 0 && missionItem.param6() == 0 && missionItem.param7() == 0) { + MissionItem& nextMissionItem = nextItem->missionItem(); + if (nextMissionItem.command() == MAV_CMD_IMAGE_STOP_CAPTURE && nextMissionItem.param1() == 0 && nextMissionItem.param2() == 0 && nextMissionItem.param3() == 0 && nextMissionItem.param4() == 0 && nextMissionItem.param5() == 0 && nextMissionItem.param6() == 0 && nextMissionItem.param7() == 0) { + // We found a stop taking photos pair foundCameraAction = true; cameraAction()->setRawValue(StopTakingPhotos); visualItems->removeAt(scanIndex)->deleteLater(); visualItems->removeAt(scanIndex)->deleteLater(); + stopLooking = true; break; } } } - // We didn't find a stop taking photos pair, so this is a regular trigger distance item - foundCameraAction = true; - cameraAction()->setRawValue(TakePhotoIntervalDistance); - cameraPhotoIntervalDistance()->setRawValue(missionItem.param1()); - visualItems->removeAt(scanIndex)->deleteLater(); - break; + // We didn't find a stop taking photos pair, check for trigger distance + if (missionItem.param1() > 0) { + foundCameraAction = true; + cameraAction()->setRawValue(TakePhotoIntervalDistance); + cameraPhotoIntervalDistance()->setRawValue(missionItem.param1()); + visualItems->removeAt(scanIndex)->deleteLater(); + stopLooking = true; + break; + } } stopLooking = true; break; @@ -268,9 +272,8 @@ bool CameraSection::scanForCameraSection(QmlObjectListModel* visualItems, int sc foundCameraAction = true; cameraAction()->setRawValue(TakeVideo); visualItems->removeAt(scanIndex)->deleteLater(); - } else { - stopLooking = true; } + stopLooking = true; break; case MAV_CMD_VIDEO_STOP_CAPTURE: @@ -278,9 +281,8 @@ bool CameraSection::scanForCameraSection(QmlObjectListModel* visualItems, int sc foundCameraAction = true; cameraAction()->setRawValue(StopTakingVideo); visualItems->removeAt(scanIndex)->deleteLater(); - } else { - stopLooking = true; } + stopLooking = true; break; default: diff --git a/src/MissionManager/MavCmdInfoCommon.json b/src/MissionManager/MavCmdInfoCommon.json index 87f41e2a7df0da61e82fa644d46fc45ef48c343f..c76fae56790f0cf7709f794a90b4d841bf682c91 100644 --- a/src/MissionManager/MavCmdInfoCommon.json +++ b/src/MissionManager/MavCmdInfoCommon.json @@ -817,11 +817,11 @@ } }, { - "id": 206, - "rawName": "MAV_CMD_DO_SET_CAM_TRIGG_DIST", - "friendlyName": "Camera trigger distance", - "description": "Set camera trigger distance.", - "category": "Camera", + "id": 206, + "rawName": "MAV_CMD_DO_SET_CAM_TRIGG_DIST", + "friendlyName": "Camera trigger distance", + "description": "Set camera trigger distance.", + "category": "Camera", "param1": { "label": "Distance", "default": 25, diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 295822b4e719b6a0cfd29b5f3a85237e37e82b5e..95cfdfb35281c5fafad242c5fa0d3d1d6ffd9332 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -1555,10 +1555,8 @@ void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualIte } SimpleMissionItem* simpleItem = qobject_cast(visualItem); - if (simpleItem && simpleItem->cameraSection()->available()) { - scanIndex++; - simpleItem->scanForSections(visualItems, scanIndex, vehicle); - continue; + if (simpleItem) { + simpleItem->scanForSections(visualItems, scanIndex + 1, vehicle); } scanIndex++; diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc index 4be427d181f4b4272defa4d2179ec67b33d53d99..69050570e6151e28caa5bffb2c66c128ca3addfc 100644 --- a/src/MissionManager/SimpleMissionItem.cc +++ b/src/MissionManager/SimpleMissionItem.cc @@ -655,16 +655,17 @@ double SimpleMissionItem::specifiedGimbalYaw(void) return _cameraSection->available() ? _cameraSection->specifiedGimbalYaw() : missionItem().specifiedGimbalYaw(); } -void SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle) +bool SimpleMissionItem::scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle) { - Q_UNUSED(vehicle); + bool sectionFound = false; - qDebug() << "SimpleMissionItem::scanForSections" << scanIndex << _cameraSection->available(); + Q_UNUSED(vehicle); if (_cameraSection->available()) { - bool sectionFound = _cameraSection->scanForCameraSection(visualItems, scanIndex); - qDebug() << sectionFound; + sectionFound = _cameraSection->scanForCameraSection(visualItems, scanIndex); } + + return sectionFound; } void SimpleMissionItem::_updateCameraSection(void) diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index 7dcea5f4ac17b22c260fe1c069a52a279d4bf935..528d78530bda97c903305641eb094716368864d7 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -48,7 +48,8 @@ public: /// @param visualItems List of all visual items /// @param scanIndex Index to start scanning from /// @param vehicle Vehicle associated with this mission - void scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle); + /// @return true: section found + bool scanForSections(QmlObjectListModel* visualItems, int scanIndex, Vehicle* vehicle); // Property accesors diff --git a/src/MissionManager/Survey.SettingsGroup.json b/src/MissionManager/Survey.SettingsGroup.json index 42f8704fb1395bf5ab2e6a0c27206448e7b94c96..422f87c861b9f4304191bbb006c79a8f0a227e9a 100644 --- a/src/MissionManager/Survey.SettingsGroup.json +++ b/src/MissionManager/Survey.SettingsGroup.json @@ -130,6 +130,18 @@ "units": "m", "defaultValue": 25 }, +{ + "name": "CameraTriggerInTurnaround", + "shortDescription": "Camera continues taking images in turnarounds.", + "type": "bool", + "defaultValue": false +}, +{ + "name": "HoverAndCapture", + "shortDescription": "Hover at each image point and take image", + "type": "bool", + "defaultValue": false +}, { "name": "CameraOrientationLandscape", "shortDescription": "Camera on vehicle is in landscape orientation.", @@ -140,7 +152,7 @@ "name": "FixedValueIsAltitude", "shortDescription": "The altitude is kep constant while ground resolution changes.", "type": "bool", - "defaultValue": 0 + "defaultValue": false }, { "name": "Camera", diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index d23d60eca6c39e52581d33aeeb9ee3ab8593ed49..ac84b1cbd7504b8191e063852a749bd846c5bc2d 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -30,6 +30,8 @@ const char* SurveyMissionItem::_jsonGridSpacingKey = "spacing"; const char* SurveyMissionItem::_jsonTurnaroundDistKey = "turnAroundDistance"; const char* SurveyMissionItem::_jsonCameraTriggerKey = "cameraTrigger"; const char* SurveyMissionItem::_jsonCameraTriggerDistanceKey = "cameraTriggerDistance"; +const char* SurveyMissionItem::_jsonCameraTriggerInTurnaroundKey = "cameraTriggerInTurnaround"; +const char* SurveyMissionItem::_jsonHoverAndCaptureKey = "hoverAndCapture"; const char* SurveyMissionItem::_jsonGroundResolutionKey = "groundResolution"; const char* SurveyMissionItem::_jsonFrontalOverlapKey = "imageFrontalOverlap"; const char* SurveyMissionItem::_jsonSideOverlapKey = "imageSideOverlap"; @@ -52,6 +54,8 @@ const char* SurveyMissionItem::gridAngleName = "GridAngle"; const char* SurveyMissionItem::gridSpacingName = "GridSpacing"; const char* SurveyMissionItem::turnaroundDistName = "TurnaroundDist"; const char* SurveyMissionItem::cameraTriggerDistanceName = "CameraTriggerDistance"; +const char* SurveyMissionItem::cameraTriggerInTurnaroundName = "CameraTriggerInTurnaround"; +const char* SurveyMissionItem::hoverAndCaptureName = "HoverAndCapture"; const char* SurveyMissionItem::groundResolutionName = "GroundResolution"; const char* SurveyMissionItem::frontalOverlapName = "FrontalOverlap"; const char* SurveyMissionItem::sideOverlapName = "SideOverlap"; @@ -83,6 +87,8 @@ SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) , _turnaroundDistFact (settingsGroup, _metaDataMap[turnaroundDistName]) , _cameraTriggerFact (settingsGroup, _metaDataMap[cameraTriggerName]) , _cameraTriggerDistanceFact (settingsGroup, _metaDataMap[cameraTriggerDistanceName]) + , _cameraTriggerInTurnaroundFact (settingsGroup, _metaDataMap[cameraTriggerInTurnaroundName]) + , _hoverAndCaptureFact (settingsGroup, _metaDataMap[hoverAndCaptureName]) , _groundResolutionFact (settingsGroup, _metaDataMap[groundResolutionName]) , _frontalOverlapFact (settingsGroup, _metaDataMap[frontalOverlapName]) , _sideOverlapFact (settingsGroup, _metaDataMap[sideOverlapName]) @@ -102,10 +108,13 @@ SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) _turnaroundDistFact.setRawValue(0); } - connect(&_gridSpacingFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_turnaroundDistFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(&_gridSpacingFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(&_gridAngleFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(&_turnaroundDistFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(&_cameraTriggerDistanceFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(&_cameraTriggerInTurnaroundFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(&_gridAltitudeFact, &Fact::valueChanged, this, &SurveyMissionItem::_updateCoordinateAltitude); // Signal to Qml when camera value changes so it can recalc @@ -261,8 +270,8 @@ int SurveyMissionItem::lastSequenceNumber(void) const { int lastSeq = _sequenceNumber; - if (_gridPoints.count()) { - lastSeq += _gridPoints.count() - 1; + if (_simpleGridPoints.count()) { + lastSeq += _simpleGridPoints.count() - 1; if (_cameraTriggerFact.rawValue().toBool()) { // Account for two trigger messages lastSeq += 2; @@ -495,8 +504,8 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumbe double SurveyMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const { double greatestDistance = 0.0; - for (int i=0; i<_gridPoints.count(); i++) { - QGeoCoordinate currentCoord = _gridPoints[i].value(); + for (int i=0; i<_simpleGridPoints.count(); i++) { + QGeoCoordinate currentCoord = _simpleGridPoints[i].value(); double distance = currentCoord.distanceTo(other); if (distance > greatestDistance) { greatestDistance = distance; @@ -521,11 +530,16 @@ bool SurveyMissionItem::specifiesCoordinate(void) const void SurveyMissionItem::_clearGrid(void) { // Bug workaround - while (_gridPoints.count() > 1) { - _gridPoints.takeLast(); + while (_simpleGridPoints.count() > 1) { + _simpleGridPoints.takeLast(); } emit gridPointsChanged(); - _gridPoints.clear(); + _simpleGridPoints.clear(); +} + +void _calcCameraShots() +{ + } void SurveyMissionItem::_generateGrid(void) @@ -535,10 +549,11 @@ void SurveyMissionItem::_generateGrid(void) return; } - _gridPoints.clear(); + _simpleGridPoints.clear(); - QList polygonPoints; - QList gridPoints; + QList polygonPoints; + QList gridPoints; + QList> gridSegments; // Convert polygon to Qt coordinate system (y positive is down) qCDebug(SurveyMissionItemLog) << "Convert polygon"; @@ -561,7 +576,7 @@ void SurveyMissionItem::_generateGrid(void) _setCoveredArea(0.5 * fabs(coveredArea)); // Generate grid - _gridGenerator(polygonPoints, gridPoints); + _gridGenerator(polygonPoints, gridPoints, gridSegments); double surveyDistance = 0.0; // Convert to Geo and set altitude @@ -574,11 +589,16 @@ void SurveyMissionItem::_generateGrid(void) QGeoCoordinate geoCoord; convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord); - _gridPoints += QVariant::fromValue(geoCoord); + _simpleGridPoints += QVariant::fromValue(geoCoord); } _setSurveyDistance(surveyDistance); - if (_cameraTriggerDistanceFact.rawValue().toDouble() > 0) { - _setCameraShots((int)floor(surveyDistance / _cameraTriggerDistanceFact.rawValue().toDouble())); + + // Set camera shots here if taking images in turnaround (otherwise it's in _gridGenerator) + double triggerDistance = _cameraTriggerDistanceFact.rawValue().toDouble(); + if (triggerDistance > 0) { + if (_cameraTriggerInTurnaroundFact.rawValue().toBool()) { + _setCameraShots((int)ceil(surveyDistance / triggerDistance)); + } } else { _setCameraShots(0); } @@ -586,11 +606,11 @@ void SurveyMissionItem::_generateGrid(void) emit gridPointsChanged(); emit lastSequenceNumberChanged(lastSequenceNumber()); - if (_gridPoints.count()) { - QGeoCoordinate coordinate = _gridPoints.first().value(); + if (_simpleGridPoints.count()) { + QGeoCoordinate coordinate = _simpleGridPoints.first().value(); coordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); setCoordinate(coordinate); - QGeoCoordinate exitCoordinate = _gridPoints.last().value(); + QGeoCoordinate exitCoordinate = _simpleGridPoints.last().value(); exitCoordinate.setAltitude(_gridAltitudeFact.rawValue().toDouble()); _setExitCoordinate(exitCoordinate); } @@ -718,14 +738,15 @@ void SurveyMissionItem::_adjustLineDirection(const QList& lineList, QLis } } -void SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QList& gridPoints) +void SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QList& simpleGridPoints, QList>& gridSegments) { double gridAngle = _gridAngleFact.rawValue().toDouble(); double gridSpacing = _gridSpacingFact.rawValue().toDouble(); qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle" << gridSpacing << gridAngle; - gridPoints.clear(); + simpleGridPoints.clear(); + gridSegments.clear(); // Convert polygon to bounding rect @@ -778,74 +799,139 @@ void SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QLi QList resultLines; _adjustLineDirection(intersectLines, resultLines); - // Turn into a path float turnaroundDist = _turnaroundDistFact.rawValue().toDouble(); + // Calc camera shots here if there are no images in turnaround + double triggerDistance = _cameraTriggerDistanceFact.rawValue().toDouble(); + if (triggerDistance > 0 && !_cameraTriggerInTurnaroundFact.rawValue().toBool()) { + int cameraShots = 0; + for (int i=0; i segmentPoints; const QLineF& line = resultLines[i]; QPointF turnaroundOffset = line.p2() - line.p1(); - turnaroundOffset = turnaroundOffset * turnaroundDist / sqrt(pow(turnaroundOffset.x(),2.0) + pow(turnaroundOffset.y(),2.0)); + turnaroundOffset = turnaroundOffset * turnaroundDist / sqrt(pow(turnaroundOffset.x(), 2) + pow(turnaroundOffset.y(), 2)); if (i & 1) { if (turnaroundDist > 0.0) { - gridPoints << line.p2() + turnaroundOffset << line.p2() << line.p1() << line.p1() - turnaroundOffset; + simpleGridPoints << line.p2() + turnaroundOffset << line.p2() << line.p1() << line.p1() - turnaroundOffset; + segmentPoints << line.p2() + turnaroundOffset << line.p2() << line.p1() << line.p1() - turnaroundOffset; } else { - gridPoints << line.p2() << line.p1(); + simpleGridPoints << line.p2() << line.p1(); + segmentPoints << line.p2() << line.p1(); } } else { if (turnaroundDist > 0.0) { - gridPoints << line.p1() - turnaroundOffset << line.p1() << line.p2() << line.p2() + turnaroundOffset; + simpleGridPoints << line.p1() - turnaroundOffset << line.p1() << line.p2() << line.p2() + turnaroundOffset; + segmentPoints << line.p1() - turnaroundOffset << line.p1() << line.p2() << line.p2() + turnaroundOffset; } else { - gridPoints << line.p1() << line.p2(); + simpleGridPoints << line.p1() << line.p2(); + segmentPoints << line.p1() << line.p2(); } } + gridSegments.append(segmentPoints); + } +} + +int SurveyMissionItem::_appendWaypointToMission(QList& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent) +{ + double altitude = _gridAltitudeFact.rawValue().toDouble(); + bool altitudeRelative = _gridAltitudeRelativeFact.rawValue().toBool(); + double triggerDistance = _cameraTriggerDistanceFact.rawValue().toDouble(); + + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_NAV_WAYPOINT, + altitudeRelative ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, + 0.0, 0.0, 0.0, 0.0, // param 1-4 unused + coord.latitude(), + coord.longitude(), + altitude, + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); + + if (cameraTrigger != CameraTriggerNone) { + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + cameraTrigger == CameraTriggerOn ? triggerDistance : 0, + 0, 0, 0, 0, 0, 0, // param 2-7 unused + true, // autoContinue + false, // isCurrentItem + missionItemParent); + items.append(item); } + + return seqNum; } void SurveyMissionItem::appendMissionItems(QList& items, QObject* missionItemParent) { - int seqNum = _sequenceNumber; - - for (int i=0; i<_gridPoints.count(); i++) { - QGeoCoordinate coord = _gridPoints[i].value(); - double altitude = _gridAltitudeFact.rawValue().toDouble(); - - MissionItem* item = new MissionItem(seqNum++, // sequence number - MAV_CMD_NAV_WAYPOINT, // MAV_CMD - _gridAltitudeRelativeFact.rawValue().toBool() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, // MAV_FRAME - 0.0, 0.0, 0.0, 0.0, // param 1-4 - coord.latitude(), - coord.longitude(), - altitude, - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + int seqNum = _sequenceNumber; + bool hasTurnaround = _turnaroundDistFact.rawValue().toDouble() > 0; + bool triggerCamera = _cameraTriggerFact.rawValue().toBool(); + bool imagesEverywhere = triggerCamera && _cameraTriggerInTurnaroundFact.rawValue().toBool(); + + int i = 0; + while (i < _simpleGridPoints.count()) { + CameraTriggerCode cameraTrigger; + QGeoCoordinate coord = _simpleGridPoints[i].value(); + + // Either waypoint entering polygon of turnaround point for start of new transect + cameraTrigger = imagesEverywhere ? (i == 0 ? CameraTriggerOn : CameraTriggerNone) : (hasTurnaround ? CameraTriggerNone : CameraTriggerOn); + seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); + + if (hasTurnaround) { + // Waypoint entering the polygon + if (++i >= _simpleGridPoints.count()) { + qWarning() << "Bad grid generation"; + break; + } + coord = _simpleGridPoints[i].value(); + cameraTrigger = imagesEverywhere ? CameraTriggerNone : CameraTriggerOn; + seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); + } - if (_cameraTriggerFact.rawValue().toBool() && i == 0) { - // Turn on camera - MissionItem* item = new MissionItem(seqNum++, // sequence number - MAV_CMD_DO_SET_CAM_TRIGG_DIST, // MAV_CMD - MAV_FRAME_MISSION, // MAV_FRAME - _cameraTriggerDistanceFact.rawValue().toDouble(), // trigger distance - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 2-7 - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); + // Waypoint exiting polygon + if (++i >= _simpleGridPoints.count()) { + qWarning() << "Bad grid generation"; + break; + } + coord = _simpleGridPoints[i].value(); + cameraTrigger = imagesEverywhere ? CameraTriggerNone : CameraTriggerOff; + seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); + + if (hasTurnaround) { + // Waypoint at the end of transect + if (++i >= _simpleGridPoints.count()) { + qWarning() << "Bad grid generation"; + break; + } + coord = _simpleGridPoints[i].value(); + cameraTrigger = CameraTriggerNone; + seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); } + + i++; } - if (_cameraTriggerFact.rawValue().toBool()) { - // Turn off camera - MissionItem* item = new MissionItem(seqNum++, // sequence number - MAV_CMD_DO_SET_CAM_TRIGG_DIST, // MAV_CMD - MAV_FRAME_MISSION, // MAV_FRAME - 0.0, // trigger distance - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // param 2-7 - true, // autoContinue - false, // isCurrentItem + if (imagesEverywhere) { + // Turn off camera at end of survey + MissionItem* item = new MissionItem(seqNum++, + MAV_CMD_DO_SET_CAM_TRIGG_DIST, + MAV_FRAME_MISSION, + 0.0, // trigger distance (off) + 0, 0, 0, 0, 0, 0, // param 2-7 unused + true, // autoContinue + false, // isCurrentItem missionItemParent); items.append(item); } @@ -883,3 +969,8 @@ void SurveyMissionItem::setMissionFlightStatus (MissionController::MissionFligh emit timeBetweenShotsChanged(); } } + +void SurveyMissionItem::_setDirty(void) +{ + setDirty(true); +} diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h index b83916a62bc1e6399345e1805cad89e64694cf40..c96e32a003319dc53821a9d0888273ecde63b763 100644 --- a/src/MissionManager/SurveyMissionItem.h +++ b/src/MissionManager/SurveyMissionItem.h @@ -32,6 +32,8 @@ public: Q_PROPERTY(Fact* turnaroundDist READ turnaroundDist CONSTANT) Q_PROPERTY(Fact* cameraTrigger READ cameraTrigger CONSTANT) Q_PROPERTY(Fact* cameraTriggerDistance READ cameraTriggerDistance CONSTANT) + Q_PROPERTY(Fact* cameraTriggerInTurnaround READ cameraTriggerInTurnaround CONSTANT) + Q_PROPERTY(Fact* hoverAndCapture READ hoverAndCapture CONSTANT) Q_PROPERTY(Fact* groundResolution READ groundResolution CONSTANT) Q_PROPERTY(Fact* frontalOverlap READ frontalOverlap CONSTANT) Q_PROPERTY(Fact* sideOverlap READ sideOverlap CONSTANT) @@ -68,7 +70,7 @@ public: QVariantList polygonPath (void) { return _polygonPath; } QmlObjectListModel* polygonModel(void) { return &_polygonModel; } - QVariantList gridPoints (void) { return _gridPoints; } + QVariantList gridPoints (void) { return _simpleGridPoints; } Fact* manualGrid (void) { return &_manualGridFact; } Fact* gridAltitude (void) { return &_gridAltitudeFact; } @@ -78,6 +80,8 @@ public: Fact* turnaroundDist (void) { return &_turnaroundDistFact; } Fact* cameraTrigger (void) { return &_cameraTriggerFact; } Fact* cameraTriggerDistance (void) { return &_cameraTriggerDistanceFact; } + Fact* cameraTriggerInTurnaround (void) { return &_cameraTriggerInTurnaroundFact; } + Fact* hoverAndCapture (void) { return &_hoverAndCaptureFact; } Fact* groundResolution (void) { return &_groundResolutionFact; } Fact* frontalOverlap (void) { return &_frontalOverlapFact; } Fact* sideOverlap (void) { return &_sideOverlapFact; } @@ -141,6 +145,8 @@ public: static const char* gridSpacingName; static const char* turnaroundDistName; static const char* cameraTriggerDistanceName; + static const char* cameraTriggerInTurnaroundName; + static const char* hoverAndCaptureName; static const char* groundResolutionName; static const char* frontalOverlapName; static const char* sideOverlapName; @@ -166,14 +172,21 @@ signals: private slots: void _cameraTriggerChanged(void); + void _setDirty(void); private: + enum CameraTriggerCode { + CameraTriggerNone, + CameraTriggerOn, + CameraTriggerOff + }; + void _clear(void); void _setExitCoordinate(const QGeoCoordinate& coordinate); void _clearGrid(void); void _generateGrid(void); void _updateCoordinateAltitude(void); - void _gridGenerator(const QList& polygonPoints, QList& gridPoints); + void _gridGenerator(const QList& polygonPoints, QList& simpleGridPoints, QList>& gridSegments); QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); void _intersectLinesWithRect(const QList& lineList, const QRectF& boundRect, QList& resultLines); void _intersectLinesWithPolygon(const QList& lineList, const QPolygonF& polygon, QList& resultLines); @@ -182,15 +195,17 @@ private: void _setCameraShots(int cameraShots); void _setCoveredArea(double coveredArea); void _cameraValueChanged(void); - - int _sequenceNumber; - bool _dirty; - QVariantList _polygonPath; - QmlObjectListModel _polygonModel; - QVariantList _gridPoints; - QGeoCoordinate _coordinate; - QGeoCoordinate _exitCoordinate; - bool _cameraOrientationFixed; + int _appendWaypointToMission(QList& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent); + + int _sequenceNumber; + bool _dirty; + QVariantList _polygonPath; + QmlObjectListModel _polygonModel; + QVariantList _simpleGridPoints; ///< Grid points for drawing simple grid visuals + QList> _gridSegments; ///< Internal grid line segments including grid exit and turnaround point + QGeoCoordinate _coordinate; + QGeoCoordinate _exitCoordinate; + bool _cameraOrientationFixed; double _surveyDistance; int _cameraShots; @@ -208,6 +223,8 @@ private: SettingsFact _turnaroundDistFact; SettingsFact _cameraTriggerFact; SettingsFact _cameraTriggerDistanceFact; + SettingsFact _cameraTriggerInTurnaroundFact; + SettingsFact _hoverAndCaptureFact; SettingsFact _groundResolutionFact; SettingsFact _frontalOverlapFact; SettingsFact _sideOverlapFact; @@ -229,6 +246,8 @@ private: static const char* _jsonTurnaroundDistKey; static const char* _jsonCameraTriggerKey; static const char* _jsonCameraTriggerDistanceKey; + static const char* _jsonCameraTriggerInTurnaroundKey; + static const char* _jsonHoverAndCaptureKey; static const char* _jsonGroundResolutionKey; static const char* _jsonFrontalOverlapKey; static const char* _jsonSideOverlapKey; diff --git a/src/PlanView/SurveyItemEditor.qml b/src/PlanView/SurveyItemEditor.qml index 65f69f05cace16a36c644fb82c343074d9e0a2e9..59bc9633665c0b1a7438b82bce35c6b3b42920b5 100644 --- a/src/PlanView/SurveyItemEditor.qml +++ b/src/PlanView/SurveyItemEditor.qml @@ -180,40 +180,73 @@ Rectangle { spacing: _margin SectionHeader { + id: cameraHeader text: qsTr("Camera") showSpacer: false } - QGCComboBox { - id: gridTypeCombo + Column { anchors.left: parent.left anchors.right: parent.right - model: _cameraList - currentIndex: -1 - - onActivated: { - if (index == _gridTypeManual) { - missionItem.manualGrid.value = true - } else if (index == _gridTypeCustomCamera) { - missionItem.manualGrid.value = false - missionItem.camera.value = gridTypeCombo.textAt(index) - missionItem.cameraOrientationFixed = false - } else { - missionItem.manualGrid.value = false - missionItem.camera.value = gridTypeCombo.textAt(index) - _noCameraValueRecalc = true - var listIndex = index - _gridTypeCamera - missionItem.cameraSensorWidth.rawValue = _vehicleCameraList[listIndex].sensorWidth - missionItem.cameraSensorHeight.rawValue = _vehicleCameraList[listIndex].sensorHeight - missionItem.cameraResolutionWidth.rawValue = _vehicleCameraList[listIndex].imageWidth - missionItem.cameraResolutionHeight.rawValue = _vehicleCameraList[listIndex].imageHeight - missionItem.cameraFocalLength.rawValue = _vehicleCameraList[listIndex].focalLength - missionItem.cameraOrientationLandscape.rawValue = _vehicleCameraList[listIndex].landscape ? 1 : 0 - missionItem.cameraOrientationFixed = _vehicleCameraList[listIndex].fixedOrientation - _noCameraValueRecalc = false - recalcFromCameraValues() + spacing: _margin + visible: cameraHeader.checked + + QGCComboBox { + id: gridTypeCombo + anchors.left: parent.left + anchors.right: parent.right + model: _cameraList + currentIndex: -1 + + onActivated: { + if (index == _gridTypeManual) { + missionItem.manualGrid.value = true + } else if (index == _gridTypeCustomCamera) { + missionItem.manualGrid.value = false + missionItem.camera.value = gridTypeCombo.textAt(index) + missionItem.cameraOrientationFixed = false + } else { + missionItem.manualGrid.value = false + missionItem.camera.value = gridTypeCombo.textAt(index) + _noCameraValueRecalc = true + var listIndex = index - _gridTypeCamera + missionItem.cameraSensorWidth.rawValue = _vehicleCameraList[listIndex].sensorWidth + missionItem.cameraSensorHeight.rawValue = _vehicleCameraList[listIndex].sensorHeight + missionItem.cameraResolutionWidth.rawValue = _vehicleCameraList[listIndex].imageWidth + missionItem.cameraResolutionHeight.rawValue = _vehicleCameraList[listIndex].imageHeight + missionItem.cameraFocalLength.rawValue = _vehicleCameraList[listIndex].focalLength + missionItem.cameraOrientationLandscape.rawValue = _vehicleCameraList[listIndex].landscape ? 1 : 0 + missionItem.cameraOrientationFixed = _vehicleCameraList[listIndex].fixedOrientation + _noCameraValueRecalc = false + recalcFromCameraValues() + } + } + } + + RowLayout { + anchors.left: parent.left + anchors.right: parent.right + spacing: _margin + visible: missionItem.manualGrid.value == true + + FactCheckBox { + anchors.baseline: cameraTriggerDistanceField.baseline + text: qsTr("Trigger Distance") + fact: missionItem.cameraTrigger + } + + FactTextField { + id: cameraTriggerDistanceField + Layout.fillWidth: true + fact: missionItem.cameraTriggerDistance + enabled: missionItem.cameraTrigger.value } } + + FactCheckBox { + text: qsTr("Hover and capture image") + fact: missionItem.hoverAndCapture + } } // Camera based grid ui @@ -343,7 +376,10 @@ Rectangle { } } - SectionHeader { text: qsTr("Grid") } + SectionHeader { + id: gridHeader + text: qsTr("Grid") + } GridLayout { anchors.left: parent.left @@ -351,6 +387,7 @@ Rectangle { columnSpacing: _margin rowSpacing: _margin columns: 2 + visible: gridHeader.checked QGCLabel { text: qsTr("Angle") } FactTextField { @@ -403,13 +440,17 @@ Rectangle { } // Manual grid ui + SectionHeader { + id: manualGridHeader + text: qsTr("Grid") + visible: gridTypeCombo.currentIndex == _gridTypeManual + } + Column { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: gridTypeCombo.currentIndex == _gridTypeManual - - SectionHeader { text: qsTr("Grid") } + visible: manualGridHeader.visible && manualGridHeader.checked FactTextFieldGrid { anchors.left: parent.left @@ -420,50 +461,21 @@ Rectangle { factLabels: [ qsTr("Angle"), qsTr("Spacing"), qsTr("Altitude"), qsTr("Turnaround dist")] } - Item { height: _margin; width: 1; visible: !ScreenTools.isTinyScreen } - FactCheckBox { anchors.left: parent.left text: qsTr("Relative altitude") fact: missionItem.gridAltitudeRelative } - - Item { height: _sectionSpacer; width: 1; visible: !ScreenTools.isTinyScreen } - - QGCLabel { text: qsTr("Camera") } - - Rectangle { - anchors.left: parent.left - anchors.right: parent.right - height: 1 - color: qgcPal.text - } - - RowLayout { - anchors.left: parent.left - anchors.right: parent.right - spacing: _margin - - FactCheckBox { - anchors.baseline: cameraTriggerDistanceField.baseline - text: qsTr("Trigger Distance") - fact: missionItem.cameraTrigger - } - - FactTextField { - id: cameraTriggerDistanceField - Layout.fillWidth: true - fact: missionItem.cameraTriggerDistance - enabled: missionItem.cameraTrigger.value - } - } } - SectionHeader { text: qsTr("Statistics") } + SectionHeader { + id: statsHeader + text: qsTr("Statistics") } Grid { columns: 2 columnSpacing: ScreenTools.defaultFontPixelWidth + visible: statsHeader.checked QGCLabel { text: qsTr("Survey area") } QGCLabel { text: QGroundControl.squareMetersToAppSettingsAreaUnits(missionItem.coveredArea).toFixed(2) + " " + QGroundControl.appSettingsAreaUnitsString }