Merge pull request #2116 from dagoodma/geodetic_math

Geodetic math
10 years ago · d44fcde43a
5 changed files with 309 additions and 8 deletions
--- a/QGCApplication.pro
+++ b/QGCApplication.pro
@ -358,6 +358,7 @@ SOURCES += \
				@@ -358,6 +358,7 @@ SOURCES += \
    src/QGCQuickWidget.cc \
    src/QGCSingleton.cc \
    src/QGCTemporaryFile.cc \
+    src/QGCGeo.cc \
    src/QmlControls/CoordinateVector.cc \
    src/QmlControls/ParameterEditorController.cc \
    src/QmlControls/ScreenToolsController.cc \
@ -456,6 +457,7 @@ HEADERS += \
				@@ -456,6 +457,7 @@ HEADERS += \
    src/MissionManager/MissionControllerTest.h \
    src/MissionManager/MissionControllerManagerTest.h \
    src/MissionManager/MissionManagerTest.h \
+    src/qgcunittest/GeoTest.h \
    src/qgcunittest/FileDialogTest.h \
    src/qgcunittest/FileManagerTest.h \
    src/qgcunittest/FlightGearTest.h \
@ -478,6 +480,7 @@ SOURCES += \
				@@ -478,6 +480,7 @@ SOURCES += \
    src/MissionManager/MissionControllerTest.cc \
    src/MissionManager/MissionControllerManagerTest.cc \
    src/MissionManager/MissionManagerTest.cc \
+    src/qgcunittest/GeoTest.cc \
    src/qgcunittest/FileDialogTest.cc \
    src/qgcunittest/FileManagerTest.cc \
    src/qgcunittest/FlightGearTest.cc \
--- a/src/QGCGeo.cc
+++ b/src/QGCGeo.cc
@ -0,0 +1,96 @@
				@@ -0,0 +1,96 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+ ======================================================================*/
+
+#include <cmath>
+#include <limits>
+
+#include "QGCGeo.h"
+
+// These defines are private
+#define M_DEG_TO_RAD (M_PI / 180.0)
+
+#define M_RAD_TO_DEG (180.0 / M_PI)
+
+#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
+#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
+
+static const float epsilon = std::numeric_limits<double>::epsilon();
+
+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;
+
+    double ref_lon_rad = origin.longitude() * M_DEG_TO_RAD;
+    double ref_lat_rad = origin.latitude() * M_DEG_TO_RAD;
+
+    double sin_lat = sin(lat_rad);
+    double cos_lat = cos(lat_rad);
+    double cos_d_lon = cos(lon_rad - ref_lon_rad);
+
+    double ref_sin_lat = sin(ref_lat_rad);
+    double ref_cos_lat = cos(ref_lat_rad);
+
+    double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon);
+    double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c));
+
+    *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());
+}
+
+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);
+    double sin_c = sin(c);
+    double cos_c = cos(c);
+
+    double ref_lon_rad = origin.longitude() * M_DEG_TO_RAD;
+    double ref_lat_rad = origin.latitude() * M_DEG_TO_RAD;
+
+    double ref_sin_lat = sin(ref_lat_rad);
+    double ref_cos_lat = cos(ref_lat_rad);
+
+    double lat_rad;
+    double lon_rad;
+
+    if (fabs(c) > epsilon) {
+        lat_rad = asin(cos_c * ref_sin_lat + (x_rad * sin_c * ref_cos_lat) / c);
+        lon_rad = (ref_lon_rad + atan2(y_rad * sin_c, c * ref_cos_lat * cos_c - x_rad * ref_sin_lat * sin_c));
+
+    } else {
+        lat_rad = ref_lat_rad;
+        lon_rad = ref_lon_rad;
+    }
+
+    coord->setLatitude(lat_rad * M_RAD_TO_DEG);
+    coord->setLongitude(lon_rad * M_RAD_TO_DEG);
+
+    coord->setAltitude(-z + origin.altitude());
+}
+
--- a/src/QGCGeo.h
+++ b/src/QGCGeo.h
@ -1,22 +1,62 @@
				@@ -1,22 +1,62 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+ ======================================================================*/
+
+/// @file
+///     @brief Coordinate transformation math functions.
+///     @link https://github.com/PX4/Firmware/blob/master/src/lib/geo/geo.c
+///     @link http://psas.pdx.edu/CoordinateSystem/Latitude_to_LocalTangent.pdf
+///     @link http://dspace.dsto.defence.gov.au/dspace/bitstream/1947/3538/1/DSTO-TN-0432.pdf
+///     @author David Goodman <dagoodma@gmail.com>
+
 #ifndef QGCGEO_H
 #define QGCGEO_H

-#define DEG2RAD (M_PI/180.0)
+#include <QGeoCoordinate>

 /* Safeguard for systems lacking sincos (e.g. Mac OS X Leopard) */
 #ifndef sincos
 #define sincos(th,x,y) { (*(x))=sin(th); (*(y))=cos(th); }
 #endif

-
 /**
- * Converting from latitude / longitude to tangent on earth surface
- * @link http://psas.pdx.edu/CoordinateSystem/Latitude_to_LocalTangent.pdf
- * @link http://dspace.dsto.defence.gov.au/dspace/bitstream/1947/3538/1/DSTO-TN-0432.pdf
+ * @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 Down component of coordinate in local plane.
 */
-//void LatLonToENU(double lat, double lon, double alt, double originLat, double originLon, double originAlt, 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 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 Down component of local coordinate in meters.
+ * @param[in] origin Geoedetic origin for LTP.
+ * @param[out] coord Geodetic coordinate to hold result.
+ */
+void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCoordinate *coord);

 #endif // QGCGEO_H
--- a/src/qgcunittest/GeoTest.cc
+++ b/src/qgcunittest/GeoTest.cc
@ -0,0 +1,108 @@
				@@ -0,0 +1,108 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+ ======================================================================*/
+
+/// @file
+///     @brief Unit test fpr QGCGeo coordinate project math.
+///
+///     @author David Goodman <dagoodma@gmail.com>
+
+#include "GeoTest.h"
+#include "QGCGeo.h"
+
+UT_REGISTER_TEST(GeoTest)
+
+/*
+GeoTest::GeoTest(void)
+{
+
+}
+*/
+
+void GeoTest::_convertGeoToNed_test(void)
+{
+    QGeoCoordinate coord(47.364869, 8.594398, 0.0);
+
+    double expectedX = -1281.152128182419801305514;
+    double expectedY = 3486.949719522415307437768;
+    double expectedZ = 0;
+
+    double x, y, z;
+    convertGeoToNed(coord, _origin, &x, &y, &z);
+
+    QCOMPARE(x, expectedX);
+    QCOMPARE(y, expectedY);
+    QCOMPARE(z, expectedZ);
+}
+
+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 x, y, z;
+    convertGeoToNed(coord, _origin, &x, &y, &z);
+
+    QCOMPARE(x, expectedX);
+    QCOMPARE(y, expectedY);
+    QCOMPARE(z, expectedZ);
+}
+
+void GeoTest::_convertNedToGeo_test(void)
+{
+    double x = -1281.152128182419801305514;
+    double y = 3486.949719522415307437768;
+    double z = 0;
+
+    double expectedLat = 47.364869;
+    double expectedLon = 8.594398;
+    double expectedAlt = 0.0;
+
+    QGeoCoordinate coord;
+    convertNedToGeo(x, y, z, _origin, &coord);
+
+    QCOMPARE(coord.latitude(), expectedLat);
+    QCOMPARE(coord.longitude(), expectedLon);
+    QCOMPARE(coord.altitude(), expectedAlt);
+}
+
+void GeoTest::_convertNedToGeoAtOrigin_test(void)
+{
+    double x = 0.0;
+    double y = 0.0;
+    double z = 0.0;
+
+    double expectedLat = _origin.latitude();
+    double expectedLon = _origin.longitude();
+    double expectedAlt = _origin.altitude();
+
+    QGeoCoordinate coord;
+    convertNedToGeo(x, y, z, _origin, &coord);
+
+    QCOMPARE(coord.latitude(), expectedLat);
+    QCOMPARE(coord.longitude(), expectedLon);
+    QCOMPARE(coord.altitude(), expectedAlt);
+}
--- a/src/qgcunittest/GeoTest.h
+++ b/src/qgcunittest/GeoTest.h
@ -0,0 +1,54 @@
				@@ -0,0 +1,54 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+ ======================================================================*/
+
+/// @file
+///     @brief Unit test for QGCGeo coordinate transformation math.
+///
+///     @author David Goodman <dagoodma@gmail.com>
+
+#ifndef GEOTEST_H
+#define GEOTEST_H
+
+#include <QGeoCoordinate>
+
+#include "UnitTest.h"
+
+class GeoTest : public UnitTest
+{
+    Q_OBJECT
+
+public:
+    GeoTest(void)
+        : _origin(47.3764, 8.5481, 0.0) /// Use ETH campus (47.3764° N, 8.5481° E)
+    { }
+
+private slots:
+    void _convertGeoToNed_test(void);
+    void _convertGeoToNedAtOrigin_test(void);
+    void _convertNedToGeo_test(void);
+    void _convertNedToGeoAtOrigin_test(void);
+private:
+    QGeoCoordinate _origin;
+};
+
+#endif // GEOTEST_H