Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
46adaf36
Commit
46adaf36
authored
Sep 25, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
wima planer, wima controller edited
parent
912e993b
Changes
20
Show whitespace changes
Inline
Side-by-side
Showing
20 changed files
with
348 additions
and
371 deletions
+348
-371
CircularSurveyItemEditor.qml
src/PlanView/CircularSurveyItemEditor.qml
+12
-5
CSWorker.cpp
src/Wima/CSWorker.cpp
+13
-9
CircularSurvey.h
src/Wima/CircularSurvey.h
+1
-1
WimaServiceArea.cc
src/Wima/Geometry/WimaServiceArea.cc
+9
-5
WimaServiceArea.h
src/Wima/Geometry/WimaServiceArea.h
+1
-1
WimaServiceAreaData.cc
src/Wima/Geometry/WimaServiceAreaData.cc
+41
-81
WimaServiceAreaData.h
src/Wima/Geometry/WimaServiceAreaData.h
+20
-26
WimaBridge.cc
src/Wima/WimaBridge.cc
+19
-40
WimaBridge.h
src/Wima/WimaBridge.h
+20
-24
WimaController.cc
src/Wima/WimaController.cc
+4
-4
WimaController.h
src/Wima/WimaController.h
+1
-1
WimaPlanData.cc
src/Wima/WimaPlanData.cc
+56
-71
WimaPlanData.h
src/Wima/WimaPlanData.h
+30
-29
WimaPlaner.cc
src/Wima/WimaPlaner.cc
+24
-25
WimaPlaner.h
src/Wima/WimaPlaner.h
+3
-2
DragCoordinate.qml
src/WimaView/DragCoordinate.qml
+10
-9
WimaMapPolygonVisuals.qml
src/WimaView/WimaMapPolygonVisuals.qml
+38
-16
WimaServiceAreaMapVisual.qml
src/WimaView/WimaServiceAreaMapVisual.qml
+41
-19
WimaToolBar.qml
src/WimaView/WimaToolBar.qml
+4
-2
WimaView.qml
src/WimaView/WimaView.qml
+1
-1
No files found.
src/PlanView/CircularSurveyItemEditor.qml
View file @
46adaf36
...
@@ -174,23 +174,30 @@ Rectangle {
...
@@ -174,23 +174,30 @@ Rectangle {
BusyIndicator
{
BusyIndicator
{
id
:
indicator
id
:
indicator
anchors.horizontalCenter
:
parent
.
horizontalCenter
anchors.horizontalCenter
:
parent
.
horizontalCenter
running
:
missionItem
.
calculating
property
bool
calculating
:
missionItem
.
calculating
running
:
calculating
on
Runn
ingChanged
:
{
on
Calculat
ingChanged
:
{
if
(
runn
ing
){
if
(
calculat
ing
){
visible
=
true
visible
=
true
}
else
{
}
else
{
timer
.
restart
()
timer
.
restart
()
}
}
}
}
Component.onCompleted
:
{
if
(
calculating
){
visible
=
true
}
}
Timer
{
Timer
{
id
:
timer
id
:
timer
interval
:
2
000
interval
:
1
000
repeat
:
false
repeat
:
false
onTriggered
:
{
onTriggered
:
{
if
(
indicator
.
runn
ing
==
false
){
if
(
indicator
.
calculat
ing
==
false
){
indicator
.
visible
=
false
indicator
.
visible
=
false
}
}
}
}
...
...
src/Wima/CSWorker.cpp
View file @
46adaf36
...
@@ -64,6 +64,9 @@ void CSWorker::update(const QGeoCoordinate &depot,
...
@@ -64,6 +64,9 @@ void CSWorker::update(const QGeoCoordinate &depot,
Lock
lk
(
this
->
_mutex
);
Lock
lk
(
this
->
_mutex
);
this
->
_depot
=
depot
;
this
->
_depot
=
depot
;
this
->
_safeArea
=
safeArea
;
this
->
_safeArea
=
safeArea
;
for
(
auto
&
v
:
this
->
_safeArea
)
{
v
.
setAltitude
(
0
);
}
this
->
_polygon
=
polygon
;
this
->
_polygon
=
polygon
;
for
(
auto
&
v
:
this
->
_polygon
)
{
for
(
auto
&
v
:
this
->
_polygon
)
{
v
.
setAltitude
(
0
);
v
.
setAltitude
(
0
);
...
@@ -96,6 +99,7 @@ void CSWorker::run() {
...
@@ -96,6 +99,7 @@ void CSWorker::run() {
depot
=
this
->
_depot
;
depot
=
this
->
_depot
;
safeArea
=
this
->
_safeArea
;
safeArea
=
this
->
_safeArea
;
}
else
{
}
else
{
depot
=
this
->
_origin
;
safeArea
=
this
->
_polygon
;
safeArea
=
this
->
_polygon
;
}
}
const
auto
polygon
=
this
->
_polygon
;
const
auto
polygon
=
this
->
_polygon
;
...
@@ -379,11 +383,11 @@ void CSWorker::run() {
...
@@ -379,11 +383,11 @@ void CSWorker::run() {
}
else
{
}
else
{
// Find index of first waypoint.
// Find index of first waypoint.
std
::
size_t
idxFirst
=
0
;
std
::
size_t
idxFirst
=
0
;
const
auto
&
info
1
=
const
auto
&
info
First
=
transectsInfo
.
front
();
this
->
_useDepotSafeArea
?
transectsInfo
[
1
]
:
transectsInfo
[
0
];
const
auto
&
firstTransect
=
transectsENU
[
infoFirst
.
index
];
const
auto
&
first
Transect
=
transectsENU
[
info1
.
index
];
const
auto
&
first
Waypoint
=
infoFirst
.
reversed
const
auto
&
firstWaypoint
=
?
firstTransect
.
back
()
info1
.
reversed
?
firstTransect
.
back
()
:
firstTransect
.
front
();
:
firstTransect
.
front
();
double
th
=
0.001
;
double
th
=
0.001
;
for
(
std
::
size_t
i
=
0
;
i
<
route
.
size
();
++
i
)
{
for
(
std
::
size_t
i
=
0
;
i
<
route
.
size
();
++
i
)
{
auto
dist
=
bg
::
distance
(
route
[
i
],
firstWaypoint
);
auto
dist
=
bg
::
distance
(
route
[
i
],
firstWaypoint
);
...
@@ -393,11 +397,11 @@ void CSWorker::run() {
...
@@ -393,11 +397,11 @@ void CSWorker::run() {
}
}
}
}
// Find index of last waypoint.
// Find index of last waypoint.
std
::
size_t
idxLast
=
0
;
std
::
size_t
idxLast
=
route
.
size
()
-
1
;
const
auto
&
info
=
transectsInfo
.
back
();
const
auto
&
info
Last
=
transectsInfo
.
back
();
const
auto
&
lastTransect
=
transectsENU
[
info
.
index
];
const
auto
&
lastTransect
=
transectsENU
[
info
Last
.
index
];
const
auto
&
lastWaypoint
=
const
auto
&
lastWaypoint
=
info
.
reversed
?
lastTransect
.
front
()
:
lastTransect
.
back
();
info
Last
.
reversed
?
lastTransect
.
front
()
:
lastTransect
.
back
();
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
for
(
long
i
=
route
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
auto
dist
=
bg
::
distance
(
route
[
i
],
lastWaypoint
);
auto
dist
=
bg
::
distance
(
route
[
i
],
lastWaypoint
);
if
(
dist
<
th
)
{
if
(
dist
<
th
)
{
...
...
src/Wima/CircularSurvey.h
View file @
46adaf36
...
@@ -29,7 +29,7 @@ public:
...
@@ -29,7 +29,7 @@ public:
Q_PROPERTY
(
Fact
*
deltaAlpha
READ
deltaAlpha
CONSTANT
)
Q_PROPERTY
(
Fact
*
deltaAlpha
READ
deltaAlpha
CONSTANT
)
Q_PROPERTY
(
Fact
*
transectMinLength
READ
transectMinLength
CONSTANT
)
Q_PROPERTY
(
Fact
*
transectMinLength
READ
transectMinLength
CONSTANT
)
Q_PROPERTY
(
bool
calculating
READ
calculating
NOTIFY
calculatingChanged
)
Q_PROPERTY
(
bool
calculating
READ
calculating
NOTIFY
calculatingChanged
)
Q_PROPERTY
(
bool
hidePolygon
READ
hidePolygon
NOTIFY
hidePolygon
)
Q_PROPERTY
(
bool
hidePolygon
READ
hidePolygon
NOTIFY
hidePolygon
Changed
)
Q_INVOKABLE
void
resetReference
(
void
);
Q_INVOKABLE
void
resetReference
(
void
);
Q_INVOKABLE
void
reverse
(
void
);
Q_INVOKABLE
void
reverse
(
void
);
...
...
src/Wima/Geometry/WimaServiceArea.cc
View file @
46adaf36
...
@@ -20,11 +20,15 @@ WimaServiceArea &WimaServiceArea::operator=(const WimaServiceArea &other) {
...
@@ -20,11 +20,15 @@ WimaServiceArea &WimaServiceArea::operator=(const WimaServiceArea &other) {
return
*
this
;
return
*
this
;
}
}
void
WimaServiceArea
::
setDepot
(
const
QGeoCoordinate
&
coordinate
)
{
bool
WimaServiceArea
::
setDepot
(
const
QGeoCoordinate
&
coordinate
)
{
if
(
_depot
!=
coordinate
)
{
if
(
_depot
!=
coordinate
)
{
if
(
this
->
containsCoordinate
(
coordinate
))
{
_depot
=
coordinate
;
_depot
=
coordinate
;
emit
depotChanged
();
emit
depotChanged
();
return
true
;
}
}
}
return
false
;
}
}
void
WimaServiceArea
::
saveToJson
(
QJsonObject
&
json
)
{
void
WimaServiceArea
::
saveToJson
(
QJsonObject
&
json
)
{
...
@@ -58,8 +62,8 @@ void print(const WimaServiceArea &area, QString &outputStr) {
...
@@ -58,8 +62,8 @@ void print(const WimaServiceArea &area, QString &outputStr) {
void
WimaServiceArea
::
init
()
{
void
WimaServiceArea
::
init
()
{
this
->
setObjectName
(
wimaServiceAreaName
);
this
->
setObjectName
(
wimaServiceAreaName
);
connect
(
this
,
&
Wima
ServiceArea
::
center
Changed
,
[
this
]
{
connect
(
this
,
&
Wima
Area
::
path
Changed
,
[
this
]
{
if
(
!
this
->
_depot
.
isValid
())
{
if
(
!
this
->
_depot
.
isValid
()
||
!
this
->
containsCoordinate
(
this
->
_depot
)
)
{
this
->
setDepot
(
this
->
center
());
this
->
setDepot
(
this
->
center
());
}
}
});
});
...
...
src/Wima/Geometry/WimaServiceArea.h
View file @
46adaf36
...
@@ -34,7 +34,7 @@ signals:
...
@@ -34,7 +34,7 @@ signals:
void
depotChanged
(
void
);
void
depotChanged
(
void
);
public
slots
:
public
slots
:
void
setDepot
(
const
QGeoCoordinate
&
coordinate
);
bool
setDepot
(
const
QGeoCoordinate
&
coordinate
);
private:
private:
// Member Methodes
// Member Methodes
...
...
src/Wima/Geometry/WimaServiceAreaData.cc
View file @
46adaf36
...
@@ -2,36 +2,32 @@
...
@@ -2,36 +2,32 @@
const
char
*
WimaServiceAreaData
::
typeString
=
"WimaServiceAreaData"
;
const
char
*
WimaServiceAreaData
::
typeString
=
"WimaServiceAreaData"
;
WimaServiceAreaData
::
WimaServiceAreaData
(
QObject
*
parent
)
WimaServiceAreaData
::
WimaServiceAreaData
(
QObject
*
parent
)
:
WimaAreaData
(
parent
)
:
WimaAreaData
(
parent
)
{}
{
}
WimaServiceAreaData
::
WimaServiceAreaData
(
const
WimaServiceAreaData
&
other
,
QObject
*
parent
)
WimaServiceAreaData
::
WimaServiceAreaData
(
const
WimaServiceAreaData
&
other
,
:
WimaAreaData
(
parent
)
QObject
*
parent
)
{
:
WimaAreaData
(
parent
)
{
*
this
=
other
;
*
this
=
other
;
}
}
WimaServiceAreaData
::
WimaServiceAreaData
(
const
WimaServiceArea
&
other
,
QObject
*
parent
)
WimaServiceAreaData
::
WimaServiceAreaData
(
const
WimaServiceArea
&
other
,
:
WimaAreaData
(
parent
)
QObject
*
parent
)
{
:
WimaAreaData
(
parent
)
{
*
this
=
other
;
*
this
=
other
;
}
}
WimaServiceAreaData
&
WimaServiceAreaData
::
operator
=
(
const
WimaServiceAreaData
&
otherData
)
WimaServiceAreaData
&
WimaServiceAreaData
::
{
operator
=
(
const
WimaServiceAreaData
&
otherData
)
{
this
->
assign
(
otherData
);
this
->
assign
(
otherData
);
this
->
setDepot
(
otherData
.
depot
());
return
*
this
;
return
*
this
;
}
}
WimaServiceAreaData
&
WimaServiceAreaData
::
operator
=
(
const
WimaServiceArea
&
otherArea
)
WimaServiceAreaData
&
WimaServiceAreaData
::
{
operator
=
(
const
WimaServiceArea
&
otherArea
)
{
this
->
assign
(
otherArea
);
this
->
assign
(
otherArea
);
this
->
setDepot
(
otherArea
.
depot
());
return
*
this
;
return
*
this
;
}
}
...
@@ -40,77 +36,41 @@ WimaServiceAreaData &WimaServiceAreaData::operator=(const WimaServiceArea &other
...
@@ -40,77 +36,41 @@ WimaServiceAreaData &WimaServiceAreaData::operator=(const WimaServiceArea &other
* Returns a constant reference to the takeOffPosition.
* Returns a constant reference to the takeOffPosition.
*
*
*/
*/
const
QGeoCoordinate
&
WimaServiceAreaData
::
takeOffPosition
()
const
const
QGeoCoordinate
&
WimaServiceAreaData
::
depot
()
const
{
return
_depot
;
}
{
return
_takeOffPosition
;
}
QString
WimaServiceAreaData
::
type
()
const
{
return
this
->
typeString
;
}
/*!
/*!
* \fn const QGeoCoordinate &WimaServiceAreaData::landOffPosition() const
* \fn void WimaServiceAreaData::setTakeOffPosition(const QGeoCoordinate
* Returns a constant reference to the landOffPosition.
* &newCoordinate) Sets the takeoff position to the \a newCoordinate and emits
* the takeOffPositionChanged() signal, if newCoordinate differs from the member
* value.
*
*
*/
*/
const
QGeoCoordinate
&
WimaServiceAreaData
::
landPosition
()
const
void
WimaServiceAreaData
::
setDepot
(
const
QGeoCoordinate
&
newCoordinate
)
{
{
if
(
_depot
!=
newCoordinate
)
{
return
_landPosition
;
_depot
=
newCoordinate
;
}
emit
depotChanged
(
_depot
);
QString
WimaServiceAreaData
::
type
()
const
{
return
this
->
typeString
;
}
/*!
* \fn void WimaServiceAreaData::setTakeOffPosition(const QGeoCoordinate &newCoordinate)
* Sets the takeoff position to the \a newCoordinate and emits the takeOffPositionChanged() signal,
* if newCoordinate differs from the member value.
*
*/
void
WimaServiceAreaData
::
setTakeOffPosition
(
const
QGeoCoordinate
&
newCoordinate
)
{
if
(
_takeOffPosition
!=
newCoordinate
)
{
_takeOffPosition
=
newCoordinate
;
emit
takeOffPositionChanged
(
_takeOffPosition
);
}
}
/*!
* \fn void WimaServiceAreaData::setLandOffPosition(const QGeoCoordinate &newCoordinate)
* Sets the land position to the \a newCoordinate and emits the landOffPositionChanged() signal,
* if newCoordinate differs from the member value.
*
*/
void
WimaServiceAreaData
::
setLandPosition
(
const
QGeoCoordinate
&
newCoordinate
)
{
if
(
_landPosition
!=
newCoordinate
)
{
_landPosition
=
newCoordinate
;
emit
landPositionChanged
(
_landPosition
);
}
}
}
}
void
WimaServiceAreaData
::
assign
(
const
WimaServiceAreaData
&
other
)
void
WimaServiceAreaData
::
assign
(
const
WimaServiceAreaData
&
other
)
{
{
WimaAreaData
::
assign
(
other
);
WimaAreaData
::
assign
(
other
);
setLandPosition
(
other
.
landPosition
());
setDepot
(
other
.
depot
());
setTakeOffPosition
(
other
.
takeOffPosition
());
}
}
void
WimaServiceAreaData
::
assign
(
const
WimaServiceArea
&
other
)
void
WimaServiceAreaData
::
assign
(
const
WimaServiceArea
&
other
)
{
{
WimaAreaData
::
assign
(
other
);
WimaAreaData
::
assign
(
other
);
setLandPosition
(
other
.
landPosition
());
setDepot
(
other
.
depot
());
setTakeOffPosition
(
other
.
depot
());
}
}
/*!
/*!
* \class WimaAreaData::WimaServiceAreaData
* \class WimaAreaData::WimaServiceAreaData
* \brief Class to store and exchange data of a \c WimaServiceArea Object.
* \brief Class to store and exchange data of a \c WimaServiceArea Object.
* Class to store and exchange data of a \c WimaServiceArea Object. In contrast to \c WimaServiceArea this class
* Class to store and exchange data of a \c WimaServiceArea Object. In contrast
* does not provied any interface to a grafical user interface, neiter it uses the QGC Fact System.
* to \c WimaServiceArea this class does not provied any interface to a grafical
* It is designed to exchange data between the \c WimaPlaner and the \c WimaController class. And it
* user interface, neiter it uses the QGC Fact System. It is designed to
* is the derived from WimaAreaData.
* exchange data between the \c WimaPlaner and the \c WimaController class. And
* it is the derived from WimaAreaData.
*
*
* \sa WimaServiceArea, WimaAreaData
* \sa WimaServiceArea, WimaAreaData
*/
*/
src/Wima/Geometry/WimaServiceAreaData.h
View file @
46adaf36
#pragma once
#pragma once
#include <QObject>
#include "QGeoCoordinate"
#include "QGeoCoordinate"
#include <QObject>
#include "WimaAreaData.h"
#include "WimaAreaData.h"
#include "WimaServiceArea.h"
#include "WimaServiceArea.h"
class
WimaServiceAreaData
:
public
WimaAreaData
{
class
WimaServiceAreaData
:
public
WimaAreaData
{
Q_OBJECT
Q_OBJECT
public:
public:
WimaServiceAreaData
(
QObject
*
parent
=
nullptr
);
WimaServiceAreaData
(
QObject
*
parent
=
nullptr
);
WimaServiceAreaData
(
const
WimaServiceAreaData
&
other
,
QObject
*
parent
=
nullptr
);
WimaServiceAreaData
(
const
WimaServiceAreaData
&
other
,
QObject
*
parent
=
nullptr
);
WimaServiceAreaData
(
const
WimaServiceArea
&
other
,
QObject
*
parent
=
nullptr
);
WimaServiceAreaData
(
const
WimaServiceArea
&
other
,
QObject
*
parent
=
nullptr
);
WimaServiceAreaData
&
operator
=
(
const
WimaServiceAreaData
&
otherData
);
WimaServiceAreaData
&
operator
=
(
const
WimaServiceAreaData
&
otherData
);
WimaServiceAreaData
&
operator
=
(
const
WimaServiceArea
&
otherArea
);
WimaServiceAreaData
&
operator
=
(
const
WimaServiceArea
&
otherArea
);
const
QGeoCoordinate
&
takeOffPosition
()
const
;
const
QGeoCoordinate
&
depot
()
const
;
const
QGeoCoordinate
&
landPosition
()
const
;
QString
type
()
const
;
QString
type
()
const
;
WimaServiceAreaData
*
Clone
()
const
{
return
new
WimaServiceAreaData
();
}
WimaServiceAreaData
*
Clone
()
const
{
return
new
WimaServiceAreaData
();
}
static
const
char
*
typeString
;
static
const
char
*
typeString
;
signals:
signals:
void
takeOffPositionChanged
(
const
QGeoCoordinate
&
other
);
void
depotChanged
(
const
QGeoCoordinate
&
other
);
void
landPositionChanged
(
const
QGeoCoordinate
&
other
);
public
slots
:
public
slots
:
void
setTakeOffPosition
(
const
QGeoCoordinate
&
newCoordinate
);
void
setDepot
(
const
QGeoCoordinate
&
newCoordinate
);
void
setLandPosition
(
const
QGeoCoordinate
&
newCoordinate
);
protected:
protected:
void
assign
(
const
WimaServiceAreaData
&
other
);
void
assign
(
const
WimaServiceAreaData
&
other
);
void
assign
(
const
WimaServiceArea
&
other
);
void
assign
(
const
WimaServiceArea
&
other
);
private:
private:
// see WimaServieArea.h for explanation
// see WimaServieArea.h for explanation
QGeoCoordinate
_takeOffPosition
;
QGeoCoordinate
_depot
;
QGeoCoordinate
_landPosition
;
};
};
src/Wima/WimaBridge.cc
View file @
46adaf36
#include "WimaBridge.h"
#include "WimaBridge.h"
#include "WimaController.h"
#include "WimaController.h"
WimaBridge
::
WimaBridge
(
QObject
*
parent
)
WimaBridge
::
WimaBridge
(
QObject
*
parent
)
:
QObject
(
parent
)
{}
:
QObject
(
parent
)
{
}
WimaController
*
WimaBridge
::
wimaController
()
{
return
_wimaController
;
}
WimaController
*
WimaBridge
::
wimaController
()
WimaPlaner
*
WimaBridge
::
wimaPlaner
()
{
return
_wimaPlaner
;
}
{
return
_wimaController
;
}
WimaPlaner
*
WimaBridge
::
wimaPlaner
()
WimaBridge
*
WimaBridge
::
thisPointer
()
{
return
this
;
}
{
return
_wimaPlaner
;
}
WimaBridge
*
WimaBridge
::
thisPointer
()
{
return
this
;
}
void
WimaBridge
::
setWimaController
(
WimaController
*
controller
)
void
WimaBridge
::
setWimaController
(
WimaController
*
controller
)
{
{
if
(
_wimaController
!=
controller
)
{
if
(
_wimaController
!=
controller
){
_wimaController
=
controller
;
_wimaController
=
controller
;
emit
wimaControllerChanged
(
_wimaController
);
emit
wimaControllerChanged
(
_wimaController
);
}
}
}
}
void
WimaBridge
::
setWimaPlaner
(
WimaPlaner
*
planer
)
void
WimaBridge
::
setWimaPlaner
(
WimaPlaner
*
planer
)
{
{
if
(
_wimaPlaner
!=
planer
)
{
if
(
_wimaPlaner
!=
planer
){
_wimaPlaner
=
planer
;
_wimaPlaner
=
planer
;
emit
wimaPlanerChanged
(
_wimaPlaner
);
emit
wimaPlanerChanged
(
_wimaPlaner
);
}
}
}
}
bool
WimaBridge
::
setWimaPlanData
(
const
WimaPlanData
&
planData
)
bool
WimaBridge
::
setWimaPlanData
(
QSharedPointer
<
WimaPlanData
>
planData
)
{
{
if
(
_wimaController
!=
nullptr
)
{
if
(
_wimaController
!=
nullptr
)
{
return
_wimaController
->
setWimaPlanData
(
planData
);
return
_wimaController
->
setWimaPlanData
(
planData
);
}
}
return
false
;
return
false
;
}
}
src/Wima/WimaBridge.h
View file @
46adaf36
#pragma once
#pragma once
#include <QObject>
#include <QObject>
#include <QSharedPointer>
#include "WimaPlanData.h"
#include "WimaPlanData.h"
...
@@ -11,38 +12,33 @@ class WimaPlaner;
...
@@ -11,38 +12,33 @@ class WimaPlaner;
//! \brief The WimaBridge class
//! \brief The WimaBridge class
//!
//!
//! A bridge establishing a link between WimaController and WimaPlaner
//! A bridge establishing a link between WimaController and WimaPlaner
class
WimaBridge
:
public
QObject
class
WimaBridge
:
public
QObject
{
{
Q_OBJECT
Q_OBJECT
public:
public:
WimaBridge
(
QObject
*
parent
=
nullptr
);
WimaBridge
(
QObject
*
parent
=
nullptr
);
WimaBridge
(
WimaBridge
&
other
)
=
delete
;
WimaBridge
(
WimaBridge
&
other
)
=
delete
;
Q_PROPERTY
(
WimaPlaner
*
wimaPlaner
Q_PROPERTY
(
WimaPlaner
*
wimaPlaner
READ
wimaPlaner
WRITE
setWimaPlaner
NOTIFY
READ
wimaPlaner
wimaPlanerChanged
)
WRITE
setWimaPlaner
Q_PROPERTY
(
WimaController
*
wimaController
READ
wimaController
WRITE
NOTIFY
wimaPlanerChanged
)
setWimaController
NOTIFY
wimaControllerChanged
)
Q_PROPERTY
(
WimaController
*
wimaController
READ
wimaController
WRITE
setWimaController
NOTIFY
wimaControllerChanged
)
WimaController
*
wimaController
();
WimaController
*
wimaController
();
WimaPlaner
*
wimaPlaner
();
WimaPlaner
*
wimaPlaner
();
Q_INVOKABLE
WimaBridge
*
thisPointer
();
Q_INVOKABLE
WimaBridge
*
thisPointer
();
void
setWimaController
(
WimaController
*
controller
);
void
setWimaController
(
WimaController
*
controller
);
void
setWimaPlaner
(
WimaPlaner
*
planer
);
void
setWimaPlaner
(
WimaPlaner
*
planer
);
signals:
signals:
void
wimaControllerChanged
(
WimaController
*
controller
);
void
wimaControllerChanged
(
WimaController
*
controller
);
void
wimaPlanerChanged
(
WimaPlaner
*
planer
);
void
wimaPlanerChanged
(
WimaPlaner
*
planer
);
public
slots
:
public
slots
:
bool
setWimaPlanData
(
const
WimaPlanData
&
planData
);
bool
setWimaPlanData
(
QSharedPointer
<
WimaPlanData
>
planData
);
private:
private:
WimaController
*
_wimaController
;
WimaController
*
_wimaController
;
WimaPlaner
*
_wimaPlaner
;
WimaPlaner
*
_wimaPlaner
;
};
};
src/Wima/WimaController.cc
View file @
46adaf36
...
@@ -353,7 +353,7 @@ bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
...
@@ -353,7 +353,7 @@ bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
return
retVal
;
return
retVal
;
}
}
bool
WimaController
::
setWimaPlanData
(
const
WimaPlanData
&
planData
)
{
bool
WimaController
::
setWimaPlanData
(
QSharedPointer
<
WimaPlanData
>
planData
)
{
// reset visual items
// reset visual items
_areas
.
clear
();
_areas
.
clear
();
_defaultWM
.
clear
();
_defaultWM
.
clear
();
...
@@ -368,7 +368,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
...
@@ -368,7 +368,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
_localPlanDataValid
=
false
;
_localPlanDataValid
=
false
;
// extract list with WimaAreas
// extract list with WimaAreas
QList
<
const
WimaAreaData
*>
areaList
=
planData
.
areaList
();
QList
<
const
WimaAreaData
*>
areaList
=
planData
->
areaList
();
int
areaCounter
=
0
;
int
areaCounter
=
0
;
const
int
numAreas
=
4
;
// extract only numAreas Areas, if there are more they
const
int
numAreas
=
4
;
// extract only numAreas Areas, if there are more they
...
@@ -424,7 +424,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
...
@@ -424,7 +424,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
emit
visualItemsChanged
();
emit
visualItemsChanged
();
// extract mission items
// extract mission items
QList
<
MissionItem
>
tempMissionItems
=
planData
.
missionItems
();
QList
<
MissionItem
>
tempMissionItems
=
planData
->
missionItems
();
if
(
tempMissionItems
.
size
()
<
1
)
{
if
(
tempMissionItems
.
size
()
<
1
)
{
qWarning
(
"WimaController: Mission items from WimaPlaner empty!"
);
qWarning
(
"WimaController: Mission items from WimaPlaner empty!"
);
return
false
;
return
false
;
...
@@ -435,7 +435,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
...
@@ -435,7 +435,7 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
}
}
_WMSettings
.
setHomePosition
(
QGeoCoordinate
(
_WMSettings
.
setHomePosition
(
QGeoCoordinate
(
_serviceArea
.
center
().
latitude
(),
_serviceArea
.
center
().
longitude
(),
0
));
_serviceArea
.
depot
().
latitude
(),
_serviceArea
.
depot
().
longitude
(),
0
));
if
(
!
_defaultWM
.
reset
())
{
if
(
!
_defaultWM
.
reset
())
{
Q_ASSERT
(
false
);
Q_ASSERT
(
false
);
...
...
src/Wima/WimaController.h
View file @
46adaf36
...
@@ -148,7 +148,7 @@ public:
...
@@ -148,7 +148,7 @@ public:
// Property setters
// Property setters
void
setMasterController
(
PlanMasterController
*
masterController
);
void
setMasterController
(
PlanMasterController
*
masterController
);
void
setMissionController
(
MissionController
*
missionController
);
void
setMissionController
(
MissionController
*
missionController
);
bool
setWimaPlanData
(
const
WimaPlanData
&
planData
);
bool
setWimaPlanData
(
QSharedPointer
<
WimaPlanData
>
planData
);
// Member Methodes
// Member Methodes
Q_INVOKABLE
WimaController
*
thisPointer
();
Q_INVOKABLE
WimaController
*
thisPointer
();
...
...
src/Wima/WimaPlanData.cc
View file @
46adaf36
#include "WimaPlanData.h"
#include "WimaPlanData.h"
WimaPlanData
::
WimaPlanData
(
QObject
*
parent
)
WimaPlanData
::
WimaPlanData
(
QObject
*
parent
)
:
QObject
(
parent
)
{}
:
QObject
(
parent
)
{
}
WimaPlanData
::
WimaPlanData
(
const
WimaPlanData
&
other
,
QObject
*
parent
)
WimaPlanData
::
WimaPlanData
(
const
WimaPlanData
&
other
,
QObject
*
parent
)
:
QObject
(
parent
)
:
QObject
(
parent
)
{
{
*
this
=
other
;
*
this
=
other
;
}
}
...
@@ -18,22 +13,21 @@ WimaPlanData::WimaPlanData(const WimaPlanData &other, QObject *parent)
...
@@ -18,22 +13,21 @@ WimaPlanData::WimaPlanData(const WimaPlanData &other, QObject *parent)
* Copies the data area list of \a other to the calling \c WimaPlanData object.
* Copies the data area list of \a other to the calling \c WimaPlanData object.
* Returns a reference to the calling \c WimaPlanData object.
* Returns a reference to the calling \c WimaPlanData object.
*/
*/
WimaPlanData
&
WimaPlanData
::
operator
=
(
const
WimaPlanData
&
other
)
WimaPlanData
&
WimaPlanData
::
operator
=
(
const
WimaPlanData
&
other
)
{
{
// copy wima areas
// copy wima areas
QList
<
const
WimaAreaData
*>
areaList
=
other
.
areaList
();
QList
<
const
WimaAreaData
*>
areaList
=
other
.
areaList
();
_areaList
.
clear
();
_areaList
.
clear
();
for
(
int
i
=
0
;
i
<
areaList
.
size
();
i
++
)
{
for
(
int
i
=
0
;
i
<
areaList
.
size
();
i
++
)
{
const
WimaAreaData
*
areaData
=
areaList
[
i
];
const
WimaAreaData
*
areaData
=
areaList
[
i
];
// determine area type and append
// determine area type and append
if
(
areaData
->
type
()
==
WimaJoinedAreaData
::
typeString
)
{
if
(
areaData
->
type
()
==
WimaJoinedAreaData
::
typeString
)
{
this
->
append
(
*
qobject_cast
<
const
WimaJoinedAreaData
*>
(
areaData
));
this
->
append
(
*
qobject_cast
<
const
WimaJoinedAreaData
*>
(
areaData
));
}
else
if
(
areaData
->
type
()
==
WimaServiceAreaData
::
typeString
)
{
}
else
if
(
areaData
->
type
()
==
WimaServiceAreaData
::
typeString
)
{
this
->
append
(
*
qobject_cast
<
const
WimaServiceAreaData
*>
(
areaData
));
this
->
append
(
*
qobject_cast
<
const
WimaServiceAreaData
*>
(
areaData
));
}
else
if
(
areaData
->
type
()
==
WimaMeasurementAreaData
::
typeString
)
{
}
else
if
(
areaData
->
type
()
==
WimaMeasurementAreaData
::
typeString
)
{
this
->
append
(
*
qobject_cast
<
const
WimaMeasurementAreaData
*>
(
areaData
));
this
->
append
(
*
qobject_cast
<
const
WimaMeasurementAreaData
*>
(
areaData
));
}
else
if
(
areaData
->
type
()
==
WimaCorridorData
::
typeString
)
{
}
else
if
(
areaData
->
type
()
==
WimaCorridorData
::
typeString
)
{
this
->
append
(
*
qobject_cast
<
const
WimaCorridorData
*>
(
areaData
));
this
->
append
(
*
qobject_cast
<
const
WimaCorridorData
*>
(
areaData
));
}
}
}
}
...
@@ -43,17 +37,15 @@ WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other)
...
@@ -43,17 +37,15 @@ WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other)
return
*
this
;
return
*
this
;
}
}
/*!
/*!
* \fn void WimaPlanData::append(const WimaAreaData &areaData)
* \fn void WimaPlanData::append(const WimaAreaData &areaData)
*
*
* Adds a WimaAreaData item.
* Adds a WimaAreaData item.
*/
*/
void
WimaPlanData
::
append
(
const
WimaJoinedAreaData
&
areaData
)
void
WimaPlanData
::
append
(
const
WimaJoinedAreaData
&
areaData
)
{
{
_joinedArea
=
areaData
;
_joinedArea
=
areaData
;
if
(
!
_areaList
.
contains
(
&
_joinedArea
)
)
{
if
(
!
_areaList
.
contains
(
&
_joinedArea
)
)
{
_areaList
.
append
(
&
_joinedArea
);
_areaList
.
append
(
&
_joinedArea
);
}
}
}
}
...
@@ -63,11 +55,10 @@ void WimaPlanData::append(const WimaJoinedAreaData &areaData)
...
@@ -63,11 +55,10 @@ void WimaPlanData::append(const WimaJoinedAreaData &areaData)
*
*
* Adds a WimaServiceAreaData item.
* Adds a WimaServiceAreaData item.
*/
*/
void
WimaPlanData
::
append
(
const
WimaServiceAreaData
&
areaData
)
void
WimaPlanData
::
append
(
const
WimaServiceAreaData
&
areaData
)
{
{
_serviceArea
=
areaData
;
_serviceArea
=
areaData
;
if
(
!
_areaList
.
contains
(
&
_serviceArea
)
)
{
if
(
!
_areaList
.
contains
(
&
_serviceArea
)
)
{
_areaList
.
append
(
&
_serviceArea
);
_areaList
.
append
(
&
_serviceArea
);
}
}
}
}
...
@@ -77,11 +68,10 @@ void WimaPlanData::append(const WimaServiceAreaData &areaData)
...
@@ -77,11 +68,10 @@ void WimaPlanData::append(const WimaServiceAreaData &areaData)
*
*
* Adds a WimaCorridorData item.
* Adds a WimaCorridorData item.
*/
*/
void
WimaPlanData
::
append
(
const
WimaCorridorData
&
areaData
)
void
WimaPlanData
::
append
(
const
WimaCorridorData
&
areaData
)
{
{
_corridor
=
areaData
;
_corridor
=
areaData
;
if
(
!
_areaList
.
contains
(
&
_corridor
)
)
{
if
(
!
_areaList
.
contains
(
&
_corridor
)
)
{
_areaList
.
append
(
&
_corridor
);
_areaList
.
append
(
&
_corridor
);
}
}
}
}
...
@@ -91,20 +81,18 @@ void WimaPlanData::append(const WimaCorridorData &areaData)
...
@@ -91,20 +81,18 @@ void WimaPlanData::append(const WimaCorridorData &areaData)
*
*
* Adds a WimaMeasurementAreaData item.
* Adds a WimaMeasurementAreaData item.
*/
*/
void
WimaPlanData
::
append
(
const
WimaMeasurementAreaData
&
areaData
)
void
WimaPlanData
::
append
(
const
WimaMeasurementAreaData
&
areaData
)
{
{
_measurementArea
=
areaData
;
_measurementArea
=
areaData
;
if
(
!
_areaList
.
contains
(
&
_measurementArea
)
)
{
if
(
!
_areaList
.
contains
(
&
_measurementArea
)
)
{
_areaList
.
append
(
&
_measurementArea
);
_areaList
.
append
(
&
_measurementArea
);
}
}
}
}
void
WimaPlanData
::
append
(
const
QList
<
MissionItem
*>
&
missionItems
)
void
WimaPlanData
::
append
(
const
QList
<
MissionItem
*>
&
missionItems
)
{
{
for
(
auto
*
item
:
missionItems
)
{
for
(
MissionItem
*
item
:
missionItems
)
{
item
->
setParent
(
this
);
MissionItem
copy
=
MissionItem
(
*
item
,
this
);
_missionItems
.
append
(
item
);
_missionItems
.
append
(
copy
);
}
}
}
}
...
@@ -113,19 +101,16 @@ void WimaPlanData::append(const QList<MissionItem *> &missionItems)
...
@@ -113,19 +101,16 @@ void WimaPlanData::append(const QList<MissionItem *> &missionItems)
*
*
* Clears all stored objects
* Clears all stored objects
*/
*/
void
WimaPlanData
::
clear
()
void
WimaPlanData
::
clear
()
{
{
_areaList
.
clear
();
_areaList
.
clear
();
_missionItems
.
clear
();
_missionItems
.
clear
();
}
}
QList
<
const
WimaAreaData
*>
WimaPlanData
::
areaList
()
const
const
QList
<
const
WimaAreaData
*>
&
WimaPlanData
::
areaList
()
const
{
{
return
_areaList
;
return
_areaList
;
}
}
QList
<
MissionItem
>
WimaPlanData
::
missionItems
()
const
const
QList
<
MissionItem
>
&
WimaPlanData
::
missionItems
()
const
{
{
return
_missionItems
;
return
_missionItems
;
}
}
...
@@ -135,8 +120,8 @@ QList<MissionItem> WimaPlanData::missionItems() const
...
@@ -135,8 +120,8 @@ QList<MissionItem> WimaPlanData::missionItems() const
*
*
* This class is designed to store data generated by the \c WimaPlaner class and
* This class is designed to store data generated by the \c WimaPlaner class and
* meant for data exchange between the \c WimaController and the \c WimaPlanner.
* meant for data exchange between the \c WimaController and the \c WimaPlanner.
* It stores a QList of \c WimaAreaData objects, called area data list,
containing the data of serveral \c WimaAreas
* It stores a QList of \c WimaAreaData objects, called area data list,
* generated by the \c WimaPlaner.
*
containing the data of serveral \c WimaAreas
generated by the \c WimaPlaner.
*
*
* \sa QList
* \sa QList
*/
*/
src/Wima/WimaPlanData.h
View file @
46adaf36
...
@@ -3,32 +3,33 @@
...
@@ -3,32 +3,33 @@
#include <QObject>
#include <QObject>
#include "Geometry/WimaAreaData.h"
#include "Geometry/WimaAreaData.h"
#include "Geometry/WimaServiceAreaData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaServiceAreaData.h"
#include "MissionItem.h"
#include "MissionItem.h"
class
WimaPlanData
:
QObject
class
WimaPlanData
:
public
QObject
{
{
Q_OBJECT
Q_OBJECT
public:
public:
WimaPlanData
(
QObject
*
parent
=
nullptr
);
WimaPlanData
(
QObject
*
parent
=
nullptr
);
WimaPlanData
(
const
WimaPlanData
&
other
,
QObject
*
parent
=
nullptr
);
WimaPlanData
(
const
WimaPlanData
&
other
,
QObject
*
parent
=
nullptr
);
WimaPlanData
&
operator
=
(
const
WimaPlanData
&
other
);
WimaPlanData
&
operator
=
(
const
WimaPlanData
&
other
);
// Member Methodes
// Member Methodes
void
append
(
const
WimaJoinedAreaData
&
areaData
);
void
append
(
const
WimaJoinedAreaData
&
areaData
);
void
append
(
const
WimaServiceAreaData
&
areaData
);
void
append
(
const
WimaServiceAreaData
&
areaData
);
void
append
(
const
WimaCorridorData
&
areaData
);
void
append
(
const
WimaCorridorData
&
areaData
);
void
append
(
const
WimaMeasurementAreaData
&
areaData
);
void
append
(
const
WimaMeasurementAreaData
&
areaData
);
//!
//! \brief append
//! \param missionItems
//! \note Takes owenership of MissionItems*
void
append
(
const
QList
<
MissionItem
*>
&
missionItems
);
void
append
(
const
QList
<
MissionItem
*>
&
missionItems
);
void
clear
();
void
clear
();
QList
<
const
WimaAreaData
*>
areaList
()
const
;
const
QList
<
const
WimaAreaData
*>
&
areaList
()
const
;
QList
<
MissionItem
>
missionItems
()
const
;
const
QList
<
MissionItem
>
&
missionItems
()
const
;
signals:
signals:
void
areaListChanged
();
void
areaListChanged
();
...
@@ -41,7 +42,7 @@ private:
...
@@ -41,7 +42,7 @@ private:
WimaServiceAreaData
_serviceArea
;
WimaServiceAreaData
_serviceArea
;
WimaCorridorData
_corridor
;
WimaCorridorData
_corridor
;
WimaMeasurementAreaData
_measurementArea
;
WimaMeasurementAreaData
_measurementArea
;
QList
<
const
WimaAreaData
*>
_areaList
;
QList
<
const
WimaAreaData
*>
_areaList
;
QList
<
MissionItem
>
_missionItems
;
QList
<
MissionItem
>
_missionItems
;
};
};
src/Wima/WimaPlaner.cc
View file @
46adaf36
...
@@ -25,7 +25,7 @@ WimaPlaner::WimaPlaner(QObject *parent)
...
@@ -25,7 +25,7 @@ WimaPlaner::WimaPlaner(QObject *parent)
_currentAreaIndex
(
-
1
),
_wimaBridge
(
nullptr
),
_mAreaChanged
(
true
),
_currentAreaIndex
(
-
1
),
_wimaBridge
(
nullptr
),
_mAreaChanged
(
true
),
_sAreaChanged
(
true
),
_corridorChanged
(
true
),
_joinedArea
(
this
),
_sAreaChanged
(
true
),
_corridorChanged
(
true
),
_joinedArea
(
this
),
_arrivalPathLength
(
0
),
_returnPathLength
(
0
),
_TSComplexItem
(
nullptr
),
_arrivalPathLength
(
0
),
_returnPathLength
(
0
),
_TSComplexItem
(
nullptr
),
_surveyChanged
(
true
),
_synchronized
(
false
)
{
_surveyChanged
(
true
),
_synchronized
(
false
)
,
_needsUpdate
(
true
)
{
connect
(
this
,
&
WimaPlaner
::
currentPolygonIndexChanged
,
this
,
connect
(
this
,
&
WimaPlaner
::
currentPolygonIndexChanged
,
this
,
&
WimaPlaner
::
updatePolygonInteractivity
);
&
WimaPlaner
::
updatePolygonInteractivity
);
...
@@ -37,6 +37,10 @@ WimaPlaner::WimaPlaner(QObject *parent)
...
@@ -37,6 +37,10 @@ WimaPlaner::WimaPlaner(QObject *parent)
this
->
_sAreaChanged
=
true
;
this
->
_sAreaChanged
=
true
;
this
->
setNeedsUpdate
(
true
);
this
->
setNeedsUpdate
(
true
);
});
});
connect
(
&
this
->
_serviceArea
,
&
WimaServiceArea
::
depotChanged
,
[
this
]
{
this
->
_sAreaChanged
=
true
;
this
->
setNeedsUpdate
(
true
);
});
connect
(
&
this
->
_corridor
,
&
WimaArea
::
pathChanged
,
[
this
]
{
connect
(
&
this
->
_corridor
,
&
WimaArea
::
pathChanged
,
[
this
]
{
this
->
_corridorChanged
=
true
;
this
->
_corridorChanged
=
true
;
this
->
setNeedsUpdate
(
true
);
this
->
setNeedsUpdate
(
true
);
...
@@ -255,6 +259,11 @@ bool WimaPlaner::update() {
...
@@ -255,6 +259,11 @@ bool WimaPlaner::update() {
return
false
;
return
false
;
}
}
if
(
!
_serviceArea
.
containsCoordinate
(
_serviceArea
.
depot
()))
{
qgcApp
()
->
showMessage
(
tr
(
"Depot not inside service area."
));
return
false
;
}
// Join areas.
// Join areas.
bool
jAreaChanged
=
bool
jAreaChanged
=
this
->
_mAreaChanged
||
this
->
_sAreaChanged
||
this
->
_corridorChanged
;
this
->
_mAreaChanged
||
this
->
_sAreaChanged
||
this
->
_corridorChanged
;
...
@@ -426,9 +435,9 @@ bool WimaPlaner::update() {
...
@@ -426,9 +435,9 @@ bool WimaPlaner::update() {
return
false
;
return
false
;
}
}
this
->
_surveyChanged
=
false
;
this
->
_surveyChanged
=
false
;
setNeedsUpdate
(
false
);
}
}
setNeedsUpdate
(
false
);
return
true
;
return
true
;
}
}
void
WimaPlaner
::
saveToCurrent
()
{
saveToFile
(
_currentFile
);
}
void
WimaPlaner
::
saveToCurrent
()
{
saveToFile
(
_currentFile
);
}
...
@@ -655,11 +664,11 @@ void WimaPlaner::updatePolygonInteractivity(int index) {
...
@@ -655,11 +664,11 @@ void WimaPlaner::updatePolygonInteractivity(int index) {
}
}
}
}
void
WimaPlaner
::
pushToWimaController
()
{
void
WimaPlaner
::
synchronize
()
{
if
(
_wimaBridge
!=
nullptr
)
{
if
(
_wimaBridge
!=
nullptr
)
{
if
(
!
_needsUpdate
)
if
(
_needsUpdate
)
return
;
return
;
WimaPlanData
planData
=
toPlanData
();
auto
planData
=
toPlanData
();
(
void
)
_wimaBridge
->
setWimaPlanData
(
planData
);
(
void
)
_wimaBridge
->
setWimaPlanData
(
planData
);
this
->
_synchronized
=
true
;
this
->
_synchronized
=
true
;
emit
synchronizedChanged
();
emit
synchronizedChanged
();
...
@@ -718,30 +727,20 @@ void WimaPlaner::setInteractive() {
...
@@ -718,30 +727,20 @@ void WimaPlaner::setInteractive() {
*
*
* \sa WimaController, WimaPlanData
* \sa WimaController, WimaPlanData
*/
*/
WimaPlanData
WimaPlaner
::
toPlanData
()
{
QSharedPointer
<
WimaPlanData
>
WimaPlaner
::
toPlanData
()
{
WimaPlanData
planData
;
QSharedPointer
<
WimaPlanData
>
planData
(
new
WimaPlanData
())
;
// store areas
// store areas
planData
.
append
(
WimaMeasurementAreaData
(
_measurementArea
));
planData
->
append
(
WimaMeasurementAreaData
(
_measurementArea
));
planData
.
append
(
WimaServiceAreaData
(
_serviceArea
));
planData
->
append
(
WimaServiceAreaData
(
_serviceArea
));
planData
.
append
(
WimaCorridorData
(
_corridor
));
planData
->
append
(
WimaCorridorData
(
_corridor
));
planData
.
append
(
WimaJoinedAreaData
(
_joinedArea
));
planData
->
append
(
WimaJoinedAreaData
(
_joinedArea
));
// convert mission items to mavlink commands
// convert mission items to mavlink commands
QObject
deleteObject
;
// used to automatically delete content of
QList
<
MissionItem
*>
missionItems
;
// rgMissionItems after this function call
_TSComplexItem
->
appendMissionItems
(
missionItems
,
nullptr
);
QList
<
MissionItem
*>
rgMissionItems
;
QmlObjectListModel
*
visualItems
=
_missionController
->
visualItems
();
QmlObjectListModel
visualItemsToCopy
;
for
(
unsigned
long
i
=
_arrivalPathLength
+
1
;
i
<
visualItems
->
count
()
-
_returnPathLength
;
i
++
)
visualItemsToCopy
.
append
(
visualItems
->
get
(
i
));
MissionController
::
convertToMissionItems
(
&
visualItemsToCopy
,
rgMissionItems
,
&
deleteObject
);
// store mavlink commands
// store mavlink commands
planData
.
append
(
rgMissionItems
);
planData
->
append
(
missionItems
);
return
planData
;
return
planData
;
}
}
...
@@ -749,7 +748,7 @@ WimaPlanData WimaPlaner::toPlanData() {
...
@@ -749,7 +748,7 @@ WimaPlanData WimaPlaner::toPlanData() {
void
WimaPlaner
::
autoLoadMission
()
{
void
WimaPlaner
::
autoLoadMission
()
{
loadFromFile
(
"/home/valentin/Desktop/drones/qgroundcontrol/Paths/"
loadFromFile
(
"/home/valentin/Desktop/drones/qgroundcontrol/Paths/"
"KlingenbachTest.wima"
);
"KlingenbachTest.wima"
);
pushToWimaController
();
synchronize
();
}
}
#endif
#endif
...
...
src/Wima/WimaPlaner.h
View file @
46adaf36
...
@@ -2,6 +2,7 @@
...
@@ -2,6 +2,7 @@
#include "QmlObjectListModel.h"
#include "QmlObjectListModel.h"
#include <QObject>
#include <QObject>
#include <QSharedPointer>
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaCorridorData.h"
...
@@ -82,7 +83,7 @@ public:
...
@@ -82,7 +83,7 @@ public:
/// Recalculates vehicle corridor, flight path, etc.
/// Recalculates vehicle corridor, flight path, etc.
Q_INVOKABLE
bool
update
();
Q_INVOKABLE
bool
update
();
/// Pushes the generated mission data to the wimaController.
/// Pushes the generated mission data to the wimaController.
Q_INVOKABLE
void
pushToWimaController
();
Q_INVOKABLE
void
synchronize
();
Q_INVOKABLE
void
saveToCurrent
();
Q_INVOKABLE
void
saveToCurrent
();
Q_INVOKABLE
void
saveToFile
(
const
QString
&
filename
);
Q_INVOKABLE
void
saveToFile
(
const
QString
&
filename
);
Q_INVOKABLE
bool
loadFromCurrent
();
Q_INVOKABLE
bool
loadFromCurrent
();
...
@@ -118,7 +119,7 @@ signals:
...
@@ -118,7 +119,7 @@ signals:
private:
private:
// Member Functions
// Member Functions
WimaPlanData
toPlanData
();
QSharedPointer
<
WimaPlanData
>
toPlanData
();
bool
shortestPath
(
const
QGeoCoordinate
&
start
,
bool
shortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
);
QVector
<
QGeoCoordinate
>
&
path
);
...
...
src/WimaView/DragCoordinate.qml
View file @
46adaf36
...
@@ -27,16 +27,17 @@ Item {
...
@@ -27,16 +27,17 @@ Item {
property
var
coordinate
property
var
coordinate
property
int
sequenceNumber
property
int
sequenceNumber
property
bool
checked
property
bool
checked
property
string
label
:
"
Reference
"
property
var
_itemVisual
property
var
_itemVisual
property
bool
_itemVisualShowing
:
false
property
bool
_itemVisualShowing
:
false
property
var
_dragArea
property
var
_dragArea
property
bool
_dragAreaShowing
:
false
property
bool
_dragAreaShowing
:
false
signal
clicked
(
int
sequenceNumber
)
signal
clicked
()
signal
released
(
int
sequenceNumber
)
signal
released
()
signal
entered
(
int
sequenceNumber
)
signal
entered
()
signal
exited
(
int
sequenceNumber
)
signal
exited
()
signal
dragStart
()
signal
dragStart
()
signal
dragStop
()
signal
dragStop
()
...
@@ -114,16 +115,16 @@ Item {
...
@@ -114,16 +115,16 @@ Item {
id
:
indicatorComponent
id
:
indicatorComponent
CoordinateIndicator
{
CoordinateIndicator
{
label
:
"
Reference
"
label
:
_root
.
label
checked
:
_root
.
checked
checked
:
_root
.
checked
z
:
QGroundControl
.
zOrderMapItems
z
:
QGroundControl
.
zOrderMapItems
sequenceNumber
:
_root
.
sequenceNumber
sequenceNumber
:
_root
.
sequenceNumber
coordinate
:
_root
.
coordinate
coordinate
:
_root
.
coordinate
onClicked
:
_root
.
clicked
(
_missionItem
.
sequenceNumber
)
onClicked
:
_root
.
clicked
()
onReleased
:
_root
.
released
(
_missionItem
.
sequenceNumber
)
onReleased
:
_root
.
released
()
onEntered
:
_root
.
entered
(
_missionItem
.
sequenceNumber
)
onEntered
:
_root
.
entered
()
onExited
:
_root
.
exited
(
_missionItem
.
sequenceNumber
)
onExited
:
_root
.
exited
()
}
}
}
}
}
}
src/WimaView/WimaMapPolygonVisuals.qml
View file @
46adaf36
...
@@ -42,6 +42,8 @@ Item {
...
@@ -42,6 +42,8 @@ Item {
property
bool
_circle
:
false
property
bool
_circle
:
false
property
real
_circleRadius
property
real
_circleRadius
property
bool
_editCircleRadius
:
false
property
bool
_editCircleRadius
:
false
property
bool
_handelsVisible
:
false
// state tracker
property
bool
_polygonVisible
:
false
// state tracker
property
real
_zorderDragHandle
:
QGroundControl
.
zOrderMapItems
+
3
// Highest to prevent splitting when items overlap
property
real
_zorderDragHandle
:
QGroundControl
.
zOrderMapItems
+
3
// Highest to prevent splitting when items overlap
property
real
_zorderSplitHandle
:
QGroundControl
.
zOrderMapItems
+
2
property
real
_zorderSplitHandle
:
QGroundControl
.
zOrderMapItems
+
2
...
@@ -49,19 +51,28 @@ Item {
...
@@ -49,19 +51,28 @@ Item {
signal
dragStop
// triggered if node or center handle was stopped dragging
signal
dragStop
// triggered if node or center handle was stopped dragging
function
addVisuals
()
{
function
addPolygon
()
{
if
(
!
_polygonComponent
){
_polygonComponent
=
polygonComponent
.
createObject
(
mapControl
)
_polygonComponent
=
polygonComponent
.
createObject
(
mapControl
)
mapControl
.
addMapItem
(
_polygonComponent
)
mapControl
.
addMapItem
(
_polygonComponent
)
}
}
}
function
removeVisuals
()
{
function
removePolygon
()
{
if
(
_polygonComponent
){
_polygonComponent
.
destroy
()
_polygonComponent
.
destroy
()
_polygonComponent
=
undefined
}
}
}
function
addHandles
()
{
function
addHandles
()
{
if
(
!
_dragHandlesComponent
)
{
if
(
!
_dragHandlesComponent
)
{
_dragHandlesComponent
=
dragHandlesComponent
.
createObject
(
mapControl
)
_dragHandlesComponent
=
dragHandlesComponent
.
createObject
(
mapControl
)
}
if
(
!
_splitHandlesComponent
)
{
_splitHandlesComponent
=
splitHandlesComponent
.
createObject
(
mapControl
)
_splitHandlesComponent
=
splitHandlesComponent
.
createObject
(
mapControl
)
}
if
(
!
_centerDragHandleComponent
)
{
_centerDragHandleComponent
=
centerDragHandleComponent
.
createObject
(
mapControl
)
_centerDragHandleComponent
=
centerDragHandleComponent
.
createObject
(
mapControl
)
}
}
}
}
...
@@ -153,27 +164,38 @@ Item {
...
@@ -153,27 +164,38 @@ Item {
setCircleRadius
(
center
,
radius
)
setCircleRadius
(
center
,
radius
)
}
}
onInteractiveChanged
:
{
if
(
interactive
)
{
function
updateVisibility
(){
if
(
visible
){
addPolygon
()
if
(
interactive
){
addHandles
()
addHandles
()
}
else
{
removeHandles
()
}
}
else
{
}
else
{
removeHandles
()
removeHandles
()
removePolygon
()
}
}
}
}
onInteractiveChanged
:
{
updateVisibility
()
}
onVisibleChanged
:
{
updateVisibility
()
}
Component
.
onCompleted
:
{
Component
.
onCompleted
:
{
if
(
initPolygon
){
if
(
initPolygon
){
addInitialPolygon
()
addInitialPolygon
()
}
}
updateVisibility
()
addVisuals
()
if
(
interactive
)
{
addHandles
()
}
}
}
Component
.
onDestruction
:
{
Component
.
onDestruction
:
{
remove
Visuals
()
remove
Polygon
()
removeHandles
()
removeHandles
()
}
}
...
...
src/WimaView/WimaServiceAreaMapVisual.qml
View file @
46adaf36
...
@@ -18,7 +18,6 @@ import QGroundControl.Palette 1.0
...
@@ -18,7 +18,6 @@ import QGroundControl.Palette 1.0
import
QGroundControl
.
Controls
1.0
import
QGroundControl
.
Controls
1.0
import
QGroundControl
.
FlightMap
1.0
import
QGroundControl
.
FlightMap
1.0
/// Wima Global Measurement Area visuals
Item
{
Item
{
id
:
_root
id
:
_root
...
@@ -27,9 +26,37 @@ Item {
...
@@ -27,9 +26,37 @@ Item {
property
var
areaItem
:
object
property
var
areaItem
:
object
property
var
_polygon
:
areaItem
property
var
_polygon
:
areaItem
property
var
_depot
property
bool
showDepot
:
areaItem
.
interactive
||
areaItem
.
borderPolygon
.
interactive
property
bool
_depotVisible
:
false
onShowDepotChanged
:
{
if
(
showDepot
){
if
(
!
_depotVisible
){
_addDepot
()
}
}
else
{
if
(
_depotVisible
){
_destroyDepot
()
}
}
}
signal
clicked
(
int
sequenceNumber
)
signal
clicked
(
int
sequenceNumber
)
function
_addDepot
()
{
_depot
=
depotPointComponent
.
createObject
(
map
)
map
.
addMapItem
(
_depot
)
_depotVisible
=
true
}
function
_destroyDepot
()
{
if
(
_depot
){
_depot
.
destroy
()
}
_depotVisible
=
false
}
/// Add an initial 4 sided polygon if there is none
/// Add an initial 4 sided polygon if there is none
function
_addInitialPolygon
()
{
function
_addInitialPolygon
()
{
...
@@ -73,18 +100,17 @@ Item {
...
@@ -73,18 +100,17 @@ Item {
_polygon
.
appendVertex
(
bottomLeftCoord
)
_polygon
.
appendVertex
(
bottomLeftCoord
)
}
}
}
}
}
}
Component
.
onCompleted
:
{
Component
.
onCompleted
:
{
_addInitialPolygon
()
_addInitialPolygon
()
//_addInitialPolyline()
if
(
interactive
){
_addDepot
()
}
}
}
Component
.
onDestruction
:
{
Component
.
onDestruction
:
{
_destroyDepot
()
}
}
WimaMapPolygonVisuals
{
WimaMapPolygonVisuals
{
...
@@ -107,29 +133,25 @@ Item {
...
@@ -107,29 +133,25 @@ Item {
interiorOpacity
:
1
interiorOpacity
:
1
}
}
// Depot Point.
// Depot Point.
Component
{
Component
{
id
:
depotPointComponent
id
:
depotPointComponent
DragCoordinate
{
DragCoordinate
{
property
var
depot
:
areaItem
.
depot
map
:
_root
.
map
map
:
_root
.
map
qgcView
:
_root
.
qgcView
qgcView
:
_root
.
qgcView
z
:
QGroundControl
.
zOrderMapItems
z
:
QGroundControl
.
zOrderMapItems
checked
:
areaItem
.
interactive
checked
:
showDepot
coordinate
:
areaItem
.
depot
coordinate
:
depot
label
:
"
Depot
"
property
var
point
:
areaItem
.
depot
onRefPointChanged
:
{
if
(
point
!==
coordinate
)
{
coordinate
=
point
}
}
onDragReleased
:
{
onDragReleased
:
{
if
(
areaItem
.
containsCoordinate
(
coordinate
)){
areaItem
.
depot
=
coordinate
areaItem
.
depot
=
coordinate
}
}
coordinate
=
Qt
.
binding
(
function
()
{
return
areaItem
.
depot
;
})
}
}
}
}
}
...
...
src/WimaView/WimaToolBar.qml
View file @
46adaf36
...
@@ -290,7 +290,7 @@ Rectangle {
...
@@ -290,7 +290,7 @@ Rectangle {
onClicked
:
{
onClicked
:
{
if
(
wimaPlaner
){
if
(
wimaPlaner
){
if
(
!
wimaPlaner
.
needsUpdate
)
{
if
(
!
wimaPlaner
.
needsUpdate
)
{
wimaPlaner
.
pushToWimaController
()
wimaPlaner
.
synchronize
()
}
else
{
}
else
{
wimaPlaner
.
update
()
wimaPlaner
.
update
()
}
}
...
@@ -302,7 +302,9 @@ Rectangle {
...
@@ -302,7 +302,9 @@ Rectangle {
from
:
0.5
from
:
0.5
to
:
1
to
:
1
loops
:
Animation
.
Infinite
loops
:
Animation
.
Infinite
running
:
wimaPlaner
?
wimaPlaner
.
needsUpdate
:
false
running
:
wimaPlaner
?
wimaPlaner
.
needsUpdate
||
(
!
wimaPlaner
.
needsUpdate
&&
!
wimaPlaner
.
synchronized
)
:
false
alwaysRunToEnd
:
true
alwaysRunToEnd
:
true
duration
:
2000
duration
:
2000
}
}
...
...
src/WimaView/WimaView.qml
View file @
46adaf36
...
@@ -592,7 +592,7 @@ QGCView {
...
@@ -592,7 +592,7 @@ QGCView {
// Add lines between waypoints
// Add lines between waypoints
MissionLineView
{
MissionLineView
{
model
:
_editingLayer
==
_layerMission
?
_missionController
.
waypointLines
:
undefined
model
:
_editingLayer
==
_layerMission
?
_missionController
.
waypointLines
:
[]
}
}
//Add Wima Visuals
//Add Wima Visuals
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment