Updated conversion to local NED instead of ENU.

src/QGCGeo.cc

@@ -39,7 +39,7 @@
 
 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 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());
 }
 

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

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);

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;
 };