From fa6d91b4d7536f5d9a081bd492b3bf52564f3f38 Mon Sep 17 00:00:00 2001 From: hellohyh001 Date: Sat, 26 Aug 2023 02:58:14 +0000 Subject: [PATCH] update Signed-off-by: hellohyh001 Change-Id: Ifc104969da63b2fab98e402caa919c48a2987302 --- interfaces/native/src/geomagnetic_field.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/interfaces/native/src/geomagnetic_field.cpp b/interfaces/native/src/geomagnetic_field.cpp index 31929718..eaec4c42 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) -- Gitee