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:
...
@@ -118,7 +118,7 @@ public:
QString
mapVisualQML
(
void
)
const
override
{
return
QStringLiteral
(
"SimpleItemMapVisual.qml"
);
}
QString
mapVisualQML
(
void
)
const
override
{
return
QStringLiteral
(
"SimpleItemMapVisual.qml"
);
}
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
void
applyNewAltitude
(
double
newAltitude
)
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
;
ReadyForSaveState
readyForSaveState
(
void
)
const
final
;
double
additionalTimeDelay
(
void
)
const
final
;
double
additionalTimeDelay
(
void
)
const
final
;
bool
exitCoordinateSameAsEntry
(
void
)
const
final
{
return
true
;
}
bool
exitCoordinateSameAsEntry
(
void
)
const
final
{
return
true
;
}
...
...
src/MissionManager/TransectStyleComplexItem.h
View file @
99e5ae38
...
@@ -88,8 +88,8 @@ public:
...
@@ -88,8 +88,8 @@ public:
// Overrides from VisualMissionItem
// Overrides from VisualMissionItem
void
save
(
QJsonArray
&
planItems
)
override
=
0
;
void
save
(
QJsonArray
&
planItems
)
override
=
0
;
bool
specifiesCoordinate
(
void
)
const
override
=
0
;
bool
specifiesCoordinate
(
void
)
const
override
=
0
;
v
oid
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
v
irtual
void
appendMissionItems
(
QList
<
MissionItem
*>&
items
,
QObject
*
missionItemParent
)
final
;
v
oid
applyNewAltitude
(
double
newAltitude
)
final
;
v
irtual
void
applyNewAltitude
(
double
newAltitude
)
final
;
bool
dirty
(
void
)
const
final
{
return
_dirty
;
}
bool
dirty
(
void
)
const
final
{
return
_dirty
;
}
bool
isSimpleItem
(
void
)
const
final
{
return
false
;
}
bool
isSimpleItem
(
void
)
const
final
{
return
false
;
}
bool
isStandaloneCoordinate
(
void
)
const
final
{
return
false
;
}
bool
isStandaloneCoordinate
(
void
)
const
final
{
return
false
;
}
...
@@ -100,7 +100,7 @@ public:
...
@@ -100,7 +100,7 @@ public:
double
specifiedFlightSpeed
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedFlightSpeed
(
void
)
final
{
return
std
::
numeric_limits
<
double
>::
quiet_NaN
();
}
double
specifiedGimbalYaw
(
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
();
}
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
;
ReadyForSaveState
readyForSaveState
(
void
)
const
override
;
QString
commandDescription
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
QString
commandDescription
(
void
)
const
override
{
return
tr
(
"Transect"
);
}
QString
commandName
(
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 @@
...
@@ -19,6 +19,8 @@
#include "CircularGenerator.h"
#include "CircularGenerator.h"
#include "LinearGenerator.h"
#include "LinearGenerator.h"
// ToDo: Check what happened to _transectsDirty
QGC_LOGGING_CATEGORY
(
CircularSurveyLog
,
"CircularSurveyLog"
)
QGC_LOGGING_CATEGORY
(
CircularSurveyLog
,
"CircularSurveyLog"
)
template
<
typename
T
>
template
<
typename
T
>
...
@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
...
@@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey";
const
char
*
CircularSurvey
::
CircularSurveyName
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
CircularSurveyName
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
variantName
=
"Variant"
;
const
char
*
CircularSurvey
::
variantName
=
"Variant"
;
CircularSurvey
::
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
CircularSurvey
::
CircularSurvey
(
PlanMasterController
*
masterController
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
QObject
*
parent
)
const
QString
&
kmlOrShpFile
,
QObject
*
parent
)
:
TransectStyleComplexItem
(
vehicle
,
flyView
,
settingsGroup
,
parent
),
:
TransectStyleComplexItem
(
masterController
,
flyView
,
settingsGroup
,
parent
),
_state
(
STATE
::
IDLE
),
_state
(
STATE
::
IDLE
),
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/CircularSurvey.SettingsGroup.json"
),
this
)),
QStringLiteral
(
":/json/CircularSurvey.SettingsGroup.json"
),
this
)),
...
@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
...
@@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&
CircularSurvey
::
_setTransects
);
&
CircularSurvey
::
_setTransects
);
connect
(
this
->
_pWorker
.
get
(),
&
RoutingThread
::
calculatingChanged
,
this
,
connect
(
this
->
_pWorker
.
get
(),
&
RoutingThread
::
calculatingChanged
,
this
,
&
CircularSurvey
::
calculatingChanged
);
&
CircularSurvey
::
calculatingChanged
);
this
->
_transectsDirty
=
true
;
// Altitude
connect
(
&
_cameraCalc
,
&
CameraCalc
::
distanceToSurfaceRelativeChanged
,
this
,
&
CircularSurvey
::
coordinateHasRelativeAltitudeChanged
);
connect
(
&
_cameraCalc
,
&
CameraCalc
::
distanceToSurfaceRelativeChanged
,
this
,
&
CircularSurvey
::
exitCoordinateHasRelativeAltitudeChanged
);
// Register Generators.
// Register Generators.
auto
lg
=
std
::
make_shared
<
routing
::
LinearGenerator
>
(
this
->
_pAreaData
);
auto
lg
=
std
::
make_shared
<
routing
::
LinearGenerator
>
(
this
->
_pAreaData
);
...
@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
...
@@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return
false
;
return
false
;
}
}
if
(
!
_load
(
complexObject
,
errorString
))
{
if
(
!
load
(
complexObject
,
sequenceNumber
,
errorString
))
{
_ignoreRecalc
=
false
;
_ignoreRecalc
=
false
;
return
false
;
return
false
;
}
}
...
@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
...
@@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
_ignoreRecalc
=
false
;
_ignoreRecalc
=
false
;
_recalcComplexDistance
();
if
(
_cameraShots
==
0
)
{
if
(
_cameraShots
==
0
)
{
// Shot count was possibly not available from plan file
// Shot count was possibly not available from plan file
_recalcCameraShots
();
_recalcCameraShots
();
...
@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
...
@@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
bool
CircularSurvey
::
specifiesCoordinate
()
const
{
return
true
;
}
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
(
bool
CircularSurvey
::
_switchToGenerator
(
const
CircularSurvey
::
PtrGenerator
&
newG
)
{
const
CircularSurvey
::
PtrGenerator
&
newG
)
{
if
(
this
->
_pGenerator
!=
newG
)
{
if
(
this
->
_pGenerator
!=
newG
)
{
...
@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
...
@@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() {
}
}
void
CircularSurvey
::
_updateWorker
()
{
void
CircularSurvey
::
_updateWorker
()
{
// Mark transects as dirty.
this
->
_transectsDirty
=
true
;
// Reset data.
// Reset data.
this
->
_transects
.
clear
();
this
->
_transects
.
clear
();
this
->
_variantVector
.
clear
();
this
->
_variantVector
.
clear
();
...
@@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() {
...
@@ -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
;
}
double
CircularSurvey
::
timeBetweenShots
()
{
return
1
;
}
QString
CircularSurvey
::
commandDescription
()
const
{
QString
CircularSurvey
::
commandDescription
()
const
{
...
@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
...
@@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString
CircularSurvey
::
abbreviation
()
const
{
return
tr
(
"C.S."
);
}
QString
CircularSurvey
::
abbreviation
()
const
{
return
tr
(
"C.S."
);
}
bool
CircularSurvey
::
readyForSave
()
const
{
TransectStyleComplexItem
::
ReadyForSaveState
return
TransectStyleComplexItem
::
readyForSave
()
&&
!
_transectsDirty
&&
CircularSurvey
::
readyForSaveState
()
const
{
this
->
_state
==
STATE
::
IDLE
;
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
;
}
double
CircularSurvey
::
additionalTimeDelay
()
const
{
return
0
;
}
...
@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
...
@@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
<<
" ms"
;
<<
" 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
// no cameraShots in Circular Survey, add if desired
void
CircularSurvey
::
_recalcCameraShots
()
{
_cameraShots
=
0
;
}
void
CircularSurvey
::
_recalcCameraShots
()
{
_cameraShots
=
0
;
}
...
@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
...
@@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
&
CircularSurvey
::
_changeVariant
);
&
CircularSurvey
::
_changeVariant
);
this
->
_changeVariantWorker
();
this
->
_changeVariantWorker
();
// Mark transect ready.
this
->
_transectsDirty
=
false
;
this
->
_state
=
STATE
::
SKIPP
;
this
->
_state
=
STATE
::
SKIPP
;
this
->
_rebuildTransects
();
this
->
_rebuildTransects
();
}
else
{
}
else
{
...
...
src/Wima/CircularSurvey.h
View file @
99e5ae38
...
@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
...
@@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem {
using
PtrGenerator
=
std
::
shared_ptr
<
routing
::
GeneratorBase
>
;
using
PtrGenerator
=
std
::
shared_ptr
<
routing
::
GeneratorBase
>
;
using
PtrRoutingData
=
std
::
shared_ptr
<
RoutingData
>
;
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:
public:
/// @param vehicle Vehicle which this is being contructed for
CircularSurvey
(
PlanMasterController
*
masterController
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
/// @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
,
QObject
*
parent
);
QObject
*
parent
);
~
CircularSurvey
();
~
CircularSurvey
();
...
@@ -63,14 +60,11 @@ public:
...
@@ -63,14 +60,11 @@ public:
QString
mapVisualQML
(
void
)
const
override
final
;
QString
mapVisualQML
(
void
)
const
override
final
;
void
save
(
QJsonArray
&
planItems
)
override
final
;
void
save
(
QJsonArray
&
planItems
)
override
final
;
bool
specifiesCoordinate
(
void
)
const
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
;
double
timeBetweenShots
(
void
)
override
final
;
QString
commandDescription
(
void
)
const
override
final
;
QString
commandDescription
(
void
)
const
override
final
;
QString
commandName
(
void
)
const
override
final
;
QString
commandName
(
void
)
const
override
final
;
QString
abbreviation
(
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
;
double
additionalTimeDelay
(
void
)
const
override
final
;
// Generator
// Generator
...
@@ -97,7 +91,6 @@ signals:
...
@@ -97,7 +91,6 @@ signals:
private
slots
:
private
slots
:
// Overrides from TransectStyleComplexItem
// Overrides from TransectStyleComplexItem
void
_rebuildTransectsPhase1
(
void
)
final
;
void
_rebuildTransectsPhase1
(
void
)
final
;
void
_recalcComplexDistance
(
void
)
final
;
void
_recalcCameraShots
(
void
)
final
;
void
_recalcCameraShots
(
void
)
final
;
// Worker functions.
// Worker functions.
...
@@ -108,11 +101,6 @@ private slots:
...
@@ -108,11 +101,6 @@ private slots:
void
_reverseWorker
();
void
_reverseWorker
();
private:
private:
void
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
void
_buildAndAppendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
bool
_switchToGenerator
(
const
PtrGenerator
&
newG
);
bool
_switchToGenerator
(
const
PtrGenerator
&
newG
);
// State.
// State.
...
@@ -139,7 +127,6 @@ private:
...
@@ -139,7 +127,6 @@ private:
PtrGenerator
_pGenerator
;
PtrGenerator
_pGenerator
;
// Routing.
// Routing.
using
Variant
=
Transects
;
QVector
<
Variant
>
_variantVector
;
QVector
<
Variant
>
_variantVector
;
PtrWorker
_pWorker
;
PtrWorker
_pWorker
;
};
};
src/Wima/Geometry/WimaArea.cc
View file @
99e5ae38
...
@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
...
@@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2,
}
else
if
(
retValue
==
JoinPolygonError
::
PathSizeLow
)
{
}
else
if
(
retValue
==
JoinPolygonError
::
PathSizeLow
)
{
qWarning
(
"Polygon vertex count is low."
);
qWarning
(
"Polygon vertex count is low."
);
}
else
{
}
else
{
Q
Vector
<
QGeoCoordinate
>
path
;
Q
List
<
QGeoCoordinate
>
path
;
toGeoList
(
joinedPolygon
,
origin
,
path
);
toGeoList
(
joinedPolygon
,
origin
,
path
);
// qWarning("after transform");
// qWarning("after transform");
// qWarning() << path;
// qWarning() << path;
...
@@ -491,38 +491,6 @@ void WimaArea::init() {
...
@@ -491,38 +491,6 @@ void WimaArea::init() {
&
WimaArea
::
recalcInteractivity
);
&
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
// QDoc Documentation
/*!
/*!
...
...
src/Wima/Geometry/WimaMeasurementArea.cc
View file @
99e5ae38
...
@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
...
@@ -20,7 +20,7 @@ TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData
&
TileData
::
operator
=
(
const
TileData
&
other
)
{
TileData
&
TileData
::
operator
=
(
const
TileData
&
other
)
{
this
->
tiles
.
clearAndDeleteContents
();
this
->
tiles
.
clearAndDeleteContents
();
for
(
std
::
size_t
i
=
0
;
i
<
std
::
size_t
(
other
.
tiles
.
count
());
++
i
)
{
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
);
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
if
(
tile
!=
nullptr
)
{
this
->
tiles
.
append
(
new
SnakeTile
(
*
tile
,
this
));
this
->
tiles
.
append
(
new
SnakeTile
(
*
tile
,
this
));
...
@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
...
@@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) {
}
}
bool
TileData
::
operator
==
(
const
TileData
&
other
)
const
{
bool
TileData
::
operator
==
(
const
TileData
&
other
)
const
{
return
this
->
tiles
==
other
.
tiles
&&
if
(
this
->
tileCenterPoints
==
other
.
tileCenterPoints
&&
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
{
bool
TileData
::
operator
!=
(
const
TileData
&
other
)
const
{
...
@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
...
@@ -72,7 +81,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
minTileAreaName
],
_metaDataMap
[
minTileAreaName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_state
(
STATE
::
IDLE
)
{
_state
(
STATE
::
IDLE
)
{
...
@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
...
@@ -91,7 +100,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_minTileAreaPercent
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
minTileAreaName
],
_metaDataMap
[
minTileAreaName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
_showTiles
(
SettingsFact
(
settingsGroup
,
_metaDataMap
[
showTilesName
],
this
/* QObject parent */
)),
this
/* QObject parent */
)),
_state
(
STATE
::
IDLE
)
{
_state
(
STATE
::
IDLE
)
{
...
@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
...
@@ -157,12 +166,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void
WimaMeasurementArea
::
saveToJson
(
QJsonObject
&
json
)
{
void
WimaMeasurementArea
::
saveToJson
(
QJsonObject
&
json
)
{
if
(
ready
())
{
if
(
ready
())
{
this
->
WimaArea
::
saveToJson
(
json
);
this
->
WimaArea
::
saveToJson
(
json
);
json
[
tileHeightName
]
=
_tileHeight
.
rawValue
().
toDouble
();
json
[
tileHeightName
]
=
_tileHeight
.
rawValue
().
toDouble
();
json
[
tileWidthName
]
=
_tileWidth
.
rawValue
().
toDouble
();
json
[
tileWidthName
]
=
_tileWidth
.
rawValue
().
toDouble
();
json
[
minTileAreaName
]
=
_minTileAreaPercent
.
rawValue
().
toDouble
();
json
[
minTileAreaName
]
=
_minTileAreaPercent
.
rawValue
().
toDouble
();
json
[
showTilesName
]
=
_showTiles
.
rawValue
().
toBool
();
json
[
showTilesName
]
=
_showTiles
.
rawValue
().
toBool
();
json
[
areaTypeName
]
=
WimaMeasurementAreaName
;
json
[
areaTypeName
]
=
WimaMeasurementAreaName
;
}
else
{
}
else
{
qCDebug
(
WimaMeasurementAreaLog
)
<<
"saveToJson(): not ready for saveing."
;
qCDebug
(
WimaMeasurementAreaLog
)
<<
"saveToJson(): not ready for saveing."
;
}
}
...
@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
...
@@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() {
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
if
(
this
->
_state
!=
STATE
::
UPDATEING
&&
this
->
_state
!=
STATE
::
STOP
)
{
if
(
this
->
_state
!=
STATE
::
UPDATEING
&&
this
->
_state
!=
STATE
::
STOP
)
{
const
auto
height
=
this
->
_tileHeight
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
height
=
this
->
_tileHeight
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
width
=
this
->
_tileWidth
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
width
=
this
->
_tileWidth
.
rawValue
().
toDouble
()
*
si
::
meter
;
const
auto
tileArea
=
width
*
height
;
const
auto
tileArea
=
width
*
height
;
const
auto
totalArea
=
this
->
area
()
*
si
::
meter
*
si
::
meter
;
const
auto
totalArea
=
this
->
area
()
*
si
::
meter
*
si
::
meter
;
const
auto
estNumTiles
=
totalArea
/
tileArea
;
const
auto
estNumTiles
=
totalArea
/
tileArea
;
// Check some conditions.
// Check some conditions.
if
(
long
(
std
::
ceil
(
estNumTiles
.
value
()))
<=
SNAKE_MAX_TILES
&&
if
(
long
(
std
::
ceil
(
estNumTiles
.
value
()))
<=
SNAKE_MAX_TILES
&&
this
->
count
()
>=
3
&&
this
->
isSimplePolygon
())
{
this
->
count
()
>=
3
&&
this
->
isSimplePolygon
())
{
setState
(
STATE
::
UPDATEING
);
setState
(
STATE
::
UPDATEING
);
auto
polygon
=
this
->
coordinateList
();
auto
polygon
=
this
->
coordinateList
();
for
(
auto
&
v
:
polygon
)
{
for
(
auto
&
v
:
polygon
)
{
v
.
setAltitude
(
0
);
v
.
setAltitude
(
0
);
}
}
const
auto
minArea
=
const
auto
minArea
=
this
->
_minTileAreaPercent
.
rawValue
().
toDouble
()
/
100
*
tileArea
;
this
->
_minTileAreaPercent
.
rawValue
().
toDouble
()
/
100
*
tileArea
;
auto
*
th
=
this
->
thread
();
auto
*
th
=
this
->
thread
();
auto
future
=
QtConcurrent
::
run
([
polygon
,
th
,
height
,
width
,
minArea
]
{
auto
future
=
QtConcurrent
::
run
([
polygon
,
th
,
height
,
width
,
minArea
]
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
DataPtr
pData
(
new
TileData
());
DataPtr
pData
(
new
TileData
());
// Convert to ENU system.
// Convert to ENU system.
QGeoCoordinate
origin
=
polygon
.
first
();
QGeoCoordinate
origin
=
polygon
.
first
();
FPolygon
polygonENU
;
FPolygon
polygonENU
;
areaToEnu
(
origin
,
polygon
,
polygonENU
);
areaToEnu
(
origin
,
polygon
,
polygonENU
);
std
::
vector
<
FPolygon
>
tilesENU
;
std
::
vector
<
FPolygon
>
tilesENU
;
BoundingBox
bbox
;
BoundingBox
bbox
;
std
::
string
errorString
;
std
::
string
errorString
;
// Generate tiles.
// Generate tiles.
if
(
snake
::
tiles
(
polygonENU
,
height
,
width
,
minArea
,
tilesENU
,
bbox
,
if
(
snake
::
tiles
(
polygonENU
,
height
,
width
,
minArea
,
tilesENU
,
bbox
,
errorString
))
{
errorString
))
{
// Convert to geo system.
// Convert to geo system.
for
(
const
auto
&
t
:
tilesENU
)
{
for
(
const
auto
&
t
:
tilesENU
)
{
auto
geoTile
=
new
SnakeTile
(
pData
.
get
());
auto
geoTile
=
new
SnakeTile
(
pData
.
get
());
for
(
const
auto
&
v
:
t
.
outer
())
{
for
(
const
auto
&
v
:
t
.
outer
())
{
QGeoCoordinate
geoVertex
;
QGeoCoordinate
geoVertex
;
fromENU
(
origin
,
v
,
geoVertex
);
fromENU
(
origin
,
v
,
geoVertex
);
geoTile
->
push_back
(
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
)
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): update time: "
<<
"doUpdate(): update time: "
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
<<
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
std
::
chrono
::
high_resolution_clock
::
now
()
-
start
)
.
count
()
.
count
()
<<
" ms"
;
<<
" ms"
;
return
pData
;
return
pData
;
});
// QtConcurrent::run()
});
// QtConcurrent::run()
this
->
_watcher
.
setFuture
(
future
);
this
->
_watcher
.
setFuture
(
future
);
}
}
}
}
qCDebug
(
WimaMeasurementAreaLog
)
qCDebug
(
WimaMeasurementAreaLog
)
<<
"doUpdate(): execution time: "
<<
"doUpdate(): execution time: "
...
...
src/Wima/Geometry/WimaServiceArea.cc
View file @
99e5ae38
#include "WimaServiceArea.h"
#include "WimaServiceArea.h"
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY
(
WimaServiceAreaLog
,
"WimaServiceAreaLog"
)
const
char
*
WimaServiceArea
::
wimaServiceAreaName
=
"Service Area"
;
const
char
*
WimaServiceArea
::
wimaServiceAreaName
=
"Service Area"
;
const
char
*
WimaServiceArea
::
depotLatitudeName
=
"DepotLatitude"
;
const
char
*
WimaServiceArea
::
depotLatitudeName
=
"DepotLatitude"
;
const
char
*
WimaServiceArea
::
depotLongitudeName
=
"DepotLongitude"
;
const
char
*
WimaServiceArea
::
depotLongitudeName
=
"DepotLongitude"
;
...
@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
...
@@ -102,20 +106,24 @@ void WimaServiceArea::init() {
}
else
if
(
this
->
_depot
.
isValid
())
{
}
else
if
(
this
->
_depot
.
isValid
())
{
// Use nearest coordinate.
// Use nearest coordinate.
auto
minDist
=
std
::
numeric_limits
<
double
>::
infinity
();
auto
minDist
=
std
::
numeric_limits
<
double
>::
infinity
();
auto
minIt
=
this
->
pathReference
().
begin
();
int
minIndex
=
0
;
for
(
auto
it
=
this
->
pathReference
().
begin
();
for
(
int
idx
=
0
;
idx
<
this
->
pathModel
().
count
();
++
idx
)
{
it
<
this
->
pathReference
().
end
();
++
it
)
{
const
QObject
*
obj
=
this
->
pathModel
()[
idx
];
const
auto
vertex
=
it
->
value
<
QGeoCoordinate
>
();
const
auto
*
vertex
=
qobject_cast
<
const
QGeoCoordinate
*>
(
obj
);
auto
d
=
vertex
.
distanceTo
(
this
->
_depot
);
if
(
vertex
!=
nullptr
)
{
if
(
d
<
minDist
)
{
auto
d
=
vertex
->
distanceTo
(
this
->
_depot
);
minDist
=
d
;
if
(
d
<
minDist
)
{
minIt
=
it
;
minDist
=
d
;
minIndex
=
idx
;
}
}
else
{
qCCritical
(
WimaServiceAreaLog
)
<<
"init(): nullptr catched!"
;
}
}
}
}
this
->
setDepot
(
minIt
->
value
<
QGeoCoordinate
>
(
));
this
->
setDepot
(
*
this
->
pathModel
().
value
<
QGeoCoordinate
*>
(
minIndex
));
}
else
if
(
this
->
path
Reference
().
size
()
>
0
)
{
}
else
if
(
this
->
path
Model
().
count
()
>
0
)
{
// Use first coordinate.
// 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) {
...
@@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) {
if
(
progress
.
size
()
==
tiles
->
count
())
{
if
(
progress
.
size
()
==
tiles
->
count
())
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
if
(
progress
[
i
]
==
100
)
{
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
)
{
if
(
tile
!=
nullptr
)
{
snake
::
FPolygon
tileENU
;
snake
::
FPolygon
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
...
...
src/Wima/Snake/LinearGenerator.cpp
View file @
99e5ae38
...
@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
...
@@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) {
if
(
progress
.
size
()
==
tiles
->
count
())
{
if
(
progress
.
size
()
==
tiles
->
count
())
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
for
(
int
i
=
0
;
i
<
tiles
->
count
();
++
i
)
{
if
(
progress
[
i
]
==
100
)
{
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
)
{
if
(
tile
!=
nullptr
)
{
snake
::
FPolygon
tileENU
;
snake
::
FPolygon
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
);
...
...
src/Wima/Snake/NemoInterface.cpp
View file @
99e5ae38
...
@@ -97,7 +97,7 @@ private:
...
@@ -97,7 +97,7 @@ private:
};
};
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
using
StatusMap
=
std
::
map
<
NemoInterface
::
STATUS
,
QString
>
;
StatusMap
statusMap
{
static
StatusMap
statusMap
{
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
NemoInterface
::
STATUS
::
NOT_CONNECTED
,
"Not Connected"
),
NemoInterface
::
STATUS
::
NOT_CONNECTED
,
"Not Connected"
),
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
std
::
make_pair
<
NemoInterface
::
STATUS
,
QString
>
(
...
@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
...
@@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p)
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
auto
connectionString
=
connectionStringFact
->
rawValue
().
toString
();
if
(
ros_bridge
::
isValidConnectionString
(
if
(
ros_bridge
::
isValidConnectionString
(
connectionString
.
toLocal8Bit
().
data
()))
{
connectionString
.
toLocal8Bit
().
data
()))
{
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
}
else
{
}
else
{
QString
defaultString
(
"localhost:9090"
);
qgcApp
()
->
warningMessageBoxOnMainThread
(
qgcApp
()
->
showMessage
(
"ROS Bridge connection string invalid: "
+
"Nemo Interface"
,
connectionString
);
"Websocket connection string possibly invalid: "
+
connectionString
+
qgcApp
()
->
showMessage
(
"Resetting connection string to: "
+
defaultString
);
". Trying to connect anyways."
);
connectionStringFact
->
setRawValue
(
QVariant
(
defaultString
));
// calls this function recursively
}
}
this
->
pRosBridge
.
reset
(
new
ros_bridge
::
ROSBridge
(
connectionString
.
toLocal8Bit
().
data
()));
};
};
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
connect
(
connectionStringFact
,
&
SettingsFact
::
rawValueChanged
,
setConnectionString
);
setConnectionString
);
...
@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
...
@@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
UniqueLock
lk1
(
this
->
ENUOriginMutex
,
std
::
adopt_lock
);
UniqueLock
lk1
(
this
->
ENUOriginMutex
,
std
::
adopt_lock
);
UniqueLock
lk2
(
this
->
tilesENUMutex
,
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
);
const
auto
*
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
if
(
tile
!=
nullptr
)
{
if
(
tile
->
coordinateList
().
size
()
>
0
)
{
if
(
tile
->
coordinateList
().
size
()
>
0
)
{
if
(
tile
->
coordinateList
().
first
().
isValid
())
{
if
(
tile
->
coordinateList
().
first
().
isValid
())
{
this
->
ENUOrigin
=
tile
->
coordinateList
().
first
();
this
->
ENUOrigin
=
tile
->
coordinateList
().
first
();
const
auto
&
origin
=
this
->
ENUOrigin
;
const
auto
&
origin
=
this
->
ENUOrigin
;
this
->
tilesENU
.
polygons
().
clear
();
this
->
tilesENU
.
polygons
().
clear
();
for
(
int
i
=
0
;
i
<
tileData
.
tiles
.
count
();
++
i
)
{
for
(
int
i
=
0
;
i
<
tileData
.
tiles
.
count
();
++
i
)
{
obj
=
tileData
.
tiles
.
get
(
i
)
;
obj
=
tileData
.
tiles
[
i
]
;
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
tile
=
qobject_cast
<
const
SnakeTile
*>
(
obj
);
if
(
tile
!=
nullptr
)
{
if
(
tile
!=
nullptr
)
{
SnakeTileLocal
tileENU
;
SnakeTileLocal
tileENU
;
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
.
path
());
snake
::
areaToEnu
(
origin
,
tile
->
coordinateList
(),
tileENU
.
path
());
this
->
tilesENU
.
polygons
().
push_back
(
std
::
move
(
tileENU
));
this
->
tilesENU
.
polygons
().
push_back
(
std
::
move
(
tileENU
));
}
else
{
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): nullptr."
;
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): nullptr."
;
break
;
break
;
}
}
}
}
}
else
{
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): Origin invalid."
;
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): Origin invalid."
;
}
}
}
else
{
}
else
{
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): tile empty."
;
qCDebug
(
NemoInterfaceLog
)
<<
"Impl::setTileData(): tile empty."
;
}
}
}
}
}
else
{
}
else
{
this
->
tileData
.
clear
();
this
->
tileData
.
clear
();
...
@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
...
@@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() {
progressMsg
.
progress
().
size
()
!=
progressMsg
.
progress
().
size
()
!=
requiredSize
)
{
// Some error occured.
requiredSize
)
{
// Some error occured.
progressMsg
.
progress
().
clear
();
progressMsg
.
progress
().
clear
();
qgcApp
()
->
showMessage
(
"Invalid progress message received."
);
qgcApp
()
->
informationMessageBoxOnMainThread
(
"Nemo Interface"
,
"Invalid progress message received."
);
}
}
emit
this
->
parent
->
progressChanged
();
emit
this
->
parent
->
progressChanged
();
...
@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
...
@@ -307,7 +306,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if
(
geographic_msgs
::
geo_point
::
toJson
(
origin
,
jOrigin
,
if
(
geographic_msgs
::
geo_point
::
toJson
(
origin
,
jOrigin
,
pDoc
->
GetAllocator
()))
{
pDoc
->
GetAllocator
()))
{
lk
.
unlock
();
lk
.
unlock
();
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
"origin"
,
jOrigin
,
pDoc
->
GetAllocator
());
}
else
{
}
else
{
lk
.
unlock
();
lk
.
unlock
();
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
...
@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
...
@@ -330,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
if
(
jsk_recognition_msgs
::
polygon_array
::
toJson
(
if
(
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
jSnakeTiles
,
pDoc
->
GetAllocator
()))
{
this
->
tilesENU
,
jSnakeTiles
,
pDoc
->
GetAllocator
()))
{
lk
.
unlock
();
lk
.
unlock
();
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
pDoc
->
AddMember
(
"tiles"
,
jSnakeTiles
,
pDoc
->
GetAllocator
());
}
else
{
}
else
{
lk
.
unlock
();
lk
.
unlock
();
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
...
@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
...
@@ -350,7 +349,7 @@ void NemoInterface::Impl::loop() {
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
this
->
pRosBridge
->
connected
()
&&
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
this
->
pRosBridge
->
connected
()
&&
!
this
->
topicServiceSetupDone
)
{
!
this
->
topicServiceSetupDone
)
{
this
->
doTopicServiceSetup
();
this
->
doTopicServiceSetup
();
this
->
topicServiceSetupDone
=
true
;
this
->
topicServiceSetupDone
=
true
;
this
->
setStatus
(
STATUS
::
WEBSOCKET_DETECTED
);
this
->
setStatus
(
STATUS
::
WEBSOCKET_DETECTED
);
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
}
else
if
(
this
->
pRosBridge
->
isRunning
()
&&
...
@@ -395,7 +394,7 @@ void NemoInterface::Impl::publishTilesENU() {
...
@@ -395,7 +394,7 @@ void NemoInterface::Impl::publishTilesENU() {
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
if
(
jsk_recognition_msgs
::
polygon_array
::
toJson
(
if
(
jsk_recognition_msgs
::
polygon_array
::
toJson
(
this
->
tilesENU
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
()))
{
this
->
tilesENU
,
*
jSnakeTiles
,
jSnakeTiles
->
GetAllocator
()))
{
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
this
->
pRosBridge
->
publish
(
std
::
move
(
jSnakeTiles
),
"/snake/tiles"
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"Impl::publishTilesENU: could not create json document."
;
<<
"Impl::publishTilesENU: could not create json document."
;
...
@@ -408,7 +407,7 @@ void NemoInterface::Impl::publishENUOrigin() {
...
@@ -408,7 +407,7 @@ void NemoInterface::Impl::publishENUOrigin() {
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
std
::
make_unique
<
rapidjson
::
Document
>
(
rapidjson
::
kObjectType
));
if
(
geographic_msgs
::
geo_point
::
toJson
(
this
->
ENUOrigin
,
*
jOrigin
,
if
(
geographic_msgs
::
geo_point
::
toJson
(
this
->
ENUOrigin
,
*
jOrigin
,
jOrigin
->
GetAllocator
()))
{
jOrigin
->
GetAllocator
()))
{
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
this
->
pRosBridge
->
publish
(
std
::
move
(
jOrigin
),
"/snake/origin"
);
}
else
{
}
else
{
qCWarning
(
NemoInterfaceLog
)
qCWarning
(
NemoInterfaceLog
)
<<
"Impl::publishENUOrigin: could not create json document."
;
<<
"Impl::publishENUOrigin: could not create json document."
;
...
...
src/Wima/WaypointManager/DefaultManager.cpp
View file @
99e5ae38
...
@@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem(
...
@@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem(
using
namespace
WaypointManager
::
Utils
;
using
namespace
WaypointManager
::
Utils
;
if
(
!
insertMissionItem
(
c
,
index
/*insertion index*/
,
list
,
if
(
!
insertMissionItem
(
c
,
index
/*insertion index*/
,
list
,
_settings
->
vehicle
(),
_settings
->
isFlyView
(),
_settings
->
masterController
(),
_settings
->
isFlyView
(),
&
list
/*parent*/
,
doUpdate
/*do update*/
))
{
&
list
/*parent*/
,
doUpdate
/*do update*/
))
{
qWarning
(
qWarning
(
"WaypointManager::DefaultManager::next(): insertMissionItem failed."
);
"WaypointManager::DefaultManager::next(): insertMissionItem failed."
);
...
@@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() {
...
@@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() {
}
}
_currentMissionItems
.
clearAndDeleteContents
();
_currentMissionItems
.
clearAndDeleteContents
();
initialize
(
_currentMissionItems
,
_settings
->
vehicle
(),
initialize
(
_currentMissionItems
,
_settings
->
masterController
(),
_settings
->
isFlyView
());
_settings
->
isFlyView
());
// Calculate path from home to first waypoint.
// Calculate path from home to first waypoint.
...
@@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() {
...
@@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() {
Q_ASSERT
(
takeoffItem
!=
nullptr
);
Q_ASSERT
(
takeoffItem
!=
nullptr
);
return
false
;
return
false
;
}
}
makeTakeOffCmd
(
takeoffItem
,
_settings
->
masterController
()
->
managerVehicle
()
);
makeTakeOffCmd
(
takeoffItem
,
_settings
->
masterController
());
// Create change speed item.
// Create change speed item.
_insertMissionItem
(
_settings
->
homePosition
(),
2
/*insertion index*/
,
_insertMissionItem
(
_settings
->
homePosition
(),
2
/*insertion index*/
,
...
@@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() {
...
@@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() {
qWarning
(
"WaypointManager::DefaultManager::next(): nullptr."
);
qWarning
(
"WaypointManager::DefaultManager::next(): nullptr."
);
return
false
;
return
false
;
}
}
makeLandCmd
(
landItem
,
_settings
->
masterController
()
->
managerVehicle
()
);
makeLandCmd
(
landItem
,
_settings
->
masterController
());
// Set altitude.
// Set altitude.
for
(
int
i
=
1
;
i
<
_currentMissionItems
.
count
();
++
i
)
{
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,
...
@@ -88,7 +88,7 @@ bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c,
using
namespace
WaypointManager
::
Utils
;
using
namespace
WaypointManager
::
Utils
;
if
(
!
insertMissionItem
(
c
,
index
/*insertion index*/
,
list
,
if
(
!
insertMissionItem
(
c
,
index
/*insertion index*/
,
list
,
_settings
->
vehicle
(),
_settings
->
isFlyView
(),
_settings
->
masterController
(),
_settings
->
isFlyView
(),
&
list
/*parent*/
,
doUpdate
/*do update*/
))
{
&
list
/*parent*/
,
doUpdate
/*do update*/
))
{
qWarning
(
"WaypointManager::RTLManager::next(): insertMissionItem failed."
);
qWarning
(
"WaypointManager::RTLManager::next(): insertMissionItem failed."
);
Q_ASSERT
(
false
);
Q_ASSERT
(
false
);
...
@@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() {
...
@@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() {
}
}
_currentMissionItems
.
clearAndDeleteContents
();
_currentMissionItems
.
clearAndDeleteContents
();
initialize
(
_currentMissionItems
,
_settings
->
vehicle
(),
initialize
(
_currentMissionItems
,
_settings
->
masterController
(),
_settings
->
isFlyView
());
_settings
->
isFlyView
());
// Calculate path from vehicle to home position.
// Calculate path from vehicle to home position.
...
@@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() {
...
@@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() {
qWarning
(
"WaypointManager::RTLManager::next(): nullptr."
);
qWarning
(
"WaypointManager::RTLManager::next(): nullptr."
);
return
false
;
return
false
;
}
}
makeLandCmd
(
landItem
,
_settings
->
masterController
()
->
managerVehicle
()
);
makeLandCmd
(
landItem
,
_settings
->
masterController
());
// Set altitude.
// Set altitude.
for
(
int
i
=
1
;
i
<
_currentMissionItems
.
count
();
++
i
)
{
for
(
int
i
=
1
;
i
<
_currentMissionItems
.
count
();
++
i
)
{
...
...
src/Wima/WaypointManager/Utils.cpp
View file @
99e5ae38
#include "Utils.h"
#include "Utils.h"
#include "MissionSettingsItem.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include <iostream>
#include <iostream>
#include <QGeoCoordinate>
#include <QGeoCoordinate>
template
<>
QVariant
WaypointManager
::
Utils
::
getCoordinate
(
const
SimpleMissionItem
*
item
)
{
return
QVariant
::
fromValue
(
item
->
coordinate
());
}
bool
WaypointManager
::
Utils
::
insertMissionItem
(
const
QGeoCoordinate
&
coordinate
,
bool
WaypointManager
::
Utils
::
insertMissionItem
(
const
QGeoCoordinate
&
coordinate
,
long
index
,
long
index
,
QmlObjectListModel
&
list
,
QmlObjectListModel
&
list
,
Vehicle
*
vehicle
,
PlanMasterController
*
masterController
,
bool
flyView
,
bool
flyView
,
QObject
*
parent
,
QObject
*
parent
,
bool
doUpdates
)
bool
doUpdates
)
...
@@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
...
@@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate,
}
else
{
}
else
{
if
(
parent
==
nullptr
)
if
(
parent
==
nullptr
)
parent
=
&
list
;
parent
=
&
list
;
SimpleMissionItem
*
newItem
=
new
SimpleMissionItem
(
vehicle
,
flyView
,
parent
);
auto
*
newItem
=
new
SimpleMissionItem
(
masterController
,
flyView
,
MissionItem
()
,
parent
);
int
sequenceNumber
=
detail
::
nextSequenceNumber
(
list
);
int
sequenceNumber
=
detail
::
nextSequenceNumber
(
list
);
newItem
->
setSequenceNumber
(
sequenceNumber
);
newItem
->
setSequenceNumber
(
sequenceNumber
);
...
@@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list)
...
@@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list)
}
}
void
WaypointManager
::
Utils
::
initialize
(
QmlObjectListModel
&
list
,
void
WaypointManager
::
Utils
::
initialize
(
QmlObjectListModel
&
list
,
Vehicle
*
vehicle
,
PlanMasterController
*
masterController
,
bool
flyView
)
bool
flyView
)
{
{
MissionSettingsItem
*
settingsItem
=
new
MissionSettingsItem
(
vehicle
,
flyView
,
&
list
);
MissionSettingsItem
*
settingsItem
=
new
MissionSettingsItem
(
masterController
,
flyView
,
&
list
);
list
.
insert
(
0
,
settingsItem
);
list
.
insert
(
0
,
settingsItem
);
}
}
...
@@ -189,26 +185,6 @@ void WaypointManager::Utils::printList(QmlObjectListModel &list)
...
@@ -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
)
void
WaypointManager
::
Utils
::
makeSpeedCmd
(
SimpleMissionItem
*
item
,
double
speed
)
{
{
assert
(
speed
>
0
);
assert
(
speed
>
0
);
...
@@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
...
@@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed)
item
->
setCoordinate
(
c
);
item
->
setCoordinate
(
c
);
item
->
missionItem
().
setParam2
(
speed
);
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) {
...
@@ -33,8 +33,6 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
return
static_cast
<
typename
std
::
underlying_type
<
T
>::
type
>
(
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
#define EVENT_TIMER_INTERVAL 50 // ms
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
const
char
*
WimaController
::
areaItemsName
=
"AreaItems"
;
...
@@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent)
...
@@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent)
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/WimaController.SettingsGroup.json"
),
this
)),
QStringLiteral
(
":/json/WimaController.SettingsGroup.json"
),
this
)),
_enableWimaController
(
settingsGroup
,
_enableWimaController
(
settingsGroup
,
_metaDataMap
[
enableWimaControllerName
]),
_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
);
// PlanData and Progress.
// PlanData and Progress.
connect
(
WimaBridge
::
instance
(),
&
WimaBridge
::
planDataChanged
,
this
,
connect
(
WimaBridge
::
instance
(),
&
WimaBridge
::
planDataChanged
,
this
,
&
WimaController
::
planDataChangedHandler
);
&
WimaController
::
planDataChangedHandler
);
...
@@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) {
...
@@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) {
emit
missionControllerChanged
();
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
()
{
bool
WimaController
::
upload
()
{
auto
&
items
=
_currentWM
->
currentMissionItems
();
auto
&
items
=
_currentWM
->
currentMissionItems
();
if
(
_masterController
&&
_masterController
->
managerVehicle
()
&&
if
(
_masterController
&&
_masterController
->
managerVehicle
()
&&
...
@@ -141,68 +113,6 @@ bool WimaController::upload() {
...
@@ -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
,
bool
WimaController
::
_calcShortestPath
(
const
QGeoCoordinate
&
start
,
const
QGeoCoordinate
&
destination
,
const
QGeoCoordinate
&
destination
,
QVector
<
QGeoCoordinate
>
&
path
)
{
QVector
<
QGeoCoordinate
>
&
path
)
{
...
@@ -312,109 +222,3 @@ void WimaController::_updateAltitude() {
...
@@ -312,109 +222,3 @@ void WimaController::_updateAltitude() {
emit
missionItemsChanged
();
emit
missionItemsChanged
();
emit
waypointPathChanged
();
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:
...
@@ -70,10 +70,6 @@ public:
// Member Methodes
// Member Methodes
Q_INVOKABLE
WimaController
*
thisPointer
();
Q_INVOKABLE
WimaController
*
thisPointer
();
// Smart RTL.
Q_INVOKABLE
void
requestSmartRTL
();
Q_INVOKABLE
void
initSmartRTL
();
Q_INVOKABLE
void
executeSmartRTL
();
// Other.
// Other.
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
Q_INVOKABLE
void
removeVehicleTrajectoryHistory
();
Q_INVOKABLE
bool
upload
();
Q_INVOKABLE
bool
upload
();
...
@@ -97,9 +93,6 @@ signals:
...
@@ -97,9 +93,6 @@ signals:
// Waypoints.
// Waypoints.
void
missionItemsChanged
(
void
);
void
missionItemsChanged
(
void
);
void
waypointPathChanged
(
void
);
void
waypointPathChanged
(
void
);
// Smart RTL.
void
smartRTLRequestConfirm
(
void
);
void
smartRTLPathConfirm
(
void
);
// Upload.
// Upload.
void
forceUploadConfirm
(
void
);
void
forceUploadConfirm
(
void
);
...
@@ -111,11 +104,6 @@ private slots:
...
@@ -111,11 +104,6 @@ private slots:
QVector
<
QGeoCoordinate
>
&
path
);
QVector
<
QGeoCoordinate
>
&
path
);
void
_updateflightSpeed
(
void
);
void
_updateflightSpeed
(
void
);
void
_updateAltitude
(
void
);
void
_updateAltitude
(
void
);
// SMART RTL
void
_checkBatteryLevel
(
void
);
bool
_SRTLPrecondition
(
QString
&
errorString
);
void
_initSmartRTL
();
void
_smartRTLCleanUp
(
bool
flying
);
// Snake.
// Snake.
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
void
_switchWaypointManager
(
WaypointManager
::
ManagerBase
&
manager
);
// Periodic tasks.
// Periodic tasks.
...
@@ -152,10 +140,6 @@ private:
...
@@ -152,10 +140,6 @@ private:
SettingsFact
_flightSpeed
;
// mission flight speed
SettingsFact
_flightSpeed
;
// mission flight speed
SettingsFact
_altitude
;
// mission altitude
SettingsFact
_altitude
;
// mission altitude
// Smart RTL.
QTimer
_smartRTLTimer
;
bool
_lowBatteryHandlingTriggered
;
// Periodic tasks.
// Periodic tasks.
QTimer
_eventTimer
;
QTimer
_eventTimer
;
EventTicker
_batteryLevelTicker
;
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