diff --git a/QGCApplication.pro b/QGCApplication.pro index fd7169337d6905499d64fabdb885c3637af6830c..c14700d13ed94341b895a10f4132987dd605de2d 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -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 += \ 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 += \ 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 \ diff --git a/src/QGCGeo.cc b/src/QGCGeo.cc new file mode 100644 index 0000000000000000000000000000000000000000..4f9a26920130c9d7b7deb18155354838529b9e39 --- /dev/null +++ b/src/QGCGeo.cc @@ -0,0 +1,96 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +#include +#include + +#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::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()); +} + diff --git a/src/QGCGeo.h b/src/QGCGeo.h index 897910ce792eb2e608b568131e67890adbcf4f5b..f52a3bc2ab35006119de2683ddbe000479a71a13 100644 --- a/src/QGCGeo.h +++ b/src/QGCGeo.h @@ -1,22 +1,62 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +/// @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 + #ifndef QGCGEO_H #define QGCGEO_H -#define DEG2RAD (M_PI/180.0) +#include /* 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 diff --git a/src/qgcunittest/GeoTest.cc b/src/qgcunittest/GeoTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..4ae24b5162d7d883be94b10d056d9f2574c7d01a --- /dev/null +++ b/src/qgcunittest/GeoTest.cc @@ -0,0 +1,108 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +/// @file +/// @brief Unit test fpr QGCGeo coordinate project math. +/// +/// @author David Goodman + +#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); +} diff --git a/src/qgcunittest/GeoTest.h b/src/qgcunittest/GeoTest.h new file mode 100644 index 0000000000000000000000000000000000000000..230256e452e9f804f1b0edd611a8aa139316e22f --- /dev/null +++ b/src/qgcunittest/GeoTest.h @@ -0,0 +1,54 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +/// @file +/// @brief Unit test for QGCGeo coordinate transformation math. +/// +/// @author David Goodman + +#ifndef GEOTEST_H +#define GEOTEST_H + +#include + +#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