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