Commit a378cbb2 authored by Valentin Platzgummer's avatar Valentin Platzgummer

arrivalReturnSpeed added to wimamenau

parent 2a3ae917
#include <QGuiApplication>
#include <QQmlApplicationEngine>
#include "CircularSurveyComplexItem.h"
#include <QQmlContext>
#include <QObject>
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();
}
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
}
}
}
}
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/
<RCC>
<qresource prefix="/">
<file>main.qml</file>
</qresource>
</RCC>
......@@ -220,8 +220,7 @@
<file alias="QGroundControl/Controls/WimaMeasurementAreaMapVisual.qml">src/WimaView/WimaMeasurementAreaMapVisual.qml</file>
<file alias="QGroundControl/Controls/WimaCorridorMapVisual.qml">src/WimaView/WimaCorridorMapVisual.qml</file>
<file alias="QGroundControl/Controls/WimaMeasurementAreaEditor.qml">src/WimaView/WimaMeasurementAreaEditor.qml</file>
<file alias="QGroundControl/Controls/SpericalSurveyMapVisual.qml">src/WimaView/SphericalSurveyMapVisual.qml</file>
<file alias="CircularSurveyItemEditor.qml">src/PlanView/CircularSurveyItemEditor.qml</file>
<file alias="QGroundControl/Controls/CircularSurveyMapVisual.qml">src/PlanView/CircularSurveyItemEditor.qml</file>
<file alias="QGroundControl/Controls/DragCoordinate.qml">src/WimaView/DragCoordinate.qml</file>
<file alias="QGroundControl/Controls/CoordinateIndicatorDrag.qml">src/WimaView/CoordinateIndicatorDrag.qml</file>
<file alias="QGroundControl/Controls/CoordinateIndicator.qml">src/WimaView/CoordinateIndicator.qml</file>
......@@ -230,6 +229,7 @@
<file alias="QGroundControl/FlightMap/WimaPlanMapItems.qml">src/FlightMap/MapItems/WimaPlanMapItems.qml</file>
<file alias="QGroundControl/FlightMap/WimaMissionItemMapVisual.qml">src/PlanView/WimaMissionItemMapVisual.qml</file>
<file alias="QGroundControl/FlightDisplay/FlightDisplayWimaMenu.qml">src/FlightDisplay/FlightDisplayWimaMenu.qml</file>
<file>src/WimaView/CircularSurveyMapVisual.qml</file>
</qresource>
<qresource prefix="/json">
<file alias="APMMavlinkStreamRate.SettingsGroup.json">src/Settings/APMMavlinkStreamRate.SettingsGroup.json</file>
......
......@@ -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
......
......@@ -173,6 +173,19 @@ void QGCMapPolygon::setPath(const QList<QGeoCoordinate>& path)
emit pathChanged();
}
void QGCMapPolygon::setPath(const QVector<QGeoCoordinate>& 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;
......
......@@ -119,6 +119,7 @@ signals:
public slots:
void setPath (const QList<QGeoCoordinate>& path);
void setPath (const QVector<QGeoCoordinate>& path);
void setPath (const QVariantList& path);
void setCenter (QGeoCoordinate newCenter);
void setCenterDrag (bool centerDrag);
......
......@@ -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::microseconds>(std::chrono::high_resolution_clock::now() - startTime).count();
qWarning() << "TransectStyleComplexItem::_rebuildTransects(): time: " << delta << " us";
//CALLGRIND_TOGGLE_COLLECT;
if (_followTerrain) {
......
......@@ -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
......
......@@ -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<MissionItem*>&
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<MissionItem*>&
// 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<QPointF> currentSection = transectPath.takeFirst();
......@@ -619,8 +618,9 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1()
if (_reverse.rawValue().toBool())
PolygonCalculus::reversePath(optiPath);
QList<QGeoCoordinate> geoPath = toGeo(optiPath, _referencePoint);
QVector<QGeoCoordinate> geoPath = toGeo(optiPath, _referencePoint);
QList<CoordInfo_t> 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::microseconds>(std::chrono::high_resolution_clock::now() - startTime).count();
qDebug() << "CircularSurveyComplexItem::_rebuildTransectsPhase1(): time: " << delta << " ms";
//qDebug() << sizeof(QPointF);
}
......
......@@ -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));
......
......@@ -13,19 +13,22 @@ namespace GeoUtilities {
typedef QList<QVector3D> QVector3DList;
typedef QList<QPointF> QPointFList;
typedef QVector<QPointF> QPointFVector;
typedef QVector<QPointF> QPointFVector;
typedef QList<QGeoCoordinate> QGeoList;
typedef QVector<QGeoCoordinate> 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);
}
......@@ -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<QGeoCoordinate> path = toGeo(toQPointFList(joinedPolygon), origin);
QVector<QGeoCoordinate> path = toGeo(toQPointFList(joinedPolygon), origin);
// qWarning("after transform");
// qWarning() << path;
joinedArea.setPath(path);
......
......@@ -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.",
......
This diff is collapsed.
......@@ -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<QGeoCoordinate> &path);
bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path);
/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList
bool extractCoordinateList(QmlObjectListModel &missionItems, QList<QGeoCoordinate> &coordinateList);
bool extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &coordinateList);
/// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList.
bool extractCoordinateList(QmlObjectListModel &missionItems, QList<QGeoCoordinate> &coordinateList, int startIndex, int endIndex);
bool extractCoordinateList(QmlObjectListModel &missionItems, QVector<QGeoCoordinate> &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
*/
......@@ -563,7 +563,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
//_visualItems.append(&_joinedArea);
#endif
QList<QGeoCoordinate> path;
QVector<QGeoCoordinate> 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<QGeoCoordinate> &path)
bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
......
......@@ -111,7 +111,7 @@ public:
QJsonDocument saveToJson(FileType fileType);
bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList<QGeoCoordinate> &path);
bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path);
// static Members
static const char* wimaFileExtension;
......
......@@ -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()
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment