diff options
Diffstat (limited to 'src/lib/geo/geo.c')
| -rw-r--r-- | src/lib/geo/geo.c | 5 |
1 files changed, 3 insertions, 2 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index ad1ff37e3..1c8d2a2a7 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -50,6 +50,7 @@ #include <math.h> #include <stdbool.h> #include <string.h> +#include <float.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> @@ -147,7 +148,7 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref 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 = (c == 0.0) ? 1.0 : (c / sin(c)); + double k = (fabs(c) < DBL_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; @@ -175,7 +176,7 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r double lat_rad; double lon_rad; - if (c != 0.0) { + if (fabs(c) > DBL_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)); |