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
4be05982
Commit
4be05982
authored
Oct 26, 2015
by
David Goodman
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Updated conversion to local NED instead of ENU.
parent
7eea5671
Changes
4
Show whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
24 additions
and
23 deletions
+24
-23
QGCGeo.cc
src/QGCGeo.cc
+4
-4
QGCGeo.h
src/QGCGeo.h
+7
-6
GeoTest.cc
src/qgcunittest/GeoTest.cc
+9
-9
GeoTest.h
src/qgcunittest/GeoTest.h
+4
-4
No files found.
src/QGCGeo.cc
View file @
4be05982
...
@@ -39,7 +39,7 @@
...
@@ -39,7 +39,7 @@
static
const
float
epsilon
=
std
::
numeric_limits
<
double
>::
epsilon
();
static
const
float
epsilon
=
std
::
numeric_limits
<
double
>::
epsilon
();
void
convertGeoTo
Enu
(
QGeoCoordinate
coord
,
QGeoCoordinate
origin
,
double
*
x
,
double
*
y
,
double
*
z
)
{
void
convertGeoTo
Ned
(
QGeoCoordinate
coord
,
QGeoCoordinate
origin
,
double
*
x
,
double
*
y
,
double
*
z
)
{
double
lat_rad
=
coord
.
latitude
()
*
M_DEG_TO_RAD
;
double
lat_rad
=
coord
.
latitude
()
*
M_DEG_TO_RAD
;
double
lon_rad
=
coord
.
longitude
()
*
M_DEG_TO_RAD
;
double
lon_rad
=
coord
.
longitude
()
*
M_DEG_TO_RAD
;
...
@@ -60,10 +60,10 @@ void convertGeoToEnu(QGeoCoordinate coord, QGeoCoordinate origin, double* x, dou
...
@@ -60,10 +60,10 @@ void convertGeoToEnu(QGeoCoordinate coord, QGeoCoordinate origin, double* x, dou
*
x
=
k
*
(
ref_cos_lat
*
sin_lat
-
ref_sin_lat
*
cos_lat
*
cos_d_lon
)
*
CONSTANTS_RADIUS_OF_EARTH
;
*
x
=
k
*
(
ref_cos_lat
*
sin_lat
-
ref_sin_lat
*
cos_lat
*
cos_d_lon
)
*
CONSTANTS_RADIUS_OF_EARTH
;
*
y
=
k
*
cos_lat
*
sin
(
lon_rad
-
ref_lon_rad
)
*
CONSTANTS_RADIUS_OF_EARTH
;
*
y
=
k
*
cos_lat
*
sin
(
lon_rad
-
ref_lon_rad
)
*
CONSTANTS_RADIUS_OF_EARTH
;
*
z
=
coord
.
altitude
()
-
origin
.
altitude
(
);
*
z
=
-
(
coord
.
altitude
()
-
origin
.
altitude
()
);
}
}
void
convert
Enu
ToGeo
(
double
x
,
double
y
,
double
z
,
QGeoCoordinate
origin
,
QGeoCoordinate
*
coord
)
{
void
convert
Ned
ToGeo
(
double
x
,
double
y
,
double
z
,
QGeoCoordinate
origin
,
QGeoCoordinate
*
coord
)
{
double
x_rad
=
x
/
CONSTANTS_RADIUS_OF_EARTH
;
double
x_rad
=
x
/
CONSTANTS_RADIUS_OF_EARTH
;
double
y_rad
=
y
/
CONSTANTS_RADIUS_OF_EARTH
;
double
y_rad
=
y
/
CONSTANTS_RADIUS_OF_EARTH
;
double
c
=
sqrtf
(
x_rad
*
x_rad
+
y_rad
*
y_rad
);
double
c
=
sqrtf
(
x_rad
*
x_rad
+
y_rad
*
y_rad
);
...
@@ -91,6 +91,6 @@ void convertEnuToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCo
...
@@ -91,6 +91,6 @@ void convertEnuToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCo
coord
->
setLatitude
(
lat_rad
*
M_RAD_TO_DEG
);
coord
->
setLatitude
(
lat_rad
*
M_RAD_TO_DEG
);
coord
->
setLongitude
(
lon_rad
*
M_RAD_TO_DEG
);
coord
->
setLongitude
(
lon_rad
*
M_RAD_TO_DEG
);
coord
->
setAltitude
(
z
+
origin
.
altitude
());
coord
->
setAltitude
(
-
z
+
origin
.
altitude
());
}
}
src/QGCGeo.h
View file @
4be05982
...
@@ -39,23 +39,24 @@
...
@@ -39,23 +39,24 @@
#endif
#endif
/**
/**
* @brief Project a geodetic coordinate on to local tangential plane (LTP) as coordinate with East, North, and Up components in meters.
* @brief Project a geodetic coordinate on to local tangential plane (LTP) as coordinate with East,
* North, and Down components in meters.
* @param[in] coord Geodetic coordinate to project onto LTP.
* @param[in] coord Geodetic coordinate to project onto LTP.
* @param[in] origin Geoedetic origin for LTP projection.
* @param[in] origin Geoedetic origin for LTP projection.
* @param[out] x North component of coordinate in local plane.
* @param[out] x North component of coordinate in local plane.
* @param[out] y East component of coordinate in local plane.
* @param[out] y East component of coordinate in local plane.
* @param[out] z
Up (altitude)
component of coordinate in local plane.
* @param[out] z
Down
component of coordinate in local plane.
*/
*/
void
convertGeoTo
Enu
(
QGeoCoordinate
coord
,
QGeoCoordinate
origin
,
double
*
x
,
double
*
y
,
double
*
z
);
void
convertGeoTo
Ned
(
QGeoCoordinate
coord
,
QGeoCoordinate
origin
,
double
*
x
,
double
*
y
,
double
*
z
);
/**
/**
* @brief Transform a local (East, North, and
Up
) coordinate into a geodetic coordinate.
* @brief Transform a local (East, North, and
Down
) coordinate into a geodetic coordinate.
* @param[in] x North component of local coordinate in meters.
* @param[in] x North component of local coordinate in meters.
* @param[in] x East component of local coordinate in meters.
* @param[in] x East component of local coordinate in meters.
* @param[in] x
Up
component of local coordinate in meters.
* @param[in] x
Down
component of local coordinate in meters.
* @param[in] origin Geoedetic origin for LTP.
* @param[in] origin Geoedetic origin for LTP.
* @param[out] coord Geodetic coordinate to hold result.
* @param[out] coord Geodetic coordinate to hold result.
*/
*/
void
convert
Enu
ToGeo
(
double
x
,
double
y
,
double
z
,
QGeoCoordinate
origin
,
QGeoCoordinate
*
coord
);
void
convert
Ned
ToGeo
(
double
x
,
double
y
,
double
z
,
QGeoCoordinate
origin
,
QGeoCoordinate
*
coord
);
#endif // QGCGEO_H
#endif // QGCGEO_H
src/qgcunittest/GeoTest.cc
View file @
4be05982
...
@@ -38,7 +38,7 @@ GeoTest::GeoTest(void)
...
@@ -38,7 +38,7 @@ GeoTest::GeoTest(void)
}
}
*/
*/
void
GeoTest
::
_convertGeoTo
Enu
_test
(
void
)
void
GeoTest
::
_convertGeoTo
Ned
_test
(
void
)
{
{
QGeoCoordinate
coord
(
47.364869
,
8.594398
,
0.0
);
QGeoCoordinate
coord
(
47.364869
,
8.594398
,
0.0
);
...
@@ -47,31 +47,31 @@ void GeoTest::_convertGeoToEnu_test(void)
...
@@ -47,31 +47,31 @@ void GeoTest::_convertGeoToEnu_test(void)
double
expectedZ
=
0
;
double
expectedZ
=
0
;
double
x
,
y
,
z
;
double
x
,
y
,
z
;
convertGeoTo
Enu
(
coord
,
_origin
,
&
x
,
&
y
,
&
z
);
convertGeoTo
Ned
(
coord
,
_origin
,
&
x
,
&
y
,
&
z
);
QCOMPARE
(
x
,
expectedX
);
QCOMPARE
(
x
,
expectedX
);
QCOMPARE
(
y
,
expectedY
);
QCOMPARE
(
y
,
expectedY
);
QCOMPARE
(
z
,
expectedZ
);
QCOMPARE
(
z
,
expectedZ
);
}
}
void
GeoTest
::
_convertGeoTo
Enu
AtOrigin_test
(
void
)
void
GeoTest
::
_convertGeoTo
Ned
AtOrigin_test
(
void
)
{
{
QGeoCoordinate
coord
(
_origin
);
QGeoCoordinate
coord
(
_origin
);
coord
.
setAltitude
(
10.0
);
// offset altitude to test z
coord
.
setAltitude
(
10.0
);
// offset altitude to test z
double
expectedX
=
0.0
;
double
expectedX
=
0.0
;
double
expectedY
=
0.0
;
double
expectedY
=
0.0
;
double
expectedZ
=
10.0
;
double
expectedZ
=
-
10.0
;
double
x
,
y
,
z
;
double
x
,
y
,
z
;
convertGeoTo
Enu
(
coord
,
_origin
,
&
x
,
&
y
,
&
z
);
convertGeoTo
Ned
(
coord
,
_origin
,
&
x
,
&
y
,
&
z
);
QCOMPARE
(
x
,
expectedX
);
QCOMPARE
(
x
,
expectedX
);
QCOMPARE
(
y
,
expectedY
);
QCOMPARE
(
y
,
expectedY
);
QCOMPARE
(
z
,
expectedZ
);
QCOMPARE
(
z
,
expectedZ
);
}
}
void
GeoTest
::
_convert
Enu
ToGeo_test
(
void
)
void
GeoTest
::
_convert
Ned
ToGeo_test
(
void
)
{
{
double
x
=
-
1281.152128182419801305514
;
double
x
=
-
1281.152128182419801305514
;
double
y
=
3486.949719522415307437768
;
double
y
=
3486.949719522415307437768
;
...
@@ -82,14 +82,14 @@ void GeoTest::_convertEnuToGeo_test(void)
...
@@ -82,14 +82,14 @@ void GeoTest::_convertEnuToGeo_test(void)
double
expectedAlt
=
0.0
;
double
expectedAlt
=
0.0
;
QGeoCoordinate
coord
;
QGeoCoordinate
coord
;
convert
Enu
ToGeo
(
x
,
y
,
z
,
_origin
,
&
coord
);
convert
Ned
ToGeo
(
x
,
y
,
z
,
_origin
,
&
coord
);
QCOMPARE
(
coord
.
latitude
(),
expectedLat
);
QCOMPARE
(
coord
.
latitude
(),
expectedLat
);
QCOMPARE
(
coord
.
longitude
(),
expectedLon
);
QCOMPARE
(
coord
.
longitude
(),
expectedLon
);
QCOMPARE
(
coord
.
altitude
(),
expectedAlt
);
QCOMPARE
(
coord
.
altitude
(),
expectedAlt
);
}
}
void
GeoTest
::
_convert
Enu
ToGeoAtOrigin_test
(
void
)
void
GeoTest
::
_convert
Ned
ToGeoAtOrigin_test
(
void
)
{
{
double
x
=
0.0
;
double
x
=
0.0
;
double
y
=
0.0
;
double
y
=
0.0
;
...
@@ -100,7 +100,7 @@ void GeoTest::_convertEnuToGeoAtOrigin_test(void)
...
@@ -100,7 +100,7 @@ void GeoTest::_convertEnuToGeoAtOrigin_test(void)
double
expectedAlt
=
_origin
.
altitude
();
double
expectedAlt
=
_origin
.
altitude
();
QGeoCoordinate
coord
;
QGeoCoordinate
coord
;
convert
Enu
ToGeo
(
x
,
y
,
z
,
_origin
,
&
coord
);
convert
Ned
ToGeo
(
x
,
y
,
z
,
_origin
,
&
coord
);
QCOMPARE
(
coord
.
latitude
(),
expectedLat
);
QCOMPARE
(
coord
.
latitude
(),
expectedLat
);
QCOMPARE
(
coord
.
longitude
(),
expectedLon
);
QCOMPARE
(
coord
.
longitude
(),
expectedLon
);
...
...
src/qgcunittest/GeoTest.h
View file @
4be05982
...
@@ -43,10 +43,10 @@ public:
...
@@ -43,10 +43,10 @@ public:
{
}
{
}
private
slots
:
private
slots
:
void
_convertGeoTo
Enu
_test
(
void
);
void
_convertGeoTo
Ned
_test
(
void
);
void
_convertGeoTo
Enu
AtOrigin_test
(
void
);
void
_convertGeoTo
Ned
AtOrigin_test
(
void
);
void
_convert
Enu
ToGeo_test
(
void
);
void
_convert
Ned
ToGeo_test
(
void
);
void
_convert
Enu
ToGeoAtOrigin_test
(
void
);
void
_convert
Ned
ToGeoAtOrigin_test
(
void
);
private:
private:
QGeoCoordinate
_origin
;
QGeoCoordinate
_origin
;
};
};
...
...
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