1 /* Copyright (c) 2017-2020, The Linux Foundation. All rights reserved.
2 *
3 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are
5 * met:
6 * * Redistributions of source code must retain the above copyright
7 * notice, this list of conditions and the following disclaimer.
8 * * Redistributions in binary form must reproduce the above
9 * copyright notice, this list of conditions and the following
10 * disclaimer in the documentation and/or other materials provided
11 * with the distribution.
12 * * Neither the name of The Linux Foundation, nor the names of its
13 * contributors may be used to endorse or promote products derived
14 * from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
17 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
20 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
25 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
26 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 *
28 */
29
30 #include <LocationUtil.h>
31 #include <log_util.h>
32 #include <inttypes.h>
33 #include <gps_extended_c.h>
34 #include <loc_misc_utils.h>
35
36 namespace android {
37 namespace hardware {
38 namespace gnss {
39 namespace V2_0 {
40 namespace implementation {
41
42 using ::android::hardware::gnss::V2_0::GnssLocation;
43 using ::android::hardware::gnss::V2_0::ElapsedRealtimeFlags;
44 using ::android::hardware::gnss::V2_0::GnssConstellationType;
45 using ::android::hardware::gnss::V1_0::GnssLocationFlags;
46
convertGnssLocation(Location & in,V1_0::GnssLocation & out)47 void convertGnssLocation(Location& in, V1_0::GnssLocation& out)
48 {
49 memset(&out, 0, sizeof(V1_0::GnssLocation));
50 if (in.flags & LOCATION_HAS_LAT_LONG_BIT) {
51 out.gnssLocationFlags |= GnssLocationFlags::HAS_LAT_LONG;
52 out.latitudeDegrees = in.latitude;
53 out.longitudeDegrees = in.longitude;
54 }
55 if (in.flags & LOCATION_HAS_ALTITUDE_BIT) {
56 out.gnssLocationFlags |= GnssLocationFlags::HAS_ALTITUDE;
57 out.altitudeMeters = in.altitude;
58 }
59 if (in.flags & LOCATION_HAS_SPEED_BIT) {
60 out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED;
61 out.speedMetersPerSec = in.speed;
62 }
63 if (in.flags & LOCATION_HAS_BEARING_BIT) {
64 out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING;
65 out.bearingDegrees = in.bearing;
66 }
67 if (in.flags & LOCATION_HAS_ACCURACY_BIT) {
68 out.gnssLocationFlags |= GnssLocationFlags::HAS_HORIZONTAL_ACCURACY;
69 out.horizontalAccuracyMeters = in.accuracy;
70 }
71 if (in.flags & LOCATION_HAS_VERTICAL_ACCURACY_BIT) {
72 out.gnssLocationFlags |= GnssLocationFlags::HAS_VERTICAL_ACCURACY;
73 out.verticalAccuracyMeters = in.verticalAccuracy;
74 }
75 if (in.flags & LOCATION_HAS_SPEED_ACCURACY_BIT) {
76 out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED_ACCURACY;
77 out.speedAccuracyMetersPerSecond = in.speedAccuracy;
78 }
79 if (in.flags & LOCATION_HAS_BEARING_ACCURACY_BIT) {
80 out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING_ACCURACY;
81 out.bearingAccuracyDegrees = in.bearingAccuracy;
82 }
83
84 out.timestamp = static_cast<V1_0::GnssUtcTime>(in.timestamp);
85 }
86
getCurrentTime(struct timespec & currentTime,int64_t & sinceBootTimeNanos)87 bool getCurrentTime(struct timespec& currentTime, int64_t& sinceBootTimeNanos)
88 {
89 struct timespec sinceBootTime;
90 struct timespec sinceBootTimeTest;
91 bool clockGetTimeSuccess = false;
92 const uint32_t MAX_TIME_DELTA_VALUE_NANOS = 10000;
93 const uint32_t MAX_GET_TIME_COUNT = 20;
94 /* Attempt to get CLOCK_REALTIME and CLOCK_BOOTIME in succession without an interruption
95 or context switch (for up to MAX_GET_TIME_COUNT times) to avoid errors in the calculation */
96 for (uint32_t i = 0; i < MAX_GET_TIME_COUNT; i++) {
97 if (clock_gettime(CLOCK_BOOTTIME, &sinceBootTime) != 0) {
98 break;
99 };
100 if (clock_gettime(CLOCK_REALTIME, ¤tTime) != 0) {
101 break;
102 }
103 if (clock_gettime(CLOCK_BOOTTIME, &sinceBootTimeTest) != 0) {
104 break;
105 };
106 sinceBootTimeNanos = sinceBootTime.tv_sec * 1000000000 + sinceBootTime.tv_nsec;
107 int64_t sinceBootTimeTestNanos =
108 sinceBootTimeTest.tv_sec * 1000000000 + sinceBootTimeTest.tv_nsec;
109 int64_t sinceBootTimeDeltaNanos = sinceBootTimeTestNanos - sinceBootTimeNanos;
110
111 /* sinceBootTime and sinceBootTimeTest should have a close value if there was no
112 interruption or context switch between clock_gettime for CLOCK_BOOTIME and
113 clock_gettime for CLOCK_REALTIME */
114 if (sinceBootTimeDeltaNanos < MAX_TIME_DELTA_VALUE_NANOS) {
115 clockGetTimeSuccess = true;
116 break;
117 } else {
118 LOC_LOGd("Delta:%" PRIi64 "ns time too large, retry number #%u...",
119 sinceBootTimeDeltaNanos, i + 1);
120 }
121 }
122 return clockGetTimeSuccess;
123 }
124
convertGnssLocation(Location & in,V2_0::GnssLocation & out)125 void convertGnssLocation(Location& in, V2_0::GnssLocation& out)
126 {
127 memset(&out, 0, sizeof(V2_0::GnssLocation));
128 convertGnssLocation(in, out.v1_0);
129
130 struct timespec currentTime;
131 int64_t sinceBootTimeNanos;
132
133 if (getCurrentTime(currentTime, sinceBootTimeNanos)) {
134 if (in.flags & LOCATION_HAS_ELAPSED_REAL_TIME) {
135 uint64_t qtimerDiff = 0;
136 uint64_t qTimerTickCount = getQTimerTickCount();
137 if (qTimerTickCount >= in.elapsedRealTime) {
138 qtimerDiff = qTimerTickCount - in.elapsedRealTime;
139 }
140 LOC_LOGv("sinceBootTimeNanos:%" PRIi64 " in.elapsedRealTime=%" PRIi64 ""
141 " qTimerTickCount=%" PRIi64 " qtimerDiff=%" PRIi64 "",
142 sinceBootTimeNanos, in.elapsedRealTime, qTimerTickCount, qtimerDiff);
143 uint64_t qTimerDiffNanos = qTimerTicksToNanos(double(qtimerDiff));
144 if (sinceBootTimeNanos >= qTimerDiffNanos) {
145 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIMESTAMP_NS;
146 out.elapsedRealtime.timestampNs = sinceBootTimeNanos - qTimerDiffNanos;
147 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS;
148 out.elapsedRealtime.timeUncertaintyNs = in.elapsedRealTimeUnc;
149 }
150 } else {
151 int64_t currentTimeNanos = currentTime.tv_sec*1000000000 + currentTime.tv_nsec;
152 int64_t locationTimeNanos = in.timestamp*1000000;
153 LOC_LOGv("sinceBootTimeNanos:%" PRIi64 " currentTimeNanos:%" PRIi64 ""
154 " locationTimeNanos:%" PRIi64 "",
155 sinceBootTimeNanos, currentTimeNanos, locationTimeNanos);
156 if (currentTimeNanos >= locationTimeNanos) {
157 int64_t ageTimeNanos = currentTimeNanos - locationTimeNanos;
158 LOC_LOGv("ageTimeNanos:%" PRIi64 ")", ageTimeNanos);
159 // the max trusted propagation time 100ms for ageTimeNanos to avoid user setting
160 // wrong time, it will affect elapsedRealtimeNanos
161 if (ageTimeNanos <= 100000000) {
162 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIMESTAMP_NS;
163 out.elapsedRealtime.timestampNs = sinceBootTimeNanos - ageTimeNanos;
164 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS;
165 // time uncertainty is the max value between abs(AP_UTC - MP_UTC) and 100ms, to
166 // verify if user change the sys time
167 out.elapsedRealtime.timeUncertaintyNs =
168 std::max(ageTimeNanos, (int64_t)100000000);
169 }
170 }
171 }
172 }
173 LOC_LOGv("out.elapsedRealtime.timestampNs=%" PRIi64 ""
174 " out.elapsedRealtime.timeUncertaintyNs=%" PRIi64 ""
175 " out.elapsedRealtime.flags=0x%X",
176 out.elapsedRealtime.timestampNs,
177 out.elapsedRealtime.timeUncertaintyNs, out.elapsedRealtime.flags);
178 }
179
convertGnssLocation(const V1_0::GnssLocation & in,Location & out)180 void convertGnssLocation(const V1_0::GnssLocation& in, Location& out)
181 {
182 memset(&out, 0, sizeof(out));
183 if (in.gnssLocationFlags & GnssLocationFlags::HAS_LAT_LONG) {
184 out.flags |= LOCATION_HAS_LAT_LONG_BIT;
185 out.latitude = in.latitudeDegrees;
186 out.longitude = in.longitudeDegrees;
187 }
188 if (in.gnssLocationFlags & GnssLocationFlags::HAS_ALTITUDE) {
189 out.flags |= LOCATION_HAS_ALTITUDE_BIT;
190 out.altitude = in.altitudeMeters;
191 }
192 if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED) {
193 out.flags |= LOCATION_HAS_SPEED_BIT;
194 out.speed = in.speedMetersPerSec;
195 }
196 if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING) {
197 out.flags |= LOCATION_HAS_BEARING_BIT;
198 out.bearing = in.bearingDegrees;
199 }
200 if (in.gnssLocationFlags & GnssLocationFlags::HAS_HORIZONTAL_ACCURACY) {
201 out.flags |= LOCATION_HAS_ACCURACY_BIT;
202 out.accuracy = in.horizontalAccuracyMeters;
203 }
204 if (in.gnssLocationFlags & GnssLocationFlags::HAS_VERTICAL_ACCURACY) {
205 out.flags |= LOCATION_HAS_VERTICAL_ACCURACY_BIT;
206 out.verticalAccuracy = in.verticalAccuracyMeters;
207 }
208 if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED_ACCURACY) {
209 out.flags |= LOCATION_HAS_SPEED_ACCURACY_BIT;
210 out.speedAccuracy = in.speedAccuracyMetersPerSecond;
211 }
212 if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING_ACCURACY) {
213 out.flags |= LOCATION_HAS_BEARING_ACCURACY_BIT;
214 out.bearingAccuracy = in.bearingAccuracyDegrees;
215 }
216
217 out.timestamp = static_cast<uint64_t>(in.timestamp);
218 }
219
convertGnssLocation(const V2_0::GnssLocation & in,Location & out)220 void convertGnssLocation(const V2_0::GnssLocation& in, Location& out)
221 {
222 memset(&out, 0, sizeof(out));
223 convertGnssLocation(in.v1_0, out);
224 }
225
convertGnssConstellationType(GnssSvType & in,V1_0::GnssConstellationType & out)226 void convertGnssConstellationType(GnssSvType& in, V1_0::GnssConstellationType& out)
227 {
228 switch(in) {
229 case GNSS_SV_TYPE_GPS:
230 out = V1_0::GnssConstellationType::GPS;
231 break;
232 case GNSS_SV_TYPE_SBAS:
233 out = V1_0::GnssConstellationType::SBAS;
234 break;
235 case GNSS_SV_TYPE_GLONASS:
236 out = V1_0::GnssConstellationType::GLONASS;
237 break;
238 case GNSS_SV_TYPE_QZSS:
239 out = V1_0::GnssConstellationType::QZSS;
240 break;
241 case GNSS_SV_TYPE_BEIDOU:
242 out = V1_0::GnssConstellationType::BEIDOU;
243 break;
244 case GNSS_SV_TYPE_GALILEO:
245 out = V1_0::GnssConstellationType::GALILEO;
246 break;
247 case GNSS_SV_TYPE_UNKNOWN:
248 default:
249 out = V1_0::GnssConstellationType::UNKNOWN;
250 break;
251 }
252 }
253
convertGnssConstellationType(GnssSvType & in,V2_0::GnssConstellationType & out)254 void convertGnssConstellationType(GnssSvType& in, V2_0::GnssConstellationType& out)
255 {
256 switch(in) {
257 case GNSS_SV_TYPE_GPS:
258 out = V2_0::GnssConstellationType::GPS;
259 break;
260 case GNSS_SV_TYPE_SBAS:
261 out = V2_0::GnssConstellationType::SBAS;
262 break;
263 case GNSS_SV_TYPE_GLONASS:
264 out = V2_0::GnssConstellationType::GLONASS;
265 break;
266 case GNSS_SV_TYPE_QZSS:
267 out = V2_0::GnssConstellationType::QZSS;
268 break;
269 case GNSS_SV_TYPE_BEIDOU:
270 out = V2_0::GnssConstellationType::BEIDOU;
271 break;
272 case GNSS_SV_TYPE_GALILEO:
273 out = V2_0::GnssConstellationType::GALILEO;
274 break;
275 case GNSS_SV_TYPE_NAVIC:
276 out = V2_0::GnssConstellationType::IRNSS;
277 break;
278 case GNSS_SV_TYPE_UNKNOWN:
279 default:
280 out = V2_0::GnssConstellationType::UNKNOWN;
281 break;
282 }
283 }
284
convertGnssSvid(GnssSv & in,int16_t & out)285 void convertGnssSvid(GnssSv& in, int16_t& out)
286 {
287 switch (in.type) {
288 case GNSS_SV_TYPE_GPS:
289 out = in.svId;
290 break;
291 case GNSS_SV_TYPE_SBAS:
292 out = in.svId;
293 break;
294 case GNSS_SV_TYPE_GLONASS:
295 out = in.svId - GLO_SV_PRN_MIN + 1;
296 break;
297 case GNSS_SV_TYPE_QZSS:
298 out = in.svId;
299 break;
300 case GNSS_SV_TYPE_BEIDOU:
301 out = in.svId - BDS_SV_PRN_MIN + 1;
302 break;
303 case GNSS_SV_TYPE_GALILEO:
304 out = in.svId - GAL_SV_PRN_MIN + 1;
305 break;
306 case GNSS_SV_TYPE_NAVIC:
307 /*Android doesn't define Navic svid range yet, use Naviv svid [1, 14] now
308 will update this once Android give Navic svid definiitons */
309 out = in.svId - NAVIC_SV_PRN_MIN + 1;
310 break;
311 default:
312 out = in.svId;
313 break;
314 }
315 }
316
convertGnssSvid(GnssMeasurementsData & in,int16_t & out)317 void convertGnssSvid(GnssMeasurementsData& in, int16_t& out)
318 {
319 switch (in.svType) {
320 case GNSS_SV_TYPE_GPS:
321 out = in.svId;
322 break;
323 case GNSS_SV_TYPE_SBAS:
324 out = in.svId;
325 break;
326 case GNSS_SV_TYPE_GLONASS:
327 if (in.svId != 255) { // OSN is known
328 out = in.svId - GLO_SV_PRN_MIN + 1;
329 } else { // OSN is not known, report FCN
330 out = in.gloFrequency + 92;
331 }
332 break;
333 case GNSS_SV_TYPE_QZSS:
334 out = in.svId;
335 break;
336 case GNSS_SV_TYPE_BEIDOU:
337 out = in.svId - BDS_SV_PRN_MIN + 1;
338 break;
339 case GNSS_SV_TYPE_GALILEO:
340 out = in.svId - GAL_SV_PRN_MIN + 1;
341 break;
342 case GNSS_SV_TYPE_NAVIC:
343 /*Android doesn't define Navic svid range yet, use Naviv svid [1, 14] now
344 will update this once Android give Navic svid definiitons */
345 out = in.svId - NAVIC_SV_PRN_MIN + 1;
346 break;
347 default:
348 out = in.svId;
349 break;
350 }
351 }
352
convertGnssEphemerisType(GnssEphemerisType & in,GnssDebug::SatelliteEphemerisType & out)353 void convertGnssEphemerisType(GnssEphemerisType& in, GnssDebug::SatelliteEphemerisType& out)
354 {
355 switch(in) {
356 case GNSS_EPH_TYPE_EPHEMERIS:
357 out = GnssDebug::SatelliteEphemerisType::EPHEMERIS;
358 break;
359 case GNSS_EPH_TYPE_ALMANAC:
360 out = GnssDebug::SatelliteEphemerisType::ALMANAC_ONLY;
361 break;
362 case GNSS_EPH_TYPE_UNKNOWN:
363 default:
364 out = GnssDebug::SatelliteEphemerisType::NOT_AVAILABLE;
365 break;
366 }
367 }
368
convertGnssEphemerisSource(GnssEphemerisSource & in,GnssDebug::SatelliteEphemerisSource & out)369 void convertGnssEphemerisSource(GnssEphemerisSource& in, GnssDebug::SatelliteEphemerisSource& out)
370 {
371 switch(in) {
372 case GNSS_EPH_SOURCE_DEMODULATED:
373 out = GnssDebug::SatelliteEphemerisSource::DEMODULATED;
374 break;
375 case GNSS_EPH_SOURCE_SUPL_PROVIDED:
376 out = GnssDebug::SatelliteEphemerisSource::SUPL_PROVIDED;
377 break;
378 case GNSS_EPH_SOURCE_OTHER_SERVER_PROVIDED:
379 out = GnssDebug::SatelliteEphemerisSource::OTHER_SERVER_PROVIDED;
380 break;
381 case GNSS_EPH_SOURCE_LOCAL:
382 case GNSS_EPH_SOURCE_UNKNOWN:
383 default:
384 out = GnssDebug::SatelliteEphemerisSource::OTHER;
385 break;
386 }
387 }
388
convertGnssEphemerisHealth(GnssEphemerisHealth & in,GnssDebug::SatelliteEphemerisHealth & out)389 void convertGnssEphemerisHealth(GnssEphemerisHealth& in, GnssDebug::SatelliteEphemerisHealth& out)
390 {
391 switch(in) {
392 case GNSS_EPH_HEALTH_GOOD:
393 out = GnssDebug::SatelliteEphemerisHealth::GOOD;
394 break;
395 case GNSS_EPH_HEALTH_BAD:
396 out = GnssDebug::SatelliteEphemerisHealth::BAD;
397 break;
398 case GNSS_EPH_HEALTH_UNKNOWN:
399 default:
400 out = GnssDebug::SatelliteEphemerisHealth::UNKNOWN;
401 break;
402 }
403 }
404
405 } // namespace implementation
406 } // namespace V2_0
407 } // namespace gnss
408 } // namespace hardware
409 } // namespace android
410