diff --git a/src/shared/utils/AeroUtils/AeroUtils.cpp b/src/shared/utils/AeroUtils/AeroUtils.cpp index a937551372be3dbd79ac7cfa9e849c04335be597..bd9f699214efad55a8ea2b20d5c95dd39847287d 100644 --- a/src/shared/utils/AeroUtils/AeroUtils.cpp +++ b/src/shared/utils/AeroUtils/AeroUtils.cpp @@ -71,15 +71,15 @@ float verticalSpeed(float p, float dpDt, float pRef, float tRef) return -(tRef * dpDt * powf(p / pRef, nInv)) / (a * n * p); } -Vector2f geodetic2NED(const Vector2f& gpsData, const Vector2f& offset) +Vector2f geodetic2NED(const Vector2f& target, const Vector2f& origin) { float mPerDegLat = 111132.95225; float mPerDegLon = - fabsf(111412.87733 * cosf(gpsData[0] * Constants::DEGREES_TO_RADIANS)); + fabsf(111412.87733 * cosf(target[0] * Constants::DEGREES_TO_RADIANS)); return { - mPerDegLat * (gpsData[0] - offset[0]), - mPerDegLon * (gpsData[1] - offset[1]), + mPerDegLat * (target[0] - origin[0]), + mPerDegLon * (target[1] - origin[1]), }; } diff --git a/src/shared/utils/AeroUtils/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h index ae170e3de0785fac4cb1734360e12531762cb983..e3e61661d46e90ee85f0de8b0a6b269683f316ba 100644 --- a/src/shared/utils/AeroUtils/AeroUtils.h +++ b/src/shared/utils/AeroUtils/AeroUtils.h @@ -173,12 +173,13 @@ float verticalSpeed(float p, float dpDt, float pRef, float tRef); * @brief Converts decimal degrees of latitude and longitude into displacement * in meters between two positions the with an ellipsoidal earth model. * - * @param position1 Latitude and longitude of current position [lat lon][deg] - * @param position2 Initial position used as an offset [lat lon][deg] - * @return Distance between the two coordinates [n e][m] + * @param target Coordinates of target position [lat lon][deg] + * @param origin Coordinates of the Initial position used as the origin of the + * NED frame [lat lon][deg] + * @return Target NED position with respect to the origin coordinates [n e][m] */ -Eigen::Vector2f geodetic2NED(const Eigen::Vector2f& position1, - const Eigen::Vector2f& position2); +Eigen::Vector2f geodetic2NED(const Eigen::Vector2f& target, + const Eigen::Vector2f& origin); /** * @brief Computes the rho (air density) of air at the given altitude.