1 /* Copyright (c) 2017-2019, 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 
34 namespace android {
35 namespace hardware {
36 namespace gnss {
37 namespace V2_0 {
38 namespace implementation {
39 
40 using ::android::hardware::gnss::V2_0::GnssLocation;
41 using ::android::hardware::gnss::V2_0::GnssConstellationType;
42 using ::android::hardware::gnss::V1_0::GnssLocationFlags;
43 
convertGnssLocation(Location & in,V1_0::GnssLocation & out)44 void convertGnssLocation(Location& in, V1_0::GnssLocation& out)
45 {
46     memset(&out, 0, sizeof(V1_0::GnssLocation));
47     if (in.flags & LOCATION_HAS_LAT_LONG_BIT) {
48         out.gnssLocationFlags |= GnssLocationFlags::HAS_LAT_LONG;
49         out.latitudeDegrees = in.latitude;
50         out.longitudeDegrees = in.longitude;
51     }
52     if (in.flags & LOCATION_HAS_ALTITUDE_BIT) {
53         out.gnssLocationFlags |= GnssLocationFlags::HAS_ALTITUDE;
54         out.altitudeMeters = in.altitude;
55     }
56     if (in.flags & LOCATION_HAS_SPEED_BIT) {
57         out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED;
58         out.speedMetersPerSec = in.speed;
59     }
60     if (in.flags & LOCATION_HAS_BEARING_BIT) {
61         out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING;
62         out.bearingDegrees = in.bearing;
63     }
64     if (in.flags & LOCATION_HAS_ACCURACY_BIT) {
65         out.gnssLocationFlags |= GnssLocationFlags::HAS_HORIZONTAL_ACCURACY;
66         out.horizontalAccuracyMeters = in.accuracy;
67     }
68     if (in.flags & LOCATION_HAS_VERTICAL_ACCURACY_BIT) {
69         out.gnssLocationFlags |= GnssLocationFlags::HAS_VERTICAL_ACCURACY;
70         out.verticalAccuracyMeters = in.verticalAccuracy;
71     }
72     if (in.flags & LOCATION_HAS_SPEED_ACCURACY_BIT) {
73         out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED_ACCURACY;
74         out.speedAccuracyMetersPerSecond = in.speedAccuracy;
75     }
76     if (in.flags & LOCATION_HAS_BEARING_ACCURACY_BIT) {
77         out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING_ACCURACY;
78         out.bearingAccuracyDegrees = in.bearingAccuracy;
79     }
80 
81     out.timestamp = static_cast<V1_0::GnssUtcTime>(in.timestamp);
82 }
83 
getCurrentTime(struct timespec & currentTime,int64_t & sinceBootTimeNanos)84 bool getCurrentTime(struct timespec& currentTime, int64_t& sinceBootTimeNanos)
85 {
86     struct timespec sinceBootTime;
87     struct timespec sinceBootTimeTest;
88     bool clockGetTimeSuccess = false;
89     const uint32_t MAX_TIME_DELTA_VALUE_NANOS = 10000;
90     const uint32_t MAX_GET_TIME_COUNT = 20;
91     /* Attempt to get CLOCK_REALTIME and CLOCK_BOOTIME in succession without an interruption
92     or context switch (for up to MAX_GET_TIME_COUNT times) to avoid errors in the calculation */
93     for (uint32_t i = 0; i < MAX_GET_TIME_COUNT; i++) {
94         if (clock_gettime(CLOCK_BOOTTIME, &sinceBootTime) != 0) {
95             break;
96         };
97         if (clock_gettime(CLOCK_REALTIME, &currentTime) != 0) {
98             break;
99         }
100         if (clock_gettime(CLOCK_BOOTTIME, &sinceBootTimeTest) != 0) {
101             break;
102         };
103         sinceBootTimeNanos = sinceBootTime.tv_sec * 1000000000 + sinceBootTime.tv_nsec;
104         int64_t sinceBootTimeTestNanos =
105             sinceBootTimeTest.tv_sec * 1000000000 + sinceBootTimeTest.tv_nsec;
106         int64_t sinceBootTimeDeltaNanos = sinceBootTimeTestNanos - sinceBootTimeNanos;
107 
108         /* sinceBootTime and sinceBootTimeTest should have a close value if there was no
109         interruption or context switch between clock_gettime for CLOCK_BOOTIME and
110         clock_gettime for CLOCK_REALTIME */
111         if (sinceBootTimeDeltaNanos < MAX_TIME_DELTA_VALUE_NANOS) {
112             clockGetTimeSuccess = true;
113             break;
114         } else {
115             LOC_LOGd("Delta:%" PRIi64 "ns time too large, retry number #%u...",
116                      sinceBootTimeDeltaNanos, i + 1);
117         }
118     }
119     return clockGetTimeSuccess;
120 }
121 
convertGnssLocation(Location & in,V2_0::GnssLocation & out)122 void convertGnssLocation(Location& in, V2_0::GnssLocation& out)
123 {
124     memset(&out, 0, sizeof(V2_0::GnssLocation));
125     convertGnssLocation(in, out.v1_0);
126 
127     struct timespec currentTime;
128     int64_t sinceBootTimeNanos;
129 
130     if (getCurrentTime(currentTime, sinceBootTimeNanos)) {
131         int64_t currentTimeNanos = currentTime.tv_sec*1000000000 + currentTime.tv_nsec;
132         int64_t locationTimeNanos = in.timestamp*1000000;
133         LOC_LOGD("%s]: sinceBootTimeNanos:%" PRIi64 " currentTimeNanos:%" PRIi64 ""
134                 " locationTimeNanos:%" PRIi64 "",
135                 __FUNCTION__, sinceBootTimeNanos, currentTimeNanos, locationTimeNanos);
136         if (currentTimeNanos >= locationTimeNanos) {
137             int64_t ageTimeNanos = currentTimeNanos - locationTimeNanos;
138             LOC_LOGD("%s]: ageTimeNanos:%" PRIi64 ")", __FUNCTION__, ageTimeNanos);
139             if (ageTimeNanos >= 0 && ageTimeNanos <= sinceBootTimeNanos) {
140                 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIMESTAMP_NS;
141                 out.elapsedRealtime.timestampNs = sinceBootTimeNanos - ageTimeNanos;
142                 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS;
143                 // time uncertainty is 1 ms since it is calculated from utc time that is in ms
144                 out.elapsedRealtime.timeUncertaintyNs = 1000000;
145                 LOC_LOGD("%s]: timestampNs:%" PRIi64 ")",
146                         __FUNCTION__, out.elapsedRealtime.timestampNs);
147             }
148         }
149     } else {
150         LOC_LOGe("Failed to calculate elapsedRealtimeNanos timestamp");
151     }
152 }
153 
convertGnssLocation(const V1_0::GnssLocation & in,Location & out)154 void convertGnssLocation(const V1_0::GnssLocation& in, Location& out)
155 {
156     memset(&out, 0, sizeof(out));
157     if (in.gnssLocationFlags & GnssLocationFlags::HAS_LAT_LONG) {
158         out.flags |= LOCATION_HAS_LAT_LONG_BIT;
159         out.latitude = in.latitudeDegrees;
160         out.longitude = in.longitudeDegrees;
161     }
162     if (in.gnssLocationFlags & GnssLocationFlags::HAS_ALTITUDE) {
163         out.flags |= LOCATION_HAS_ALTITUDE_BIT;
164         out.altitude = in.altitudeMeters;
165     }
166     if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED) {
167         out.flags |= LOCATION_HAS_SPEED_BIT;
168         out.speed = in.speedMetersPerSec;
169     }
170     if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING) {
171         out.flags |= LOCATION_HAS_BEARING_BIT;
172         out.bearing = in.bearingDegrees;
173     }
174     if (in.gnssLocationFlags & GnssLocationFlags::HAS_HORIZONTAL_ACCURACY) {
175         out.flags |= LOCATION_HAS_ACCURACY_BIT;
176         out.accuracy = in.horizontalAccuracyMeters;
177     }
178     if (in.gnssLocationFlags & GnssLocationFlags::HAS_VERTICAL_ACCURACY) {
179         out.flags |= LOCATION_HAS_VERTICAL_ACCURACY_BIT;
180         out.verticalAccuracy = in.verticalAccuracyMeters;
181     }
182     if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED_ACCURACY) {
183         out.flags |= LOCATION_HAS_SPEED_ACCURACY_BIT;
184         out.speedAccuracy = in.speedAccuracyMetersPerSecond;
185     }
186     if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING_ACCURACY) {
187         out.flags |= LOCATION_HAS_BEARING_ACCURACY_BIT;
188         out.bearingAccuracy = in.bearingAccuracyDegrees;
189     }
190 
191     out.timestamp = static_cast<uint64_t>(in.timestamp);
192 }
193 
convertGnssLocation(const V2_0::GnssLocation & in,Location & out)194 void convertGnssLocation(const V2_0::GnssLocation& in, Location& out)
195 {
196     memset(&out, 0, sizeof(out));
197     convertGnssLocation(in.v1_0, out);
198 }
199 
convertGnssConstellationType(GnssSvType & in,V1_0::GnssConstellationType & out)200 void convertGnssConstellationType(GnssSvType& in, V1_0::GnssConstellationType& out)
201 {
202     switch(in) {
203         case GNSS_SV_TYPE_GPS:
204             out = V1_0::GnssConstellationType::GPS;
205             break;
206         case GNSS_SV_TYPE_SBAS:
207             out = V1_0::GnssConstellationType::SBAS;
208             break;
209         case GNSS_SV_TYPE_GLONASS:
210             out = V1_0::GnssConstellationType::GLONASS;
211             break;
212         case GNSS_SV_TYPE_QZSS:
213             out = V1_0::GnssConstellationType::QZSS;
214             break;
215         case GNSS_SV_TYPE_BEIDOU:
216             out = V1_0::GnssConstellationType::BEIDOU;
217             break;
218         case GNSS_SV_TYPE_GALILEO:
219             out = V1_0::GnssConstellationType::GALILEO;
220             break;
221         case GNSS_SV_TYPE_UNKNOWN:
222         default:
223             out = V1_0::GnssConstellationType::UNKNOWN;
224             break;
225     }
226 }
227 
convertGnssConstellationType(GnssSvType & in,V2_0::GnssConstellationType & out)228 void convertGnssConstellationType(GnssSvType& in, V2_0::GnssConstellationType& out)
229 {
230     switch(in) {
231         case GNSS_SV_TYPE_GPS:
232             out = V2_0::GnssConstellationType::GPS;
233             break;
234         case GNSS_SV_TYPE_SBAS:
235             out = V2_0::GnssConstellationType::SBAS;
236             break;
237         case GNSS_SV_TYPE_GLONASS:
238             out = V2_0::GnssConstellationType::GLONASS;
239             break;
240         case GNSS_SV_TYPE_QZSS:
241             out = V2_0::GnssConstellationType::QZSS;
242             break;
243         case GNSS_SV_TYPE_BEIDOU:
244             out = V2_0::GnssConstellationType::BEIDOU;
245             break;
246         case GNSS_SV_TYPE_GALILEO:
247             out = V2_0::GnssConstellationType::GALILEO;
248             break;
249         case GNSS_SV_TYPE_NAVIC:
250             out = V2_0::GnssConstellationType::IRNSS;
251             break;
252         case GNSS_SV_TYPE_UNKNOWN:
253         default:
254             out = V2_0::GnssConstellationType::UNKNOWN;
255             break;
256     }
257 }
258 
convertGnssEphemerisType(GnssEphemerisType & in,GnssDebug::SatelliteEphemerisType & out)259 void convertGnssEphemerisType(GnssEphemerisType& in, GnssDebug::SatelliteEphemerisType& out)
260 {
261     switch(in) {
262         case GNSS_EPH_TYPE_EPHEMERIS:
263             out = GnssDebug::SatelliteEphemerisType::EPHEMERIS;
264             break;
265         case GNSS_EPH_TYPE_ALMANAC:
266             out = GnssDebug::SatelliteEphemerisType::ALMANAC_ONLY;
267             break;
268         case GNSS_EPH_TYPE_UNKNOWN:
269         default:
270             out = GnssDebug::SatelliteEphemerisType::NOT_AVAILABLE;
271             break;
272     }
273 }
274 
convertGnssEphemerisSource(GnssEphemerisSource & in,GnssDebug::SatelliteEphemerisSource & out)275 void convertGnssEphemerisSource(GnssEphemerisSource& in, GnssDebug::SatelliteEphemerisSource& out)
276 {
277     switch(in) {
278         case GNSS_EPH_SOURCE_DEMODULATED:
279             out = GnssDebug::SatelliteEphemerisSource::DEMODULATED;
280             break;
281         case GNSS_EPH_SOURCE_SUPL_PROVIDED:
282             out = GnssDebug::SatelliteEphemerisSource::SUPL_PROVIDED;
283             break;
284         case GNSS_EPH_SOURCE_OTHER_SERVER_PROVIDED:
285             out = GnssDebug::SatelliteEphemerisSource::OTHER_SERVER_PROVIDED;
286             break;
287         case GNSS_EPH_SOURCE_LOCAL:
288         case GNSS_EPH_SOURCE_UNKNOWN:
289         default:
290             out = GnssDebug::SatelliteEphemerisSource::OTHER;
291             break;
292     }
293 }
294 
convertGnssEphemerisHealth(GnssEphemerisHealth & in,GnssDebug::SatelliteEphemerisHealth & out)295 void convertGnssEphemerisHealth(GnssEphemerisHealth& in, GnssDebug::SatelliteEphemerisHealth& out)
296 {
297     switch(in) {
298         case GNSS_EPH_HEALTH_GOOD:
299             out = GnssDebug::SatelliteEphemerisHealth::GOOD;
300             break;
301         case GNSS_EPH_HEALTH_BAD:
302             out = GnssDebug::SatelliteEphemerisHealth::BAD;
303             break;
304         case GNSS_EPH_HEALTH_UNKNOWN:
305         default:
306             out = GnssDebug::SatelliteEphemerisHealth::UNKNOWN;
307             break;
308     }
309 }
310 
311 }  // namespace implementation
312 }  // namespace V2_0
313 }  // namespace gnss
314 }  // namespace hardware
315 }  // namespace android
316