Commit 4be05982 authored by David Goodman's avatar David Goodman

Updated conversion to local NED instead of ENU.

parent 7eea5671
...@@ -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 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 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 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 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());
} }
...@@ -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 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 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 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 #endif // QGCGEO_H
...@@ -38,7 +38,7 @@ GeoTest::GeoTest(void) ...@@ -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); 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;
convertGeoToEnu(coord, _origin, &x, &y, &z); convertGeoToNed(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::_convertGeoToEnuAtOrigin_test(void) void GeoTest::_convertGeoToNedAtOrigin_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;
convertGeoToEnu(coord, _origin, &x, &y, &z); convertGeoToNed(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::_convertEnuToGeo_test(void) void GeoTest::_convertNedToGeo_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;
convertEnuToGeo(x, y, z, _origin, &coord); convertNedToGeo(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::_convertEnuToGeoAtOrigin_test(void) void GeoTest::_convertNedToGeoAtOrigin_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;
convertEnuToGeo(x, y, z, _origin, &coord); convertNedToGeo(x, y, z, _origin, &coord);
QCOMPARE(coord.latitude(), expectedLat); QCOMPARE(coord.latitude(), expectedLat);
QCOMPARE(coord.longitude(), expectedLon); QCOMPARE(coord.longitude(), expectedLon);
......
...@@ -43,10 +43,10 @@ public: ...@@ -43,10 +43,10 @@ public:
{ } { }
private slots: private slots:
void _convertGeoToEnu_test(void); void _convertGeoToNed_test(void);
void _convertGeoToEnuAtOrigin_test(void); void _convertGeoToNedAtOrigin_test(void);
void _convertEnuToGeo_test(void); void _convertNedToGeo_test(void);
void _convertEnuToGeoAtOrigin_test(void); void _convertNedToGeoAtOrigin_test(void);
private: private:
QGeoCoordinate _origin; QGeoCoordinate _origin;
}; };
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment