1 /*
2  * Copyright (c) 2021 Huawei Device Co., Ltd.
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15 
16 #include "geomagnetic_field.h"
17 
18 #include <cmath>
19 #include <mutex>
20 
21 #include "sensor_errors.h"
22 #include "sensor_utils.h"
23 
24 using namespace std;
25 using namespace OHOS::Sensors;
26 namespace {
27 constexpr float EARTH_MAJOR_AXIS_RADIUS = 6378.137f;
28 constexpr float EARTH_MINOR_AXIS_RADIUS = 6356.7523142f;
29 constexpr float EARTH_REFERENCE_RADIUS = 6371.2f;
30 constexpr float PRECISION = 1e-5f;
31 constexpr float LATITUDE_MAX = 90.0f;
32 constexpr float LATITUDE_MIN = -90.0f;
33 constexpr float CONVERSION_FACTOR = 1000.0f;
34 constexpr float DERIVATIVE_FACTOR = 1.0f;
35 // the time from 1970-01-01 to 2020-01-01 as UTC milliseconds from the epoch
36 constexpr int64_t WMM_BASE_TIME = 1580486400000;
37 // The following Gaussian coefficients are derived from the US/ United Kingdom World Magnetic Model 2020-2025.
38 constexpr float GAUSS_COEFFICIENT_G[13][13] = {
39     {0.0f},
40     {-29404.5f, -1450.7f},
41     {-2500.0f, 2982.0f, 1676.8f},
42     {1363.9f, -2381.0f, 1236.2f, 525.7f},
43     {903.1f, 809.4f, 86.2f, -309.4f, 47.9f},
44     {-234.4f, 363.1f, 187.8f, -140.7f, -151.2f, 13.7f},
45     {65.9f, 65.6f, 73.0f, -121.5f, -36.2f, 13.5f, -64.7f},
46     {80.6f, -76.8f, -8.3f, 56.5f, 15.8f, 6.4f, -7.2f, 9.8f},
47     {23.6f, 9.8f, -17.5f, -0.4f, -21.1f, 15.3f, 13.7f, -16.5f, -0.3f},
48     {5.0f, 8.2f, 2.9f, -1.4f, -1.1f, -13.3f, 1.1f, 8.9f, -9.3f, -11.9f},
49     {-1.9f, -6.2f, -0.1f, 1.7f, -0.9f, 0.6f, -0.9f, 1.9f, 1.4f, -2.4f, -3.9f},
50     {3.0f, -1.4f, -2.5f, 2.4f, -0.9f, 0.3f, -0.7f, -0.1f, 1.4f, -0.6f, 0.2f, 3.1f},
51     {-2.0f, -0.1f, 0.5f, 1.3f, -1.2f, 0.7f, 0.3f, 0.5f, -0.2f, -0.5f, 0.1f, -1.1f, -0.3f}
52 };
53 constexpr float GAUSS_COEFFICIENT_H[13][13] = {
54     {0.0f},
55     {0.0f, 4652.9f},
56     {0.0f, -2991.6f, -734.8f},
57     {0.0f, -82.2f, 241.8f, -542.9f},
58     {0.0f, 282.0f, -158.4f, 199.8f, -350.1f},
59     {0.0f, 47.7f, 208.4f, -121.3f, 32.2f, 99.1f},
60     {0.0f, -19.1f, 25.0f, 52.7f, -64.4f, 9.0f, 68.1f},
61     {0.0f, -51.4f, -16.8f, 2.3f, 23.5f, -2.2f, -27.2f, -1.9f},
62     {0.0f, 8.4f, -15.3f, 12.8f, -11.8f, 14.9f, 3.6f, -6.9f, 2.8f},
63     {0.0f, -23.3f, 11.1f, 9.8f, -5.1f, -6.2f, 7.8f, 0.4f, -1.5f, 9.7f},
64     {0.0f, 3.4f, -0.2f, 3.5f, 4.8f, -8.6f, -0.1f, -4.2f, -3.4f, -0.1f, -8.8f},
65     {0.0f, 0.0f, 2.6f, -0.5f, -0.4f, 0.6f, -0.2f, -1.7f, -1.6f, -3.0f, -2.0f, -2.6f},
66     {0.0f, -1.2f, 0.5f, 1.3f, -1.8f, 0.1f, 0.7f, -0.1f, 0.6f, 0.2f, -0.9f, 0.0f, 0.5f}
67 };
68 constexpr float DELTA_GAUSS_COEFFICIENT_G[13][13] = {
69     {0.0f},
70     {6.7f, 7.7f},
71     {-11.5f, -7.1f, -2.2f},
72     {2.8f, -6.2f, 3.4f, -12.2f},
73     {-1.1f, -1.6f, -6.0f, 5.4f, -5.5f},
74     {-0.3f, 0.6f, -0.7f, 0.1f, 1.2f, 1.0f},
75     {-0.6f, -0.4f, 0.5f, 1.4f, -1.4f, 0.0f, 0.8f},
76     {-0.1f, -0.3f, -0.1f, 0.7f, 0.2f, -0.5f, -0.8f, 1.0f},
77     {-0.1f, 0.1f, -0.1f, 0.5f, -0.1f, 0.4f, 0.5f, 0.0f, 0.4f},
78     {-0.1f, -0.2f, 0.0f, 0.4f, -0.3f, 0.0f, 0.3f, 0.0f, 0.0f, -0.4f},
79     {0.0f, 0.0f, 0.0f, 0.2f, -0.1f, -0.2f, 0.0f, -0.1f, -0.2f, -0.1f, 0.0f},
80     {0.0f, -0.1f, 0.0f, 0.0f, 0.0f, -0.1f, 0.0f, 0.0f, -0.1f, -0.1f, -0.1f, -0.1f},
81     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -0.1f}
82 };
83 constexpr float DELTA_GAUSS_COEFFICIENT_H[13][13] = {
84     {0.0f},
85     {0.0f, -25.1f},
86     {0.0f, -30.2f, -23.9f},
87     {0.0f, 5.7f, -1.0f, 1.1f},
88     {0.0f, 0.2f, 6.9f, 3.7f, -5.6f},
89     {0.0f, 0.1f, 2.5f, -0.9f, 3.0f, 0.5f},
90     {0.0f, 0.1f, -1.8f, -1.4f, 0.9f, 0.1f, 1.0f},
91     {0.0f, 0.5f, 0.6f, -0.7f, -0.2f, -1.2f, 0.2f, 0.3f},
92     {0.0f, -0.3f, 0.7f, -0.2f, 0.5f, -0.3f, -0.5f, 0.4f, 0.1f},
93     {0.0f, -0.3f, 0.2f, -0.4f, 0.4f, 0.1f, 0.0f, -0.2f, 0.5f, 0.2f},
94     {0.0f, 0.0f, 0.1f, -0.3f, 0.1f, -0.2f, 0.1f, 0.0f, -0.1f, 0.2f, 0.0f},
95     {0.0f, 0.0f, 0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.1f, 0.0f, -0.1f, 0.0f, 0.0f},
96     {0.0f, 0.0f, 0.0f, -0.1f, 0.1f, 0.0f, 0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f, -0.1f}
97 };
98 constexpr int32_t GAUSSIAN_COEFFICIENT_DIMENSION = 13;
99 std::mutex g_mutex;
100 
101 float g_northComponent;
102 float g_eastComponent;
103 float g_downComponent;
104 float g_geocentricLatitude;
105 float g_geocentricLongitude;
106 float g_geocentricRadius;
107 
108 std::vector<std::vector<float>> schmidtQuasiNormalFactors;
109 std::vector<std::vector<float>> polynomials(GAUSSIAN_COEFFICIENT_DIMENSION);
110 std::vector<std::vector<float>> polynomialsDerivative(GAUSSIAN_COEFFICIENT_DIMENSION);
111 std::vector<float> relativeRadiusPower(GAUSSIAN_COEFFICIENT_DIMENSION + 2);
112 std::vector<float> sinMLongitude(GAUSSIAN_COEFFICIENT_DIMENSION);
113 std::vector<float> cosMLongitude(GAUSSIAN_COEFFICIENT_DIMENSION);
114 }
115 
GeomagneticField(float latitude,float longitude,float altitude,int64_t timeMillis)116 GeomagneticField::GeomagneticField(float latitude, float longitude, float altitude, int64_t timeMillis)
117 {
118     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
119     schmidtQuasiNormalFactors = GetSchmidtQuasiNormalFactors(GAUSSIAN_COEFFICIENT_DIMENSION);
120     float gcLatitude = fmax(LATITUDE_MIN + PRECISION, fmin(LATITUDE_MAX - PRECISION, latitude));
121     CalibrateGeocentricCoordinates(gcLatitude, longitude, altitude);
122     InitLegendreTable(GAUSSIAN_COEFFICIENT_DIMENSION - 1, static_cast<float>(M_PI / 2.0 - g_geocentricLatitude));
123     GetRelativeRadiusPower();
124     double latDiffRad = ToRadians(gcLatitude) - g_geocentricLatitude;
125     CalculateGeomagneticComponent(latDiffRad, timeMillis);
126 }
127 
GetSchmidtQuasiNormalFactors(int32_t expansionDegree)128 std::vector<std::vector<float>> GeomagneticField::GetSchmidtQuasiNormalFactors(int32_t expansionDegree)
129 {
130     std::vector<std::vector<float>> schmidtQuasiNormFactors(expansionDegree + 1);
131     schmidtQuasiNormFactors[0].resize(1);
132     schmidtQuasiNormFactors[0][0] = 1.0f;
133     for (int32_t row = 1; row <= expansionDegree; row++) {
134         schmidtQuasiNormFactors[row].resize(row + 1);
135         schmidtQuasiNormFactors[row][0] =
136             schmidtQuasiNormFactors[row - 1][0] * (2 * row - 1) / static_cast<float>(row);
137         for (int32_t column = 1; column <= row; column++) {
138             schmidtQuasiNormFactors[row][column] = schmidtQuasiNormFactors[row][column - 1]
139                 * static_cast<float>(sqrt((row - column + 1) * ((column == 1) ? 2 : 1)
140                 / static_cast<float>(row + column)));
141         }
142     }
143     return schmidtQuasiNormFactors;
144 }
145 
CalculateGeomagneticComponent(double latDiffRad,int64_t timeMillis)146 void GeomagneticField::CalculateGeomagneticComponent(double latDiffRad, int64_t timeMillis)
147 {
148     float yearsSinceBase = (timeMillis - WMM_BASE_TIME) / (365.0f * 24.0f * 60.0f * 60.0f * 1000.0f);
149     float inverseCosLatitude = IsEqual(static_cast<float>(cos(g_geocentricLatitude)), 0.0f) ?
150         std::numeric_limits<float>::max() : DERIVATIVE_FACTOR / static_cast<float>(cos(g_geocentricLatitude));
151     GetLongitudeTrigonometric();
152     float gcX = 0.0f;
153     float gcY = 0.0f;
154     float gcZ = 0.0f;
155     for (int32_t row = 1; row < GAUSSIAN_COEFFICIENT_DIMENSION; row++) {
156         for (int32_t column = 0; column <= row; column++) {
157             float g = GAUSS_COEFFICIENT_G[row][column] + yearsSinceBase
158                 * DELTA_GAUSS_COEFFICIENT_G[row][column];
159             float h = GAUSS_COEFFICIENT_H[row][column] + yearsSinceBase
160                 * DELTA_GAUSS_COEFFICIENT_H[row][column];
161             gcX += relativeRadiusPower[row + 2]
162                 * (g * cosMLongitude[column] + h * sinMLongitude[column])
163                 * polynomialsDerivative[row][column]
164                 * schmidtQuasiNormalFactors[row][column];
165             gcY += relativeRadiusPower[row + 2] * column
166                 * (g * sinMLongitude[column] - h * cosMLongitude[column])
167                 * polynomials[row][column]
168                 * schmidtQuasiNormalFactors[row][column]
169                 * inverseCosLatitude;
170             gcZ -= (row + 1) * relativeRadiusPower[row + 2]
171                 * (g * cosMLongitude[column] + h * sinMLongitude[column])
172                 * polynomials[row][column]
173                 * schmidtQuasiNormalFactors[row][column];
174         }
175         g_northComponent = static_cast<float>(gcX * cos(latDiffRad) + gcZ * sin(latDiffRad));
176         g_eastComponent = gcY;
177         g_downComponent = static_cast<float>(-gcX * sin(latDiffRad) + gcZ * cos(latDiffRad));
178     }
179 }
180 
GetLongitudeTrigonometric()181 void GeomagneticField::GetLongitudeTrigonometric()
182 {
183     sinMLongitude[0] = 0.0f;
184     cosMLongitude[0] = 1.0f;
185     sinMLongitude[1] = static_cast<float>(sin(g_geocentricLongitude));
186     cosMLongitude[1] = static_cast<float>(cos(g_geocentricLongitude));
187     for (uint32_t index = 2; index < GAUSSIAN_COEFFICIENT_DIMENSION; ++index) {
188         uint32_t x = index >> 1;
189         sinMLongitude[index] = (sinMLongitude[index - x] * cosMLongitude[x]
190             + cosMLongitude[index - x] * sinMLongitude[x]);
191         cosMLongitude[index] = (cosMLongitude[index - x] * cosMLongitude[x]
192             - sinMLongitude[index - x] * sinMLongitude[x]);
193     }
194 }
195 
GetRelativeRadiusPower()196 void GeomagneticField::GetRelativeRadiusPower()
197 {
198     relativeRadiusPower[0] = 1.0f;
199     relativeRadiusPower[1] = IsEqual(g_geocentricRadius, 0.0f) ? std::numeric_limits<float>::max() :
200         EARTH_REFERENCE_RADIUS / g_geocentricRadius;
201     for (int32_t index = 2; index < static_cast<int32_t>(relativeRadiusPower.size()); ++index) {
202         relativeRadiusPower[index] = relativeRadiusPower[index - 1] * relativeRadiusPower[1];
203     }
204 }
205 
CalibrateGeocentricCoordinates(float latitude,float longitude,float altitude)206 void GeomagneticField::CalibrateGeocentricCoordinates(float latitude, float longitude, float altitude)
207 {
208     float altitudeKm = altitude / CONVERSION_FACTOR;
209     float a2 = EARTH_MAJOR_AXIS_RADIUS * EARTH_MAJOR_AXIS_RADIUS;
210     float b2 = EARTH_MINOR_AXIS_RADIUS * EARTH_MINOR_AXIS_RADIUS;
211     double gdLatRad = ToRadians(latitude);
212     float clat = static_cast<float>(cos(gdLatRad));
213     float slat = static_cast<float>(sin(gdLatRad));
214     float tlat = IsEqual(clat, 0.0f) ? std::numeric_limits<float>::max() : slat / clat;
215     float latRad = static_cast<float>(sqrt(a2 * clat * clat + b2 * slat * slat));
216     g_geocentricLatitude = static_cast<float>(atan(tlat * (latRad * altitudeKm + b2)
217         / (latRad * altitudeKm + a2)));
218     g_geocentricLongitude = static_cast<float>(ToRadians(longitude));
219     float radSq = altitudeKm * altitudeKm + 2 * altitudeKm
220         * latRad + (a2 * a2 * clat * clat + b2 * b2 * slat * slat)
221         / (a2 * clat * clat + b2 * slat * slat);
222     g_geocentricRadius = static_cast<float>(sqrt(radSq));
223 }
224 
InitLegendreTable(int32_t expansionDegree,float thetaRad)225 void GeomagneticField::InitLegendreTable(int32_t expansionDegree, float thetaRad)
226 {
227     polynomials[0].resize(1);
228     polynomials[0][0] = 1.0f;
229     polynomialsDerivative[0].resize(1);
230     polynomialsDerivative[0][0] = 0.0f;
231     float cosValue = static_cast<float>(cos(thetaRad));
232     float sinValue = static_cast<float>(sin(thetaRad));
233     for (int32_t row = 1; row <= expansionDegree; row++) {
234         polynomials[row].resize(row + 1);
235         polynomialsDerivative[row].resize(row + 1);
236         for (int32_t column = 0; column <= row; column++) {
237             if (row == column) {
238                 polynomials[row][column] = sinValue * polynomials[row - 1][column - 1];
239                 polynomialsDerivative[row][column] = cosValue * polynomials[row - 1][column - 1]
240                     + sinValue * polynomialsDerivative[row - 1][column - 1];
241             } else if (row == 1 || column == row - 1) {
242                 polynomials[row][column] = cosValue * polynomials[row - 1][column];
243                 polynomialsDerivative[row][column] = -sinValue * polynomials[row - 1][column]
244                     + cosValue * polynomialsDerivative[row - 1][column];
245             } else {
246                 float k = ((row - 1) * (row - 1) - column * column)
247                     / static_cast<float>((2 * row - 1) * (2 * row - 3));
248                 polynomials[row][column] = cosValue * polynomials[row - 1][column]
249                     - k * polynomials[row - 2][column];
250                 polynomialsDerivative[row][column] = -sinValue * polynomials[row - 1][column]
251                     + cosValue * polynomialsDerivative[row - 1][column]
252                     - k * polynomialsDerivative[row - 2][column];
253             }
254         }
255     }
256 }
257 
ObtainX()258 float GeomagneticField::ObtainX()
259 {
260     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
261     return g_northComponent;
262 }
263 
ObtainY()264 float GeomagneticField::ObtainY()
265 {
266     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
267     return g_eastComponent;
268 }
269 
ObtainZ()270 float GeomagneticField::ObtainZ()
271 {
272     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
273     return g_downComponent;
274 }
275 
ObtainGeomagneticDip()276 float GeomagneticField::ObtainGeomagneticDip()
277 {
278     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
279     float horizontalIntensity = hypot(g_northComponent, g_eastComponent);
280     return static_cast<float>(ToDegrees(atan2(g_downComponent, horizontalIntensity)));
281 }
282 
ToDegrees(double angrad)283 double GeomagneticField::ToDegrees(double angrad)
284 {
285     return angrad * 180.0 / M_PI;
286 }
287 
ToRadians(double angdeg)288 double GeomagneticField::ToRadians(double angdeg)
289 {
290     return angdeg / 180.0 * M_PI;
291 }
292 
ObtainDeflectionAngle()293 float GeomagneticField::ObtainDeflectionAngle()
294 {
295     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
296     return static_cast<float>(ToDegrees(atan2(g_eastComponent, g_northComponent)));
297 }
298 
ObtainLevelIntensity()299 float GeomagneticField::ObtainLevelIntensity()
300 {
301     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
302     float horizontalIntensity = hypot(g_northComponent, g_eastComponent);
303     return horizontalIntensity;
304 }
305 
ObtainTotalIntensity()306 float GeomagneticField::ObtainTotalIntensity()
307 {
308     std::lock_guard<std::mutex> geomagneticLock(g_mutex);
309     float sumOfSquares = g_northComponent * g_northComponent + g_eastComponent * g_eastComponent
310         + g_downComponent * g_downComponent;
311     float totalIntensity = static_cast<float>(sqrt(sumOfSquares));
312     return totalIntensity;
313 }
314 
315