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
99e5ae38
Commit
99e5ae38
authored
Nov 10, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
addapting wima stuff to qgc master branch, code not ready
parent
5e177398
Changes
17
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
17 changed files
with
320 additions
and
702 deletions
+320
-702
SimpleMissionItem.h
src/MissionManager/SimpleMissionItem.h
+1
-1
TransectStyleComplexItem.h
src/MissionManager/TransectStyleComplexItem.h
+3
-3
CircularSurvey.cc
src/Wima/CircularSurvey.cc
+17
-99
CircularSurvey.h
src/Wima/CircularSurvey.h
+5
-18
WimaArea.cc
src/Wima/Geometry/WimaArea.cc
+1
-33
WimaMeasurementArea.cc
src/Wima/Geometry/WimaMeasurementArea.cc
+71
-62
WimaServiceArea.cc
src/Wima/Geometry/WimaServiceArea.cc
+19
-11
CircularGenerator.cpp
src/Wima/Snake/CircularGenerator.cpp
+3
-1
LinearGenerator.cpp
src/Wima/Snake/LinearGenerator.cpp
+3
-1
NemoInterface.cpp
src/Wima/Snake/NemoInterface.cpp
+31
-32
DefaultManager.cpp
src/Wima/WaypointManager/DefaultManager.cpp
+4
-4
RTLManager.cpp
src/Wima/WaypointManager/RTLManager.cpp
+3
-3
Utils.cpp
src/Wima/WaypointManager/Utils.cpp
+6
-43
Utils.h
src/Wima/WaypointManager/Utils.h
+96
-119
WimaController.cc
src/Wima/WimaController.cc
+1
-197
WimaController.h
src/Wima/WimaController.h
+0
-16
WimaPlaner.cc
src/Wima/WimaPlaner.cc
+56
-59
No files found.
src/MissionManager/SimpleMissionItem.h
View file @
99e5ae38
...
...
@@ -118,7 +118,7 @@ public:
QString
mapVisualQML
(
void
)
const
override
{
return
QStringLiteral
(
"SimpleItemMapVisual.qml"
);
}
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
final
;
v
oid
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
v
irtual
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
ReadyForSaveState
readyForSaveState
(
void
)
const
final
;
double
additionalTimeDelay
(
void
)
const
final
;
bool
exitCoordinateSameAsEntry
(
void
)
const
final
{
return
true
;
}
...
...
src/MissionManager/TransectStyleComplexItem.h
View file @
99e5ae38
...
...
@@ -88,8 +88,8 @@ public:
// Overrides from VisualMissionItem
void
save
(
QJsonArray
&
planItems
)
override
=
0
;
bool
specifiesCoordinate
(
void
)
const
override
=
0
;
v
oid
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
v
oid
applyNewAltitude
(
double
newAltitude
)
final
;
v
irtual
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
v
irtual
void
applyNewAltitude
(
double
newAltitude
)
final
;
bool
dirty
(
void
)
const
final
{
return
_dirty
;
}
bool
isSimpleItem
(
void
)
const
final
{
return
false
;
}
bool
isStandaloneCoordinate
(
void
)
const
final
{
return
false
;
}
...
...
@@ -100,7 +100,7 @@ public:
double
specifiedFlightSpeed
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalYaw
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalPitch
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
v
oid
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
v
irtual
void
setMissionFlightStatus
(
MissionController
::
MissionFlightStatus_t
&
missionFlightStatus
)
final
;
ReadyForSaveState
readyForSaveState
(
void
)
const
override
;
QString
commandDescription
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
QString
commandName
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
...
...
src/Wima/CircularSurvey.cc
View file @
99e5ae38
...
...
@@ -19,6 +19,8 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY
(
CircularSurveyLog
,
"CircularSurveyLog"
)
template
<
typename
T
>
...
...
@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const
char
*
CircularSurvey
::
CircularSurveyName
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
variantName
=
"Variant"
;
CircularSurvey
::
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
CircularSurvey
::
CircularSurvey
(
PlanMasterController
*
masterController
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
QObject
*
parent
)
:
TransectStyleComplexItem
(
vehicle
,
flyView
,
settingsGroup
,
parent
),
:
TransectStyleComplexItem
(
masterController
,
flyView
,
settingsGroup
,
parent
),
_state
(
STATE
::
IDLE
),
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/CircularSurvey.SettingsGroup.json"
),
this
)),
...
...
@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&
CircularSurvey
::
_setTransects
);
connect
(
this
->
_pWorker
.
get
(),
&
RoutingThread
::
calculatingChanged
,
this
,
&
CircularSurvey
::
calculatingChanged
);
this
->
_transectsDirty
=
true
;
// Altitude
connect
(
&
_cameraCalc
,
&
CameraCalc
::
distanceToSurfaceRelativeChanged
,
this
,
&
CircularSurvey
::
coordinateHasRelativeAltitudeChanged
);
connect
(
&
_cameraCalc
,
&
CameraCalc
::
distanceToSurfaceRelativeChanged
,
this
,
&
CircularSurvey
::
exitCoordinateHasRelativeAltitudeChanged
);
// Register Generators.
auto
lg
=
std
::
make_shared
<
routing
::
LinearGenerator
>
(
this
->
_pAreaData
);
...
...
@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return
false
;
}
if
(
!
_load
(
complexObject
,
errorString
))
{
if
(
!
load
(
complexObject
,
sequenceNumber
,
errorString
))
{
_ignoreRecalc
=
false
;
return
false
;
}
...
...
@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_ignoreRecalc
=
false
;
_recalcComplexDistance
();
if
(
_cameraShots
==
0
)
{
// Shot count was possibly not available from plan file
_recalcCameraShots
();
...
...
@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
bool
CircularSurvey
::
specifiesCoordinate
()
const
{
return
true
;
}
void
CircularSurvey
::
appendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
{
if
(
_transectsDirty
)
return
;
if
(
_loadedMissionItems
.
count
())
{
// We have mission items from the loaded plan, use those
_appendLoadedMissionItems
(
items
,
missionItemParent
);
}
else
{
// Build the mission items on the fly
_buildAndAppendMissionItems
(
items
,
missionItemParent
);
}
}
void
CircularSurvey
::
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
{
if
(
_transectsDirty
)
return
;
int
seqNum
=
_sequenceNumber
;
for
(
const
MissionItem
*
loadedMissionItem
:
_loadedMissionItems
)
{
MissionItem
*
item
=
new
MissionItem
(
*
loadedMissionItem
,
missionItemParent
);
item
->
setSequenceNumber
(
seqNum
++
);
items
.
append
(
item
);
}
}
void
CircularSurvey
::
_buildAndAppendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
{
if
(
_transectsDirty
||
_transects
.
count
()
==
0
)
return
;
MissionItem
*
item
;
int
seqNum
=
_sequenceNumber
;
MAV_FRAME
mavFrame
=
followTerrain
()
||
!
_cameraCalc
.
distanceToSurfaceRelative
()
?
MAV_FRAME_GLOBAL
:
MAV_FRAME_GLOBAL_RELATIVE_ALT
;
for
(
const
QList
<
TransectStyleComplexItem
::
CoordInfo_t
>
&
transect
:
_transects
)
{
// bool transectEntry = true;
for
(
const
CoordInfo_t
&
transectCoordInfo
:
transect
)
{
item
=
new
MissionItem
(
seqNum
++
,
MAV_CMD_NAV_WAYPOINT
,
mavFrame
,
0
,
// Hold time (delay for hover and capture to settle vehicle
// before image is taken)
0.0
,
// No acceptance radius specified
0.0
,
// Pass through waypoint
std
::
numeric_limits
<
double
>::
quiet_NaN
(),
// Yaw unchanged
transectCoordInfo
.
coord
.
latitude
(),
transectCoordInfo
.
coord
.
longitude
(),
transectCoordInfo
.
coord
.
altitude
(),
true
,
// autoContinue
false
,
// isCurrentItem
missionItemParent
);
items
.
append
(
item
);
}
}
}
bool
CircularSurvey
::
_switchToGenerator
(
const
CircularSurvey
::
PtrGenerator
&
newG
)
{
if
(
this
->
_pGenerator
!=
newG
)
{
...
...
@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
}
void
CircularSurvey
::
_updateWorker
()
{
// Mark transects as dirty.
this
->
_transectsDirty
=
true
;
// Reset data.
this
->
_transects
.
clear
();
this
->
_variantVector
.
clear
();
...
...
@@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() {
}
}
void
CircularSurvey
::
applyNewAltitude
(
double
newAltitude
)
{
_cameraCalc
.
valueSetIsDistance
()
->
setRawValue
(
true
);
_cameraCalc
.
distanceToSurface
()
->
setRawValue
(
newAltitude
);
_cameraCalc
.
setDistanceToSurfaceRelative
(
true
);
}
double
CircularSurvey
::
timeBetweenShots
()
{
return
1
;
}
QString
CircularSurvey
::
commandDescription
()
const
{
...
...
@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString
CircularSurvey
::
abbreviation
()
const
{
return
tr
(
"C.S."
);
}
bool
CircularSurvey
::
readyForSave
()
const
{
return
TransectStyleComplexItem
::
readyForSave
()
&&
!
_transectsDirty
&&
this
->
_state
==
STATE
::
IDLE
;
TransectStyleComplexItem
::
ReadyForSaveState
CircularSurvey
::
readyForSaveState
()
const
{
if
(
TransectStyleComplexItem
::
readyForSaveState
()
==
TransectStyleComplexItem
::
ReadyForSaveState
::
ReadyForSave
)
{
if
(
this
->
_state
==
STATE
::
IDLE
)
{
return
ReadyForSaveState
::
ReadyForSave
;
}
else
{
return
ReadyForSaveState
::
NotReadyForSaveData
;
}
}
else
{
return
TransectStyleComplexItem
::
readyForSaveState
();
}
}
double
CircularSurvey
::
additionalTimeDelay
()
const
{
return
0
;
}
...
...
@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
<<
" ms"
;
}
void
CircularSurvey
::
_recalcComplexDistance
()
{
_complexDistance
=
0
;
if
(
_transectsDirty
)
return
;
for
(
int
i
=
0
;
i
<
_visualTransectPoints
.
count
()
-
1
;
i
++
)
{
_complexDistance
+=
_visualTransectPoints
[
i
].
value
<
QGeoCoordinate
>
().
distanceTo
(
_visualTransectPoints
[
i
+
1
].
value
<
QGeoCoordinate
>
());
}
emit
complexDistanceChanged
();
}
// no cameraShots in Circular Survey, add if desired
void
CircularSurvey
::
_recalcCameraShots
()
{
_cameraShots
=
0
;
}
...
...
@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
&
CircularSurvey
::
_changeVariant
);
this
->
_changeVariantWorker
();
// Mark transect ready.
this
->
_transectsDirty
=
false
;
this
->
_state
=
STATE
::
SKIPP
;
this
->
_rebuildTransects
();
}
else
{
...
...
src/Wima/CircularSurvey.h
View file @
99e5ae38
...
...
@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
using
PtrGenerator
=
std
::
shared_ptr
<
routing
::
GeneratorBase
>
;
using
PtrRoutingData
=
std
::
shared_ptr
<
RoutingData
>
;
using
PtrWorker
=
std
::
shared_ptr
<
RoutingThread
>
;
using
PtrWorker
=
std
::
unique_ptr
<
RoutingThread
>
;
using
Transects
=
QList
<
QList
<
CoordInfo_t
>>
;
using
Variant
=
Transects
;
public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
/// @param kmlOrShpFile Polygon comes from this file, empty for default
/// polygon
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
CircularSurvey
(
PlanMasterController
*
masterController
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
QObject
*
parent
);
~
CircularSurvey
();
...
...
@@ -63,14 +60,11 @@ public:
QString
mapVisualQML
(
void
)
const
override
final
;
void
save
(
QJsonArray
&
planItems
)
override
final
;
bool
specifiesCoordinate
(
void
)
const
override
final
;
void
appendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
)
override
final
;
void
applyNewAltitude
(
double
newAltitude
)
override
final
;
double
timeBetweenShots
(
void
)
override
final
;
QString
commandDescription
(
void
)
const
override
final
;
QString
commandName
(
void
)
const
override
final
;
QString
abbreviation
(
void
)
const
override
final
;
bool
readyForSav
e
(
void
)
const
override
final
;
ReadyForSaveState
readyForSaveStat
e
(
void
)
const
override
final
;
double
additionalTimeDelay
(
void
)
const
override
final
;
// Generator
...
...
@@ -97,7 +91,6 @@ signals:
private
slots
:
// Overrides from TransectStyleComplexItem
void
_rebuildTransectsPhase1
(
void
)
final
;
void
_recalcComplexDistance
(
void
)
final
;
void
_recalcCameraShots
(
void
)
final
;
// Worker functions.
...
...
@@ -108,11 +101,6 @@ private slots:
void
_reverseWorker
();
private:
void
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
void
_buildAndAppendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
bool
_switchToGenerator
(
const
PtrGenerator
&
newG
);
// State.
...
...
@@ -139,7 +127,6 @@ private:
PtrGenerator
_pGenerator
;
// Routing.
using
Variant
=
Transects
;
QVector
<
Variant
>
_variantVector
;
PtrWorker
_pWorker
;
};
src/Wima/Geometry/WimaArea.cc
View file @
99e5ae38
...
...
@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
}
else
if
(
retValue
==
JoinPolygonError
::
PathSizeLow
)
{
qWarning
(
"Polygon vertex count is low."
);
}
else
{
Q
Vector
<
QGeoCoordinate
>
path
;
Q
List
<
QGeoCoordinate
>
path
;
toGeoList
(
joinedPolygon
,
origin
,
path
);
// qWarning("after transform");
// qWarning() << path;
...
...
@@ -491,38 +491,6 @@ void WimaArea::init() {
&
WimaArea
::
recalcInteractivity
);
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the console.
*/
void
print
(
const
WimaArea
&
area
)
{
QString
message
;
print
(
area
,
message
);
qWarning
()
<<
message
;
}
/*!
* \fn void print(const WimaArea &area)
* Prints the data contained in \a area to the \a outputString.
*/
void
print
(
const
WimaArea
&
area
,
QString
&
outputString
)
{
outputString
.
append
(
QString
(
"Type: %1
\n
"
).
arg
(
area
.
objectName
()));
print
(
static_cast
<
const
QGCMapPolygon
&>
(
area
),
outputString
);
outputString
.
append
(
QString
(
"Maximum Altitude: %1
\n
"
).
arg
(
area
.
_maxAltitude
));
outputString
.
append
(
QString
(
"Border Polygon Offset: %1
\n
"
)
.
arg
(
area
.
_borderPolygonOffset
.
rawValue
().
toDouble
()));
outputString
.
append
(
QString
(
"Border Polygon Coordinates
\n
"
)
.
arg
(
area
.
_borderPolygonOffset
.
rawValue
().
toDouble
()));
for
(
int
i
=
0
;
i
<
area
.
_borderPolygon
.
count
();
i
++
)
{
QGeoCoordinate
coordinate
=
area
.
_borderPolygon
.
vertexCoordinate
(
i
);
outputString
.
append
(
QString
(
"%1
\n
"
).
arg
(
coordinate
.
toString
(
QGeoCoordinate
::
Degrees
)));
}
}
// QDoc Documentation
/*!
...
...
src/Wima/Geometry/WimaMeasurementArea.cc
View file @
99e5ae38
...
...
@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData
&
TileData
::
operator
=
(
const
TileData
&
other
)
{
this
->
tiles
.
clearAndDeleteContents
();
for
(
std
::
size_t
i
=
0
;
i
<
std
::
size_t
(
other
.
tiles
.
count
());
++
i
)
{
const
auto
*
obj
=
other
.
tiles
.
get
(
i
)
;
const
auto
*
obj
=
other
.
tiles
[
i
]
;
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
this
->
tiles
.
append
(
new
SnakeTile
(
*
tile
,
this
));
...
...
@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
}
bool
TileData
::
operator
==
(
const
TileData
&
other
)
const
{
return
this
->
tiles
==
other
.
tiles
&&
this
->
tileCenterPoints
==
other
.
tileCenterPoints
;
if
(
this
->
tileCenterPoints
==
other
.
tileCenterPoints
&&
this
->
tiles
.
count
()
==
other
.
tiles
.
count
())
{
for
(
int
i
=
0
;
i
<
other
.
tiles
.
count
();
++
i
)
{
if
(
this
->
tiles
[
i
]
!=
other
.
tiles
[
i
])
{
return
false
;
}
}
return
true
;
}
else
{
return
false
;
}
}
bool
TileData
::
operator
!=
(
const
TileData
&
other
)
const
{
...
...
@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this
/* QObject parent */
)),
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
minTileAreaName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
this
/* QObject parent */
)),
_state
(
STATE
::
IDLE
)
{
...
...
@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this
/* QObject parent */
)),
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
minTileAreaName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
this
/* QObject parent */
)),
_state
(
STATE
::
IDLE
)
{
...
...
@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void
WimaMeasurementArea
::
saveToJson
(
QJsonObject
&
json
)
{
if
(
ready
())
{
this
->
WimaArea
::
saveToJson
(
json
);
json
[
tileHeightName
]
=
_tileHeight
.
rawValue
().
toDouble
();
json
[
tileWidthName
]
=
_tileWidth
.
rawValue
().
toDouble
();
this
->
WimaArea
::
saveToJson
(
json
);
json
[
tileHeightName
]
=
_tileHeight
.
rawValue
().
toDouble
();
json
[
tileWidthName
]
=
_tileWidth
.
rawValue
().
toDouble
();
json
[
minTileAreaName
]
=
_minTileAreaPercent
.
rawValue
().
toDouble
();
json
[
showTilesName
]
=
_showTiles
.
rawValue
().
toBool
();
json
[
areaTypeName
]
=
WimaMeasurementAreaName
;
json
[
showTilesName
]
=
_showTiles
.
rawValue
().
toBool
();
json
[
areaTypeName
]
=
WimaMeasurementAreaName
;
}
else
{
qCDebug
(
WimaMeasurementAreaLog
)
<<
"saveToJson(): not ready for saveing."
;
}
...
...
@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
if
(
this
->
_state
!=
STATE
::
UPDATEING
&&
this
->
_state
!=
STATE
::
STOP
)
{
const
auto
height
=
this
->
_tileHeight
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
width
=
this
->
_tileWidth
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
tileArea
=
width
*
height
;
const
auto
totalArea
=
this
->
area
()
*
si
::
meter
*
si
::
meter
;
const
auto
estNumTiles
=
totalArea
/
tileArea
;
const
auto
height
=
this
->
_tileHeight
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
width
=
this
->
_tileWidth
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
tileArea
=
width
*
height
;
const
auto
totalArea
=
this
->
area
()
*
si
::
meter
*
si
::
meter
;
const
auto
estNumTiles
=
totalArea
/
tileArea
;
// Check some conditions.
if
(
long
(
std
::
ceil
(
estNumTiles
.
value
()))
<=
SNAKE_MAX_TILES
&&
this
->
count
()
>=
3
&&
this
->
isSimplePolygon
())
{
this
->
count
()
>=
3
&&
this
->
isSimplePolygon
())
{
setState
(
STATE
::
UPDATEING
);
auto
polygon
=
this
->
coordinateList
();
for
(
auto
&
v
:
polygon
)
{
v
.
setAltitude
(
0
);
}
const
auto
minArea
=
auto
polygon
=
this
->
coordinateList
();
for
(
auto
&
v
:
polygon
)
{
v
.
setAltitude
(
0
);
}
const
auto
minArea
=
this
->
_minTileAreaPercent
.
rawValue
().
toDouble
()
/
100
*
tileArea
;
auto
*
th
=
this
->
thread
();
auto
future
=
QtConcurrent
::
run
([
polygon
,
th
,
height
,
width
,
minArea
]
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
DataPtr
pData
(
new
TileData
());
// Convert to ENU system.
QGeoCoordinate
origin
=
polygon
.
first
();
FPolygon
polygonENU
;
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
vector
<
FPolygon
>
tilesENU
;
BoundingBox
bbox
;
std
::
string
errorString
;
// Generate tiles.
if
(
snake
::
tiles
(
polygonENU
,
height
,
width
,
minArea
,
tilesENU
,
bbox
,
errorString
))
{
// Convert to geo system.
for
(
const
auto
&
t
:
tilesENU
)
{
auto
geoTile
=
new
SnakeTile
(
pData
.
get
());
for
(
const
auto
&
v
:
t
.
outer
())
{
QGeoCoordinate
geoVertex
;
fromENU
(
origin
,
v
,
geoVertex
);
geoTile
->
push_back
(
geoVertex
);
auto
*
th
=
this
->
thread
();
auto
future
=
QtConcurrent
::
run
([
polygon
,
th
,
height
,
width
,
minArea
]
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
DataPtr
pData
(
new
TileData
());
// Convert to ENU system.
QGeoCoordinate
origin
=
polygon
.
first
();
FPolygon
polygonENU
;
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
vector
<
FPolygon
>
tilesENU
;
BoundingBox
bbox
;
std
::
string
errorString
;
// Generate tiles.
if
(
snake
::
tiles
(
polygonENU
,
height
,
width
,
minArea
,
tilesENU
,
bbox
,
errorString
))
{
// Convert to geo system.
for
(
const
auto
&
t
:
tilesENU
)
{
auto
geoTile
=
new
SnakeTile
(
pData
.
get
());
for
(
const
auto
&
v
:
t
.
outer
())
{
QGeoCoordinate
geoVertex
;
fromENU
(
origin
,
v
,
geoVertex
);
geoTile
->
push_back
(
geoVertex
);
}
pData
->
tiles
.
append
(
geoTile
);
// Calculate center.
snake
::
FPoint
center
;
snake
::
polygonCenter
(
t
,
center
);
QGeoCoordinate
geoCenter
;
fromENU
(
origin
,
center
,
geoCenter
);
pData
->
tileCenterPoints
.
append
(
QVariant
::
fromValue
(
geoCenter
));
}
pData
->
tiles
.
append
(
geoTile
);
// Calculate center.
snake
::
FPoint
center
;
snake
::
polygonCenter
(
t
,
center
);
QGeoCoordinate
geoCenter
;
fromENU
(
origin
,
center
,
geoCenter
);
pData
->
tileCenterPoints
.
append
(
QVariant
::
fromValue
(
geoCenter
));
}
}
pData
->
moveToThread
(
th
);
pData
->
moveToThread
(
th
);
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): update time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
.
count
()
<<
" ms"
;
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): update time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
.
count
()
<<
" ms"
;
return
pData
;
});
// QtConcurrent::run()
return
pData
;
});
// QtConcurrent::run()
this
->
_watcher
.
setFuture
(
future
);
}
this
->
_watcher
.
setFuture
(
future
);
}
}
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): execution time: "
...
...
src/Wima/Geometry/WimaServiceArea.cc
View file @
99e5ae38
#include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY
(
WimaServiceAreaLog
,
"WimaServiceAreaLog"
)
const
char
*
WimaServiceArea
::
wimaServiceAreaName
=
"Service Area"
;
const
char
*
WimaServiceArea
::
depotLatitudeName
=
"DepotLatitude"
;
const
char
*
WimaServiceArea
::
depotLongitudeName
=
"DepotLongitude"
;
...
...
@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
}
else
if
(
this
->
_depot
.
isValid
())
{
// Use nearest coordinate.
auto
minDist
=
std
::
numeric_limits
<
double
>::
infinity
();
auto
minIt
=
this
->
pathReference
().
begin
();
for
(
auto
it
=
this
->
pathReference
().
begin
();
it
<
this
->
pathReference
().
end
();
++
it
)
{
const
auto
vertex
=
it
->
value
<
QGeoCoordinate
>
();
auto
d
=
vertex
.
distanceTo
(
this
->
_depot
);
if
(
d
<
minDist
)
{
minDist
=
d
;
minIt
=
it
;
int
minIndex
=
0
;
for
(
int
idx
=
0
;
idx
<
this
->
pathModel
().
count
();
++
idx
)
{
const
QObject
*
obj
=
this
->
pathModel
()[
idx
];
const
auto
*
vertex
=
qobject_cast
<
const
QGeoCoordinate
*>
(
obj
);
if
(
vertex
!=
nullptr
)
{
auto
d
=
vertex
->
distanceTo
(
this
->
_depot
);
if
(
d
<
minDist
)
{
minDist
=
d
;
minIndex
=
idx
;
}
}
else
{
qCCritical
(
WimaServiceAreaLog
)
<<
"init(): nullptr catched!"
;
}
}
this
->
setDepot
(
minIt
->
value
<
QGeoCoordinate
>
(
));
}
else
if
(
this
->
path
Reference
().
size
()
>
0
)
{
this
->
setDepot
(
*
this
->
pathModel
().
value
<
QGeoCoordinate
*>
(
minIndex
));
}
else
if
(
this
->
path
Model
().
count
()
>
0
)
{
// Use first coordinate.
this
->
setDepot
(
this
->
pathReference
().
value
(
0
).
value
<
QGeoCoordinate
>
(
));
this
->
setDepot
(
*
this
->
pathModel
().
value
<
QGeoCoordinate
*>
(
0
));
}
}
});
...
...
src/Wima/Snake/CircularGenerator.cpp
View file @
99e5ae38
...
...
@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) {
if
(
progress
.
size
()
==
tiles
->
count
())
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
if
(
progress
[
i
]
==
100
)
{
const
auto
*
tile
=
tiles
->
value
<
const
SnakeTile
*>
(
i
);
const
auto
*
obj
=
(
*
tiles
)[
int
(
i
)];
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
snake
::
FPolygon
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
...
...
src/Wima/Snake/LinearGenerator.cpp
View file @
99e5ae38
...
...
@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
if
(
progress
.
size
()
==
tiles
->
count
())
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
if
(
progress
[
i
]
==
100
)
{
const
auto
*
tile
=
tiles
->
value
<
const
SnakeTile
*>
(
i
);
const
QObject
*
obj
=
(
*
tiles
)[
int
(
i
)];
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
snake
::
FPolygon
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
...
...
src/Wima/Snake/NemoInterface.cpp
View file @
99e5ae38
...
...
@@ -97,7 +97,7 @@ private:
};
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
StatusMap
statusMap
{
static
StatusMap
statusMap
{
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
NOT_CONNECTED
,
"Not Connected"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
...
...
@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
if
(
ros_bridge
::
isValidConnectionString
(
connectionString
.
toLocal8Bit
().
data
()))
{
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
}
else
{
QString
defaultString
(
"localhost:9090"
);
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
connectionString
);
qgcApp
()
->
showMessage
(
"Resetting connection string to: "
+
defaultString
);
connectionStringFact
->
setRawValue
(
QVariant
(
defaultString
));
// calls this function recursively
qgcApp
()
->
warningMessageBoxOnMainThread
(
"Nemo Interface"
,
"Websocket connection string possibly invalid: "
+
connectionString
+
". Trying to connect anyways."
);
}
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
};
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
...
...
@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
UniqueLock
lk1
(
this
->
ENUOriginMutex
,
std
::
adopt_lock
);
UniqueLock
lk2
(
this
->
tilesENUMutex
,
std
::
adopt_lock
);
const
auto
*
obj
=
tileData
.
tiles
.
get
(
0
)
;
const
auto
*
obj
=
tileData
.
tiles
[
0
]
;
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
if
(
tile
->
coordinateList
().
size
()
>
0
)
{
if
(
tile
->
coordinateList
().
first
().
isValid
())
{
this
->
ENUOrigin
=
tile
->
coordinateList
().
first
();
const
auto
&
origin
=
this
->
ENUOrigin
;
this
->
tilesENU
.
polygons
().
clear
();
for
(
int
i
=
0
;
i
<
tileData
.
tiles
.
count
();
++
i
)
{
obj
=
tileData
.
tiles
.
get
(
i
)
;
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
SnakeTileLocal
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
.
path
());
this
->
tilesENU
.
polygons
().
push_back
(
std
::
move
(
tileENU
));
}
else
{
this
->
ENUOrigin
=
tile
->
coordinateList
().
first
();
const
auto
&
origin
=
this
->
ENUOrigin
;
this
->
tilesENU
.
polygons
().
clear
();
for
(
int
i
=
0
;
i
<
tileData
.
tiles
.
count
();
++
i
)
{
obj
=
tileData
.
tiles
[
i
]
;
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
SnakeTileLocal
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
.
path
());
this
->
tilesENU
.
polygons
().
push_back
(
std
::
move
(
tileENU
));
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): nullptr."
;
break
;
break
;
}
}
}
}
else
{
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): Origin invalid."
;
}
}
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): tile empty."
;
}
}
}
}
else
{
this
->
tileData
.
clear
();
...
...
@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
progressMsg
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
progressMsg
.
progress
().
clear
();
qgcApp
()
->
showMessage
(
"Invalid progress message received."
);
qgcApp
()
->
informationMessageBoxOnMainThread
(
"Nemo Interface"
,
"Invalid progress message received."
);
}
emit
this
->
parent
->
progressChanged
();
...
...
@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if
(
geographic_msgs
::
geo_point
::
toJson
(
origin
,
jOrigin
,
pDoc
->
GetAllocator
()))
{
lk
.
unlock
();
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
}
else
{
lk
.
unlock
();
qCWarning
(
NemoInterfaceLog
)
...
...
@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if
(
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
jSnakeTiles
,
pDoc
->
GetAllocator
()))
{
lk
.
unlock
();
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
}
else
{
lk
.
unlock
();
qCWarning
(
NemoInterfaceLog
)
...
...
@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
this
->
pRosBridge
->
connected
()
&&
!
this
->
topicServiceSetupDone
)
{
this
->
doTopicServiceSetup
();
this
->
topicServiceSetupDone
=
true
;
this
->
topicServiceSetupDone
=
true
;
this
->
setStatus
(
STATUS
::
WEBSOCKET_DETECTED
);
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
...
...
@@ -395,7 +394,7 @@ void NemoInterface::Impl::publishTilesENU() {
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
if
(
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
()))
{
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"Impl::publishTilesENU: could not create json document."
;
...
...
@@ -408,7 +407,7 @@ void NemoInterface::Impl::publishENUOrigin() {
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
if
(
geographic_msgs
::
geo_point
::
toJson
(
this
->
ENUOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
()))
{
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
}
else
{
qCWarning
(
NemoInterfaceLog
)
<<
"Impl::publishENUOrigin: could not create json document."
;
...
...
src/Wima/WaypointManager/DefaultManager.cpp
View file @
99e5ae38
...
...
@@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem(
using
namespace
WaypointManager
::
Utils
;
if
(
!
insertMissionItem
(
c
,
index
/*insertion index*/
,
list
,
_settings
->
vehicle
(),
_settings
->
isFlyView
(),
_settings
->
masterController
(),
_settings
->
isFlyView
(),
&
list
/*parent*/
,
doUpdate
/*do update*/
))
{
qWarning
(
"WaypointManager::DefaultManager::next(): insertMissionItem failed."
);
...
...
@@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() {
}
_currentMissionItems
.
clearAndDeleteContents
();
initialize
(
_currentMissionItems
,
_settings
->
vehicle
(),
initialize
(
_currentMissionItems
,
_settings
->
masterController
(),
_settings
->
isFlyView
());
// Calculate path from home to first waypoint.
...
...
@@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() {
Q_ASSERT
(
takeoffItem
!=
nullptr
);
return
false
;
}
makeTakeOffCmd
(
takeoffItem
,
_settings
->
masterController
()
->
managerVehicle
()
);
makeTakeOffCmd
(
takeoffItem
,
_settings
->
masterController
());
// Create change speed item.
_insertMissionItem
(
_settings
->
homePosition
(),
2
/*insertion index*/
,
...
...
@@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() {
qWarning
(
"WaypointManager::DefaultManager::next(): nullptr."
);
return
false
;
}
makeLandCmd
(
landItem
,
_settings
->
masterController
()
->
managerVehicle
()
);
makeLandCmd
(
landItem
,
_settings
->
masterController
());
// Set altitude.
for
(
int
i
=
1
;
i
<
_currentMissionItems
.
count
();
++
i
)
{
...
...
src/Wima/WaypointManager/RTLManager.cpp
View file @
99e5ae38
...
...
@@ -88,7 +88,7 @@ bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
using
namespace
WaypointManager
::
Utils
;
if
(
!
insertMissionItem
(
c
,
index
/*insertion index*/
,
list
,
_settings
->
vehicle
(),
_settings
->
isFlyView
(),
_settings
->
masterController
(),
_settings
->
isFlyView
(),
&
list
/*parent*/
,
doUpdate
/*do update*/
))
{
qWarning
(
"WaypointManager::RTLManager::next(): insertMissionItem failed."
);
Q_ASSERT
(
false
);
...
...
@@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() {
}
_currentMissionItems
.
clearAndDeleteContents
();
initialize
(
_currentMissionItems
,
_settings
->
vehicle
(),
initialize
(
_currentMissionItems
,
_settings
->
masterController
(),
_settings
->
isFlyView
());
// Calculate path from vehicle to home position.
...
...
@@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() {
qWarning
(
"WaypointManager::RTLManager::next(): nullptr."
);
return
false
;
}
makeLandCmd
(
landItem
,
_settings
->
masterController
()
->
managerVehicle
()
);
makeLandCmd
(
landItem
,
_settings
->
masterController
());
// Set altitude.
for
(
int
i
=
1
;
i
<
_currentMissionItems
.
count
();
++
i
)
{
...
...
src/Wima/WaypointManager/Utils.cpp
View file @
99e5ae38
#include "Utils.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include <iostream>
#include <QGeoCoordinate>
template
<>
QVariant
WaypointManager
::
Utils
::
getCoordinate
(
const
SimpleMissionItem
*
item
)
{
return
QVariant
::
fromValue
(
item
->
coordinate
());
}
bool
WaypointManager
::
Utils
::
insertMissionItem
(
const
QGeoCoordinate
&
coordinate
,
long
index
,
QmlObjectListModel
&
list
,
Vehicle
*
vehicle
,
PlanMasterController
*
masterController
,
bool
flyView
,
QObject
*
parent
,
bool
doUpdates
)
...
...
@@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
}
else
{
if
(
parent
==
nullptr
)
parent
=
&
list
;
SimpleMissionItem
*
newItem
=
new
SimpleMissionItem
(
vehicle
,
flyView
,
parent
);
auto
*
newItem
=
new
SimpleMissionItem
(
masterController
,
flyView
,
MissionItem
()
,
parent
);
int
sequenceNumber
=
detail
::
nextSequenceNumber
(
list
);
newItem
->
setSequenceNumber
(
sequenceNumber
);
...
...
@@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list)
}
void
WaypointManager
::
Utils
::
initialize
(
QmlObjectListModel
&
list
,
Vehicle
*
vehicle
,
PlanMasterController
*
masterController
,
bool
flyView
)
{
MissionSettingsItem
*
settingsItem
=
new
MissionSettingsItem
(
vehicle
,
flyView
,
&
list
);
MissionSettingsItem
*
settingsItem
=
new
MissionSettingsItem
(
masterController
,
flyView
,
&
list
);
list
.
insert
(
0
,
settingsItem
);
}
...
...
@@ -189,26 +185,6 @@ void WaypointManager::Utils::printList(QmlObjectListModel &list)
}
}
bool
WaypointManager
::
Utils
::
makeTakeOffCmd
(
SimpleMissionItem
*
item
,
Vehicle
*
vehicle
)
{
if
(
vehicle
->
fixedWing
()
||
vehicle
->
vtol
()
||
vehicle
->
multiRotor
()
)
{
MAV_CMD
takeoffCmd
=
vehicle
->
vtol
()
?
MAV_CMD_NAV_VTOL_TAKEOFF
:
MAV_CMD_NAV_TAKEOFF
;
if
(
vehicle
->
firmwarePlugin
()
->
supportedMissionCommands
().
contains
(
takeoffCmd
))
{
auto
c
=
item
->
coordinate
();
item
->
setCommand
(
takeoffCmd
);
item
->
setCoordinate
(
c
);
return
true
;
}
}
return
false
;
}
void
WaypointManager
::
Utils
::
makeSpeedCmd
(
SimpleMissionItem
*
item
,
double
speed
)
{
assert
(
speed
>
0
);
...
...
@@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
item
->
setCoordinate
(
c
);
item
->
missionItem
().
setParam2
(
speed
);
}
bool
WaypointManager
::
Utils
::
makeLandCmd
(
SimpleMissionItem
*
item
,
Vehicle
*
vehicle
)
{
MAV_CMD
landCmd
=
vehicle
->
vtol
()
?
MAV_CMD_NAV_VTOL_LAND
:
MAV_CMD_NAV_LAND
;
if
(
vehicle
->
firmwarePlugin
()
->
supportedMissionCommands
().
contains
(
landCmd
))
{
item
->
setCommand
(
landCmd
);
}
else
{
Q_ASSERT
(
false
);
qWarning
(
"WaypointManager::Utils::makeLandCmd(): Land command not supported!"
);
return
false
;
}
return
true
;
}
src/Wima/WaypointManager/Utils.h
View file @
99e5ae38
This diff is collapsed.
Click to expand it.
src/Wima/WimaController.cc
View file @
99e5ae38
...
...
@@ -33,8 +33,6 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
return
static_cast
<
typename
std
::
underlying_type
<
T
>::
type
>
(
value
);
}
#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
...
...
@@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent)
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/WimaController.SettingsGroup.json"
),
this
)),
_enableWimaController
(
settingsGroup
,
_metaDataMap
[
enableWimaControllerName
]),
_flightSpeed
(
settingsGroup
,
_metaDataMap
[
flightSpeedName
]),
_altitude
(
settingsGroup
,
_metaDataMap
[
altitudeName
]),
_lowBatteryHandlingTriggered
(
false
),
_batteryLevelTicker
(
EVENT_TIMER_INTERVAL
,
1000
/*ms*/
)
{
// Set up facts for waypoint manager.
connect
(
&
_flightSpeed
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateflightSpeed
);
connect
(
&
_altitude
,
&
Fact
::
rawValueChanged
,
this
,
&
WimaController
::
_updateAltitude
);
// Periodic.
connect
(
&
_eventTimer
,
&
QTimer
::
timeout
,
this
,
&
WimaController
::
_eventTimerHandler
);
_eventTimer
.
start
(
EVENT_TIMER_INTERVAL
);
_metaDataMap
[
enableWimaControllerName
]){
// PlanData and Progress.
connect
(
WimaBridge
::
instance
(),
&
WimaBridge
::
planDataChanged
,
this
,
&
WimaController
::
planDataChangedHandler
);
...
...
@@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) {
emit
missionControllerChanged
();
}
void
WimaController
::
requestSmartRTL
()
{
qCWarning
(
WimaControllerLog
)
<<
"requestSmartRTL() called"
;
QString
errorString
(
"Smart RTL requested."
);
if
(
!
_SRTLPrecondition
(
errorString
))
{
qgcApp
()
->
showMessage
(
errorString
);
return
;
}
emit
smartRTLRequestConfirm
();
}
bool
WimaController
::
upload
()
{
auto
&
items
=
_currentWM
->
currentMissionItems
();
if
(
_masterController
&&
_masterController
->
managerVehicle
()
&&
...
...
@@ -141,68 +113,6 @@ bool WimaController::upload() {
}
}
bool
WimaController
::
forceUpload
()
{
auto
&
currentMissionItems
=
_currentWM
->
currentMissionItems
();
if
(
currentMissionItems
.
count
()
<
1
||
!
_missionController
||
!
_masterController
)
{
qCWarning
(
WimaControllerLog
)
<<
"forceUpload(): error:"
<<
"currentMissionItems.count(): "
<<
currentMissionItems
.
count
()
<<
"_missionController: "
<<
_missionController
<<
"_masterController: "
<<
_masterController
;
return
false
;
}
else
{
_missionController
->
removeAll
();
// Set homeposition of settingsItem.
QmlObjectListModel
*
visuals
=
_missionController
->
visualItems
();
MissionSettingsItem
*
settingsItem
=
visuals
->
value
<
MissionSettingsItem
*>
(
0
);
if
(
settingsItem
==
nullptr
)
{
Q_ASSERT
(
false
);
qCWarning
(
WimaControllerLog
)
<<
"updateCurrentMissionItems(): nullptr"
;
return
false
;
}
else
{
settingsItem
->
setCoordinate
(
_WMSettings
.
homePosition
());
// Copy mission items to _missionController and send them.
for
(
int
i
=
1
;
i
<
currentMissionItems
.
count
();
i
++
)
{
auto
*
item
=
currentMissionItems
.
value
<
const
SimpleMissionItem
*>
(
i
);
_missionController
->
insertSimpleMissionItem
(
*
item
,
visuals
->
count
());
}
_masterController
->
sendToVehicle
();
return
true
;
}
}
}
void
WimaController
::
removeFromVehicle
()
{
if
(
_masterController
&&
_missionController
)
{
_masterController
->
removeAllFromVehicle
();
_missionController
->
removeAll
();
}
}
void
WimaController
::
executeSmartRTL
()
{
qCWarning
(
WimaControllerLog
)
<<
"executeSmartRTL() called"
;
if
(
_masterController
&&
_masterController
->
managerVehicle
())
{
forceUpload
();
_masterController
->
managerVehicle
()
->
startMission
();
}
}
void
WimaController
::
initSmartRTL
()
{
qCWarning
(
WimaControllerLog
)
<<
"initSmartRTL() called"
;
_initSmartRTL
();
}
void
WimaController
::
removeVehicleTrajectoryHistory
()
{
if
(
_masterController
&&
_masterController
->
managerVehicle
())
{
_masterController
->
managerVehicle
()
->
trajectoryPoints
()
->
clearAndDeleteContents
();
}
}
bool
WimaController
::
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
)
{
...
...
@@ -312,109 +222,3 @@ void WimaController::_updateAltitude() {
emit
missionItemsChanged
();
emit
waypointPathChanged
();
}
void
WimaController
::
_checkBatteryLevel
()
{
if
(
_missionController
&&
_masterController
&&
_masterController
->
managerVehicle
())
{
Vehicle
*
managerVehicle
=
masterController
()
->
managerVehicle
();
WimaSettings
*
wimaSettings
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
();
int
threshold
=
wimaSettings
->
lowBatteryThreshold
()
->
rawValue
().
toInt
();
bool
enabled
=
_enableWimaController
.
rawValue
().
toBool
();
unsigned
int
minTime
=
wimaSettings
->
minimalRemainingMissionTime
()
->
rawValue
().
toUInt
();
if
(
enabled
)
{
Fact
*
battery1percentRemaining
=
managerVehicle
->
battery1FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
Fact
*
battery2percentRemaining
=
managerVehicle
->
battery2FactGroup
()
->
getFact
(
VehicleBatteryFactGroup
::
_percentRemainingFactName
);
if
(
battery1percentRemaining
->
rawValue
().
toDouble
()
<
threshold
&&
battery2percentRemaining
->
rawValue
().
toDouble
()
<
threshold
)
{
if
(
!
_lowBatteryHandlingTriggered
)
{
_lowBatteryHandlingTriggered
=
true
;
if
(
!
(
_missionController
->
remainingTime
()
<=
minTime
))
{
requestSmartRTL
();
}
}
}
else
{
_lowBatteryHandlingTriggered
=
false
;
}
}
}
}
void
WimaController
::
_eventTimerHandler
()
{
// Battery level check necessary?
Fact
*
enableLowBatteryHandling
=
qgcApp
()
->
toolbox
()
->
settingsManager
()
->
wimaSettings
()
->
enableLowBatteryHandling
();
if
(
enableLowBatteryHandling
->
rawValue
().
toBool
()
&&
_batteryLevelTicker
.
ready
())
_checkBatteryLevel
();
}
void
WimaController
::
_smartRTLCleanUp
(
bool
flying
)
{
if
(
!
flying
&&
_missionController
)
{
// vehicle has landed
_switchWaypointManager
(
_emptyWM
);
_missionController
->
removeAll
();
disconnect
(
masterController
()
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
_smartRTLCleanUp
);
}
}
bool
WimaController
::
_SRTLPrecondition
(
QString
&
errorString
)
{
if
(
!
_planDataValid
)
{
errorString
.
append
(
tr
(
"No WiMA data available. Please define at least a "
"measurement and a service area."
));
return
false
;
}
return
_rtlWM
.
checkPrecondition
(
errorString
);
}
void
WimaController
::
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
)
{
if
(
_currentWM
!=
&
manager
)
{
_currentWM
=
&
manager
;
emit
missionItemsChanged
();
emit
waypointPathChanged
();
qCWarning
(
WimaControllerLog
)
<<
"_switchWaypointManager: statistics update "
"missing."
;
}
}
void
WimaController
::
_initSmartRTL
()
{
static
int
attemptCounter
=
0
;
attemptCounter
++
;
QString
errorString
;
if
(
_SRTLPrecondition
(
errorString
))
{
if
(
_missionController
&&
_masterController
&&
_masterController
->
managerVehicle
())
{
_masterController
->
managerVehicle
()
->
pauseVehicle
();
connect
(
_masterController
->
managerVehicle
(),
&
Vehicle
::
flyingChanged
,
this
,
&
WimaController
::
_smartRTLCleanUp
);
if
(
_rtlWM
.
update
())
{
// Calculate return path.
_switchWaypointManager
(
_rtlWM
);
removeFromVehicle
();
attemptCounter
=
0
;
emit
smartRTLPathConfirm
();
}
}
}
else
if
(
attemptCounter
>
SMART_RTL_MAX_ATTEMPTS
)
{
errorString
.
append
(
tr
(
"Smart RTL: No success after maximum number of attempts."
));
qgcApp
()
->
showMessage
(
errorString
);
attemptCounter
=
0
;
}
else
{
_smartRTLTimer
.
singleShot
(
SMART_RTL_ATTEMPT_INTERVAL
,
this
,
&
WimaController
::
_initSmartRTL
);
}
}
src/Wima/WimaController.h
View file @
99e5ae38
...
...
@@ -70,10 +70,6 @@ public:
// Member Methodes
Q_INVOKABLE
WimaController
*
thisPointer
();
// Smart RTL.
Q_INVOKABLE
void
requestSmartRTL
();
Q_INVOKABLE
void
initSmartRTL
();
Q_INVOKABLE
void
executeSmartRTL
();
// Other.
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
Q_INVOKABLE
bool
upload
();
...
...
@@ -97,9 +93,6 @@ signals:
// Waypoints.
void
missionItemsChanged
(
void
);
void
waypointPathChanged
(
void
);
// Smart RTL.
void
smartRTLRequestConfirm
(
void
);
void
smartRTLPathConfirm
(
void
);
// Upload.
void
forceUploadConfirm
(
void
);
...
...
@@ -111,11 +104,6 @@ private slots:
QVector
<
QGeoCoordinate
>
&
path
);
void
_updateflightSpeed
(
void
);
void
_updateAltitude
(
void
);
// SMART RTL
void
_checkBatteryLevel
(
void
);
bool
_SRTLPrecondition
(
QString
&
errorString
);
void
_initSmartRTL
();
void
_smartRTLCleanUp
(
bool
flying
);
// Snake.
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
// Periodic tasks.
...
...
@@ -152,10 +140,6 @@ private:
SettingsFact
_flightSpeed
;
// mission flight speed
SettingsFact
_altitude
;
// mission altitude
// Smart RTL.
QTimer
_smartRTLTimer
;
bool
_lowBatteryHandlingTriggered
;
// Periodic tasks.
QTimer
_eventTimer
;
EventTicker
_batteryLevelTicker
;
...
...
src/Wima/WimaPlaner.cc
View file @
99e5ae38
This diff is collapsed.
Click to expand it.
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