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
60b604d9
Commit
60b604d9
authored
Oct 19, 2020
by
Valentin Platzgummer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
temp, with errors
parent
58b2b82d
Changes
8
Hide whitespace changes
Inline
Side-by-side
Showing
8 changed files
with
236 additions
and
340 deletions
+236
-340
CircularSurvey.cc
src/Wima/CircularSurvey.cc
+176
-247
CircularSurvey.h
src/Wima/CircularSurvey.h
+50
-89
RoutingThread.h
src/Wima/RoutingThread.h
+1
-1
CircularGenerator.cpp
src/Wima/Snake/CircularGenerator.cpp
+3
-0
CircularGenerator.h
src/Wima/Snake/CircularGenerator.h
+3
-0
LinkConfiguration.h
src/comm/LinkConfiguration.h
+1
-1
LinkInterface.h
src/comm/LinkInterface.h
+1
-1
UnitTest.h
src/qgcunittest/UnitTest.h
+1
-1
No files found.
src/Wima/CircularSurvey.cc
View file @
60b604d9
...
...
@@ -9,10 +9,6 @@
#define CLIPPER_SCALE 1000000
#include "clipper/clipper.hpp"
using
namespace
ClipperLib
;
template
<>
inline
auto
get
<
0
>
(
const
IntPoint
&
p
)
{
return
p
.
X
;
}
template
<>
inline
auto
get
<
1
>
(
const
IntPoint
&
p
)
{
return
p
.
Y
;
}
#include "Geometry/GenericCircle.h"
#include "Snake/SnakeTile.h"
...
...
@@ -20,6 +16,9 @@ template <> inline auto get<1>(const IntPoint &p) { return p.Y; }
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
#include "CircularGenerator.h"
#include "LinearGenerator.h"
QGC_LOGGING_CATEGORY
(
CircularSurveyLog
,
"CircularSurveyLog"
)
template
<
typename
T
>
...
...
@@ -27,72 +26,30 @@ constexpr typename std::underlying_type<T>::type integral(T value) {
return
static_cast
<
typename
std
::
underlying_type
<
T
>::
type
>
(
value
);
}
bool
circularTransects
(
const
snake
::
FPolygon
&
polygon
,
const
std
::
vector
<
snake
::
FPolygon
>
&
tiles
,
snake
::
Length
deltaR
,
snake
::
Angle
deltaAlpha
,
snake
::
Length
minLength
,
snake
::
Transects
&
transects
);
bool
linearTransects
(
const
snake
::
FPolygon
&
polygon
,
const
std
::
vector
<
snake
::
FPolygon
>
&
tiles
,
snake
::
Length
distance
,
snake
::
Angle
angle
,
snake
::
Length
minLength
,
snake
::
Transects
&
transects
);
const
char
*
CircularSurvey
::
settingsGroup
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
transectDistanceName
=
"TransectDistance"
;
const
char
*
CircularSurvey
::
alphaName
=
"Alpha"
;
const
char
*
CircularSurvey
::
minLengthName
=
"MinLength"
;
const
char
*
CircularSurvey
::
typeName
=
"Type"
;
const
char
*
CircularSurvey
::
CircularSurveyName
=
"CircularSurvey"
;
const
char
*
CircularSurvey
::
refPointLatitudeName
=
"ReferencePointLat"
;
const
char
*
CircularSurvey
::
refPointLongitudeName
=
"ReferencePointLong"
;
const
char
*
CircularSurvey
::
refPointAltitudeName
=
"ReferencePointAlt"
;
const
char
*
CircularSurvey
::
variantName
=
"Variant"
;
const
char
*
CircularSurvey
::
numRunsName
=
"NumRuns"
;
const
char
*
CircularSurvey
::
runName
=
"Run"
;
CircularSurvey
::
CircularSurvey
(
Vehicle
*
vehicle
,
bool
flyView
,
const
QString
&
kmlOrShpFile
,
QObject
*
parent
)
:
TransectStyleComplexItem
(
vehicle
,
flyView
,
settingsGroup
,
parent
),
_
referencePoint
(
QGeoCoordinate
(
0
,
0
,
0
)
),
_
state
(
STATE
::
IDLE
),
_metaDataMap
(
FactMetaData
::
createMapFromJsonFile
(
QStringLiteral
(
":/json/CircularSurvey.SettingsGroup.json"
),
this
)),
_transectDistance
(
settingsGroup
,
_metaDataMap
[
transectDistanceName
]),
_alpha
(
settingsGroup
,
_metaDataMap
[
alphaName
]),
_minLength
(
settingsGroup
,
_metaDataMap
[
minLengthName
]),
_type
(
settingsGroup
,
_metaDataMap
[
typeName
]),
_variant
(
settingsGroup
,
_metaDataMap
[
variantName
]),
_numRuns
(
settingsGroup
,
_metaDataMap
[
numRunsName
]),
_run
(
settingsGroup
,
_metaDataMap
[
runName
]),
_pWorker
(
std
::
make_unique
<
RoutingThread
>
()),
_state
(
STATE
::
DEFAULT
),
_hidePolygon
(
false
)
{
_areaData
(
std
::
make_shared
<
WimaPlanData
>
()),
_pWorker
(
std
::
make_unique
<
RoutingThread
>
())
{
Q_UNUSED
(
kmlOrShpFile
)
_editorQml
=
"qrc:/qml/CircularSurveyItemEditor.qml"
;
// Connect facts.
connect
(
&
_transectDistance
,
&
Fact
::
valueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
&
_alpha
,
&
Fact
::
valueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
&
_minLength
,
&
Fact
::
valueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
this
,
&
CircularSurvey
::
refPointChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
this
,
&
CircularSurvey
::
depotChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
&
this
->
_type
,
&
Fact
::
rawValueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
&
this
->
_variant
,
&
Fact
::
rawValueChanged
,
this
,
&
CircularSurvey
::
_changeVariant
);
connect
(
&
this
->
_run
,
&
Fact
::
rawValueChanged
,
this
,
&
CircularSurvey
::
_changeRun
);
connect
(
&
this
->
_numRuns
,
&
Fact
::
rawValueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
// Areas.
connect
(
this
,
&
CircularSurvey
::
measurementAreaChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
connect
(
this
,
&
CircularSurvey
::
joinedAreaChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
// Connect worker.
connect
(
this
->
_pWorker
.
get
(),
&
RoutingThread
::
result
,
this
,
...
...
@@ -106,88 +63,27 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&
CircularSurvey
::
coordinateHasRelativeAltitudeChanged
);
connect
(
&
_cameraCalc
,
&
CameraCalc
::
distanceToSurfaceRelativeChanged
,
this
,
&
CircularSurvey
::
exitCoordinateHasRelativeAltitudeChanged
);
// Register Generators.
auto
cg
=
std
::
make_shared
<
routing
::
CircularGenerator
>
(
this
->
_areaData
);
registerGenerator
(
cg
->
name
(),
cg
);
auto
lg
=
std
::
make_shared
<
routing
::
LinearGenerator
>
(
this
->
_areaData
);
registerGenerator
(
lg
->
name
(),
lg
);
}
CircularSurvey
::~
CircularSurvey
()
{}
void
CircularSurvey
::
resetReference
()
{
setRefPoint
(
_mArea
.
center
());
}
void
CircularSurvey
::
reverse
()
{
this
->
_state
=
STATE
::
REVERSE
;
this
->
_rebuildTransects
();
}
void
CircularSurvey
::
setRefPoint
(
const
QGeoCoordinate
&
refPt
)
{
if
(
refPt
!=
_referencePoint
)
{
_referencePoint
=
refPt
;
_referencePoint
.
setAltitude
(
0
);
emit
refPointChanged
();
}
void
CircularSurvey
::
setPlanData
(
const
WimaPlanData
&
d
)
{
*
this
->
_areaData
=
d
;
}
QGeoCoordinate
CircularSurvey
::
refPoint
()
const
{
return
_referencePoint
;
}
Fact
*
CircularSurvey
::
transectDistance
()
{
return
&
_transectDistance
;
}
Fact
*
CircularSurvey
::
alpha
()
{
return
&
_alpha
;
}
bool
CircularSurvey
::
hidePolygon
()
const
{
return
_hidePolygon
;
}
QList
<
QString
>
CircularSurvey
::
variantNames
()
const
{
return
_variantNames
;
}
QList
<
QString
>
CircularSurvey
::
runNames
()
const
{
return
_runNames
;
}
QGeoCoordinate
CircularSurvey
::
depot
()
const
{
return
this
->
_depot
;
}
const
QList
<
QList
<
QGeoCoordinate
>>
&
CircularSurvey
::
rawTransects
()
const
{
return
this
->
_rawTransects
;
}
void
CircularSurvey
::
setHidePolygon
(
bool
hide
)
{
if
(
this
->
_hidePolygon
!=
hide
)
{
this
->
_hidePolygon
=
hide
;
emit
hidePolygonChanged
();
}
}
void
CircularSurvey
::
setMeasurementArea
(
const
WimaMeasurementAreaData
&
mArea
)
{
if
(
this
->
_mArea
!=
mArea
)
{
this
->
_mArea
=
mArea
;
emit
measurementAreaChanged
();
}
}
void
CircularSurvey
::
setJoinedArea
(
const
WimaJoinedAreaData
&
jArea
)
{
if
(
this
->
_jArea
!=
jArea
)
{
this
->
_jArea
=
jArea
;
emit
joinedAreaChanged
();
}
}
void
CircularSurvey
::
setMeasurementArea
(
const
WimaMeasurementArea
&
mArea
)
{
if
(
this
->
_mArea
!=
mArea
)
{
this
->
_mArea
=
mArea
;
emit
measurementAreaChanged
();
}
}
void
CircularSurvey
::
setJoinedArea
(
const
WimaJoinedArea
&
jArea
)
{
if
(
this
->
_jArea
!=
jArea
)
{
this
->
_jArea
=
jArea
;
emit
joinedAreaChanged
();
}
}
void
CircularSurvey
::
setDepot
(
const
QGeoCoordinate
&
depot
)
{
if
(
this
->
_depot
.
latitude
()
!=
depot
.
latitude
()
||
this
->
_depot
.
longitude
()
!=
depot
.
longitude
())
{
this
->
_depot
=
depot
;
this
->
_depot
.
setAltitude
(
0
);
emit
depotChanged
();
}
}
bool
CircularSurvey
::
load
(
const
QJsonObject
&
complexObject
,
int
sequenceNumber
,
QString
&
errorString
)
{
// We need to pull version first to determine what validation/conversion
...
...
@@ -253,9 +149,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return
false
;
}
_transectDistance
.
setRawValue
(
complexObject
[
transectDistanceName
].
toDouble
());
_alpha
.
setRawValue
(
complexObject
[
alphaName
].
toDouble
());
_minLength
.
setRawValue
(
complexObject
[
minLengthName
].
toDouble
());
_type
.
setRawValue
(
complexObject
[
typeName
].
toInt
());
_variant
.
setRawValue
(
complexObject
[
variantName
].
toInt
());
_numRuns
.
setRawValue
(
complexObject
[
numRunsName
].
toInt
());
...
...
@@ -289,9 +182,6 @@ void CircularSurvey::save(QJsonArray &planItems) {
VisualMissionItem
::
jsonTypeComplexItemValue
;
saveObject
[
ComplexMissionItem
::
jsonComplexItemTypeKey
]
=
CircularSurveyName
;
saveObject
[
transectDistanceName
]
=
_transectDistance
.
rawValue
().
toDouble
();
saveObject
[
alphaName
]
=
_alpha
.
rawValue
().
toDouble
();
saveObject
[
minLengthName
]
=
_minLength
.
rawValue
().
toDouble
();
saveObject
[
typeName
]
=
double
(
_type
.
rawValue
().
toUInt
());
saveObject
[
variantName
]
=
double
(
_variant
.
rawValue
().
toUInt
());
saveObject
[
numRunsName
]
=
double
(
_numRuns
.
rawValue
().
toUInt
());
...
...
@@ -370,6 +260,27 @@ void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
}
}
bool
CircularSurvey
::
_switchToGenerator
(
const
CircularSurvey
::
PtrGenerator
&
newG
)
{
if
(
this
->
_pGenerator
!=
newG
)
{
if
(
this
->
_pGenerator
!=
nullptr
)
{
disconnect
(
this
->
_pGenerator
.
get
(),
&
GeneratorBase
::
generatorChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
}
this
->
_pGenerator
=
newG
;
connect
(
this
->
_pGenerator
.
get
(),
&
GeneratorBase
::
generatorChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
this
->
_state
=
STATE
::
IDLE
;
_rebuildTransects
();
return
true
;
}
else
{
return
false
;
}
}
void
CircularSurvey
::
_changeVariant
()
{
this
->
_state
=
STATE
::
VARIANT_CHANGE
;
this
->
_rebuildTransects
();
...
...
@@ -385,130 +296,53 @@ void CircularSurvey::_updateWorker() {
this
->
_transectsDirty
=
true
;
// Reset data.
this
->
_transects
.
clear
();
this
->
_rawTransects
.
clear
();
this
->
_variantVector
.
clear
();
this
->
_variantNames
.
clear
();
this
->
_runNames
.
clear
();
emit
variantNamesChanged
();
emit
runNamesChanged
();
// Prepare data.
auto
ref
=
this
->
_referencePoint
;
auto
geoPolygon
=
this
->
_mArea
.
coordinateList
();
for
(
auto
&
v
:
geoPolygon
)
{
v
.
setAltitude
(
0
);
}
auto
pPolygon
=
std
::
make_shared
<
snake
::
FPolygon
>
();
snake
::
areaToEnu
(
ref
,
geoPolygon
,
*
pPolygon
);
// Progress and tiles.
const
auto
&
progress
=
this
->
_mArea
.
progress
();
const
auto
*
tiles
=
this
->
_mArea
.
tiles
();
auto
pTiles
=
std
::
make_shared
<
std
::
vector
<
snake
::
FPolygon
>>
();
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
);
if
(
tile
!=
nullptr
)
{
snake
::
FPolygon
tileENU
;
snake
::
areaToEnu
(
ref
,
tile
->
coordinateList
(),
tileENU
);
pTiles
->
push_back
(
std
::
move
(
tileENU
));
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): progress.size() != tiles->count()."
;
return
;
}
if
(
this
->
_areaData
->
isValid
())
{
// Prepare data.
auto
origin
=
this
->
_areaData
->
origin
();
// Convert safe area.
auto
geoSafeArea
=
this
->
_jArea
.
coordinateList
();
if
(
!
(
geoSafeArea
.
size
()
>=
3
))
{
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): safe area invalid."
<<
geoSafeArea
;
return
;
}
for
(
auto
&
v
:
geoSafeArea
)
{
if
(
v
.
isValid
())
{
v
.
setAltitude
(
0
);
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): safe area contains invalid coordinate."
<<
geoSafeArea
;
return
;
}
}
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): progress.size() != tiles->count()."
;
return
;
}
// Convert safe area.
auto
geoDepot
=
this
->
_depot
;
auto
geoSafeArea
=
this
->
_jArea
.
coordinateList
();
if
(
!
geoDepot
.
isValid
())
{
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): depot invalid."
<<
geoDepot
;
return
;
}
if
(
!
(
geoSafeArea
.
size
()
>=
3
))
{
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): safe area invalid."
<<
geoSafeArea
;
return
;
}
for
(
auto
&
v
:
geoSafeArea
)
{
v
.
setAltitude
(
0
);
}
snake
::
FPoint
depot
;
snake
::
toENU
(
ref
,
geoDepot
,
depot
);
// Routing par.
RoutingParameter
par
;
par
.
numSolutions
=
5
;
if
(
this
->
_numRuns
.
rawValue
().
toUInt
()
<
1
)
{
disconnect
(
&
this
->
_numRuns
,
&
Fact
::
rawValueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
this
->
_numRuns
.
setCookedValue
(
QVariant
(
1
));
connect
(
&
this
->
_numRuns
,
&
Fact
::
rawValueChanged
,
this
,
&
CircularSurvey
::
_rebuildTransects
);
}
par
.
numRuns
=
this
->
_numRuns
.
rawValue
().
toUInt
();
auto
&
safeAreaENU
=
par
.
safeArea
;
snake
::
areaToEnu
(
ref
,
geoSafeArea
,
safeAreaENU
);
// Fetch transect parameter.
auto
distance
=
snake
::
Length
(
this
->
_transectDistance
.
rawValue
().
toDouble
()
*
bu
::
si
::
meter
);
auto
minLength
=
snake
::
Length
(
this
->
_minLength
.
rawValue
().
toDouble
()
*
bu
::
si
::
meter
);
auto
alpha
=
snake
::
Angle
(
this
->
_alpha
.
rawValue
().
toDouble
()
*
bu
::
degree
::
degree
);
// Select survey type.
if
(
this
->
_type
.
rawValue
().
toUInt
()
==
integral
(
Type
::
Circular
))
{
// Clip angle.
if
(
alpha
>=
snake
::
Angle
(
0.3
*
bu
::
degree
::
degree
)
&&
alpha
<=
snake
::
Angle
(
45
*
bu
::
degree
::
degree
))
{
auto
generator
=
[
depot
,
pPolygon
,
pTiles
,
distance
,
alpha
,
minLength
](
snake
::
Transects
&
transects
)
->
bool
{
bool
value
=
circularTransects
(
*
pPolygon
,
*
pTiles
,
distance
,
alpha
,
minLength
,
transects
);
transects
.
insert
(
transects
.
begin
(),
snake
::
FLineString
{
depot
});
return
value
;
};
// Start routing worker.
this
->
_pWorker
->
route
(
par
,
generator
);
// Routing par.
RoutingParameter
par
;
par
.
numSolutions
=
5
;
auto
&
safeAreaENU
=
par
.
safeArea
;
snake
::
areaToEnu
(
origin
,
geoSafeArea
,
safeAreaENU
);
// Create generator.
GeneratorBase
::
Generator
g
;
// Transect generator.
if
(
_pGenerator
->
get
(
g
))
{
// Start/Restart routing worker.
this
->
_pWorker
->
route
(
par
,
g
);
}
else
{
if
(
alpha
<
snake
::
Angle
(
0.3
*
bu
::
degree
::
degree
))
{
this
->
_alpha
.
setCookedValue
(
QVariant
(
0.3
));
}
else
{
this
->
_alpha
.
setCookedValue
(
QVariant
(
45
));
}
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): generator creation failed."
;
}
}
else
if
(
this
->
_type
.
rawValue
().
toUInt
()
==
integral
(
Type
::
Linear
))
{
auto
generator
=
[
depot
,
pPolygon
,
pTiles
,
distance
,
alpha
,
minLength
](
snake
::
Transects
&
transects
)
->
bool
{
bool
value
=
linearTransects
(
*
pPolygon
,
*
pTiles
,
distance
,
alpha
,
minLength
,
transects
);
transects
.
insert
(
transects
.
begin
(),
snake
::
FLineString
{
depot
});
return
value
;
};
// Start routing worker.
this
->
_pWorker
->
route
(
par
,
generator
);
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"CircularSurvey::rebuildTransectsPhase1(): invalid survey type:"
<<
this
->
_type
.
rawValue
().
toUInt
();
qCWarning
(
CircularSurveyLog
)
<<
"_updateWorker(): plan data invalid."
;
}
}
void
CircularSurvey
::
_changeVariant
Run
Worker
()
{
void
CircularSurvey
::
_changeVariantWorker
()
{
auto
variant
=
this
->
_variant
.
rawValue
().
toUInt
();
auto
run
=
this
->
_run
.
rawValue
().
toUInt
();
...
...
@@ -582,7 +416,7 @@ void CircularSurvey::_changeVariantRunWorker() {
&
CircularSurvey
::
_changeRun
);
if
(
this
->
_variantVector
.
size
()
>
0
&&
this
->
_variantVector
.
front
().
size
()
>
0
)
{
this
->
_changeVariant
Run
Worker
();
this
->
_changeVariantWorker
();
}
}
}
...
...
@@ -621,13 +455,13 @@ void CircularSurvey::_storeWorker() {
}
// Store solutions.
QVector
<
Runs
>
solutionVector
;
QVector
<
Variant
>
solutionVector
;
const
auto
nSolutions
=
pRoutingData
->
solutionVector
.
size
();
for
(
std
::
size_t
j
=
0
;
j
<
nSolutions
;
++
j
)
{
const
auto
&
solution
=
pRoutingData
->
solutionVector
.
at
(
j
);
const
auto
nRuns
=
solution
.
size
();
// Store runs.
Runs
runs
(
nRuns
,
Transects
{
QList
<
CoordInfo_t
>
()});
Variant
runs
(
nRuns
,
Transects
{
QList
<
CoordInfo_t
>
()});
for
(
std
::
size_t
k
=
0
;
k
<
nRuns
;
++
k
)
{
const
auto
&
route
=
solution
.
at
(
k
);
const
auto
&
path
=
route
.
path
;
...
...
@@ -737,7 +571,7 @@ void CircularSurvey::_storeWorker() {
&
CircularSurvey
::
_changeVariant
);
connect
(
&
this
->
_run
,
&
Fact
::
rawValueChanged
,
this
,
&
CircularSurvey
::
_changeRun
);
this
->
_changeVariant
Run
Worker
();
this
->
_changeVariantWorker
();
// Mark transect as stored and ready.
this
->
_transectsDirty
=
false
;
}
...
...
@@ -760,11 +594,108 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
QString
CircularSurvey
::
abbreviation
()
const
{
return
tr
(
"C.S."
);
}
bool
CircularSurvey
::
readyForSave
()
const
{
return
TransectStyleComplexItem
::
readyForSave
()
&&
!
_transectsDirty
;
return
TransectStyleComplexItem
::
readyForSave
()
&&
!
_transectsDirty
&&
this
->
_state
==
STATE
::
IDLE
;
}
double
CircularSurvey
::
additionalTimeDelay
()
const
{
return
0
;
}
bool
CircularSurvey
::
registerGenerator
(
const
QString
&
name
,
std
::
shared_ptr
<
GeneratorBase
>
g
)
{
if
(
name
.
isEmpty
())
{
qCWarning
(
CircularSurveyLog
)
<<
"registerGenerator(): empty name string."
;
return
false
;
}
if
(
g
)
{
qCWarning
(
CircularSurveyLog
)
<<
"registerGenerator(): empty generator."
;
return
false
;
}
if
(
this
->
_generatorNameList
.
contains
(
name
))
{
qCWarning
(
CircularSurveyLog
)
<<
"registerGenerator(): generator "
"already registered."
;
return
false
;
}
else
{
this
->
_generatorNameList
.
push_back
(
name
);
this
->
_generatorList
.
push_back
(
g
);
if
(
this
->
_generatorList
.
size
()
==
1
)
{
this
->
_pGenerator
=
g
;
}
emit
generatorNameListChanged
();
return
true
;
}
}
bool
CircularSurvey
::
unregisterGenerator
(
const
QString
&
name
)
{
auto
index
=
this
->
_generatorNameList
.
indexOf
(
name
);
if
(
index
>=
0
)
{
// Is this the current generator?
const
auto
&
g
=
this
->
_generatorList
.
at
(
index
);
if
(
g
.
get
()
==
this
->
_pGenerator
.
get
())
{
if
(
index
>
0
)
{
_switchToGenerator
(
this
->
_generatorList
.
at
(
index
-
1
));
}
else
{
_switchToGenerator
(
nullptr
);
qCWarning
(
CircularSurveyLog
)
<<
"unregisterGenerator(): last generator unregistered."
;
}
}
this
->
_generatorNameList
.
removeAt
(
index
);
this
->
_generatorList
.
removeAt
(
index
);
emit
generatorNameListChanged
();
return
true
;
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"unregisterGenerator(): generator "
<<
name
<<
" not registered."
;
return
false
;
}
}
bool
CircularSurvey
::
unregisterGenerator
(
int
index
)
{
if
(
index
>
0
&&
index
<
this
->
_generatorNameList
.
size
())
{
return
unregisterGenerator
(
this
->
_generatorNameList
.
at
(
index
));
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"unregisterGenerator(): index ("
<<
index
<<
") out"
"of bounds ( "
<<
this
->
_generatorList
.
size
()
<<
" )."
;
return
false
;
}
}
bool
CircularSurvey
::
switchToGenerator
(
const
QString
&
name
)
{
auto
index
=
this
->
_generatorNameList
.
indexOf
(
name
);
if
(
index
>=
0
)
{
_switchToGenerator
(
this
->
_generatorList
.
at
(
index
));
return
true
;
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"switchToGenerator(): generator "
<<
name
<<
" not registered."
;
return
false
;
}
}
bool
CircularSurvey
::
switchToGenerator
(
int
index
)
{
if
(
index
>=
0
)
{
_switchToGenerator
(
this
->
_generatorList
.
at
(
index
));
return
true
;
}
else
{
qCWarning
(
CircularSurveyLog
)
<<
"unregisterGenerator(): index ("
<<
index
<<
") out"
"of bounds ( "
<<
this
->
_generatorNameList
.
size
()
<<
" )."
;
return
false
;
}
}
QList
<
QString
>
CircularSurvey
::
generatorNameList
()
{
return
this
->
_generatorNameList
;
}
void
CircularSurvey
::
_rebuildTransectsPhase1
(
void
)
{
auto
start
=
std
::
chrono
::
high_resolution_clock
::
now
();
...
...
@@ -775,22 +706,22 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
break
;
case
STATE
:
:
VARIANT_CHANGE
:
qCWarning
(
CircularSurveyLog
)
<<
"rebuildTransectsPhase1: variant change."
;
this
->
_changeVariant
Run
Worker
();
this
->
_changeVariantWorker
();
break
;
case
STATE
:
:
RUN_CHANGE
:
qCWarning
(
CircularSurveyLog
)
<<
"rebuildTransectsPhase1: run change."
;
this
->
_changeVariant
Run
Worker
();
this
->
_changeVariantWorker
();
break
;
case
STATE
:
:
REVERSE
:
qCWarning
(
CircularSurveyLog
)
<<
"rebuildTransectsPhase1: reverse."
;
this
->
_reverseWorker
();
break
;
case
STATE
:
:
DEFAULT
:
case
STATE
:
:
IDLE
:
qCWarning
(
CircularSurveyLog
)
<<
"rebuildTransectsPhase1: update."
;
this
->
_updateWorker
();
break
;
}
this
->
_state
=
STATE
::
DEFAULT
;
this
->
_state
=
STATE
::
IDLE
;
qCWarning
(
CircularSurveyLog
)
<<
"rebuildTransectsPhase1(): "
...
...
@@ -821,8 +752,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
this
->
_rebuildTransects
();
}
Fact
*
CircularSurvey
::
minLength
()
{
return
&
_minLength
;
}
Fact
*
CircularSurvey
::
type
()
{
return
&
_type
;
}
Fact
*
CircularSurvey
::
variant
()
{
return
&
_variant
;
}
...
...
src/Wima/CircularSurvey.h
View file @
60b604d9
...
...
@@ -2,26 +2,26 @@
#include <QFutureWatcher>
#include <QVector>
#include <memory>
#include "SettingsFact.h"
#include "TransectStyleComplexItem.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "WimaPlanData.h"
class
RoutingThread
;
class
RoutingData
;
class
GeneratorBase
;
class
CircularSurvey
:
public
TransectStyleComplexItem
{
Q_OBJECT
public:
using
PtrRoutingData
=
QSharedPointer
<
RoutingData
>
;
enum
class
Type
{
Circular
=
0
,
Linear
=
1
,
Count
=
2
// Must me last, onyl for counting
};
using
PtrGenerator
=
std
::
shared_ptr
<
GeneratorBase
>
;
using
PtrRoutingData
=
std
::
shared_ptr
<
RoutingData
>
;
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
...
...
@@ -31,50 +31,25 @@ public:
QObject
*
parent
);
~
CircularSurvey
();
Q_PROPERTY
(
QGeoCoordinate
refPoint
READ
refPoint
WRITE
setRefPoint
NOTIFY
refPointChanged
)
Q_PROPERTY
(
Fact
*
transectDistance
READ
transectDistance
CONSTANT
)
Q_PROPERTY
(
Fact
*
alpha
READ
alpha
CONSTANT
)
Q_PROPERTY
(
Fact
*
minLength
READ
minLength
CONSTANT
)
Q_PROPERTY
(
Fact
*
type
READ
type
CONSTANT
)
Q_PROPERTY
(
Fact
*
variant
READ
variant
CONSTANT
)
Q_PROPERTY
(
Fact
*
numRuns
READ
numRuns
CONSTANT
)
Q_PROPERTY
(
Fact
*
run
READ
run
CONSTANT
)
Q_PROPERTY
(
int
typeCount
READ
typeCount
CONSTANT
)
Q_PROPERTY
(
bool
calculating
READ
calculating
NOTIFY
calculatingChanged
)
Q_PROPERTY
(
bool
hidePolygon
READ
hidePolygon
NOTIFY
hidePolygonChanged
)
Q_PROPERTY
(
Fact
*
variant
READ
variant
CONSTANT
)
Q_PROPERTY
(
QList
<
QString
>
variantNames
READ
variantNames
NOTIFY
variantNamesChanged
)
Q_PROPERTY
(
QList
<
QString
>
runNames
READ
runNames
NOTIFY
runNamesChanged
)
Q_PROPERTY
(
QList
<
QString
>
generatorNameList
READ
generatorNameList
NOTIFY
generatorNameListChanged
)
Q_PROPERTY
(
bool
calculating
READ
calculating
NOTIFY
calculatingChanged
)
Q_INVOKABLE
void
resetReference
(
void
);
Q_INVOKABLE
void
reverse
(
void
);
// Property setters
void
setRefPoint
(
const
QGeoCoordinate
&
refPt
);
void
setHidePolygon
(
bool
hide
);
void
setMeasurementArea
(
const
WimaMeasurementAreaData
&
mArea
);
void
setJoinedArea
(
const
WimaJoinedAreaData
&
jArea
);
void
setMeasurementArea
(
const
WimaMeasurementArea
&
mArea
);
void
setJoinedArea
(
const
WimaJoinedArea
&
jArea
);
void
setDepot
(
const
QGeoCoordinate
&
depot
);
void
setPlanData
(
const
WimaPlanData
&
d
);
// Property getters
QGeoCoordinate
refPoint
()
const
;
Fact
*
transectDistance
();
Fact
*
alpha
();
Fact
*
minLength
();
Fact
*
type
();
Fact
*
variant
();
Fact
*
numRuns
();
Fact
*
run
();
int
typeCount
()
const
;
bool
calculating
()
const
;
bool
hidePolygon
()
const
;
Fact
*
variant
();
QList
<
QString
>
variantNames
()
const
;
QList
<
QString
>
runNames
()
const
;
QGeoCoordinate
depot
()
const
;
const
QList
<
QList
<
QGeoCoordinate
>>
&
rawTransects
()
const
;
// Overrides
bool
load
(
const
QJsonObject
&
complexObject
,
int
sequenceNumber
,
...
...
@@ -92,91 +67,77 @@ public:
bool
readyForSave
(
void
)
const
override
final
;
double
additionalTimeDelay
(
void
)
const
override
final
;
// Generator
bool
registerGenerator
(
const
QString
&
name
,
std
::
shared_ptr
<
GeneratorBase
>
g
);
bool
unregisterGenerator
(
const
QString
&
name
);
bool
unregisterGenerator
(
int
index
);
Q_INVOKABLE
bool
switchToGenerator
(
const
QString
&
name
);
Q_INVOKABLE
bool
switchToGenerator
(
int
index
);
QList
<
QString
>
generatorNameList
();
static
const
char
*
settingsGroup
;
static
const
char
*
transectDistanceName
;
static
const
char
*
alphaName
;
static
const
char
*
minLengthName
;
static
const
char
*
typeName
;
static
const
char
*
variantName
;
static
const
char
*
numRunsName
;
static
const
char
*
runName
;
static
const
char
*
CircularSurveyName
;
static
const
char
*
refPointLongitudeName
;
static
const
char
*
refPointLatitudeName
;
static
const
char
*
refPointAltitudeName
;
signals:
void
refPointChanged
();
void
calculatingChanged
();
void
hidePolygonChanged
();
void
depotChanged
();
void
variantNamesChanged
();
void
runNamesChanged
();
void
measurementAreaChanged
();
void
joinedAreaChanged
();
void
generatorNameListChanged
();
void
generatorChanged
();
private
slots
:
// Overrides from TransectStyleComplexItem
void
_rebuildTransectsPhase1
(
void
)
final
;
void
_recalcComplexDistance
(
void
)
final
;
void
_recalcCameraShots
(
void
)
final
;
// Worker functions.
void
_setTransects
(
PtrRoutingData
pRoute
);
void
_changeVariant
();
void
_updateWorker
();
void
_changeVariantWorker
();
void
_reverseWorker
();
void
_storeWorker
();
private:
void
_appendLoadedMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
void
_buildAndAppendMissionItems
(
QList
<
MissionItem
*>
&
items
,
QObject
*
missionItemParent
);
void
_changeVariant
();
void
_changeRun
();
void
_updateWorker
();
void
_changeVariantRunWorker
();
void
_reverseWorker
();
void
_storeWorker
();
void
_changeRunWorker
();
bool
_switchToGenerator
(
const
PtrGenerator
&
newG
);
// State.
enum
class
STATE
{
IDLE
,
STORE
,
REVERSE
,
VARIANT_CHANGE
,
RUN_CHANGE
,
};
STATE
_state
;
// center of the circular lanes, e.g. base station
QGeoCoordinate
_referencePoint
;
QMap
<
QString
,
FactMetaData
*>
_metaDataMap
;
// distance between two neighbour circles
SettingsFact
_transectDistance
;
// angle discretisation of the circles
SettingsFact
_alpha
;
// minimal transect lenght, transects are rejected if they are shorter than
// this value
SettingsFact
_minLength
;
SettingsFact
_type
;
SettingsFact
_variant
;
QList
<
QString
>
_variantNames
;
SettingsFact
_numRuns
;
SettingsFact
_run
;
QList
<
QString
>
_runNames
;
// Area data
WimaMeasurementAreaData
_mArea
;
WimaJoinedAreaData
_jArea
;
QGeoCoordinate
_depot
;
std
::
shared_ptr
<
WimaAreaData
>
_areaData
;
// Generators
QList
<
PtrGenerator
>
_generatorList
;
QList
<
QString
>
_generatorNameList
;
PtrGenerator
_pGenerator
;
// Worker
using
PtrWorker
=
std
::
shared_ptr
<
RoutingThread
>
;
PtrWorker
_pWorker
;
PtrRoutingData
_pRoutingData
;
PtrRoutingData
_pRoutingData
;
// remove this, not necessary.
// Routing data.
QList
<
QList
<
QGeoCoordinate
>>
_rawTransects
;
using
Runs
=
QVector
<
Transects
>
;
QVector
<
Runs
>
_variantVector
;
// State.
enum
class
STATE
{
DEFAULT
,
STORE
,
REVERSE
,
VARIANT_CHANGE
,
RUN_CHANGE
,
};
STATE
_state
;
bool
_hidePolygon
;
using
Variant
=
Transects
;
QVector
<
Variant
>
_variantVector
;
};
src/Wima/RoutingThread.h
View file @
60b604d9
...
...
@@ -31,7 +31,7 @@ class RoutingThread : public QThread {
using
Lock
=
std
::
unique_lock
<
std
::
mutex
>
;
public:
using
PtrRoutingData
=
QSharedPointe
r
<
RoutingData
>
;
using
PtrRoutingData
=
shared_pt
r
<
RoutingData
>
;
using
Generator
=
std
::
function
<
bool
(
snake
::
Transects
&
)
>
;
using
Consumer
=
std
::
function
<
void
(
const
RoutingData
&
)
>
;
...
...
src/Wima/Snake/CircularGenerator.cpp
View file @
60b604d9
...
...
@@ -26,6 +26,9 @@ const char *CircularGenerator::settingsGroup = "CircularGenerator";
const
char
*
CircularGenerator
::
distanceName
=
"TransectDistance"
;
const
char
*
CircularGenerator
::
deltaAlphaName
=
"DeltaAlpha"
;
const
char
*
CircularGenerator
::
minLengthName
=
"MinLength"
;
const
char
*
CircularGenerator
::
refPointLatitudeName
=
"ReferencePointLat"
;
const
char
*
CircularGenerator
::
refPointLongitudeName
=
"ReferencePointLong"
;
const
char
*
CircularGenerator
::
refPointAltitudeName
=
"ReferencePointAlt"
;
CircularGenerator
::
CircularGenerator
(
QObject
*
parent
)
:
CircularGenerator
(
nullptr
,
parent
)
{}
...
...
src/Wima/Snake/CircularGenerator.h
View file @
60b604d9
...
...
@@ -36,6 +36,9 @@ public:
static
const
char
*
distanceName
;
static
const
char
*
deltaAlphaName
;
static
const
char
*
minLengthName
;
static
const
char
*
refPointLongitudeName
;
static
const
char
*
refPointLatitudeName
;
static
const
char
*
refPointAltitudeName
;
signals:
void
referenceChanged
();
...
...
src/comm/LinkConfiguration.h
View file @
60b604d9
...
...
@@ -213,5 +213,5 @@ private:
bool
_highLatency
;
};
typedef
QSharedPointe
r
<
LinkConfiguration
>
SharedLinkConfigurationPointer
;
typedef
shared_pt
r
<
LinkConfiguration
>
SharedLinkConfigurationPointer
;
src/comm/LinkInterface.h
View file @
60b604d9
...
...
@@ -305,5 +305,5 @@ private:
QMap
<
int
/* vehicle id */
,
MavlinkMessagesTimer
*>
_mavlinkMessagesTimers
;
};
typedef
QSharedPointe
r
<
LinkInterface
>
SharedLinkInterfacePointer
;
typedef
shared_pt
r
<
LinkInterface
>
SharedLinkInterfacePointer
;
src/qgcunittest/UnitTest.h
View file @
60b604d9
...
...
@@ -215,7 +215,7 @@ public:
}
private:
QSharedPointe
r
<
T
>
_unitTest
;
shared_pt
r
<
T
>
_unitTest
;
};
#endif
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