diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index 57c85261c9c456700ea9772443a7cbda0ad0c15c..86d03389b1a5aa3f61efed3a9c4fefcfbf09a6d4 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -34,14 +34,6 @@ extern "C" { // eigen #include <Eigen/Dense> -#ifndef RAD2DEG -#define RAD2DEG 180.0 / 3.14159265358979323846 -#endif - -#ifndef DEG2RAD -#define DEG2RAD 3.14159265358979323846 / 180.0 -#endif - namespace GnssUtils { @@ -226,5 +218,6 @@ enum Combination }; +#undef PI // avoid conflict with other libraries that may define PI } diff --git a/src/utils/transformations.cpp b/src/utils/transformations.cpp index 0cfd24af3648e2eb301dccb9ba63bf705b600aac..3c9b1198cfd8ba60e8b2f1c4897a686ccbb654ea 100644 --- a/src/utils/transformations.cpp +++ b/src/utils/transformations.cpp @@ -29,25 +29,22 @@ namespace GnssUtils Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef, bool in_rads) { Eigen::Vector3d latlonalt; - ecef2pos(_ecef.data(), latlonalt.data()); + ecef2pos(_ecef.data(), latlonalt.data()); // in rads + + if (not in_rads) // requested in degrees + latlonalt.head<2>() *= 180.0 / M_PI; - if (not in_rads) - { - latlonalt(0) = RAD2DEG * latlonalt(0); - latlonalt(1) = RAD2DEG * latlonalt(1); - } return latlonalt; } Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon, bool in_rads) { Eigen::Vector3d ecef; - if (not in_rads) + if (not in_rads) // provided in degrees { Eigen::Vector3d latlon_rads = _latlon; - latlon_rads(0) = DEG2RAD * _latlon(0); - latlon_rads(1) = DEG2RAD * _latlon(1); - latlon_rads(2) = _latlon(2); + latlon_rads.head<2>() *= M_PI / 180.0; + pos2ecef(latlon_rads.data(), ecef.data()); } else