From dacd3c2b4efbca2f923c80779c56bbd8a3f6bbce Mon Sep 17 00:00:00 2001 From: Holden Date: Mon, 11 Mar 2024 05:08:26 -0400 Subject: [PATCH] CMake: Fetch geographiclib --- CMakeLists.txt | 2 +- QGCExternalLibs.pri | 21 ++ {src/Geo => libs/GeographicLib}/Constants.hpp | 0 {src/Geo => libs/GeographicLib}/MGRS.cpp | 0 {src/Geo => libs/GeographicLib}/MGRS.hpp | 0 {src/Geo => libs/GeographicLib}/Math.cpp | 0 {src/Geo => libs/GeographicLib}/Math.hpp | 0 .../GeographicLib}/PolarStereographic.cpp | 0 .../GeographicLib}/PolarStereographic.hpp | 0 .../GeographicLib}/TransverseMercator.cpp | 0 .../GeographicLib}/TransverseMercator.hpp | 0 {src/Geo => libs/GeographicLib}/UTMUPS.cpp | 0 {src/Geo => libs/GeographicLib}/UTMUPS.hpp | 0 {src/Geo => libs/GeographicLib}/Utility.cpp | 0 {src/Geo => libs/GeographicLib}/Utility.h | 0 {src/Geo => libs/GeographicLib}/Utility.hpp | 0 qgroundcontrol.pro | 13 - src/Geo/CMakeLists.txt | 44 ++-- src/Geo/QGCGeo.cc | 224 +++++++++--------- src/Geo/QGCGeo.h | 18 +- src/MissionManager/QGCMapPolygon.cc | 8 +- src/MissionManager/QGCMapPolyline.cc | 12 +- src/MissionManager/SurveyComplexItem.cc | 14 +- .../EditPositionDialogController.cc | 8 +- src/Utilities/SHPFileHelper.cc | 2 +- .../FactGroups/VehicleGPS2FactGroup.cc | 4 +- src/Vehicle/FactGroups/VehicleGPSFactGroup.cc | 6 +- test/Geo/GeoTest.cc | 112 ++++++--- test/Geo/GeoTest.h | 14 +- 29 files changed, 279 insertions(+), 223 deletions(-) rename {src/Geo => libs/GeographicLib}/Constants.hpp (100%) rename {src/Geo => libs/GeographicLib}/MGRS.cpp (100%) rename {src/Geo => libs/GeographicLib}/MGRS.hpp (100%) rename {src/Geo => libs/GeographicLib}/Math.cpp (100%) rename {src/Geo => libs/GeographicLib}/Math.hpp (100%) rename {src/Geo => libs/GeographicLib}/PolarStereographic.cpp (100%) rename {src/Geo => libs/GeographicLib}/PolarStereographic.hpp (100%) rename {src/Geo => libs/GeographicLib}/TransverseMercator.cpp (100%) rename {src/Geo => libs/GeographicLib}/TransverseMercator.hpp (100%) rename {src/Geo => libs/GeographicLib}/UTMUPS.cpp (100%) rename {src/Geo => libs/GeographicLib}/UTMUPS.hpp (100%) rename {src/Geo => libs/GeographicLib}/Utility.cpp (100%) rename {src/Geo => libs/GeographicLib}/Utility.h (100%) rename {src/Geo => libs/GeographicLib}/Utility.hpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index f28598b028f..906742ee510 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR) +cmake_minimum_required(VERSION 3.22.1) set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release") if(NOT CMAKE_BUILD_TYPE) diff --git a/QGCExternalLibs.pri b/QGCExternalLibs.pri index f592ea3c2a0..390c347a5a3 100644 --- a/QGCExternalLibs.pri +++ b/QGCExternalLibs.pri @@ -231,3 +231,24 @@ contains (DEFINES, CONFIG_UTM_ADAPTER){ INCLUDEPATH += $$PWD/libs/libevents/libevents/libs/cpp/parse/nlohmann_json/include LIBS += -lboost_system -lboost_thread -lssl -lcrypto } + +HEADERS += \ + libs/GeographicLib/Constants.hpp \ + libs/GeographicLib/Math.hpp \ + libs/GeographicLib/Utility.hpp \ + libs/GeographicLib/UTMUPS.hpp \ + libs/GeographicLib/MGRS.hpp \ + libs/GeographicLib/TransverseMercator.hpp \ + libs/GeographicLib/PolarStereographic.hpp \ + +SOURCES += \ + libs/GeographicLib/Math.cpp \ + libs/GeographicLib/Utility.cpp \ + libs/GeographicLib/UTMUPS.cpp \ + libs/GeographicLib/MGRS.cpp \ + libs/GeographicLib/TransverseMercator.cpp \ + libs/GeographicLib/PolarStereographic.cpp \ + +INCLUDEPATH += \ + libs \ + libs/GeographicLib diff --git a/src/Geo/Constants.hpp b/libs/GeographicLib/Constants.hpp similarity index 100% rename from src/Geo/Constants.hpp rename to libs/GeographicLib/Constants.hpp diff --git a/src/Geo/MGRS.cpp b/libs/GeographicLib/MGRS.cpp similarity index 100% rename from src/Geo/MGRS.cpp rename to libs/GeographicLib/MGRS.cpp diff --git a/src/Geo/MGRS.hpp b/libs/GeographicLib/MGRS.hpp similarity index 100% rename from src/Geo/MGRS.hpp rename to libs/GeographicLib/MGRS.hpp diff --git a/src/Geo/Math.cpp b/libs/GeographicLib/Math.cpp similarity index 100% rename from src/Geo/Math.cpp rename to libs/GeographicLib/Math.cpp diff --git a/src/Geo/Math.hpp b/libs/GeographicLib/Math.hpp similarity index 100% rename from src/Geo/Math.hpp rename to libs/GeographicLib/Math.hpp diff --git a/src/Geo/PolarStereographic.cpp b/libs/GeographicLib/PolarStereographic.cpp similarity index 100% rename from src/Geo/PolarStereographic.cpp rename to libs/GeographicLib/PolarStereographic.cpp diff --git a/src/Geo/PolarStereographic.hpp b/libs/GeographicLib/PolarStereographic.hpp similarity index 100% rename from src/Geo/PolarStereographic.hpp rename to libs/GeographicLib/PolarStereographic.hpp diff --git a/src/Geo/TransverseMercator.cpp b/libs/GeographicLib/TransverseMercator.cpp similarity index 100% rename from src/Geo/TransverseMercator.cpp rename to libs/GeographicLib/TransverseMercator.cpp diff --git a/src/Geo/TransverseMercator.hpp b/libs/GeographicLib/TransverseMercator.hpp similarity index 100% rename from src/Geo/TransverseMercator.hpp rename to libs/GeographicLib/TransverseMercator.hpp diff --git a/src/Geo/UTMUPS.cpp b/libs/GeographicLib/UTMUPS.cpp similarity index 100% rename from src/Geo/UTMUPS.cpp rename to libs/GeographicLib/UTMUPS.cpp diff --git a/src/Geo/UTMUPS.hpp b/libs/GeographicLib/UTMUPS.hpp similarity index 100% rename from src/Geo/UTMUPS.hpp rename to libs/GeographicLib/UTMUPS.hpp diff --git a/src/Geo/Utility.cpp b/libs/GeographicLib/Utility.cpp similarity index 100% rename from src/Geo/Utility.cpp rename to libs/GeographicLib/Utility.cpp diff --git a/src/Geo/Utility.h b/libs/GeographicLib/Utility.h similarity index 100% rename from src/Geo/Utility.h rename to libs/GeographicLib/Utility.h diff --git a/src/Geo/Utility.hpp b/libs/GeographicLib/Utility.hpp similarity index 100% rename from src/Geo/Utility.hpp rename to libs/GeographicLib/Utility.hpp diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 53e3f1b8105..9e054e3f22d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -493,13 +493,6 @@ HEADERS += \ src/PositionManager/PositionManager.h \ src/PositionManager/SimulatedPosition.h \ src/Geo/QGCGeo.h \ - src/Geo/Constants.hpp \ - src/Geo/Math.hpp \ - src/Geo/Utility.hpp \ - src/Geo/UTMUPS.hpp \ - src/Geo/MGRS.hpp \ - src/Geo/TransverseMercator.hpp \ - src/Geo/PolarStereographic.hpp \ src/Utilities/QGC.h \ src/QGCApplication.h \ src/Utilities/QGCCachedFileDownload.h \ @@ -763,12 +756,6 @@ SOURCES += \ src/PositionManager/PositionManager.cpp \ src/PositionManager/SimulatedPosition.cc \ src/Geo/QGCGeo.cc \ - src/Geo/Math.cpp \ - src/Geo/Utility.cpp \ - src/Geo/UTMUPS.cpp \ - src/Geo/MGRS.cpp \ - src/Geo/TransverseMercator.cpp \ - src/Geo/PolarStereographic.cpp \ src/Utilities/QGC.cc \ src/QGCApplication.cc \ src/Utilities/QGCCachedFileDownload.cc \ diff --git a/src/Geo/CMakeLists.txt b/src/Geo/CMakeLists.txt index c0d6da2fd83..8a6050f2613 100644 --- a/src/Geo/CMakeLists.txt +++ b/src/Geo/CMakeLists.txt @@ -1,36 +1,34 @@ find_package(Qt6 REQUIRED COMPONENTS Core Positioning) -# include(FetchContent) -# FetchContent_Declare(geographiclib -# GIT_REPOSITORY https://github.com/geographiclib/geographiclib.git -# GIT_TAG v2.3 -# GIT_SHALLOW TRUE -# ) -# FetchContent_MakeAvailable(geographiclib) - qt_add_library(Geo STATIC - Constants.hpp - Math.cpp - Math.hpp - MGRS.cpp - MGRS.hpp - PolarStereographic.cpp - PolarStereographic.hpp QGCGeo.cc QGCGeo.h - TransverseMercator.cpp - TransverseMercator.hpp - Utility.cpp - Utility.h - Utility.hpp - UTMUPS.cpp - UTMUPS.hpp ) +set(BUILD_SHARED_LIBS OFF CACHE INTERNAL "") +set(BUILD_BOTH_LIBS OFF CACHE INTERNAL "") +set(BUILD_DOCUMENTATION OFF CACHE INTERNAL "") +set(BUILD_MANPAGES OFF CACHE INTERNAL "") + +include(FetchContent) +FetchContent_Declare(geographiclib + GIT_REPOSITORY https://github.com/geographiclib/geographiclib.git + GIT_TAG r2.3 + GIT_SHALLOW TRUE +) +FetchContent_GetProperties(geographiclib) +if(NOT geographiclib_POPULATED) + FetchContent_Populate(geographiclib) + add_subdirectory(${geographiclib_SOURCE_DIR} ${geographiclib_BINARY_DIR} EXCLUDE_FROM_ALL) +endif() +# FetchContent_MakeAvailable(geographiclib) + target_link_libraries(Geo PRIVATE - Qt6::Core + GeographicLib::GeographicLib + Utilities PUBLIC + Qt6::Core Qt6::Positioning ) diff --git a/src/Geo/QGCGeo.cc b/src/Geo/QGCGeo.cc index 4ee23a54649..c303233cc34 100644 --- a/src/Geo/QGCGeo.cc +++ b/src/Geo/QGCGeo.cc @@ -7,149 +7,153 @@ * ****************************************************************************/ +// TODO: Use GeoCoords from GeographicLib? + #include "QGCGeo.h" -#include "UTMUPS.hpp" -#include "MGRS.hpp" +#include "QGCLoggingCategory.h" #include +#include + +#include +#include +#include -#include #include -// These defines are private -#define M_DEG_TO_RAD (M_PI / 180.0) -#define M_RAD_TO_DEG (180.0 / M_PI) -#define CONSTANTS_RADIUS_OF_EARTH 6371000 // meters (m) +QGC_LOGGING_CATEGORY(QGCGeoLog, "qgc.geo.qgcgeo") -static const double epsilon = std::numeric_limits::epsilon(); +static constexpr double epsilon = std::numeric_limits::epsilon(); -void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z) -{ - if (coord == origin) { - // Short circuit to prevent NaNs in calculation - *x = *y = *z = 0; - return; - } +namespace QGCGeo { - double lat_rad = coord.latitude() * M_DEG_TO_RAD; - double lon_rad = coord.longitude() * M_DEG_TO_RAD; + void convertGeoToNed(const QGeoCoordinate &coord, const QGeoCoordinate &origin, double &x, double &y, double &z) { + if (coord == origin) { + // Prevent NaNs in calculation + x = y = z = 0; + return; + } - double ref_lon_rad = origin.longitude() * M_DEG_TO_RAD; - double ref_lat_rad = origin.latitude() * M_DEG_TO_RAD; + const double lat_rad = qDegreesToRadians(coord.latitude()); + const double lon_rad = qDegreesToRadians(coord.longitude()); - double sin_lat = sin(lat_rad); - double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref_lon_rad); + const double ref_lon_rad = qDegreesToRadians(origin.longitude()); + const double ref_lat_rad = qDegreesToRadians(origin.latitude()); - double ref_sin_lat = sin(ref_lat_rad); - double ref_cos_lat = cos(ref_lat_rad); + const double sin_lat = sin(lat_rad); + const double cos_lat = cos(lat_rad); + const double cos_d_lon = cos(lon_rad - ref_lon_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)); + const double ref_sin_lat = sin(ref_lat_rad); + const double ref_cos_lat = cos(ref_lat_rad); - *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; + const double c = acos(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon); + const double k = (fabs(c) < epsilon) ? 1.0 : (c / sin(c)); - *z = -(coord.altitude() - origin.altitude()); -} + x = k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * GeographicLib::Constants::WGS84_a(); + y = k * cos_lat * sin(lon_rad - ref_lon_rad) * GeographicLib::Constants::WGS84_a(); + 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 = sqrt(x_rad * x_rad + y_rad * y_rad); - double sin_c = sin(c); - double cos_c = cos(c); + void convertNedToGeo(double x, double y, double z, const QGeoCoordinate &origin, QGeoCoordinate &coord) { + const double x_rad = x / GeographicLib::Constants::WGS84_a(); + const double y_rad = y / GeographicLib::Constants::WGS84_a(); + const double c = sqrt(x_rad * x_rad + y_rad * y_rad); + const double sin_c = sin(c); + const 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; + const double ref_lon_rad = qDegreesToRadians(origin.longitude()); + const double ref_lat_rad = qDegreesToRadians(origin.latitude()); - double ref_sin_lat = sin(ref_lat_rad); - double ref_cos_lat = cos(ref_lat_rad); + const double ref_sin_lat = sin(ref_lat_rad); + const double ref_cos_lat = cos(ref_lat_rad); - double lat_rad; - double lon_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)); + 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; + } - } else { - lat_rad = ref_lat_rad; - lon_rad = ref_lon_rad; + coord.setLatitude(qRadiansToDegrees(lat_rad)); + coord.setLongitude(qRadiansToDegrees(lon_rad)); + coord.setAltitude(-z + origin.altitude()); } - coord->setLatitude(lat_rad * M_RAD_TO_DEG); - coord->setLongitude(lon_rad * M_RAD_TO_DEG); + int convertGeoToUTM(const QGeoCoordinate& coord, double &easting, double &northing) + { + try { + int zone; + bool northp; + GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, easting, northing); + return zone; + } catch(GeographicLib::GeographicErr) { + return 0; + } + } - coord->setAltitude(-z + origin.altitude()); -} + bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate &coord) + { + double lat, lon; -int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northing) -{ - try { - int zone; - bool northp; - GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, easting, northing); - return zone; - } catch(...) { - return 0; - } -} + try { + GeographicLib::UTMUPS::Reverse(zone, !southhemi, easting, northing, lat, lon); + } catch(GeographicLib::GeographicErr) { + return false; + } + + coord.setLatitude(lat); + coord.setLongitude(lon); -bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord) -{ - double lat, lon; - try { - GeographicLib::UTMUPS::Reverse(zone, !southhemi, easting, northing, lat, lon); - } catch(...) { - return false; + return true; } - coord.setLatitude(lat); - coord.setLongitude(lon); - return true; -} + QString convertGeoToMGRS(const QGeoCoordinate &coord) + { + std::string mgrs; + + try { + int zone; + bool northp; + double x, y; + GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, x, y); + GeographicLib::MGRS::Forward(zone, northp, x, y, coord.latitude(), 5, mgrs); + } catch(GeographicLib::GeographicErr) { + mgrs = ""; + } -QString convertGeoToMGRS(const QGeoCoordinate& coord) -{ - int zone; - bool northp; - double x, y; - std::string mgrs; - - try { - GeographicLib::UTMUPS::Forward(coord.latitude(), coord.longitude(), zone, northp, x, y); - GeographicLib::MGRS::Forward(zone, northp, x, y, coord.latitude(), 5, mgrs); - } catch(...) { - mgrs = ""; + const QString qstr = QString::fromStdString(mgrs); + for (int i = qstr.length() - 1; i >= 0; i--) { + if (!qstr.at(i).isDigit()) { + const int l = (qstr.length() - i) / 2; + return qstr.left(i + 1) + " " + qstr.mid(i + 1, l) + " " + qstr.mid(i + 1 + l); + } + } + + return qstr; } - QString qstr = QString::fromStdString(mgrs); - for (int i = qstr.length() - 1; i >= 0; i--) { - if (!qstr.at(i).isDigit()) { - int l = (qstr.length() - i) / 2; - return qstr.left(i + 1) + " " + qstr.mid(i + 1, l) + " " + qstr.mid(i + 1 + l); + bool convertMGRSToGeo(const QString &mgrs, QGeoCoordinate &coord) + { + double lat, lon; + + try { + int zone, prec; + bool northp; + double x, y; + GeographicLib::MGRS::Reverse(mgrs.simplified().replace(" ", "").toStdString(), zone, northp, x, y, prec); + GeographicLib::UTMUPS::Reverse(zone, northp, x, y, lat, lon); + } catch(GeographicLib::GeographicErr) { + return false; } - } - return qstr; -} + coord.setLatitude(lat); + coord.setLongitude(lon); -bool convertMGRSToGeo(QString mgrs, QGeoCoordinate& coord) -{ - int zone, prec; - bool northp; - double x, y; - double lat, lon; - - try { - GeographicLib::MGRS::Reverse(mgrs.simplified().replace(" ", "").toStdString(), zone, northp, x, y, prec); - GeographicLib::UTMUPS::Reverse(zone, northp, x, y, lat, lon); - } catch(...) { - return false; + return true; } - coord.setLatitude(lat); - coord.setLongitude(lon); - - return true; } diff --git a/src/Geo/QGCGeo.h b/src/Geo/QGCGeo.h index f55d6507cf7..b5f460173a3 100644 --- a/src/Geo/QGCGeo.h +++ b/src/Geo/QGCGeo.h @@ -18,6 +18,11 @@ #pragma once #include +#include + +Q_DECLARE_LOGGING_CATEGORY(QGCGeoLog) + +namespace QGCGeo { /** * @brief Project a geodetic coordinate on to local tangential plane (LTP) as coordinate with East, @@ -28,7 +33,7 @@ * @param[out] y East component of coordinate in local plane. * @param[out] z Down component of coordinate in local plane. */ -void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, double* y, double* z); + void convertGeoToNed(const QGeoCoordinate &coord, const QGeoCoordinate &origin, double &x, double &y, double &z); /** * @brief Transform a local (East, North, and Down) coordinate into a geodetic coordinate. @@ -38,7 +43,7 @@ void convertGeoToNed(QGeoCoordinate coord, QGeoCoordinate origin, double* x, dou * @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); + void convertNedToGeo(double x, double y, double z, const QGeoCoordinate &origin, QGeoCoordinate &coord); // LatLonToUTMXY // Converts a latitude/longitude pair to x and y coordinates in the @@ -58,7 +63,7 @@ void convertNedToGeo(double x, double y, double z, QGeoCoordinate origin, QGeoCo // Returns: // The UTM zone used for calculating the values of x and y. // If conversion failed the function returns 0 -int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northing); + int convertGeoToUTM(const QGeoCoordinate& coord, double &easting, double &northing); // UTMXYToLatLon // @@ -79,7 +84,7 @@ int convertGeoToUTM(const QGeoCoordinate& coord, double& easting, double& northi // // Returns: // The function returns true if conversion succeeded. -bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate& coord); + bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, QGeoCoordinate &coord); // Converts a latitude/longitude pair to MGRS string // @@ -89,7 +94,7 @@ bool convertUTMToGeo(double easting, double northing, int zone, bool southhemi, // Returns: // The MGRS coordinate string // If conversion fails the function returns empty string -QString convertGeoToMGRS(const QGeoCoordinate& coord); + QString convertGeoToMGRS(const QGeoCoordinate &coord); // Converts MGRS string to a latitude/longitude pair. // @@ -102,4 +107,5 @@ QString convertGeoToMGRS(const QGeoCoordinate& coord); // // Returns: // The function returns true if conversion succeeded. -bool convertMGRSToGeo(QString mgrs, QGeoCoordinate& coord); + bool convertMGRSToGeo(const QString &mgrs, QGeoCoordinate &coord); +} diff --git a/src/MissionManager/QGCMapPolygon.cc b/src/MissionManager/QGCMapPolygon.cc index ae609c71920..25360e846c5 100644 --- a/src/MissionManager/QGCMapPolygon.cc +++ b/src/MissionManager/QGCMapPolygon.cc @@ -118,7 +118,7 @@ QGeoCoordinate QGCMapPolygon::_coordFromPointF(const QPointF& point) const if (_polygonPath.count() > 0) { QGeoCoordinate tangentOrigin = _polygonPath[0].value(); - convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, coord); } return coord; @@ -130,7 +130,7 @@ QPointF QGCMapPolygon::_pointFFromCoord(const QGeoCoordinate& coordinate) const double y, x, down; QGeoCoordinate tangentOrigin = _polygonPath[0].value(); - convertGeoToNed(coordinate, tangentOrigin, &y, &x, &down); + QGCGeo::convertGeoToNed(coordinate, tangentOrigin, y, x, down); return QPointF(x, -y); } @@ -412,7 +412,7 @@ QList QGCMapPolygon::nedPolygon(void) const // This avoids a nan calculation that comes out of convertGeoToNed x = y = 0; } else { - convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); + QGCGeo::convertGeoToNed(vertex, tangentOrigin, y, x, down); } nedPolygon += QPointF(x, y); } @@ -464,7 +464,7 @@ void QGCMapPolygon::offset(double distance) return; } QGeoCoordinate coord; - convertNedToGeo(newVertex.y(), newVertex.x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(newVertex.y(), newVertex.x(), 0, tangentOrigin, coord); rgNewPolygon.append(coord); } } diff --git a/src/MissionManager/QGCMapPolyline.cc b/src/MissionManager/QGCMapPolyline.cc index a77482ae19d..888323ec9a5 100644 --- a/src/MissionManager/QGCMapPolyline.cc +++ b/src/MissionManager/QGCMapPolyline.cc @@ -98,7 +98,7 @@ QGeoCoordinate QGCMapPolyline::_coordFromPointF(const QPointF& point) const if (_polylinePath.count() > 0) { QGeoCoordinate tangentOrigin = _polylinePath[0].value(); - convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, coord); } return coord; @@ -110,7 +110,7 @@ QPointF QGCMapPolyline::_pointFFromCoord(const QGeoCoordinate& coordinate) const double y, x, down; QGeoCoordinate tangentOrigin = _polylinePath[0].value(); - convertGeoToNed(coordinate, tangentOrigin, &y, &x, &down); + QGCGeo::convertGeoToNed(coordinate, tangentOrigin, y, x, down); return QPointF(x, -y); } @@ -281,7 +281,7 @@ QList QGCMapPolyline::nedPolyline(void) // This avoids a nan calculation that comes out of convertGeoToNed x = y = 0; } else { - convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); + QGCGeo::convertGeoToNed(vertex, tangentOrigin, y, x, down); } nedPolyline += QPointF(x, y); } @@ -324,7 +324,7 @@ QList QGCMapPolyline::offsetPolyline(double distance) // Add first vertex QGeoCoordinate coord; - convertNedToGeo(rgOffsetEdges[0].p1().y(), rgOffsetEdges[0].p1().x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(rgOffsetEdges[0].p1().y(), rgOffsetEdges[0].p1().x(), 0, tangentOrigin, coord); rgNewPolyline.append(coord); // Intersect the offset edges to generate new central vertices @@ -335,13 +335,13 @@ QList QGCMapPolyline::offsetPolyline(double distance) // Two lines are colinear newVertex = rgOffsetEdges[i].p2(); } - convertNedToGeo(newVertex.y(), newVertex.x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(newVertex.y(), newVertex.x(), 0, tangentOrigin, coord); rgNewPolyline.append(coord); } // Add last vertex int lastIndex = rgOffsetEdges.count() - 1; - convertNedToGeo(rgOffsetEdges[lastIndex].p2().y(), rgOffsetEdges[lastIndex].p2().x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(rgOffsetEdges[lastIndex].p2().y(), rgOffsetEdges[lastIndex].p2().x(), 0, tangentOrigin, coord); rgNewPolyline.append(coord); } diff --git a/src/MissionManager/SurveyComplexItem.cc b/src/MissionManager/SurveyComplexItem.cc index 312da416773..359f26dddde 100644 --- a/src/MissionManager/SurveyComplexItem.cc +++ b/src/MissionManager/SurveyComplexItem.cc @@ -709,7 +709,7 @@ void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(bool refly) // This avoids a nan calculation that comes out of convertGeoToNed x = y = 0; } else { - convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); + QGCGeo::convertGeoToNed(vertex, tangentOrigin, y, x, down); } polygonPoints += QPointF(x, y); qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y(); @@ -800,9 +800,9 @@ void SurveyComplexItem::_rebuildTransectsPhase1WorkerSinglePolygon(bool refly) QGeoCoordinate coord; QList transect; - convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, coord); transect.append(coord); - convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, coord); transect.append(coord); transects.append(transect); @@ -928,7 +928,7 @@ void SurveyComplexItem::_rebuildTransectsPhase1WorkerSplitPolygons(bool refly) // This avoids a nan calculation that comes out of convertGeoToNed x = y = 0; } else { - convertGeoToNed(vertex, tangentOrigin, &y, &x, &down); + convertGeoToNed(vertex, tangentOrigin, y, x, down); } polygonPoints += QPointF(x, y); qCDebug(SurveyComplexItemLog) << "_rebuildTransectsPhase1 vertex:x:y" << vertex << polygonPoints.last().x() << polygonPoints.last().y(); @@ -1197,7 +1197,7 @@ void SurveyComplexItem::_rebuildTransectsFromPolygon(bool refly, const QPolygonF if (transitionPoint != nullptr) { QList transect; QGeoCoordinate coord; - convertNedToGeo(transitionPoint->y(), transitionPoint->x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(transitionPoint->y(), transitionPoint->x(), 0, tangentOrigin, coord); transect.append(coord); transect.append(coord); //TODO transects.append(transect); @@ -1207,9 +1207,9 @@ void SurveyComplexItem::_rebuildTransectsFromPolygon(bool refly, const QPolygonF QList transect; QGeoCoordinate coord; - convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(line.p1().y(), line.p1().x(), 0, tangentOrigin, coord); transect.append(coord); - convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, &coord); + QGCGeo::convertNedToGeo(line.p2().y(), line.p2().x(), 0, tangentOrigin, coord); transect.append(coord); transects.append(transect); diff --git a/src/QmlControls/EditPositionDialogController.cc b/src/QmlControls/EditPositionDialogController.cc index b84e8c99ad5..d95f8898430 100644 --- a/src/QmlControls/EditPositionDialogController.cc +++ b/src/QmlControls/EditPositionDialogController.cc @@ -60,14 +60,14 @@ void EditPositionDialogController::initValues(void) _longitudeFact.setRawValue(_coordinate.longitude()); double easting, northing; - int zone = convertGeoToUTM(_coordinate, easting, northing); + int zone = QGCGeo::convertGeoToUTM(_coordinate, easting, northing); if (zone >= 1 && zone <= 60) { _zoneFact.setRawValue(zone); _hemisphereFact.setRawValue(_coordinate.latitude() < 0); _eastingFact.setRawValue(easting); _northingFact.setRawValue(northing); } - QString mgrs = convertGeoToMGRS(_coordinate); + QString mgrs = QGCGeo::convertGeoToMGRS(_coordinate); if (!mgrs.isEmpty()) { _mgrsFact.setRawValue(mgrs); } @@ -83,7 +83,7 @@ void EditPositionDialogController::setFromGeo(void) void EditPositionDialogController::setFromUTM(void) { qDebug() << _eastingFact.rawValue().toDouble() << _northingFact.rawValue().toDouble() << _zoneFact.rawValue().toInt() << (_hemisphereFact.rawValue().toInt() == 1); - if (convertUTMToGeo(_eastingFact.rawValue().toDouble(), _northingFact.rawValue().toDouble(), _zoneFact.rawValue().toInt(), _hemisphereFact.rawValue().toInt() == 1, _coordinate)) { + if (QGCGeo::convertUTMToGeo(_eastingFact.rawValue().toDouble(), _northingFact.rawValue().toDouble(), _zoneFact.rawValue().toInt(), _hemisphereFact.rawValue().toInt() == 1, _coordinate)) { qDebug() << _eastingFact.rawValue().toDouble() << _northingFact.rawValue().toDouble() << _zoneFact.rawValue().toInt() << (_hemisphereFact.rawValue().toInt() == 1) << _coordinate; emit coordinateChanged(_coordinate); } else { @@ -93,7 +93,7 @@ void EditPositionDialogController::setFromUTM(void) void EditPositionDialogController::setFromMGRS(void) { - if (convertMGRSToGeo(_mgrsFact.rawValue().toString(), _coordinate)) { + if (QGCGeo::convertMGRSToGeo(_mgrsFact.rawValue().toString(), _coordinate)) { emit coordinateChanged(_coordinate); } else { initValues(); diff --git a/src/Utilities/SHPFileHelper.cc b/src/Utilities/SHPFileHelper.cc index dfec0060713..c37f403d595 100644 --- a/src/Utilities/SHPFileHelper.cc +++ b/src/Utilities/SHPFileHelper.cc @@ -141,7 +141,7 @@ bool SHPFileHelper::loadPolygonFromFile(const QString& shpFile, QListnVertices; i++) { QGeoCoordinate coord; - if (!utmZone || !convertUTMToGeo(shpObject->padfX[i], shpObject->padfY[i], utmZone, utmSouthernHemisphere, coord)) { + if (!utmZone || !QGCGeo::convertUTMToGeo(shpObject->padfX[i], shpObject->padfY[i], utmZone, utmSouthernHemisphere, coord)) { coord.setLatitude(shpObject->padfY[i]); coord.setLongitude(shpObject->padfX[i]); } diff --git a/src/Vehicle/FactGroups/VehicleGPS2FactGroup.cc b/src/Vehicle/FactGroups/VehicleGPS2FactGroup.cc index c88d70f2181..571f6385cf0 100644 --- a/src/Vehicle/FactGroups/VehicleGPS2FactGroup.cc +++ b/src/Vehicle/FactGroups/VehicleGPS2FactGroup.cc @@ -32,10 +32,10 @@ void VehicleGPS2FactGroup::_handleGps2Raw(mavlink_message_t& message) lat()->setRawValue (gps2Raw.lat * 1e-7); lon()->setRawValue (gps2Raw.lon * 1e-7); - mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(gps2Raw.lat * 1e-7, gps2Raw.lon * 1e-7))); + mgrs()->setRawValue (QGCGeo::convertGeoToMGRS(QGeoCoordinate(gps2Raw.lat * 1e-7, gps2Raw.lon * 1e-7))); count()->setRawValue (gps2Raw.satellites_visible == 255 ? 0 : gps2Raw.satellites_visible); hdop()->setRawValue (gps2Raw.eph == UINT16_MAX ? qQNaN() : gps2Raw.eph / 100.0); vdop()->setRawValue (gps2Raw.epv == UINT16_MAX ? qQNaN() : gps2Raw.epv / 100.0); courseOverGround()->setRawValue (gps2Raw.cog == UINT16_MAX ? qQNaN() : gps2Raw.cog / 100.0); lock()->setRawValue (gps2Raw.fix_type); -} \ No newline at end of file +} diff --git a/src/Vehicle/FactGroups/VehicleGPSFactGroup.cc b/src/Vehicle/FactGroups/VehicleGPSFactGroup.cc index 2a5448640bd..963351cfefb 100644 --- a/src/Vehicle/FactGroups/VehicleGPSFactGroup.cc +++ b/src/Vehicle/FactGroups/VehicleGPSFactGroup.cc @@ -63,7 +63,7 @@ void VehicleGPSFactGroup::_handleGpsRawInt(mavlink_message_t& message) lat()->setRawValue (gpsRawInt.lat * 1e-7); lon()->setRawValue (gpsRawInt.lon * 1e-7); - mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(gpsRawInt.lat * 1e-7, gpsRawInt.lon * 1e-7))); + mgrs()->setRawValue (QGCGeo::convertGeoToMGRS(QGeoCoordinate(gpsRawInt.lat * 1e-7, gpsRawInt.lon * 1e-7))); count()->setRawValue (gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible); hdop()->setRawValue (gpsRawInt.eph == UINT16_MAX ? qQNaN() : gpsRawInt.eph / 100.0); vdop()->setRawValue (gpsRawInt.epv == UINT16_MAX ? qQNaN() : gpsRawInt.epv / 100.0); @@ -88,7 +88,7 @@ void VehicleGPSFactGroup::_handleHighLatency(mavlink_message_t& message) lat()->setRawValue (coordinate.latitude); lon()->setRawValue (coordinate.longitude); - mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(coordinate.latitude, coordinate.longitude))); + mgrs()->setRawValue (QGCGeo::convertGeoToMGRS(QGeoCoordinate(coordinate.latitude, coordinate.longitude))); count()->setRawValue(0); } @@ -99,7 +99,7 @@ void VehicleGPSFactGroup::_handleHighLatency2(mavlink_message_t& message) lat()->setRawValue (highLatency2.latitude * 1e-7); lon()->setRawValue (highLatency2.longitude * 1e-7); - mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(highLatency2.latitude * 1e-7, highLatency2.longitude * 1e-7))); + mgrs()->setRawValue (QGCGeo::convertGeoToMGRS(QGeoCoordinate(highLatency2.latitude * 1e-7, highLatency2.longitude * 1e-7))); count()->setRawValue(0); hdop()->setRawValue (highLatency2.eph == UINT8_MAX ? qQNaN() : highLatency2.eph / 10.0); vdop()->setRawValue (highLatency2.epv == UINT8_MAX ? qQNaN() : highLatency2.epv / 10.0); diff --git a/test/Geo/GeoTest.cc b/test/Geo/GeoTest.cc index f54240dfae5..25bbe21b64a 100644 --- a/test/Geo/GeoTest.cc +++ b/test/Geo/GeoTest.cc @@ -18,23 +18,16 @@ #include -/* -GeoTest::GeoTest(void) -{ - -} -*/ - void GeoTest::_convertGeoToNed_test(void) { - QGeoCoordinate coord(47.364869, 8.594398, 0.0); + const QGeoCoordinate coord(47.364869, 8.594398, 0.0); - double expectedX = -1281.152128182419801305514; - double expectedY = 3486.949719522415307437768; - double expectedZ = 0; + const double expectedX = -1281.152128182419801305514; + const double expectedY = 3486.949719522415307437768; + const double expectedZ = 0; double x, y, z; - convertGeoToNed(coord, _origin, &x, &y, &z); + QGCGeo::convertGeoToNed(coord, m_origin, x, y, z); QCOMPARE(x, expectedX); QCOMPARE(y, expectedY); @@ -43,15 +36,15 @@ void GeoTest::_convertGeoToNed_test(void) void GeoTest::_convertGeoToNedAtOrigin_test(void) { - QGeoCoordinate coord(_origin); + QGeoCoordinate coord(m_origin); coord.setAltitude(10.0); // offset altitude to test z - double expectedX = 0.0; - double expectedY = 0.0; - double expectedZ = -10.0; + const double expectedX = 0.0; + const double expectedY = 0.0; + const double expectedZ = -10.0; double x, y, z; - convertGeoToNed(coord, _origin, &x, &y, &z); + QGCGeo::convertGeoToNed(coord, m_origin, x, y, z); QCOMPARE(x, expectedX); QCOMPARE(y, expectedY); @@ -60,36 +53,79 @@ void GeoTest::_convertGeoToNedAtOrigin_test(void) 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; + const double x = -1281.152128182419801305514; + const double y = 3486.949719522415307437768; + const double z = 0; QGeoCoordinate coord; - convertNedToGeo(x, y, z, _origin, &coord); + QGCGeo::convertNedToGeo(x, y, z, m_origin, coord); - QCOMPARE(coord.latitude(), expectedLat); - QCOMPARE(coord.longitude(), expectedLon); - QCOMPARE(coord.altitude(), expectedAlt); + QVERIFY(coord.isValid()); + QCOMPARE(coord.latitude(), m_origin.latitude()); + QCOMPARE(coord.longitude(), m_origin.longitude()); + QCOMPARE(coord.altitude(), m_origin.altitude()); } void GeoTest::_convertNedToGeoAtOrigin_test(void) { - double x = 0.0; - double y = 0.0; - double z = 0.0; + const double x = 0.0; + const double y = 0.0; + const double z = 0.0; + + QGeoCoordinate coord; + QGCGeo::convertNedToGeo(x, y, z, m_origin, coord); + + QVERIFY(coord.isValid()); + QCOMPARE(coord.latitude(), m_origin.latitude()); + QCOMPARE(coord.longitude(), m_origin.longitude()); + QCOMPARE(coord.altitude(), m_origin.altitude()); +} + +void GeoTest::_convertGeoToUTM_test(void) { + const QGeoCoordinate coord(m_origin); + + double easting; + double northing; + const int zone = QGCGeo::convertGeoToUTM(coord, easting, northing); + + QCOMPARE(zone, 32); + QCOMPARE(easting, 465886.09); + QCOMPARE(northing, 5247092.45); +} + +void GeoTest::_convertUTMToGeo_test(void) { + const double easting = 465886.09; + const double northing = 5247092.45; + const int zone = 32; + const bool southhemi = false; + + QGeoCoordinate coord; + const bool result = QGCGeo::convertUTMToGeo(easting, northing, zone, southhemi, coord); + + QVERIFY(result); + QVERIFY(coord.isValid()); + QCOMPARE(coord.latitude(), m_origin.latitude()); + QCOMPARE(coord.longitude(), m_origin.longitude()); + QCOMPARE(coord.altitude(), m_origin.altitude()); +} + +void GeoTest::_convertGeoToMGRS_test(void) { + const QGeoCoordinate coord(m_origin); + + const QString result = QGCGeo::convertGeoToMGRS(coord); + + QCOMPARE(result, "32T MT 65886 47092"); +} - double expectedLat = _origin.latitude(); - double expectedLon = _origin.longitude(); - double expectedAlt = _origin.altitude(); +void GeoTest::_convertMGRSToGeo_test(void) { + const QString mgrs("32T MT 65886 47092"); QGeoCoordinate coord; - convertNedToGeo(x, y, z, _origin, &coord); + const bool result = QGCGeo::convertMGRSToGeo(mgrs, coord); - QCOMPARE(coord.latitude(), expectedLat); - QCOMPARE(coord.longitude(), expectedLon); - QCOMPARE(coord.altitude(), expectedAlt); + QVERIFY(result); + QVERIFY(coord.isValid()); + QCOMPARE(coord.latitude(), m_origin.latitude()); + QCOMPARE(coord.longitude(), m_origin.longitude()); + QCOMPARE(coord.altitude(), m_origin.altitude()); } diff --git a/test/Geo/GeoTest.h b/test/Geo/GeoTest.h index a78ab77504d..d14306c210b 100644 --- a/test/Geo/GeoTest.h +++ b/test/Geo/GeoTest.h @@ -24,16 +24,20 @@ 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) - { } + GeoTest(void) = default; private slots: void _convertGeoToNed_test(void); void _convertGeoToNedAtOrigin_test(void); void _convertNedToGeo_test(void); void _convertNedToGeoAtOrigin_test(void); + + void _convertGeoToUTM_test(void); + void _convertUTMToGeo_test(void); + void _convertGeoToMGRS_test(void); + void _convertMGRSToGeo_test(void); + private: - QGeoCoordinate _origin; + /// Use ETH campus (47.3764° N, 8.5481° E) + const QGeoCoordinate m_origin{47.3764, 8.5481, 0.0}; }; -