From a378cbb2c6edb602369dabd94e72a31561ea2d49 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Thu, 9 Jan 2020 19:21:32 +0100 Subject: [PATCH] arrivalReturnSpeed added to wimamenau --- .../optimisation/main.cpp | 29 ---- .../optimisation/main.qml | 32 ---- .../optimisation/optimisation.pro | 37 ----- .../optimisation/qml.qrc | 5 - qgroundcontrol.qrc | 4 +- src/FlightDisplay/FlightDisplayWimaMenu.qml | 11 +- src/MissionManager/QGCMapPolygon.cc | 13 ++ src/MissionManager/QGCMapPolygon.h | 1 + .../TransectStyleComplexItem.cc | 7 +- .../QGroundControl.Controls.qmldir | 2 +- src/Wima/CircularSurveyComplexItem.cc | 13 +- src/Wima/GeoUtilities.cc | 16 +- src/Wima/GeoUtilities.h | 11 +- src/Wima/WimaArea.cc | 2 +- src/Wima/WimaController.SettingsGroup.json | 10 +- src/Wima/WimaController.cc | 144 +++++++++++++----- src/Wima/WimaController.h | 46 +++++- src/Wima/WimaPlaner.cc | 4 +- src/Wima/WimaPlaner.h | 2 +- ...Visual.qml => CircularSurveyMapVisual.qml} | 4 +- 20 files changed, 214 insertions(+), 179 deletions(-) delete mode 100644 microProjects/CircularSurveyOptimisation/optimisation/main.cpp delete mode 100644 microProjects/CircularSurveyOptimisation/optimisation/main.qml delete mode 100644 microProjects/CircularSurveyOptimisation/optimisation/optimisation.pro delete mode 100644 microProjects/CircularSurveyOptimisation/optimisation/qml.qrc rename src/WimaView/{SphericalSurveyMapVisual.qml => CircularSurveyMapVisual.qml} (99%) diff --git a/microProjects/CircularSurveyOptimisation/optimisation/main.cpp b/microProjects/CircularSurveyOptimisation/optimisation/main.cpp deleted file mode 100644 index 69d2cfa1f..000000000 --- a/microProjects/CircularSurveyOptimisation/optimisation/main.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include -#include "CircularSurveyComplexItem.h" -#include -#include - -int main(int argc, char *argv[]) -{ - QCoreApplication::setAttribute(Qt::AA_EnableHighDpiScaling); - - QGuiApplication app(argc, argv); - - QQmlApplicationEngine engine; - - CircularSurveyComplexItem survey; - - const QUrl url(QStringLiteral("qrc:/main.qml")); - QObject::connect(&engine, &QQmlApplicationEngine::objectCreated, - &app, [url](QObject *obj, const QUrl &objUrl) { - if (!obj && url == objUrl) - QCoreApplication::exit(-1); - - }, Qt::QueuedConnection); - - engine.rootContext()->setContextProperty("survey", &survey); - engine.load(url); - - return app.exec(); -} diff --git a/microProjects/CircularSurveyOptimisation/optimisation/main.qml b/microProjects/CircularSurveyOptimisation/optimisation/main.qml deleted file mode 100644 index 5a551210c..000000000 --- a/microProjects/CircularSurveyOptimisation/optimisation/main.qml +++ /dev/null @@ -1,32 +0,0 @@ -import QtQuick 2.11 -import QtQuick.Window 2.11 -import QtLocation 5.5 - -import "../../../src/WimaView" - -Window { - visible: true - width: 640 - height: 480 - title: qsTr("Hello World") - - Map{ - id: map - anchors.fill: parent - - plugin: Plugin { - name: "osm" // "mapboxgl", "esri", ... - // specify plugin parameters if necessary - // PluginParameter { - // name: - // value: - // } - } - Repeater{ - model: survey - SphericalSurveyMapVisual{ - map: map - } - } - } -} diff --git a/microProjects/CircularSurveyOptimisation/optimisation/optimisation.pro b/microProjects/CircularSurveyOptimisation/optimisation/optimisation.pro deleted file mode 100644 index bcf4de3d9..000000000 --- a/microProjects/CircularSurveyOptimisation/optimisation/optimisation.pro +++ /dev/null @@ -1,37 +0,0 @@ -QT += quick -QT += positioning - -CONFIG += c++11 - -# The following define makes your compiler emit warnings if you use -# any Qt feature that has been marked deprecated (the exact warnings -# depend on your compiler). Refer to the documentation for the -# deprecated API to know how to port your code away from it. -DEFINES += QT_DEPRECATED_WARNINGS - -# You can also make your code fail to compile if it uses deprecated APIs. -# In order to do so, uncomment the following line. -# You can also select to disable deprecated APIs only up to a certain version of Qt. -#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 - -SOURCES += \ - main.cpp - -RESOURCES += qml.qrc - -# Additional import path used to resolve QML modules in Qt Creator's code model -QML_IMPORT_PATH = $$PWD/../../../src/WimaView - -# Additional import path used to resolve QML modules just for Qt Quick Designer -QML_DESIGNER_IMPORT_PATH = - -# Default rules for deployment. -qnx: target.path = /tmp/$${TARGET}/bin -else: unix:!android: target.path = /opt/$${TARGET}/bin -!isEmpty(target.path): INSTALLS += target - -INCLUDEPATH += $$PWD/../../../src/Wima -INCLUDEPATH += $$PWD/../../../src/MissionManager -INCLUDEPATH += $$PWD/../../../src/comm -INCLUDEPATH += $$PWD/../../../libs/mavlink/include/mavlink/v2.0/ - diff --git a/microProjects/CircularSurveyOptimisation/optimisation/qml.qrc b/microProjects/CircularSurveyOptimisation/optimisation/qml.qrc deleted file mode 100644 index 5f6483ac3..000000000 --- a/microProjects/CircularSurveyOptimisation/optimisation/qml.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - main.qml - - diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 91d97879e..fb874cc97 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -220,8 +220,7 @@ src/WimaView/WimaMeasurementAreaMapVisual.qml src/WimaView/WimaCorridorMapVisual.qml src/WimaView/WimaMeasurementAreaEditor.qml - src/WimaView/SphericalSurveyMapVisual.qml - src/PlanView/CircularSurveyItemEditor.qml + src/PlanView/CircularSurveyItemEditor.qml src/WimaView/DragCoordinate.qml src/WimaView/CoordinateIndicatorDrag.qml src/WimaView/CoordinateIndicator.qml @@ -230,6 +229,7 @@ src/FlightMap/MapItems/WimaPlanMapItems.qml src/PlanView/WimaMissionItemMapVisual.qml src/FlightDisplay/FlightDisplayWimaMenu.qml + src/WimaView/CircularSurveyMapVisual.qml src/Settings/APMMavlinkStreamRate.SettingsGroup.json diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index d84b73865..21578b413 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -225,7 +225,7 @@ Item { width: parent.width QGCLabel { - text: qsTr("Speed") + text: qsTr("Phase Speed") Layout.fillWidth: true } FactTextField { @@ -233,6 +233,15 @@ Item { Layout.fillWidth: true } + QGCLabel { + text: qsTr("Arrival and Return Speed") + Layout.fillWidth: true + } + FactTextField { + fact: wimaController.arrivalReturnSpeed + Layout.fillWidth: true + } + QGCLabel { text: qsTr("Altitude") Layout.fillWidth: true diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index 3ac733b71..83b2e4d04 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -173,6 +173,19 @@ void QGCMapPolygon::setPath(const QList& path) emit pathChanged(); } +void QGCMapPolygon::setPath(const QVector& path) +{ + _polygonPath.clear(); + _polygonModel.clearAndDeleteContents(); + for(const QGeoCoordinate& coord: path) { + _polygonPath.append(QVariant::fromValue(coord)); + _polygonModel.append(new QGCQGeoCoordinate(coord, this)); + } + + setDirty(true); + emit pathChanged(); +} + void QGCMapPolygon::setPath(const QVariantList& path) { _polygonPath = path; diff --git a/src/MissionManager/QGCMapPolygon.h b/src/MissionManager/QGCMapPolygon.h index b6651588b..803cf70ad 100644 --- a/src/MissionManager/QGCMapPolygon.h +++ b/src/MissionManager/QGCMapPolygon.h @@ -119,6 +119,7 @@ signals: public slots: void setPath (const QList& path); + void setPath (const QVector& path); void setPath (const QVariantList& path); void setCenter (QGeoCoordinate newCenter); void setCenterDrag (bool centerDrag); diff --git a/src/MissionManager/TransectStyleComplexItem.cc b/src/MissionManager/TransectStyleComplexItem.cc index 8b2204ffd..36ec81206 100644 --- a/src/MissionManager/TransectStyleComplexItem.cc +++ b/src/MissionManager/TransectStyleComplexItem.cc @@ -357,8 +357,11 @@ void TransectStyleComplexItem::_rebuildTransects(void) return; } - //CALLGRIND_TOGGLE_COLLECT; - _rebuildTransectsPhase1(); + //CALLGRIND_TOGGLE_COLLECT; + auto startTime = std::chrono::high_resolution_clock::now(); + _rebuildTransectsPhase1(); + auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - startTime).count(); + qWarning() << "TransectStyleComplexItem::_rebuildTransects(): time: " << delta << " us"; //CALLGRIND_TOGGLE_COLLECT; if (_followTerrain) { diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index e78fa1814..b4bf5211b 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -96,7 +96,7 @@ WimaCorridorMapVisual 1.0 WimaCorridorMapVisual.qml WimaItemEditor 1.0 WimaItemEditor.qml WimaMapPolygonVisuals 1.0 WimaMapPolygonVisuals.qml WimaMapPolylineVisuals 1.0 WimaMapPolylineVisuals.qml -SphericalSurveyMapVisual 1.0 SphericalSurveyMapVisual.qml +CircularSurveyMapVisual 1.0 CircularSurveyMapVisual.qml DragCoordinate 1.0 DragCoordinate.qml CoordinateIndicator 1.0 CoordinateIndicator.qml CoordinateIndicatorDrag 1.0 CoordinateIndicatorDrag.qml diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index c31ba470b..121a1061e 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -35,6 +35,7 @@ CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyV , _referencePointBeingChanged (false) , _updateCounter (0) { + Q_UNUSED(kmlOrShpFile) _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml"; connect(&_deltaR, &Fact::valueChanged, this, &CircularSurveyComplexItem::_rebuildTransects); connect(&_deltaAlpha, &Fact::valueChanged, this, &CircularSurveyComplexItem::_rebuildTransects); @@ -231,7 +232,7 @@ void CircularSurveyComplexItem::_buildAndAppendMissionItems(QList& int seqNum = _sequenceNumber; // bool imagesEverywhere = _cameraTriggerInTurnAroundFact.rawValue().toBool(); // bool addTriggerAtBeginning = !hoverAndCaptureEnabled() && imagesEverywhere; - bool firstOverallPoint = true; + //bool firstOverallPoint = true; MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; @@ -283,7 +284,7 @@ void CircularSurveyComplexItem::_buildAndAppendMissionItems(QList& // missionItemParent); // items.append(item); // } - firstOverallPoint = false; + //firstOverallPoint = false; // // Possibly add trigger start/stop to survey area entrance/exit // if (triggerCamera() && !hoverAndCaptureEnabled() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) { @@ -365,7 +366,6 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() using namespace GeoUtilities; using namespace PolygonCalculus; using namespace PlanimetryCalculus; - auto startTime = std::chrono::high_resolution_clock::now(); // rebuild not necessary? if (!_isInitialized || _referencePointBeingChanged) @@ -576,7 +576,6 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() if (transectPath.size() == 0) return; - // optimize path to snake or zig-zag pattern bool isSnakePattern = _isSnakePath.rawValue().toBool(); QVector currentSection = transectPath.takeFirst(); @@ -619,8 +618,9 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() if (_reverse.rawValue().toBool()) PolygonCalculus::reversePath(optiPath); - QList geoPath = toGeo(optiPath, _referencePoint); + QVector geoPath = toGeo(optiPath, _referencePoint); QList transectList; + transectList.reserve(optiPath.size()); for ( const QGeoCoordinate &coordinate : geoPath) { CoordInfo_t coordinfo = {coordinate, CoordTypeInterior}; transectList.append(coordinfo); @@ -628,9 +628,6 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() _transects.append(transectList); qDebug() << "CircularSurveyComplexItem::_rebuildTransectsPhase1(): calls: " << _updateCounter; - auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - startTime).count(); - qDebug() << "CircularSurveyComplexItem::_rebuildTransectsPhase1(): time: " << delta << " ms"; - //qDebug() << sizeof(QPointF); } diff --git a/src/Wima/GeoUtilities.cc b/src/Wima/GeoUtilities.cc index 86eabe995..7372f848e 100644 --- a/src/Wima/GeoUtilities.cc +++ b/src/Wima/GeoUtilities.cc @@ -27,9 +27,10 @@ namespace GeoUtilities { return point; } - QGeoList toGeo(const QVector3DList &points, const QGeoCoordinate &origin) + QGeoVector toGeo(const QVector3DList &points, const QGeoCoordinate &origin) { - QGeoList coordinates; + QGeoVector coordinates; + coordinates.reserve(points.size()); for (auto point : points) coordinates.append(toGeo(point, origin)); @@ -67,6 +68,7 @@ namespace GeoUtilities { QPointFList toCartesian2D(const QGeoList &coordinates, const QGeoCoordinate &origin) { QPointFList listF; + listF.reserve(coordinates.size()); for ( auto coordinate : coordinates) listF.append(toCartesian2D(coordinate, origin)); @@ -92,18 +94,20 @@ namespace GeoUtilities { return QGeoCoordinate(); // singularity occurred (1/cos(pi/2) = inf) } - QGeoList toGeo(const QPointFList &points, const QGeoCoordinate &origin) + QGeoVector toGeo(const QPointFList &points, const QGeoCoordinate &origin) { - QGeoList coordinates; + QGeoVector coordinates; + coordinates.reserve(points.size()); for ( auto point : points) coordinates.append(toGeo(point, origin)); return coordinates; } - QGeoList toGeo(const QPointFVector &points, const QGeoCoordinate &origin) + QGeoVector toGeo(const QPointFVector &points, const QGeoCoordinate &origin) { - QGeoList coordinates; + QGeoVector coordinates; + coordinates.reserve(points.size()); for ( auto point : points) coordinates.append(toGeo(point, origin)); diff --git a/src/Wima/GeoUtilities.h b/src/Wima/GeoUtilities.h index 0cf3b3ad9..60566b9f7 100644 --- a/src/Wima/GeoUtilities.h +++ b/src/Wima/GeoUtilities.h @@ -13,19 +13,22 @@ namespace GeoUtilities { typedef QList QVector3DList; typedef QList QPointFList; - typedef QVector QPointFVector; + typedef QVector QPointFVector; typedef QList QGeoList; + typedef QVector QGeoVector; const double earthRadius = 6378137; // meter QGeoCoordinate toGeo (const QVector3D &point, const QGeoCoordinate &origin); - QGeoList toGeo (const QVector3DList &points, const QGeoCoordinate &origin); + QGeoVector toGeo (const QVector3DList &points, const QGeoCoordinate &origin); QGeoCoordinate toGeo (const QPointF &point, const QGeoCoordinate &origin); - QGeoList toGeo (const QPointFList &points, const QGeoCoordinate &origin); - QGeoList toGeo (const QPointFVector &points, const QGeoCoordinate &origin); + QGeoVector toGeo (const QPointFList &points, const QGeoCoordinate &origin); + QGeoVector toGeo (const QPointFVector &points, const QGeoCoordinate &origin); QVector3D toCartesian (const QGeoCoordinate &coordinate, const QGeoCoordinate &origin); QVector3DList toCartesian (const QGeoList &coordinates, const QGeoCoordinate &origin); + QVector3DList toCartesian (const QGeoVector &coordinates, const QGeoCoordinate &origin); QPointF toCartesian2D (const QGeoCoordinate &point, const QGeoCoordinate &origin); QPointFList toCartesian2D (const QGeoList &coordinates, const QGeoCoordinate &origin); + QPointFList toCartesian2D (const QGeoVector &coordinates, const QGeoCoordinate &origin); } diff --git a/src/Wima/WimaArea.cc b/src/Wima/WimaArea.cc index 5f8e19607..31dd51754 100644 --- a/src/Wima/WimaArea.cc +++ b/src/Wima/WimaArea.cc @@ -267,7 +267,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, WimaArea &join } else if (retValue == JoinPolygonError::PathSizeLow) { qWarning("Polygon vertex count is low."); } else { - QList path = toGeo(toQPointFList(joinedPolygon), origin); + QVector path = toGeo(toQPointFList(joinedPolygon), origin); // qWarning("after transform"); // qWarning() << path; joinedArea.setPath(path); diff --git a/src/Wima/WimaController.SettingsGroup.json b/src/Wima/WimaController.SettingsGroup.json index de38c201b..149c57682 100644 --- a/src/Wima/WimaController.SettingsGroup.json +++ b/src/Wima/WimaController.SettingsGroup.json @@ -39,12 +39,20 @@ }, { "name": "FlightSpeed", - "shortDescription": "The mission flight speed.", + "shortDescription": "The phase flight speed.", "type": "double", "min" : 0.3, "max" : 5, "defaultValue": 2 }, +{ + "name": "ArrivalReturnSpeed", + "shortDescription": "The flight speed for arrival and return path.", + "type": "double", + "min" : 0.3, + "max" : 5, + "defaultValue": 5 +}, { "name": "Altitude", "shortDescription": "The mission altitude.", diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index a320207ed..9c140f4fe 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -11,6 +11,7 @@ const char* WimaController::startWaypointIndexName = "StartWaypointIndex"; const char* WimaController::showAllMissionItemsName = "ShowAllMissionItems"; const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems"; const char* WimaController::flightSpeedName = "FlightSpeed"; +const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed"; const char* WimaController::altitudeName = "Altitude"; const char* WimaController::reverseName = "Reverse"; @@ -30,6 +31,7 @@ WimaController::WimaController(QObject *parent) , _showAllMissionItems (settingsGroup, _metaDataMap[showAllMissionItemsName]) , _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName]) , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) + , _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName]) , _altitude (settingsGroup, _metaDataMap[altitudeName]) , _reverse (settingsGroup, _metaDataMap[reverseName]) , _endWaypointIndex (0) @@ -48,7 +50,7 @@ WimaController::WimaController(QObject *parent) connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::updateNextWaypoint); connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::recalcCurrentPhase); connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase); - connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::updateSpeed); + connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::updateflightSpeed); connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::updateAltitude); connect(&_reverse, &Fact::rawValueChanged, this, &WimaController::reverseChangedHandler); @@ -137,6 +139,11 @@ Fact *WimaController::flightSpeed() return &_flightSpeed; } +Fact *WimaController::arrivalReturnSpeed() +{ + return &_arrivalReturnSpeed; +} + Fact *WimaController::altitude() { return &_altitude; @@ -369,7 +376,7 @@ QJsonDocument WimaController::saveToJson(FileType fileType) return QJsonDocument(); } -bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path) +bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { using namespace GeoUtilities; using namespace PolygonCalculus; @@ -384,12 +391,12 @@ bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoo return retVal; } -bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList) +bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVector &coordinateList) { return extractCoordinateList(missionItems, coordinateList, 0, missionItems.count()-1); } -bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList, int startIndex, int endIndex) +bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVector &coordinateList, int startIndex, int endIndex) { if ( startIndex >= 0 && startIndex < missionItems.count() @@ -424,7 +431,7 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex) { - QList geoCoordintateList; + QVector geoCoordintateList; bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex); @@ -594,10 +601,7 @@ bool WimaController::calcNextPhase() } _startWaypointIndex = startIndex; - - int maxWaypointsPerPhase = _maxWaypointsPerPhase.rawValue().toInt(); - // determine end waypoint index bool lastMissionPhaseReached = false; if (!reverse) { @@ -613,25 +617,25 @@ bool WimaController::calcNextPhase() // extract waypoints - QList geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems + QVector CSWpList; // list with potential waypoints (from _missionItems), for _currentMissionItems if (!reverse) { - if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex, _endWaypointIndex)) { + if (!extractCoordinateList(_missionItems, CSWpList, _startWaypointIndex, _endWaypointIndex)) { qWarning("WimaController::calcNextPhase(): error on waypoint extraction."); return false; } } else { - if (!extractCoordinateList(_missionItems, geoCoordinateList, _endWaypointIndex, _startWaypointIndex)) { + if (!extractCoordinateList(_missionItems, CSWpList, _endWaypointIndex, _startWaypointIndex)) { qWarning("WimaController::calcNextPhase(): error on waypoint extraction."); return false; } // reverse path - QList reversePath; - for (QGeoCoordinate c : geoCoordinateList) + QVector reversePath; + for (QGeoCoordinate c : CSWpList) reversePath.prepend(c); - geoCoordinateList.clear(); - geoCoordinateList.append(reversePath); + CSWpList.clear(); + CSWpList.append(reversePath); } @@ -652,27 +656,27 @@ bool WimaController::calcNextPhase() } // calculate path from home to first waypoint - QList path; + QVector arrivalPath; if (!_takeoffLandPostion.isValid()){ qWarning("WimaController::calcNextPhase(): _takeoffLandPostion not valid."); return false; } - if ( !calcShortestPath(_takeoffLandPostion, geoCoordinateList[0], path) ) { + if ( !calcShortestPath(_takeoffLandPostion, CSWpList.first(), arrivalPath) ) { qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint."); return false; } - // prepend to geoCoordinateList - for (int i = path.size()-2; i >= 0; i--) // i = path.size()-2 : last coordinate already in geoCoordinateList - geoCoordinateList.prepend(path[i]); + arrivalPath.removeFirst(); + arrivalPath.removeLast(); // calculate path from last waypoint to home - path.clear(); - if ( !calcShortestPath(geoCoordinateList.last(), _takeoffLandPostion, path) ) { + QVector returnPath; + if ( !calcShortestPath(CSWpList.last(), _takeoffLandPostion, returnPath) ) { qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint."); return false; - } - path.removeFirst(); // first coordinate already in geoCoordinateList - geoCoordinateList.append(path); + } + int returnPathLength = returnPath.size()-2; + returnPath.removeFirst(); + returnPath.removeLast(); // create Mission Items @@ -687,11 +691,8 @@ bool WimaController::calcNextPhase() } settingsItem->setCoordinate(_takeoffLandPostion); - for (auto coordinate : geoCoordinateList) { - _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); - } - // set takeoff position for first mission item (bug) + missionController()->insertSimpleMissionItem(_takeoffLandPostion, 1); SimpleMissionItem *takeoffItem = missionControllerVisuals->value(1); if (takeoffItem == nullptr) { qWarning("WimaController::calcNextPhase(): nullptr"); @@ -699,7 +700,7 @@ bool WimaController::calcNextPhase() } takeoffItem->setCoordinate(_takeoffLandPostion); - // create change speed item + // create change speed item, after take off _missionController->insertSimpleMissionItem(_takeoffLandPostion, 2); SimpleMissionItem *speedItem = missionControllerVisuals->value(2); if (speedItem == nullptr) { @@ -708,11 +709,48 @@ bool WimaController::calcNextPhase() } speedItem->setCoordinate(_takeoffLandPostion); speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); + speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble()); + + // insert arrival path + for (auto coordinate : arrivalPath) + _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); + + // create change speed item, after arrival path + int index = missionControllerVisuals->count(); + _missionController->insertSimpleMissionItem(CSWpList.first(), index); + speedItem = missionControllerVisuals->value(index); + if (speedItem == nullptr) { + qWarning("WimaController::calcNextPhase(): nullptr"); + return false; + } + speedItem->setCoordinate(CSWpList.first()); + speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble()); + // insert Circular Survey coordinates + for (auto coordinate : CSWpList) + _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); + + // create change speed item, after circular survey + index = missionControllerVisuals->count(); + _missionController->insertSimpleMissionItem(CSWpList.last(), index); + speedItem = missionControllerVisuals->value(index); + if (speedItem == nullptr) { + qWarning("WimaController::calcNextPhase(): nullptr"); + return false; + } + speedItem->setCoordinate(_takeoffLandPostion); + speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); + speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble()); + + // insert return path coordinates + for (auto coordinate : returnPath) + _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); // set land command for last mission item - SimpleMissionItem *landItem = missionControllerVisuals->value(missionControllerVisuals->count()-1); + index = missionControllerVisuals->count(); + _missionController->insertSimpleMissionItem(_takeoffLandPostion, index); + SimpleMissionItem *landItem = missionControllerVisuals->value(index); if (landItem == nullptr) { qWarning("WimaController::calcNextPhase(): nullptr"); return false; @@ -784,16 +822,44 @@ bool WimaController::setTakeoffLandPosition() return true; } -void WimaController::updateSpeed() +void WimaController::updateflightSpeed() { - SimpleMissionItem *item = _currentMissionItems.value(1); // speed item - if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) { - item->missionItem().setParam2(_flightSpeed.rawValue().toDouble()); - _setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()); + int speedItemCounter = 0; + for (int i = 0; i < _currentMissionItems.count(); i++) { + SimpleMissionItem *item = _currentMissionItems.value(i); + if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) { + speedItemCounter++; + if (speedItemCounter == 2) { + item->missionItem().setParam2(_flightSpeed.rawValue().toDouble()); + _setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()); + break; + } + } } - else { - qWarning("WimaController::updateSpeed(): internal error."); + + if (speedItemCounter != 3) + qWarning("WimaController::updateflightSpeed(): internal error."); +} + +void WimaController::updateArrivalReturnSpeed() +{ + int speedItemCounter = 0; + for (int i = 0; i < _currentMissionItems.count(); i++) { + SimpleMissionItem *item = _currentMissionItems.value(i); + if (item != nullptr && item->command() == MAV_CMD_DO_CHANGE_SPEED) { + speedItemCounter++; + if (speedItemCounter != 2) { + item->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble()); + } + + if (speedItemCounter == 3) + break; + } } + + if (speedItemCounter != 3) + qWarning("WimaController::updateArrivalReturnSpeed(): internal error."); + } void WimaController::updateAltitude() @@ -923,7 +989,7 @@ bool WimaController::_calcReturnPath(QString &errorSring) } // calculate return path - QList returnPath; + QVector returnPath; calcShortestPath(currentVehiclePosition, _takeoffLandPostion, returnPath); // successful? if (returnPath.isEmpty()) { diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 152f0d73b..4657d430f 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -55,6 +55,7 @@ public: Q_PROPERTY(Fact* showCurrentMissionItems READ showCurrentMissionItems CONSTANT) Q_PROPERTY(Fact* flightSpeed READ flightSpeed CONSTANT) Q_PROPERTY(Fact* altitude READ altitude CONSTANT) + Q_PROPERTY(Fact* arrivalReturnSpeed READ arrivalReturnSpeed CONSTANT) Q_PROPERTY(Fact* reverse READ reverse CONSTANT) Q_PROPERTY(bool uploadOverrideRequired READ uploadOverrideRequired WRITE setUploadOverrideRequired NOTIFY uploadOverrideRequiredChanged) Q_PROPERTY(double phaseDistance READ phaseDistance NOTIFY phaseDistanceChanged) @@ -84,6 +85,7 @@ public: Fact* showAllMissionItems (void); Fact* showCurrentMissionItems(void); Fact* flightSpeed (void); + Fact* arrivalReturnSpeed (void); Fact* altitude (void); Fact* reverse (void); bool uploadOverrideRequired (void) const; @@ -130,17 +132,18 @@ public: static const char* showAllMissionItemsName; static const char* showCurrentMissionItemsName; static const char* flightSpeedName; + static const char* arrivalReturnSpeedName; static const char* altitudeName; static const char* reverseName; // Member Methodes QJsonDocument saveToJson(FileType fileType); - bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path); + bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path); /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList - bool extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList); + bool extractCoordinateList(QmlObjectListModel &missionItems, QVector &coordinateList); /// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList. - bool extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList, int startIndex, int endIndex); + bool extractCoordinateList(QmlObjectListModel &missionItems, QVector &coordinateList, int startIndex, int endIndex); /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList); /// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList. @@ -172,7 +175,8 @@ private slots: void updateNextWaypoint (void); void recalcCurrentPhase (void); bool setTakeoffLandPosition (void); - void updateSpeed (void); + void updateflightSpeed (void); + void updateArrivalReturnSpeed (void); void updateAltitude (void); void checkBatteryLevel (void); void smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL @@ -218,12 +222,13 @@ private: SettingsFact _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not. SettingsFact _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not. SettingsFact _flightSpeed; // mission flight speed + SettingsFact _arrivalReturnSpeed; // arrival and return path speed SettingsFact _altitude; // mission altitude SettingsFact _reverse; // Reverses the phase direction. Phases go from high to low waypoint numbers, if true. - int _endWaypointIndex; // indes of the mission item stored in _missionItems defining the last element + int _endWaypointIndex; // index of the mission item stored in _missionItems defining the last element // (which is not part of the return path) of _currentMissionItem - int _startWaypointIndex; // indes of the mission item stored in _missionItems defining the first element + int _startWaypointIndex; // index of the mission item stored in _missionItems defining the first element // (which is not part of the arrival path) of _currentMissionItem bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area. // The user can override the upload lock with a slider, this will reset this variable to false. @@ -238,4 +243,33 @@ private: }; +/* + * The following explains the structure of + * _missionController.visualItems(). The indices + * are not that important and only specified for + * reasons of completeness. + * + * Index Description + * -------------------------------------------- + * 0 MissionSettingsItem + * 1 Takeoff Command + * 2 Speed Command: arrivalReturnSpeed + * 3 Arrival Path Waypoint 0 + * ... + * 3+n-1 Arrival Path Waypoint n-1 + * 3+n Speed Command: flightSpeed + * 3+n+1 Circular Survey Waypoint 0 + * ... + * 3+n+m Circular Survey Waypoint m-1 + * 3+n+m+1 Speed Command: arrivalReturnSpeed + * 3+n+m+2 Return Path Waypoint 0 + * ... + * 3+n+m+2+l Return Path Waypoint l-1 + * 3+n+m+2+l+1 Land command + * + * _currentMissionItems is equal to + * _missionController.visualItems() except that it + * is missing the MissionSettingsItem + */ + diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 910c15153..814fff11c 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -563,7 +563,7 @@ bool WimaPlaner::calcArrivalAndReturnPath() //_visualItems.append(&_joinedArea); #endif - QList path; + QVector path; if ( !calcShortestPath(start, end, path)) { qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data()); return false; @@ -667,7 +667,7 @@ void WimaPlaner::pushToContainer() } } -bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path) +bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { using namespace GeoUtilities; using namespace PolygonCalculus; diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index f916d7710..fde1c40ed 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -111,7 +111,7 @@ public: QJsonDocument saveToJson(FileType fileType); - bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path); + bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path); // static Members static const char* wimaFileExtension; diff --git a/src/WimaView/SphericalSurveyMapVisual.qml b/src/WimaView/CircularSurveyMapVisual.qml similarity index 99% rename from src/WimaView/SphericalSurveyMapVisual.qml rename to src/WimaView/CircularSurveyMapVisual.qml index 8f3677b5c..7c592f2ba 100644 --- a/src/WimaView/SphericalSurveyMapVisual.qml +++ b/src/WimaView/CircularSurveyMapVisual.qml @@ -82,7 +82,7 @@ Item { _mapPolygon.appendVertex(topRightCoord) _mapPolygon.appendVertex(bottomRightCoord) _mapPolygon.appendVertex(bottomLeftCoord) - } + } } function _setRefPoint() { @@ -91,7 +91,7 @@ Item { Component.onCompleted: { if ( !_missionItem.isInitialized ) { - _addInitialPolygon() + _addInitialPolygon() _missionItem.isInitialized = true // set isInitialized to true, to trigger _rebuildTransectsPhase1 in the last line _setRefPoint() } -- 2.22.0