diff --git a/QGCApplication.pro b/QGCApplication.pro index fd7169337d6905499d64fabdb885c3637af6830c..f6e1434e4948053d87c9dc3d07d1aa83d3a1d486 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -272,7 +272,8 @@ HEADERS += \ src/ui/uas/QGCUnconnectedInfoWidget.h \ src/ui/uas/UASMessageView.h \ src/MissionItem.h \ - src/AutoPilotPlugins/PX4/PX4AirframeLoader.h + src/AutoPilotPlugins/PX4/PX4AirframeLoader.h \ + src/qgcunittest/GeoTest.h WindowsBuild { PRECOMPILED_HEADER += src/stable_headers.h @@ -384,7 +385,9 @@ SOURCES += \ src/ui/uas/QGCUnconnectedInfoWidget.cc \ src/ui/uas/UASMessageView.cc \ src/MissionItem.cc \ - src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc + src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc \ + src/QGCGeo.cc \ + src/qgcunittest/GeoTest.cc !iOSBuild { SOURCES += \ diff --git a/src/QGCGeo.cc b/src/QGCGeo.cc new file mode 100644 index 0000000000000000000000000000000000000000..b3c1f072fc6c60dad760fcfa49886994ab5fe131 --- /dev/null +++ b/src/QGCGeo.cc @@ -0,0 +1,95 @@ +/*===================================================================== + + 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 "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 convertGeoToEnu(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 convertEnuToGeo(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..84322ab8f536e4e1cfc43648c8da000cd17c377c 100644 --- a/src/QGCGeo.h +++ b/src/QGCGeo.h @@ -1,22 +1,61 @@ +/*===================================================================== + + 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 Up 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. */ -//void LatLonToENU(double lat, double lon, double alt, double originLat, double originLon, double originAlt, double* x, double* y, double* z) -//{ +void convertGeoToEnu(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z); -//} +/** + * @brief Transform a local (East, North, and Up) 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] 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); #endif // QGCGEO_H diff --git a/src/qgcunittest/GeoTest.cc b/src/qgcunittest/GeoTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..6ff25476889506a80aa02429011f3e17f253e289 --- /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::_convertGeoToEnu_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; + convertGeoToEnu(coord, _origin, &x, &y, &z); + + QCOMPARE(x, expectedX); + QCOMPARE(y, expectedY); + QCOMPARE(z, expectedZ); +} + +void GeoTest::_convertGeoToEnuAtOrigin_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; + convertGeoToEnu(coord, _origin, &x, &y, &z); + + QCOMPARE(x, expectedX); + QCOMPARE(y, expectedY); + QCOMPARE(z, expectedZ); +} + +void GeoTest::_convertEnuToGeo_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; + convertEnuToGeo(x, y, z, _origin, &coord); + + QCOMPARE(coord.latitude(), expectedLat); + QCOMPARE(coord.longitude(), expectedLon); + QCOMPARE(coord.altitude(), expectedAlt); +} + +void GeoTest::_convertEnuToGeoAtOrigin_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; + convertEnuToGeo(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..4c7455c2b8a3195edd2e3e9cbee9f423bd3cd639 --- /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 _convertGeoToEnu_test(void); + void _convertGeoToEnuAtOrigin_test(void); + void _convertEnuToGeo_test(void); + void _convertEnuToGeoAtOrigin_test(void); +private: + QGeoCoordinate _origin; +}; + +#endif // GEOTEST_H