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