diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 16d17199e2f28e4ce82f1ad8df85ef0015d784ce..10909b627d54c618ffbdc05b81b61f2f63c74147 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -221,6 +221,7 @@ src/WimaView/WimaCorridorMapVisual.qml src/WimaView/WimaMeasurementAreaEditor.qml src/WimaView/SphericalSurveyMapVisual.qml + src/PlanView/CircularSurveyItemEditor.qml src/Settings/APMMavlinkStreamRate.SettingsGroup.json @@ -266,6 +267,7 @@ src/Vehicle/WindFact.json src/Settings/Video.SettingsGroup.json src/Wima/WimaMeasurementArea.SettingsGroup.json + src/Wima/CircularSurvey.SettingsGroup.json src/comm/APMArduCopterMockLink.params diff --git a/src/PlanView/CircularSurveyItemEditor.qml b/src/PlanView/CircularSurveyItemEditor.qml new file mode 100644 index 0000000000000000000000000000000000000000..a7ae505d73f2d164c0ff44ec67a32cc3fc5a8a55 --- /dev/null +++ b/src/PlanView/CircularSurveyItemEditor.qml @@ -0,0 +1,209 @@ +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.4 +import QtQuick.Dialogs 1.2 +import QtQuick.Extras 1.4 +import QtQuick.Layouts 1.2 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Vehicle 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.FlightMap 1.0 + +Rectangle { + id: _root + height: visible ? (editorColumn.height + (_margin * 2)) : 0 + width: availableWidth + color: qgcPal.windowShadeDark + radius: _radius + + // The following properties must be available up the hierarchy chain + //property real availableWidth ///< Width for control + //property var missionItem ///< Mission Item for editor + + property real _margin: ScreenTools.defaultFontPixelWidth / 2 + property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5 + property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle + property real _cameraMinTriggerInterval: missionItem.cameraCalc.minTriggerInterval.rawValue + + function polygonCaptureStarted() { + missionItem.clearPolygon() + } + + function polygonCaptureFinished(coordinates) { + for (var i=0; i Circle::approximateSektor(double angleDiscretisation, double alph } // check if input is valid - if ( fabs(angleDiscretisation) > fabs(deltaAlpha) - || qFuzzyIsNull(angleDiscretisation)) + if ( qFuzzyIsNull(angleDiscretisation)) return QList(); diff --git a/src/Wima/CircularSurvey.SettingsGroup.json b/src/Wima/CircularSurvey.SettingsGroup.json new file mode 100644 index 0000000000000000000000000000000000000000..3b74e7374c4f54339fa29ffb3dc0e515a55d9db4 --- /dev/null +++ b/src/Wima/CircularSurvey.SettingsGroup.json @@ -0,0 +1,20 @@ +[ +{ + "name": "DeltaR", + "shortDescription": "The distance between two consecutive circles.", + "type": "double", + "units": "m", + "min": 0.3, + "decimalPlaces": 1, + "defaultValue": 3 +}, +{ + "name": "DeltaAlpha", + "shortDescription": "Angle discretisation of the circles.", + "type": "double", + "units": "Deg", + "min": 0.3, + "decimalPlaces": 1, + "defaultValue": 0.5 +} +] diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index 0c776075649a82d5e4e4477dc7a6cb950958943d..590041a92981f7bda95586a7f879656f8d67370d 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -2,13 +2,24 @@ const char* CircularSurveyComplexItem::settingsGroup = "Survey"; +const char* CircularSurveyComplexItem::deltaRName = "DeltaR"; +const char* CircularSurveyComplexItem::deltaAlphaName = "DeltaAlpha"; CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, QObject *parent) - : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) - , _referencePoint(QGeoCoordinate(47.770859, 16.531076,0)) - + : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) + , _referencePoint (QGeoCoordinate(47.770859, 16.531076,0)) + , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)) + , _deltaR (settingsGroup, _metaDataMap[deltaRName]) + , _deltaAlpha (settingsGroup, _metaDataMap[deltaAlphaName]) { + _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml"; + + //connect(&_deltaR, &Fact::valueChanged, this, &CircularSurveyComplexItem::_setDirty); + //connect(&_deltaAlpha, &Fact::valueChanged, this, &CircularSurveyComplexItem::_setDirty); + + connect(&_updateTimer, &QTimer::timeout, this, &CircularSurveyComplexItem::_updateItem); + _updateTimer.start(100); } void CircularSurveyComplexItem::setRefPoint(const QGeoCoordinate &refPt) @@ -25,6 +36,16 @@ QGeoCoordinate CircularSurveyComplexItem::refPoint() const return _referencePoint; } +Fact *CircularSurveyComplexItem::deltaR() +{ + return &_deltaR; +} + +Fact *CircularSurveyComplexItem::deltaAlpha() +{ + return &_deltaAlpha; +} + bool CircularSurveyComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) { return false; @@ -75,8 +96,17 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() QVector distances; for (const QPointF &p : surveyPolygon) distances.append(norm(p)); - double dalpha = 2.1/180*M_PI; // radiants - double dr = 3; // meter + // check if input is valid + if ( _deltaAlpha.rawValue() > _deltaAlpha.rawMax() + && _deltaAlpha.rawValue() < _deltaAlpha.rawMin()) + return; + if ( _deltaR.rawValue() > _deltaR.rawMax() + && _deltaR.rawValue() < _deltaR.rawMin()) + return; + + + double dalpha = _deltaAlpha.rawValue().toDouble()/180*M_PI; // radiants + double dr = _deltaR.rawValue().toDouble(); // meter double r_min = dr; // meter double r_max = (*std::max_element(distances.begin(), distances.end())); // meter @@ -88,16 +118,16 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() for (const QPointF &p : surveyPolygon) angles.append(truncateAngle(angle(p))); // determine r_min by successive approximation - double r = r_max - dr; - while ( r > r_min) { + double r = r_min; + while ( r < r_max) { Circle circle(r, origin); - if (!intersects(circle, surveyPolygon)) { + if (intersects(circle, surveyPolygon)) { r_min = r; break; } - r -= dr; + r += dr; } originInside = false; } @@ -176,14 +206,14 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() double alpha1 = angle(entryPoints[k]); double alpha2 = angle(exitPoints[(k+offset) % entryPoints.size()]); - QList sectorPath = circle.approximateSektor(dalpha, alpha1, alpha2); + QList sectorPath = circle.approximateSektor(double(dalpha), alpha1, alpha2); // use shortestPath() here if necessary, could be a problem if dr >> if (sectorPath.size() > 0) currPolyPath.append(sectorPath); } } else if (originInside) { // circle fully inside polygon - QList sectorPath = circle.approximateSektor(dalpha, 0, 2*M_PI); + QList sectorPath = circle.approximateSektor(double(dalpha), 0, 2*M_PI); // use shortestPath() here if necessary, could be a problem if dr >> currPolyPath.append(sectorPath); } @@ -195,6 +225,8 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() } // optimize path to lawn pattern + if (fullPath.size() == 0) + return; QList currentSection = fullPath.takeFirst(); QList> optiPath; // optimized path while( !fullPath.empty() ) { @@ -249,6 +281,15 @@ void CircularSurveyComplexItem::_recalcCameraShots() } +void CircularSurveyComplexItem::_updateItem() +{ + if (_dirty) { + _rebuildTransects(); + setDirty(false); + } + +} + /*! \class CircularSurveyComplexItem \inmodule Wima diff --git a/src/Wima/CircularSurveyComplexItem.h b/src/Wima/CircularSurveyComplexItem.h index 249de0f0f3c0e688bc24ed3b761f031213c24105..014bbba13c3023005c61c32e370e75d7e1aaacb9 100644 --- a/src/Wima/CircularSurveyComplexItem.h +++ b/src/Wima/CircularSurveyComplexItem.h @@ -18,12 +18,16 @@ public: CircularSurveyComplexItem(Vehicle* vehicle, bool flyView, const QString& kmlOrShpFile, QObject* parent); Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY refPointChanged) + Q_PROPERTY(Fact* deltaR READ deltaR CONSTANT) + Q_PROPERTY(Fact* deltaAlpha READ deltaAlpha CONSTANT) // Property setters void setRefPoint(const QGeoCoordinate &refPt); // Property getters - QGeoCoordinate refPoint() const; + QGeoCoordinate refPoint() const; + Fact *deltaR(); + Fact *deltaAlpha(); // Overrides from ComplexMissionItem bool load (const QJsonObject& complexObject, int sequenceNumber, QString& errorString) final; @@ -43,7 +47,9 @@ public: bool readyForSave (void) const final; double additionalTimeDelay (void) const final; - static const char* settingsGroup; + static const char* settingsGroup; + static const char* deltaRName; + static const char* deltaAlphaName; signals: void refPointChanged(); @@ -53,9 +59,19 @@ private slots: void _rebuildTransectsPhase1 (void) final; void _recalcComplexDistance (void) final; void _recalcCameraShots (void) final; + void _updateItem (void); + +signals: private: QGeoCoordinate _referencePoint; // center of the circular lanes, e.g. base station + + QMap _metaDataMap; + + SettingsFact _deltaR; + SettingsFact _deltaAlpha; + + QTimer _updateTimer; }; diff --git a/src/Wima/CircularSurveyToDo.txt b/src/Wima/CircularSurveyToDo.txt deleted file mode 100644 index ea4e175b26a2a32bbbf0f82463071e5671f0502d..0000000000000000000000000000000000000000 --- a/src/Wima/CircularSurveyToDo.txt +++ /dev/null @@ -1 +0,0 @@ -add refpt gui and edit constructor diff --git a/src/Wima/ToDo.txt b/src/Wima/ToDo.txt new file mode 100644 index 0000000000000000000000000000000000000000..9aa7945d050ec483211c2d19e3fb39413855a7f1 --- /dev/null +++ b/src/Wima/ToDo.txt @@ -0,0 +1,4 @@ +add refpt gui and edit constructor +new path optim required for circ survey +add gui elements for circ survey +solve bug for cantFindPath.wima on Desktop/WimaPath diff --git a/src/Wima/doc/SkriptDO.pdf b/src/Wima/doc/SkriptDO.pdf new file mode 100644 index 0000000000000000000000000000000000000000..e1cc81107455b7743d262425f39f9ec454c76976 Binary files /dev/null and b/src/Wima/doc/SkriptDO.pdf differ diff --git a/src/WimaView/SphericalSurveyMapVisual.qml b/src/WimaView/SphericalSurveyMapVisual.qml index 43f23a9e6b6069f7ecaa02686eb90d83d41518ef..f48717c9e5891c3bd88fc6ecd78d003ef50e2959 100644 --- a/src/WimaView/SphericalSurveyMapVisual.qml +++ b/src/WimaView/SphericalSurveyMapVisual.qml @@ -78,12 +78,17 @@ Item { _mapPolygon.appendVertex(topRightCoord) _mapPolygon.appendVertex(bottomRightCoord) _mapPolygon.appendVertex(bottomLeftCoord) - } + } + } + + function _setRefPoint() { + _missionItem.refPoint = map.toCoordinate(map.centerViewport.x, map.centerViewport.y, false /* clipToViewPort */) } Component.onCompleted: { _addInitialPolygon() _addVisualElements() + _setRefPoint() } Component.onDestruction: { @@ -152,4 +157,24 @@ Item { } } } + + // Ref. point (Base Station) + Component { + id: refPointComponent + + MapQuickItem { + anchorPoint.x: sourceItem.anchorPointX + anchorPoint.y: sourceItem.anchorPointY + z: QGroundControl.zOrderMapItems + coordinate: _missionItem.refPoint + visible: _missionItem.refPoint.isValid + + sourceItem: MissionItemIndexLabel { + index: _missionItem.sequenceNumber + label: "Ref." + checked: _missionItem.isCurrentItem + onClicked: _root.clicked(_missionItem.sequenceNumber) + } + } + } }