From eaedc20a8ef9938d98491a980a6f7a068d72cfd6 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 6 Apr 2017 10:44:34 -0700 Subject: [PATCH] Support for reply survey at 90 degrees offset --- src/MissionManager/SurveyMissionItem.cc | 189 ++++++++++++++++-------- src/MissionManager/SurveyMissionItem.h | 22 ++- src/PlanView/SurveyItemEditor.qml | 13 ++ 3 files changed, 155 insertions(+), 69 deletions(-) diff --git a/src/MissionManager/SurveyMissionItem.cc b/src/MissionManager/SurveyMissionItem.cc index 268232c7c..1478a2975 100644 --- a/src/MissionManager/SurveyMissionItem.cc +++ b/src/MissionManager/SurveyMissionItem.cc @@ -45,6 +45,7 @@ const char* SurveyMissionItem::_jsonCameraNameKey = "name"; const char* SurveyMissionItem::_jsonManualGridKey = "manualGrid"; const char* SurveyMissionItem::_jsonCameraOrientationLandscapeKey = "orientationLandscape"; const char* SurveyMissionItem::_jsonFixedValueIsAltitudeKey = "fixedValueIsAltitude"; +const char* SurveyMissionItem::_jsonRefly90DegreesKey = "refly90Degrees"; const char* SurveyMissionItem::settingsGroup = "Survey"; const char* SurveyMissionItem::manualGridName = "ManualGrid"; @@ -75,6 +76,7 @@ SurveyMissionItem::SurveyMissionItem(Vehicle* vehicle, QObject* parent) , _dirty(false) , _cameraOrientationFixed(false) , _missionCommandCount(0) + , _refly90Degrees(false) , _surveyDistance(0.0) , _cameraShots(0) , _coveredArea(0.0) @@ -109,13 +111,14 @@ 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(&_cameraTriggerInTurnaroundFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_hoverAndCaptureFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); - connect(&_cameraTriggerFact, &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(&_cameraTriggerFact, &Fact::valueChanged, this, &SurveyMissionItem::_generateGrid); + connect(this, &SurveyMissionItem::refly90DegreesChanged, this, &SurveyMissionItem::_generateGrid); connect(&_gridAltitudeFact, &Fact::valueChanged, this, &SurveyMissionItem::_updateCoordinateAltitude); @@ -298,6 +301,7 @@ void SurveyMissionItem::save(QJsonArray& missionItems) saveObject[_jsonManualGridKey] = _manualGridFact.rawValue().toBool(); saveObject[_jsonFixedValueIsAltitudeKey] = _fixedValueIsAltitudeFact.rawValue().toBool(); saveObject[_jsonHoverAndCaptureKey] = _hoverAndCaptureFact.rawValue().toBool(); + saveObject[_jsonRefly90DegreesKey] = _refly90Degrees; if (_cameraTriggerFact.rawValue().toBool()) { saveObject[_jsonCameraTriggerDistanceKey] = _cameraTriggerDistanceFact.rawValue().toDouble(); @@ -389,6 +393,7 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumbe { _jsonManualGridKey, QJsonValue::Bool, true }, { _jsonFixedValueIsAltitudeKey, QJsonValue::Bool, true }, { _jsonHoverAndCaptureKey, QJsonValue::Bool, false }, + { _jsonRefly90DegreesKey, QJsonValue::Bool, false }, }; if (!JsonHelper::validateKeys(v2Object, mainKeyInfoList, errorString)) { return false; @@ -411,6 +416,8 @@ bool SurveyMissionItem::load(const QJsonObject& complexObject, int sequenceNumbe _gridAltitudeRelativeFact.setRawValue (v2Object[_jsonGridAltitudeRelativeKey].toBool(true)); _hoverAndCaptureFact.setRawValue (v2Object[_jsonHoverAndCaptureKey].toBool(false)); + _refly90Degrees = v2Object[_jsonRefly90DegreesKey].toBool(false); + QList gridKeyInfoList = { { _jsonGridAltitudeKey, QJsonValue::Double, true }, { _jsonGridAltitudeRelativeKey, QJsonValue::Bool, true }, @@ -538,6 +545,37 @@ void _calcCameraShots() } +void SurveyMissionItem::_convertTransectToGeo(const QList>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList>& transectSegmentsGeo) +{ + transectSegmentsGeo.clear(); + + for (int i=0; i transectCoords; + const QList& transectPoints = transectSegmentsNED[i]; + + for (int j=0; j& pointsNED, const QGeoCoordinate& tangentOrigin, QVariantList& pointsGeo) +{ + pointsGeo.clear(); + + for (int i=0; i polygonPoints; QList gridPoints; @@ -573,45 +612,34 @@ void SurveyMissionItem::_generateGrid(void) _setCoveredArea(0.5 * fabs(coveredArea)); // Generate grid - _gridGenerator(polygonPoints, gridPoints, transectSegments); + int cameraShots = 0; + cameraShots += _gridGenerator(polygonPoints, gridPoints, transectSegments, false /* refly */); + _convertPointsToGeo(gridPoints, tangentOrigin, _simpleGridPoints); + _convertTransectToGeo(transectSegments, tangentOrigin, _transectSegments); + if (_refly90Degrees) { + QVariantList reflyPointsGeo; + + gridPoints.clear(); + transectSegments.clear(); + cameraShots += _gridGenerator(polygonPoints, gridPoints, transectSegments, true /* refly */); + _convertPointsToGeo(gridPoints, tangentOrigin, reflyPointsGeo); + _convertTransectToGeo(transectSegments, tangentOrigin, _reflyTransectSegments); + _simpleGridPoints.append(reflyPointsGeo); + } - // Convert simple grid to QGeoCoordinates + // Calc survey distance double surveyDistance = 0.0; - for (int i=0; i(); + QGeoCoordinate coord2 = _simpleGridPoints[i].value(); + surveyDistance += coord1.distanceTo(coord2); } _setSurveyDistance(surveyDistance); - // Convert transect segments to QGeoCoordinate - for (int i=0; i transectCoords; - const QList& transectPoints = transectSegments[i]; - - for (int j=0; j& lineList, QLis } } -void SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QList& simpleGridPoints, QList>& transectSegments) +int SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QList& simpleGridPoints, QList>& transectSegments, bool refly) { - double gridAngle = _gridAngleFact.rawValue().toDouble(); + int cameraShots = 0; + + double gridAngle = _gridAngleFact.rawValue().toDouble() + (refly ? 90 : 0); double gridSpacing = _gridSpacingFact.rawValue().toDouble(); qCDebug(SurveyMissionItemLog) << "SurveyMissionItem::_gridGenerator gridSpacing:gridAngle" << gridSpacing << gridAngle; @@ -827,11 +859,9 @@ void SurveyMissionItem::_gridGenerator(const QList& polygonPoints, QLi // Calc camera shots here if there are no images in turnaround if (_triggerCamera() && !_imagesEverywhere()) { - int cameraShots = 0; for (int i=0; i& polygonPoints, QLi transectSegments.append(transectPoints); } + + return cameraShots; } int SurveyMissionItem::_appendWaypointToMission(QList& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent) @@ -961,13 +993,21 @@ bool SurveyMissionItem::_nextTransectCoord(const QList& transect return true; } -void SurveyMissionItem::appendMissionItems(QList& items, QObject* missionItemParent) +/// Appends the mission items for the survey +/// @param items Mission items are appended to this list +/// @param missionItemParent Parent object for newly created MissionItem objects +/// @param seqNum[in,out] Sequence number to start from +/// @param hasRefly true: misison has a refly section +/// @param buildRefly: true: build the refly section, false: build the first section +/// @return false: Generation failed +bool SurveyMissionItem::_appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly) { - int seqNum = _sequenceNumber; + qCDebug(SurveyMissionItemLog) << "hasTurnaround:triggerCamera:hoverAndCapture:imagesEverywhere:hasRefly:buildRefly" << _hasTurnaround() << _triggerCamera() << _hoverAndCaptureEnabled() << _imagesEverywhere() << hasRefly << buildRefly; - qCDebug(SurveyMissionItemLog) << "hasTurnaround:triggerCamera:hoverAndCapture:imagesEverywhere" << _hasTurnaround() << _triggerCamera() << _hoverAndCaptureEnabled() << _imagesEverywhere(); + QList>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments; - if (_imagesEverywhere()) { + if (!buildRefly && _imagesEverywhere()) { + // We are taking images in turnaround, so we start command once at beginning MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, MAV_FRAME_MISSION, @@ -979,52 +1019,52 @@ void SurveyMissionItem::appendMissionItems(QList& items, QObject* items.append(item); } - for (int segmentIndex=0; segmentIndex<_transectSegments.count(); segmentIndex++) { + for (int segmentIndex=0; segmentIndex& transectSegment = _transectSegments[segmentIndex]; + const QList& segment = transectSegments[segmentIndex]; - qCDebug(SurveyMissionItemLog) << "transectSegment.count" << transectSegment.count(); + qCDebug(SurveyMissionItemLog) << "segment.count" << segment.count(); if (_hasTurnaround()) { // Add entry turnaround point - if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) { - return; + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; } seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent); } // Add polygon entry point - if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) { - return; + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; } cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerHoverAndCapture : CameraTriggerOn); seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); // Add internal hover and capture points if (_hoverAndCaptureEnabled()) { - int lastHoverAndCaptureIndex = transectSegment.count() - 1 - (_hasTurnaround() ? 1 : 0); + int lastHoverAndCaptureIndex = segment.count() - 1 - (_hasTurnaround() ? 1 : 0); qCDebug(SurveyMissionItemLog) << "lastHoverAndCaptureIndex" << lastHoverAndCaptureIndex; for (; pointIndex < lastHoverAndCaptureIndex; pointIndex++) { - if (!_nextTransectCoord(transectSegment, pointIndex, coord)) { - return; + if (!_nextTransectCoord(segment, pointIndex, coord)) { + return false; } seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerHoverAndCapture, missionItemParent); } } // Add polygon exit point - if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) { - return; + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; } cameraTrigger = _imagesEverywhere() || !_triggerCamera() ? CameraTriggerNone : (_hoverAndCaptureEnabled() ? CameraTriggerNone : CameraTriggerOff); seqNum = _appendWaypointToMission(items, seqNum, coord, cameraTrigger, missionItemParent); if (_hasTurnaround()) { // Add exit turnaround point - if (!_nextTransectCoord(transectSegment, pointIndex++, coord)) { - return; + if (!_nextTransectCoord(segment, pointIndex++, coord)) { + return false; } seqNum = _appendWaypointToMission(items, seqNum, coord, CameraTriggerNone, missionItemParent); } @@ -1032,7 +1072,7 @@ void SurveyMissionItem::appendMissionItems(QList& items, QObject* qCDebug(SurveyMissionItemLog) << "last PointIndex" << pointIndex; } - if (_imagesEverywhere()) { + if (((hasRefly && buildRefly) || !hasRefly) && _imagesEverywhere()) { // Turn off camera at end of survey MissionItem* item = new MissionItem(seqNum++, MAV_CMD_DO_SET_CAM_TRIGG_DIST, @@ -1044,6 +1084,21 @@ void SurveyMissionItem::appendMissionItems(QList& items, QObject* missionItemParent); items.append(item); } + + return true; +} + +void SurveyMissionItem::appendMissionItems(QList& items, QObject* missionItemParent) +{ + int seqNum = _sequenceNumber; + + if (!_appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, false /* buildRefly */)) { + return; + } + + if (_refly90Degrees) { + _appendMissionItemsWorker(items, missionItemParent, seqNum, _refly90Degrees, true /* buildRefly */); + } } int SurveyMissionItem::cameraShots(void) const @@ -1113,3 +1168,11 @@ void SurveyMissionItem::applyNewAltitude(double newAltitude) { _gridAltitudeFact.setRawValue(newAltitude); } + +void SurveyMissionItem::setRefly90Degrees(bool refly90Degrees) +{ + if (refly90Degrees != _refly90Degrees) { + _refly90Degrees = refly90Degrees; + emit refly90DegreesChanged(refly90Degrees); + } +} diff --git a/src/MissionManager/SurveyMissionItem.h b/src/MissionManager/SurveyMissionItem.h index 98aeddb45..0eda14042 100644 --- a/src/MissionManager/SurveyMissionItem.h +++ b/src/MissionManager/SurveyMissionItem.h @@ -49,6 +49,7 @@ public: Q_PROPERTY(bool cameraOrientationFixed MEMBER _cameraOrientationFixed NOTIFY cameraOrientationFixedChanged) Q_PROPERTY(bool hoverAndCaptureAllowed READ hoverAndCaptureAllowed CONSTANT) + Q_PROPERTY(bool refly90Degrees READ refly90Degrees WRITE setRefly90Degrees NOTIFY refly90DegreesChanged) Q_PROPERTY(double timeBetweenShots READ timeBetweenShots NOTIFY timeBetweenShotsChanged) Q_PROPERTY(QVariantList polygonPath READ polygonPath NOTIFY polygonPathChanged) @@ -95,10 +96,13 @@ public: Fact* fixedValueIsAltitude (void) { return &_fixedValueIsAltitudeFact; } Fact* camera (void) { return &_cameraFact; } - int cameraShots(void) const; - double coveredArea(void) const { return _coveredArea; } - double timeBetweenShots(void) const; - bool hoverAndCaptureAllowed(void) const; + int cameraShots (void) const; + double coveredArea (void) const { return _coveredArea; } + double timeBetweenShots (void) const; + bool hoverAndCaptureAllowed (void) const; + bool refly90Degrees (void) const { return _refly90Degrees; } + + void setRefly90Degrees(bool refly90Degrees); // Overrides from ComplexMissionItem @@ -108,7 +112,6 @@ public: double greatestDistanceTo (const QGeoCoordinate &other) const final; QString mapVisualQML (void) const final { return QStringLiteral("SurveyMapVisual.qml"); } - // Overrides from VisualMissionItem bool dirty (void) const final { return _dirty; } @@ -172,6 +175,7 @@ signals: void gridTypeChanged (QString gridType); void timeBetweenShotsChanged (void); void cameraOrientationFixedChanged (bool cameraOrientationFixed); + void refly90DegreesChanged (bool refly90Degrees); private slots: void _setDirty(void); @@ -189,7 +193,7 @@ private: void _clearGrid(void); void _generateGrid(void); void _updateCoordinateAltitude(void); - void _gridGenerator(const QList& polygonPoints, QList& simpleGridPoints, QList>& transectSegments); + int _gridGenerator(const QList& polygonPoints, QList& simpleGridPoints, QList>& transectSegments, bool refly); 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); @@ -206,6 +210,9 @@ private: bool _hoverAndCaptureEnabled(void) const; bool _hasTurnaround(void) const; double _turnaroundDistance(void) const; + void _convertTransectToGeo(const QList>& transectSegmentsNED, const QGeoCoordinate& tangentOrigin, QList>& transectSegmentsGeo); + void _convertPointsToGeo(const QList& pointsNED, const QGeoCoordinate& tangentOrigin, QVariantList& pointsGeo); + bool _appendMissionItemsWorker(QList& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly); int _sequenceNumber; bool _dirty; @@ -213,10 +220,12 @@ private: QmlObjectListModel _polygonModel; QVariantList _simpleGridPoints; ///< Grid points for drawing simple grid visuals QList> _transectSegments; ///< Internal transect segments including grid exit, turnaround and internal camera points + QList> _reflyTransectSegments; ///< Refly segments QGeoCoordinate _coordinate; QGeoCoordinate _exitCoordinate; bool _cameraOrientationFixed; int _missionCommandCount; + bool _refly90Degrees; double _surveyDistance; int _cameraShots; @@ -272,6 +281,7 @@ private: static const char* _jsonCameraNameKey; static const char* _jsonCameraOrientationLandscapeKey; static const char* _jsonFixedValueIsAltitudeKey; + static const char* _jsonRefly90DegreesKey; }; #endif diff --git a/src/PlanView/SurveyItemEditor.qml b/src/PlanView/SurveyItemEditor.qml index 5aeb40574..d4360f0fa 100644 --- a/src/PlanView/SurveyItemEditor.qml +++ b/src/PlanView/SurveyItemEditor.qml @@ -402,6 +402,13 @@ Rectangle { Layout.fillWidth: true } + QGCCheckBox { + text: qsTr("Refly at 90 degree offset") + checked: missionItem.refly90Degrees + onClicked: missionItem.refly90Degrees = checked + Layout.columnSpan: 2 + } + QGCLabel { wrapMode: Text.WordWrap font.pointSize: ScreenTools.smallFontPointSize @@ -462,6 +469,12 @@ Rectangle { factLabels: [ qsTr("Angle"), qsTr("Spacing"), qsTr("Altitude"), qsTr("Turnaround dist")] } + QGCCheckBox { + text: qsTr("Refly at 90 degree offset") + checked: missionItem.refly90Degrees + onClicked: missionItem.refly90Degrees = checked + } + FactCheckBox { anchors.left: parent.left text: qsTr("Relative altitude") -- 2.22.0