From 4be0598254816182ed60c90fc3b88c6dbe1c5c9c Mon Sep 17 00:00:00 2001 From: David Goodman Date: Mon, 26 Oct 2015 21:24:36 -0700 Subject: [PATCH] Updated conversion to local NED instead of ENU. --- src/QGCGeo.cc | 8 ++++---- src/QGCGeo.h | 13 +++++++------ src/qgcunittest/GeoTest.cc | 18 +++++++++--------- src/qgcunittest/GeoTest.h | 8 ++++---- 4 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/QGCGeo.cc b/src/QGCGeo.cc index d47b7eda3..4f9a26920 100644 --- a/src/QGCGeo.cc +++ b/src/QGCGeo.cc @@ -39,7 +39,7 @@ static const float epsilon = std::numeric_limits::epsilon(); -void convertGeoToEnu(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z) { +void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z) { double lat_rad = coord.latitude() * 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 *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; - *z = coord.altitude() - origin.altitude(); + *z = -(coord.altitude() - origin.altitude()); } -void convertEnuToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord) { +void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord) { double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; 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 coord->setLatitude(lat_rad * M_RAD_TO_DEG); coord->setLongitude(lon_rad * M_RAD_TO_DEG); - coord->setAltitude(z + origin.altitude()); + coord->setAltitude(-z + origin.altitude()); } diff --git a/src/QGCGeo.h b/src/QGCGeo.h index 84322ab8f..f52a3bc2a 100644 --- a/src/QGCGeo.h +++ b/src/QGCGeo.h @@ -39,23 +39,24 @@ #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] origin Geoedetic origin for LTP projection. * @param[out] x North 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 convertGeoToEnu(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z); +void convertGeoToNed(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 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[out] coord Geodetic coordinate to hold result. */ -void convertEnuToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord); +void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord); #endif // QGCGEO_H diff --git a/src/qgcunittest/GeoTest.cc b/src/qgcunittest/GeoTest.cc index 6ff254768..4ae24b516 100644 --- a/src/qgcunittest/GeoTest.cc +++ b/src/qgcunittest/GeoTest.cc @@ -38,7 +38,7 @@ GeoTest::GeoTest(void) } */ -void GeoTest::_convertGeoToEnu_test(void) +void GeoTest::_convertGeoToNed_test(void) { QGeoCoordinate coord(47.364869, 8.594398, 0.0); @@ -47,31 +47,31 @@ void GeoTest::_convertGeoToEnu_test(void) double expectedZ = 0; double x, y, z; - convertGeoToEnu(coord, _origin, &x, &y, &z); + convertGeoToNed(coord, _origin, &x, &y, &z); QCOMPARE(x, expectedX); QCOMPARE(y, expectedY); QCOMPARE(z, expectedZ); } -void GeoTest::_convertGeoToEnuAtOrigin_test(void) +void GeoTest::_convertGeoToNedAtOrigin_test(void) { QGeoCoordinate coord(_origin); coord.setAltitude(10.0); // offset altitude to test z double expectedX = 0.0; double expectedY = 0.0; - double expectedZ = 10.0; + double expectedZ = -10.0; double x, y, z; - convertGeoToEnu(coord, _origin, &x, &y, &z); + convertGeoToNed(coord, _origin, &x, &y, &z); QCOMPARE(x, expectedX); QCOMPARE(y, expectedY); QCOMPARE(z, expectedZ); } -void GeoTest::_convertEnuToGeo_test(void) +void GeoTest::_convertNedToGeo_test(void) { double x = -1281.152128182419801305514; double y = 3486.949719522415307437768; @@ -82,14 +82,14 @@ void GeoTest::_convertEnuToGeo_test(void) double expectedAlt = 0.0; QGeoCoordinate coord; - convertEnuToGeo(x, y, z, _origin, &coord); + convertNedToGeo(x, y, z, _origin, &coord); QCOMPARE(coord.latitude(), expectedLat); QCOMPARE(coord.longitude(), expectedLon); QCOMPARE(coord.altitude(), expectedAlt); } -void GeoTest::_convertEnuToGeoAtOrigin_test(void) +void GeoTest::_convertNedToGeoAtOrigin_test(void) { double x = 0.0; double y = 0.0; @@ -100,7 +100,7 @@ void GeoTest::_convertEnuToGeoAtOrigin_test(void) double expectedAlt = _origin.altitude(); QGeoCoordinate coord; - convertEnuToGeo(x, y, z, _origin, &coord); + convertNedToGeo(x, y, z, _origin, &coord); QCOMPARE(coord.latitude(), expectedLat); QCOMPARE(coord.longitude(), expectedLon); diff --git a/src/qgcunittest/GeoTest.h b/src/qgcunittest/GeoTest.h index 4c7455c2b..230256e45 100644 --- a/src/qgcunittest/GeoTest.h +++ b/src/qgcunittest/GeoTest.h @@ -43,10 +43,10 @@ public: { } private slots: - void _convertGeoToEnu_test(void); - void _convertGeoToEnuAtOrigin_test(void); - void _convertEnuToGeo_test(void); - void _convertEnuToGeoAtOrigin_test(void); + void _convertGeoToNed_test(void); + void _convertGeoToNedAtOrigin_test(void); + void _convertNedToGeo_test(void); + void _convertNedToGeoAtOrigin_test(void); private: QGeoCoordinate _origin; }; -- 2.22.0