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
efbf70dc
Unverified
Commit
efbf70dc
authored
Aug 03, 2018
by
Don Gagne
Committed by
GitHub
Aug 03, 2018
Browse files
Options
Browse Files
Download
Plain Diff
Merge pull request #6755 from DonLakeFlyer/TerrainFail
Terrain: Better error handing
parents
0b049457
480da6fc
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
58 additions
and
21 deletions
+58
-21
VisualMissionItem.cc
src/MissionManager/VisualMissionItem.cc
+3
-5
TerrainQuery.cc
src/Terrain/TerrainQuery.cc
+48
-11
TerrainQuery.h
src/Terrain/TerrainQuery.h
+3
-3
TerrainTile.cc
src/TerrainTile.cc
+4
-2
No files found.
src/MissionManager/VisualMissionItem.cc
View file @
efbf70dc
...
...
@@ -195,9 +195,7 @@ void VisualMissionItem::_reallyUpdateTerrainAltitude(void)
void
VisualMissionItem
::
_terrainDataReceived
(
bool
success
,
QList
<
double
>
heights
)
{
if
(
success
)
{
_terrainAltitude
=
heights
[
0
];
emit
terrainAltitudeChanged
(
_terrainAltitude
);
sender
()
->
deleteLater
();
}
_terrainAltitude
=
success
?
heights
[
0
]
:
qQNaN
();
emit
terrainAltitudeChanged
(
_terrainAltitude
);
sender
()
->
deleteLater
();
}
src/Terrain/TerrainQuery.cc
View file @
efbf70dc
...
...
@@ -338,16 +338,23 @@ void TerrainTileManager::addCoordinateQuery(TerrainOfflineAirMapQuery* terrainQu
qCDebug
(
TerrainQueryLog
)
<<
"TerrainTileManager::addCoordinateQuery count"
<<
coordinates
.
count
();
if
(
coordinates
.
length
()
>
0
)
{
bool
error
;
QList
<
double
>
altitudes
;
if
(
!
_getAltitudesForCoordinates
(
coordinates
,
altitudes
))
{
if
(
!
_getAltitudesForCoordinates
(
coordinates
,
altitudes
,
error
))
{
QueuedRequestInfo_t
queuedRequestInfo
=
{
terrainQueryInterface
,
QueryMode
::
QueryModeCoordinates
,
0
,
0
,
coordinates
};
_requestQueue
.
append
(
queuedRequestInfo
);
return
;
}
qCDebug
(
TerrainQueryLog
)
<<
"All altitudes taken from cached data"
;
terrainQueryInterface
->
_signalCoordinateHeights
(
coordinates
.
count
()
==
altitudes
.
count
(),
altitudes
);
if
(
error
)
{
QList
<
double
>
noAltitudes
;
qCWarning
(
TerrainQueryLog
)
<<
"addCoordinateQuery: signalling failure due to internal error"
;
terrainQueryInterface
->
_signalCoordinateHeights
(
false
,
noAltitudes
);
}
else
{
qCDebug
(
TerrainQueryLog
)
<<
"addCoordinateQuery: All altitudes taken from cached data"
;
terrainQueryInterface
->
_signalCoordinateHeights
(
coordinates
.
count
()
==
altitudes
.
count
(),
altitudes
);
}
}
}
...
...
@@ -370,19 +377,28 @@ void TerrainTileManager::addPathQuery(TerrainOfflineAirMapQuery* terrainQueryInt
qCDebug
(
TerrainQueryLog
)
<<
"TerrainTileManager::addPathQuery start:end:coordCount"
<<
startPoint
<<
endPoint
<<
coordinates
.
count
();
bool
error
;
QList
<
double
>
altitudes
;
if
(
!
_getAltitudesForCoordinates
(
coordinates
,
altitudes
))
{
if
(
!
_getAltitudesForCoordinates
(
coordinates
,
altitudes
,
error
))
{
QueuedRequestInfo_t
queuedRequestInfo
=
{
terrainQueryInterface
,
QueryMode
::
QueryModePath
,
latStep
,
lonStep
,
coordinates
};
_requestQueue
.
append
(
queuedRequestInfo
);
return
;
}
qCDebug
(
TerrainQueryLog
)
<<
"All altitudes taken from cached data"
;
terrainQueryInterface
->
_signalPathHeights
(
coordinates
.
count
()
==
altitudes
.
count
(),
latStep
,
lonStep
,
altitudes
);
if
(
error
)
{
QList
<
double
>
noAltitudes
;
qCWarning
(
TerrainQueryLog
)
<<
"addPathQuery: signalling failure due to internal error"
;
terrainQueryInterface
->
_signalPathHeights
(
false
,
latStep
,
lonStep
,
noAltitudes
);
}
else
{
qCDebug
(
TerrainQueryLog
)
<<
"addPathQuery: All altitudes taken from cached data"
;
terrainQueryInterface
->
_signalPathHeights
(
coordinates
.
count
()
==
altitudes
.
count
(),
latStep
,
lonStep
,
altitudes
);
}
}
bool
TerrainTileManager
::
_getAltitudesForCoordinates
(
const
QList
<
QGeoCoordinate
>&
coordinates
,
QList
<
double
>&
altitudes
)
bool
TerrainTileManager
::
_getAltitudesForCoordinates
(
const
QList
<
QGeoCoordinate
>&
coordinates
,
QList
<
double
>&
altitudes
,
bool
&
error
)
{
error
=
false
;
foreach
(
const
QGeoCoordinate
&
coordinate
,
coordinates
)
{
QString
tileHash
=
_getTileHash
(
coordinate
);
_tilesMutex
.
lock
();
...
...
@@ -406,14 +422,20 @@ bool TerrainTileManager::_getAltitudesForCoordinates(const QList<QGeoCoordinate>
return
false
;
}
else
{
if
(
_tiles
[
tileHash
].
isIn
(
coordinate
))
{
altitudes
.
push_back
(
_tiles
[
tileHash
].
elevation
(
coordinate
));
double
elevation
=
_tiles
[
tileHash
].
elevation
(
coordinate
);
if
(
elevation
<
0.0
)
{
error
=
true
;
}
altitudes
.
push_back
(
elevation
);
}
else
{
qCWarning
(
TerrainQueryLog
)
<<
"Error: coordinate not in tile region"
;
altitudes
.
push_back
(
-
1.0
);
error
=
true
;
}
}
_tilesMutex
.
unlock
();
}
return
true
;
}
...
...
@@ -477,14 +499,29 @@ void TerrainTileManager::_terrainDone(QByteArray responseBytes, QNetworkReply::N
// now try to query the data again
for
(
int
i
=
_requestQueue
.
count
()
-
1
;
i
>=
0
;
i
--
)
{
bool
error
;
QList
<
double
>
altitudes
;
QueuedRequestInfo_t
&
requestInfo
=
_requestQueue
[
i
];
if
(
_getAltitudesForCoordinates
(
requestInfo
.
coordinates
,
altitudes
))
{
if
(
_getAltitudesForCoordinates
(
requestInfo
.
coordinates
,
altitudes
,
error
))
{
if
(
requestInfo
.
queryMode
==
QueryMode
::
QueryModeCoordinates
)
{
requestInfo
.
terrainQueryInterface
->
_signalCoordinateHeights
(
requestInfo
.
coordinates
.
count
()
==
altitudes
.
count
(),
altitudes
);
if
(
error
)
{
QList
<
double
>
noAltitudes
;
qCWarning
(
TerrainQueryLog
)
<<
"_terrainDone(coordinateQuery): signalling failure due to internal error"
;
requestInfo
.
terrainQueryInterface
->
_signalCoordinateHeights
(
false
,
noAltitudes
);
}
else
{
qCDebug
(
TerrainQueryLog
)
<<
"_terrainDone(coordinateQuery): All altitudes taken from cached data"
;
requestInfo
.
terrainQueryInterface
->
_signalCoordinateHeights
(
requestInfo
.
coordinates
.
count
()
==
altitudes
.
count
(),
altitudes
);
}
}
else
if
(
requestInfo
.
queryMode
==
QueryMode
::
QueryModePath
)
{
requestInfo
.
terrainQueryInterface
->
_signalPathHeights
(
requestInfo
.
coordinates
.
count
()
==
altitudes
.
count
(),
requestInfo
.
latStep
,
requestInfo
.
lonStep
,
altitudes
);
if
(
error
)
{
QList
<
double
>
noAltitudes
;
qCWarning
(
TerrainQueryLog
)
<<
"_terrainDone(coordinateQuery): signalling failure due to internal error"
;
requestInfo
.
terrainQueryInterface
->
_signalPathHeights
(
false
,
requestInfo
.
latStep
,
requestInfo
.
lonStep
,
noAltitudes
);
}
else
{
qCDebug
(
TerrainQueryLog
)
<<
"_terrainDone(coordinateQuery): All altitudes taken from cached data"
;
requestInfo
.
terrainQueryInterface
->
_signalPathHeights
(
requestInfo
.
coordinates
.
count
()
==
altitudes
.
count
(),
requestInfo
.
latStep
,
requestInfo
.
lonStep
,
altitudes
);
}
}
_requestQueue
.
removeAt
(
i
);
}
...
...
src/Terrain/TerrainQuery.h
View file @
efbf70dc
...
...
@@ -140,9 +140,9 @@ private:
QList
<
QGeoCoordinate
>
coordinates
;
}
QueuedRequestInfo_t
;
void
_tileFailed
(
void
);
bool
_getAltitudesForCoordinates
(
const
QList
<
QGeoCoordinate
>&
coordinates
,
QList
<
double
>&
altitudes
);
QString
_getTileHash
(
const
QGeoCoordinate
&
coordinate
);
/// Method to create a unique string for each tile
void
_tileFailed
(
void
);
bool
_getAltitudesForCoordinates
(
const
QList
<
QGeoCoordinate
>&
coordinates
,
QList
<
double
>&
altitudes
,
bool
&
error
);
QString
_getTileHash
(
const
QGeoCoordinate
&
coordinate
);
QList
<
QueuedRequestInfo_t
>
_requestQueue
;
State
_state
=
State
::
Idle
;
...
...
src/TerrainTile.cc
View file @
efbf70dc
...
...
@@ -134,7 +134,7 @@ TerrainTile::TerrainTile(QByteArray byteArray)
bool
TerrainTile
::
isIn
(
const
QGeoCoordinate
&
coordinate
)
const
{
if
(
!
_isValid
)
{
qC
Debu
g
(
TerrainTileLog
)
<<
"isIn requested, but tile not valid"
;
qC
Warnin
g
(
TerrainTileLog
)
<<
"isIn requested, but tile not valid"
;
return
false
;
}
bool
ret
=
coordinate
.
latitude
()
>=
_southWest
.
latitude
()
&&
coordinate
.
longitude
()
>=
_southWest
.
longitude
()
&&
...
...
@@ -157,7 +157,7 @@ double TerrainTile::elevation(const QGeoCoordinate& coordinate) const
qCDebug
(
TerrainTileLog
)
<<
"indexLat:indexLon"
<<
indexLat
<<
indexLon
<<
"elevation"
<<
_data
[
indexLat
][
indexLon
];
return
static_cast
<
double
>
(
_data
[
indexLat
][
indexLon
]);
}
else
{
qC
Debu
g
(
TerrainTileLog
)
<<
"Asking for elevation, but no valid data."
;
qC
Warnin
g
(
TerrainTileLog
)
<<
"Asking for elevation, but no valid data."
;
return
-
1.0
;
}
}
...
...
@@ -284,6 +284,7 @@ int TerrainTile::_latToDataIndex(double latitude) const
if
(
isValid
()
&&
_southWest
.
isValid
()
&&
_northEast
.
isValid
())
{
return
qRound
((
latitude
-
_southWest
.
latitude
())
/
(
_northEast
.
latitude
()
-
_southWest
.
latitude
())
*
(
_gridSizeLat
-
1
));
}
else
{
qCWarning
(
TerrainTileLog
)
<<
"TerrainTile::_latToDataIndex internal error"
<<
isValid
()
<<
_southWest
.
isValid
()
<<
_northEast
.
isValid
();
return
-
1
;
}
}
...
...
@@ -293,6 +294,7 @@ int TerrainTile::_lonToDataIndex(double longitude) const
if
(
isValid
()
&&
_southWest
.
isValid
()
&&
_northEast
.
isValid
())
{
return
qRound
((
longitude
-
_southWest
.
longitude
())
/
(
_northEast
.
longitude
()
-
_southWest
.
longitude
())
*
(
_gridSizeLon
-
1
));
}
else
{
qCWarning
(
TerrainTileLog
)
<<
"TerrainTile::_lonToDataIndex internal error"
<<
isValid
()
<<
_southWest
.
isValid
()
<<
_northEast
.
isValid
();
return
-
1
;
}
}
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