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 <loc_misc_utils.h>
34 #include <gps_extended_c.h>
35
36 namespace android {
37 namespace hardware {
38 namespace gnss {
39 namespace V2_1 {
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 using ::android::hardware::gnss::measurement_corrections::V1_0::GnssSingleSatCorrectionFlags;
47
convertGnssLocation(Location & in,V1_0::GnssLocation & out)48 void convertGnssLocation(Location& in, V1_0::GnssLocation& out)
49 {
50 memset(&out, 0, sizeof(V1_0::GnssLocation));
51 if (in.flags & LOCATION_HAS_LAT_LONG_BIT) {
52 out.gnssLocationFlags |= GnssLocationFlags::HAS_LAT_LONG;
53 out.latitudeDegrees = in.latitude;
54 out.longitudeDegrees = in.longitude;
55 }
56 if (in.flags & LOCATION_HAS_ALTITUDE_BIT) {
57 out.gnssLocationFlags |= GnssLocationFlags::HAS_ALTITUDE;
58 out.altitudeMeters = in.altitude;
59 }
60 if (in.flags & LOCATION_HAS_SPEED_BIT) {
61 out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED;
62 out.speedMetersPerSec = in.speed;
63 }
64 if (in.flags & LOCATION_HAS_BEARING_BIT) {
65 out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING;
66 out.bearingDegrees = in.bearing;
67 }
68 if (in.flags & LOCATION_HAS_ACCURACY_BIT) {
69 out.gnssLocationFlags |= GnssLocationFlags::HAS_HORIZONTAL_ACCURACY;
70 out.horizontalAccuracyMeters = in.accuracy;
71 }
72 if (in.flags & LOCATION_HAS_VERTICAL_ACCURACY_BIT) {
73 out.gnssLocationFlags |= GnssLocationFlags::HAS_VERTICAL_ACCURACY;
74 out.verticalAccuracyMeters = in.verticalAccuracy;
75 }
76 if (in.flags & LOCATION_HAS_SPEED_ACCURACY_BIT) {
77 out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED_ACCURACY;
78 out.speedAccuracyMetersPerSecond = in.speedAccuracy;
79 }
80 if (in.flags & LOCATION_HAS_BEARING_ACCURACY_BIT) {
81 out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING_ACCURACY;
82 out.bearingAccuracyDegrees = in.bearingAccuracy;
83 }
84
85 out.timestamp = static_cast<V1_0::GnssUtcTime>(in.timestamp);
86 }
87
convertGnssLocation(Location & in,V2_0::GnssLocation & out)88 void convertGnssLocation(Location& in, V2_0::GnssLocation& out)
89 {
90 memset(&out, 0, sizeof(V2_0::GnssLocation));
91 convertGnssLocation(in, out.v1_0);
92
93 if (in.flags & LOCATION_HAS_ELAPSED_REAL_TIME) {
94 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIMESTAMP_NS;
95 out.elapsedRealtime.timestampNs = in.elapsedRealTime;
96 out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS;
97 out.elapsedRealtime.timeUncertaintyNs = in.elapsedRealTimeUnc;
98 LOC_LOGd("out.elapsedRealtime.timestampNs=%" PRIi64 ""
99 " out.elapsedRealtime.timeUncertaintyNs=%" PRIi64 ""
100 " out.elapsedRealtime.flags=0x%X",
101 out.elapsedRealtime.timestampNs,
102 out.elapsedRealtime.timeUncertaintyNs, out.elapsedRealtime.flags);
103 }
104 }
105
convertGnssLocation(const V1_0::GnssLocation & in,Location & out)106 void convertGnssLocation(const V1_0::GnssLocation& in, Location& out)
107 {
108 memset(&out, 0, sizeof(out));
109 if (in.gnssLocationFlags & GnssLocationFlags::HAS_LAT_LONG) {
110 out.flags |= LOCATION_HAS_LAT_LONG_BIT;
111 out.latitude = in.latitudeDegrees;
112 out.longitude = in.longitudeDegrees;
113 }
114 if (in.gnssLocationFlags & GnssLocationFlags::HAS_ALTITUDE) {
115 out.flags |= LOCATION_HAS_ALTITUDE_BIT;
116 out.altitude = in.altitudeMeters;
117 }
118 if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED) {
119 out.flags |= LOCATION_HAS_SPEED_BIT;
120 out.speed = in.speedMetersPerSec;
121 }
122 if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING) {
123 out.flags |= LOCATION_HAS_BEARING_BIT;
124 out.bearing = in.bearingDegrees;
125 }
126 if (in.gnssLocationFlags & GnssLocationFlags::HAS_HORIZONTAL_ACCURACY) {
127 out.flags |= LOCATION_HAS_ACCURACY_BIT;
128 out.accuracy = in.horizontalAccuracyMeters;
129 }
130 if (in.gnssLocationFlags & GnssLocationFlags::HAS_VERTICAL_ACCURACY) {
131 out.flags |= LOCATION_HAS_VERTICAL_ACCURACY_BIT;
132 out.verticalAccuracy = in.verticalAccuracyMeters;
133 }
134 if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED_ACCURACY) {
135 out.flags |= LOCATION_HAS_SPEED_ACCURACY_BIT;
136 out.speedAccuracy = in.speedAccuracyMetersPerSecond;
137 }
138 if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING_ACCURACY) {
139 out.flags |= LOCATION_HAS_BEARING_ACCURACY_BIT;
140 out.bearingAccuracy = in.bearingAccuracyDegrees;
141 }
142
143 out.timestamp = static_cast<uint64_t>(in.timestamp);
144 }
145
convertGnssLocation(const V2_0::GnssLocation & in,Location & out)146 void convertGnssLocation(const V2_0::GnssLocation& in, Location& out)
147 {
148 memset(&out, 0, sizeof(out));
149 convertGnssLocation(in.v1_0, out);
150 }
151
convertGnssConstellationType(GnssSvType & in,V1_0::GnssConstellationType & out)152 void convertGnssConstellationType(GnssSvType& in, V1_0::GnssConstellationType& out)
153 {
154 switch(in) {
155 case GNSS_SV_TYPE_GPS:
156 out = V1_0::GnssConstellationType::GPS;
157 break;
158 case GNSS_SV_TYPE_SBAS:
159 out = V1_0::GnssConstellationType::SBAS;
160 break;
161 case GNSS_SV_TYPE_GLONASS:
162 out = V1_0::GnssConstellationType::GLONASS;
163 break;
164 case GNSS_SV_TYPE_QZSS:
165 out = V1_0::GnssConstellationType::QZSS;
166 break;
167 case GNSS_SV_TYPE_BEIDOU:
168 out = V1_0::GnssConstellationType::BEIDOU;
169 break;
170 case GNSS_SV_TYPE_GALILEO:
171 out = V1_0::GnssConstellationType::GALILEO;
172 break;
173 case GNSS_SV_TYPE_UNKNOWN:
174 default:
175 out = V1_0::GnssConstellationType::UNKNOWN;
176 break;
177 }
178 }
179
convertGnssConstellationType(GnssSvType & in,V2_0::GnssConstellationType & out)180 void convertGnssConstellationType(GnssSvType& in, V2_0::GnssConstellationType& out)
181 {
182 switch(in) {
183 case GNSS_SV_TYPE_GPS:
184 out = V2_0::GnssConstellationType::GPS;
185 break;
186 case GNSS_SV_TYPE_SBAS:
187 out = V2_0::GnssConstellationType::SBAS;
188 break;
189 case GNSS_SV_TYPE_GLONASS:
190 out = V2_0::GnssConstellationType::GLONASS;
191 break;
192 case GNSS_SV_TYPE_QZSS:
193 out = V2_0::GnssConstellationType::QZSS;
194 break;
195 case GNSS_SV_TYPE_BEIDOU:
196 out = V2_0::GnssConstellationType::BEIDOU;
197 break;
198 case GNSS_SV_TYPE_GALILEO:
199 out = V2_0::GnssConstellationType::GALILEO;
200 break;
201 case GNSS_SV_TYPE_NAVIC:
202 out = V2_0::GnssConstellationType::IRNSS;
203 break;
204 case GNSS_SV_TYPE_UNKNOWN:
205 default:
206 out = V2_0::GnssConstellationType::UNKNOWN;
207 break;
208 }
209 }
210
convertGnssSvid(GnssSv & in,int16_t & out)211 void convertGnssSvid(GnssSv& in, int16_t& out)
212 {
213 switch (in.type) {
214 case GNSS_SV_TYPE_GPS:
215 out = in.svId;
216 break;
217 case GNSS_SV_TYPE_SBAS:
218 out = in.svId;
219 break;
220 case GNSS_SV_TYPE_GLONASS:
221 if (!isGloSlotUnknown(in.svId)) { // OSN is known
222 out = in.svId - GLO_SV_PRN_MIN + 1;
223 } else { // OSN is not known, report FCN
224 out = in.gloFrequency + 92;
225 }
226 break;
227 case GNSS_SV_TYPE_QZSS:
228 out = in.svId;
229 break;
230 case GNSS_SV_TYPE_BEIDOU:
231 out = in.svId - BDS_SV_PRN_MIN + 1;
232 break;
233 case GNSS_SV_TYPE_GALILEO:
234 out = in.svId - GAL_SV_PRN_MIN + 1;
235 break;
236 case GNSS_SV_TYPE_NAVIC:
237 out = in.svId - NAVIC_SV_PRN_MIN + 1;
238 break;
239 default:
240 out = in.svId;
241 break;
242 }
243 }
244
convertGnssSvid(GnssMeasurementsData & in,int16_t & out)245 void convertGnssSvid(GnssMeasurementsData& in, int16_t& out)
246 {
247 switch (in.svType) {
248 case GNSS_SV_TYPE_GPS:
249 out = in.svId;
250 break;
251 case GNSS_SV_TYPE_SBAS:
252 out = in.svId;
253 break;
254 case GNSS_SV_TYPE_GLONASS:
255 if (!isGloSlotUnknown(in.svId)) { // OSN is known
256 out = in.svId - GLO_SV_PRN_MIN + 1;
257 } else { // OSN is not known, report FCN
258 out = in.gloFrequency + 92;
259 }
260 break;
261 case GNSS_SV_TYPE_QZSS:
262 out = in.svId;
263 break;
264 case GNSS_SV_TYPE_BEIDOU:
265 out = in.svId - BDS_SV_PRN_MIN + 1;
266 break;
267 case GNSS_SV_TYPE_GALILEO:
268 out = in.svId - GAL_SV_PRN_MIN + 1;
269 break;
270 case GNSS_SV_TYPE_NAVIC:
271 out = in.svId - NAVIC_SV_PRN_MIN + 1;
272 break;
273 default:
274 out = in.svId;
275 break;
276 }
277 }
278
convertGnssEphemerisType(GnssEphemerisType & in,GnssDebug::SatelliteEphemerisType & out)279 void convertGnssEphemerisType(GnssEphemerisType& in, GnssDebug::SatelliteEphemerisType& out)
280 {
281 switch(in) {
282 case GNSS_EPH_TYPE_EPHEMERIS:
283 out = GnssDebug::SatelliteEphemerisType::EPHEMERIS;
284 break;
285 case GNSS_EPH_TYPE_ALMANAC:
286 out = GnssDebug::SatelliteEphemerisType::ALMANAC_ONLY;
287 break;
288 case GNSS_EPH_TYPE_UNKNOWN:
289 default:
290 out = GnssDebug::SatelliteEphemerisType::NOT_AVAILABLE;
291 break;
292 }
293 }
294
convertGnssEphemerisSource(GnssEphemerisSource & in,GnssDebug::SatelliteEphemerisSource & out)295 void convertGnssEphemerisSource(GnssEphemerisSource& in, GnssDebug::SatelliteEphemerisSource& out)
296 {
297 switch(in) {
298 case GNSS_EPH_SOURCE_DEMODULATED:
299 out = GnssDebug::SatelliteEphemerisSource::DEMODULATED;
300 break;
301 case GNSS_EPH_SOURCE_SUPL_PROVIDED:
302 out = GnssDebug::SatelliteEphemerisSource::SUPL_PROVIDED;
303 break;
304 case GNSS_EPH_SOURCE_OTHER_SERVER_PROVIDED:
305 out = GnssDebug::SatelliteEphemerisSource::OTHER_SERVER_PROVIDED;
306 break;
307 case GNSS_EPH_SOURCE_LOCAL:
308 case GNSS_EPH_SOURCE_UNKNOWN:
309 default:
310 out = GnssDebug::SatelliteEphemerisSource::OTHER;
311 break;
312 }
313 }
314
convertGnssEphemerisHealth(GnssEphemerisHealth & in,GnssDebug::SatelliteEphemerisHealth & out)315 void convertGnssEphemerisHealth(GnssEphemerisHealth& in, GnssDebug::SatelliteEphemerisHealth& out)
316 {
317 switch(in) {
318 case GNSS_EPH_HEALTH_GOOD:
319 out = GnssDebug::SatelliteEphemerisHealth::GOOD;
320 break;
321 case GNSS_EPH_HEALTH_BAD:
322 out = GnssDebug::SatelliteEphemerisHealth::BAD;
323 break;
324 case GNSS_EPH_HEALTH_UNKNOWN:
325 default:
326 out = GnssDebug::SatelliteEphemerisHealth::UNKNOWN;
327 break;
328 }
329 }
330
convertSingleSatCorrections(const SingleSatCorrection & in,GnssSingleSatCorrection & out)331 void convertSingleSatCorrections(const SingleSatCorrection& in, GnssSingleSatCorrection& out)
332 {
333 out.flags = GNSS_MEAS_CORR_UNKNOWN_BIT;
334 if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_SAT_IS_LOS_PROBABILITY)) {
335 out.flags |= GNSS_MEAS_CORR_HAS_SAT_IS_LOS_PROBABILITY_BIT;
336 }
337 if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH)) {
338 out.flags |= GNSS_MEAS_CORR_HAS_EXCESS_PATH_LENGTH_BIT;
339 }
340 if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH_UNC)) {
341 out.flags |= GNSS_MEAS_CORR_HAS_EXCESS_PATH_LENGTH_UNC_BIT;
342 }
343 if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_REFLECTING_PLANE)) {
344 out.flags |= GNSS_MEAS_CORR_HAS_REFLECTING_PLANE_BIT;
345 }
346 switch (in.constellation) {
347 case (::android::hardware::gnss::V1_0::GnssConstellationType::GPS):
348 out.svType = GNSS_SV_TYPE_GPS;
349 break;
350 case (::android::hardware::gnss::V1_0::GnssConstellationType::SBAS):
351 out.svType = GNSS_SV_TYPE_SBAS;
352 break;
353 case (::android::hardware::gnss::V1_0::GnssConstellationType::GLONASS):
354 out.svType = GNSS_SV_TYPE_GLONASS;
355 break;
356 case (::android::hardware::gnss::V1_0::GnssConstellationType::QZSS):
357 out.svType = GNSS_SV_TYPE_QZSS;
358 break;
359 case (::android::hardware::gnss::V1_0::GnssConstellationType::BEIDOU):
360 out.svType = GNSS_SV_TYPE_BEIDOU;
361 break;
362 case (::android::hardware::gnss::V1_0::GnssConstellationType::GALILEO):
363 out.svType = GNSS_SV_TYPE_GALILEO;
364 break;
365 case (::android::hardware::gnss::V1_0::GnssConstellationType::UNKNOWN):
366 default:
367 out.svType = GNSS_SV_TYPE_UNKNOWN;
368 break;
369 }
370 out.svId = in.svid;
371 out.carrierFrequencyHz = in.carrierFrequencyHz;
372 out.probSatIsLos = in.probSatIsLos;
373 out.excessPathLengthMeters = in.excessPathLengthMeters;
374 out.excessPathLengthUncertaintyMeters = in.excessPathLengthUncertaintyMeters;
375
376 out.reflectingPlane.latitudeDegrees = in.reflectingPlane.latitudeDegrees;
377 out.reflectingPlane.longitudeDegrees = in.reflectingPlane.longitudeDegrees;
378 out.reflectingPlane.altitudeMeters = in.reflectingPlane.altitudeMeters;
379 out.reflectingPlane.azimuthDegrees = in.reflectingPlane.azimuthDegrees;
380 }
381
convertMeasurementCorrections(const MeasurementCorrectionsV1_0 & in,GnssMeasurementCorrections & out)382 void convertMeasurementCorrections(const MeasurementCorrectionsV1_0& in,
383 GnssMeasurementCorrections& out)
384 {
385 memset(&out, 0, sizeof(GnssMeasurementCorrections));
386 out.latitudeDegrees = in.latitudeDegrees;
387 out.longitudeDegrees = in.longitudeDegrees;
388 out.altitudeMeters = in.altitudeMeters;
389 out.horizontalPositionUncertaintyMeters = in.horizontalPositionUncertaintyMeters;
390 out.verticalPositionUncertaintyMeters = in.verticalPositionUncertaintyMeters;
391 out.toaGpsNanosecondsOfWeek = in.toaGpsNanosecondsOfWeek;
392
393 for (int i = 0; i < in.satCorrections.size(); i++) {
394 GnssSingleSatCorrection gnssSingleSatCorrection = {};
395
396 convertSingleSatCorrections(in.satCorrections[i], gnssSingleSatCorrection);
397 out.satCorrections.push_back(gnssSingleSatCorrection);
398 }
399 }
400
401 } // namespace implementation
402 } // namespace V2_1
403 } // namespace gnss
404 } // namespace hardware
405 } // namespace android
406