diff --git a/interfaces/native/src/geomagnetic_field.cpp b/interfaces/native/src/geomagnetic_field.cpp index 3192971842466a0d8006246b0ee2fb88dbd639e8..eaec4c42d3ffa286f025ca6d1ff329fafa5e320e 100644 --- a/interfaces/native/src/geomagnetic_field.cpp +++ b/interfaces/native/src/geomagnetic_field.cpp @@ -205,10 +205,20 @@ void GeomagneticField::CalibrateGeocentricCoordinates(float latitude, float long double gdLatRad = ToRadians(latitude); float clat = static_cast(cos(gdLatRad)); float slat = static_cast(sin(gdLatRad)); - float tlat = slat / clat; float latRad = static_cast(sqrt(a2 * clat * clat + b2 * slat * slat)); - geocentricLatitude = static_cast(atan(tlat * (latRad * altitudeKm + b2) - / (latRad * altitudeKm + a2))); + if (fabs(clat) > PRECISION) { + float tlat = slat / clat; + geocentricLatitude = static_cast(atan(tlat * (latRad * altitudeKm + b2) + / (latRad * altitudeKm + a2))); + } else { + if (fabs(latitude - 90) < PRECISION) { + geocentricLatitude = M_PI / 2.0; + } + if (fabs(latitude - 270) < PRECISION) { + geocentricLatitude = -(M_PI / 2.0); + } + } + geocentricLongitude = static_cast(ToRadians(longitude)); float radSq = altitudeKm * altitudeKm + 2 * altitudeKm * latRad + (a2 * a2 * clat * clat + b2 * b2 * slat * slat)