1 /*
2  * Copyright (C) 2019 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <Utils.h>
18 #include <android/hardware/gnss/BnGnss.h>
19 #include <android/hardware/gnss/IGnss.h>
20 #include "gtest/gtest.h"
21 
22 #include <cutils/properties.h>
23 #include <math.h>
24 #include <utils/SystemClock.h>
25 
26 namespace android {
27 namespace hardware {
28 namespace gnss {
29 namespace common {
30 
31 using android::hardware::gnss::ElapsedRealtime;
32 using android::hardware::gnss::GnssLocation;
33 
34 using namespace measurement_corrections::V1_0;
35 using V1_0::GnssLocationFlags;
36 
37 using MeasurementCorrectionsAidl =
38         android::hardware::gnss::measurement_corrections::MeasurementCorrections;
39 using ReflectingPlaneAidl = android::hardware::gnss::measurement_corrections::ReflectingPlane;
40 using SingleSatCorrectionAidl =
41         android::hardware::gnss::measurement_corrections::SingleSatCorrection;
42 using ExcessPathInfo = SingleSatCorrectionAidl::ExcessPathInfo;
43 
44 template <>
getLocationTimestampMillis(const android::hardware::gnss::GnssLocation & location)45 int64_t Utils::getLocationTimestampMillis(const android::hardware::gnss::GnssLocation& location) {
46     return location.timestampMillis;
47 }
48 
49 template <>
getLocationTimestampMillis(const V1_0::GnssLocation & location)50 int64_t Utils::getLocationTimestampMillis(const V1_0::GnssLocation& location) {
51     return location.timestamp;
52 }
53 
54 template <>
checkLocationElapsedRealtime(const V1_0::GnssLocation &)55 void Utils::checkLocationElapsedRealtime(const V1_0::GnssLocation&) {}
56 
57 template <>
checkLocationElapsedRealtime(const android::hardware::gnss::GnssLocation & location)58 void Utils::checkLocationElapsedRealtime(const android::hardware::gnss::GnssLocation& location) {
59     checkElapsedRealtime(location.elapsedRealtime);
60 }
61 
checkPositionDebug(android::hardware::gnss::IGnssDebug::DebugData data)62 void Utils::checkPositionDebug(android::hardware::gnss::IGnssDebug::DebugData data) {
63     if (data.position.valid) {
64         ASSERT_TRUE(data.position.latitudeDegrees >= -90 && data.position.latitudeDegrees <= 90);
65         ASSERT_TRUE(data.position.longitudeDegrees >= -180 &&
66                     data.position.longitudeDegrees <= 180);
67         ASSERT_TRUE(data.position.altitudeMeters >= -1000 &&  // Dead Sea: -414m
68                     data.position.altitudeMeters <= 20000);   // Mount Everest: 8850m
69         ASSERT_TRUE(data.position.speedMetersPerSec >= 0 && data.position.speedMetersPerSec <= 600);
70         ASSERT_TRUE(data.position.bearingDegrees >= -360 && data.position.bearingDegrees <= 360);
71         ASSERT_TRUE(data.position.horizontalAccuracyMeters > 0 &&
72                     data.position.horizontalAccuracyMeters <= 20000000);
73         ASSERT_TRUE(data.position.verticalAccuracyMeters > 0 &&
74                     data.position.verticalAccuracyMeters <= 20000);
75         ASSERT_TRUE(data.position.speedAccuracyMetersPerSecond > 0 &&
76                     data.position.speedAccuracyMetersPerSecond <= 500);
77         ASSERT_TRUE(data.position.bearingAccuracyDegrees > 0 &&
78                     data.position.bearingAccuracyDegrees <= 180);
79         ASSERT_TRUE(data.position.ageSeconds >= 0);
80     }
81     ASSERT_TRUE(data.time.timeEstimateMs >= 1483228800000);  // Jan 01 2017 00:00:00 GMT.
82     ASSERT_TRUE(data.time.timeUncertaintyNs > 0);
83     ASSERT_TRUE(data.time.frequencyUncertaintyNsPerSec > 0 &&
84                 data.time.frequencyUncertaintyNsPerSec <= 2.0e5);  // 200 ppm
85 }
86 
checkElapsedRealtime(const ElapsedRealtime & elapsedRealtime)87 void Utils::checkElapsedRealtime(const ElapsedRealtime& elapsedRealtime) {
88     ASSERT_TRUE(elapsedRealtime.flags >= 0 &&
89                 elapsedRealtime.flags <= (ElapsedRealtime::HAS_TIMESTAMP_NS |
90                                           ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS));
91     if (elapsedRealtime.flags & ElapsedRealtime::HAS_TIMESTAMP_NS) {
92         ASSERT_TRUE(elapsedRealtime.timestampNs > 0);
93     }
94     if (elapsedRealtime.flags & ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS) {
95         ASSERT_TRUE(elapsedRealtime.timeUncertaintyNs > 0);
96     }
97 }
98 
getMockLocation(double latitudeDegrees,double longitudeDegrees,double horizontalAccuracyMeters)99 const GnssLocation Utils::getMockLocation(double latitudeDegrees, double longitudeDegrees,
100                                           double horizontalAccuracyMeters) {
101     ElapsedRealtime elapsedRealtime;
102     elapsedRealtime.flags =
103             ElapsedRealtime::HAS_TIMESTAMP_NS | ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS;
104     elapsedRealtime.timestampNs = ::android::elapsedRealtimeNano();
105     elapsedRealtime.timeUncertaintyNs = 1000;
106     GnssLocation location;
107     location.gnssLocationFlags = 0xFF;
108     location.latitudeDegrees = latitudeDegrees;
109     location.longitudeDegrees = longitudeDegrees;
110     location.altitudeMeters = 500.0;
111     location.speedMetersPerSec = 0.0;
112     location.bearingDegrees = 0.0;
113     location.horizontalAccuracyMeters = horizontalAccuracyMeters;
114     location.verticalAccuracyMeters = 1000.0;
115     location.speedAccuracyMetersPerSecond = 1000.0;
116     location.bearingAccuracyDegrees = 90.0;
117     location.timestampMillis =
118             static_cast<int64_t>(kMockTimestamp + ::android::elapsedRealtimeNano() * 1e-6);
119     location.elapsedRealtime = elapsedRealtime;
120     return location;
121 }
122 
getMockMeasurementCorrections()123 const MeasurementCorrections Utils::getMockMeasurementCorrections() {
124     ReflectingPlane reflectingPlane = {
125             .latitudeDegrees = 37.4220039,
126             .longitudeDegrees = -122.0840991,
127             .altitudeMeters = 250.35,
128             .azimuthDegrees = 203.0,
129     };
130 
131     SingleSatCorrection singleSatCorrection1 = {
132             .singleSatCorrectionFlags = GnssSingleSatCorrectionFlags::HAS_SAT_IS_LOS_PROBABILITY |
133                                         GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH |
134                                         GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH_UNC |
135                                         GnssSingleSatCorrectionFlags::HAS_REFLECTING_PLANE,
136             .constellation = V1_0::GnssConstellationType::GPS,
137             .svid = 12,
138             .carrierFrequencyHz = 1.59975e+09,
139             .probSatIsLos = 0.50001,
140             .excessPathLengthMeters = 137.4802,
141             .excessPathLengthUncertaintyMeters = 25.5,
142             .reflectingPlane = reflectingPlane,
143     };
144     SingleSatCorrection singleSatCorrection2 = {
145             .singleSatCorrectionFlags = GnssSingleSatCorrectionFlags::HAS_SAT_IS_LOS_PROBABILITY |
146                                         GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH |
147                                         GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH_UNC,
148 
149             .constellation = V1_0::GnssConstellationType::GPS,
150             .svid = 9,
151             .carrierFrequencyHz = 1.59975e+09,
152             .probSatIsLos = 0.873,
153             .excessPathLengthMeters = 26.294,
154             .excessPathLengthUncertaintyMeters = 10.0,
155     };
156 
157     hidl_vec<SingleSatCorrection> singleSatCorrections = {singleSatCorrection1,
158                                                           singleSatCorrection2};
159     MeasurementCorrections mockCorrections = {
160             .latitudeDegrees = 37.4219999,
161             .longitudeDegrees = -122.0840575,
162             .altitudeMeters = 30.60062531,
163             .horizontalPositionUncertaintyMeters = 9.23542,
164             .verticalPositionUncertaintyMeters = 15.02341,
165             .toaGpsNanosecondsOfWeek = 2935633453L,
166             .satCorrections = singleSatCorrections,
167     };
168     return mockCorrections;
169 }
170 
171 const measurement_corrections::V1_1::MeasurementCorrections
getMockMeasurementCorrections_1_1()172 Utils::getMockMeasurementCorrections_1_1() {
173     MeasurementCorrections mockCorrections_1_0 = getMockMeasurementCorrections();
174 
175     measurement_corrections::V1_1::SingleSatCorrection singleSatCorrection1 = {
176             .v1_0 = mockCorrections_1_0.satCorrections[0],
177             .constellation = V2_0::GnssConstellationType::IRNSS,
178     };
179     measurement_corrections::V1_1::SingleSatCorrection singleSatCorrection2 = {
180             .v1_0 = mockCorrections_1_0.satCorrections[1],
181             .constellation = V2_0::GnssConstellationType::IRNSS,
182     };
183 
184     mockCorrections_1_0.satCorrections[0].constellation = V1_0::GnssConstellationType::UNKNOWN;
185     mockCorrections_1_0.satCorrections[1].constellation = V1_0::GnssConstellationType::UNKNOWN;
186 
187     hidl_vec<measurement_corrections::V1_1::SingleSatCorrection> singleSatCorrections = {
188             singleSatCorrection1, singleSatCorrection2};
189 
190     measurement_corrections::V1_1::MeasurementCorrections mockCorrections_1_1 = {
191             .v1_0 = mockCorrections_1_0,
192             .hasEnvironmentBearing = true,
193             .environmentBearingDegrees = 45.0,
194             .environmentBearingUncertaintyDegrees = 4.0,
195             .satCorrections = singleSatCorrections,
196     };
197     return mockCorrections_1_1;
198 }
199 
200 namespace {
createExcessPathInfo(float excessPathLengthMeters,float excessPathLengthUncertaintyMeters,const ReflectingPlaneAidl * reflectingPlane,float attenuationDb)201 const ExcessPathInfo createExcessPathInfo(float excessPathLengthMeters,
202                                           float excessPathLengthUncertaintyMeters,
203                                           const ReflectingPlaneAidl* reflectingPlane,
204                                           float attenuationDb) {
205     ExcessPathInfo excessPathInfo;
206     excessPathInfo.excessPathInfoFlags =
207             ExcessPathInfo::EXCESS_PATH_INFO_HAS_EXCESS_PATH_LENGTH |
208             ExcessPathInfo::EXCESS_PATH_INFO_HAS_EXCESS_PATH_LENGTH_UNC |
209             ExcessPathInfo::EXCESS_PATH_INFO_HAS_ATTENUATION |
210             (reflectingPlane == nullptr ? 0
211                                         : ExcessPathInfo::EXCESS_PATH_INFO_HAS_REFLECTING_PLANE);
212     excessPathInfo.excessPathLengthMeters = excessPathLengthMeters;
213     excessPathInfo.excessPathLengthUncertaintyMeters = excessPathLengthUncertaintyMeters;
214     if (reflectingPlane != nullptr) {
215         excessPathInfo.reflectingPlane = *reflectingPlane;
216     }
217     excessPathInfo.attenuationDb = attenuationDb;
218     return excessPathInfo;
219 }
220 }  // anonymous namespace
221 
getMockMeasurementCorrections_aidl()222 const MeasurementCorrectionsAidl Utils::getMockMeasurementCorrections_aidl() {
223     ReflectingPlaneAidl reflectingPlane;
224     reflectingPlane.latitudeDegrees = 37.4220039;
225     reflectingPlane.longitudeDegrees = -122.0840991;
226     reflectingPlane.altitudeMeters = 250.35;
227     reflectingPlane.reflectingPlaneAzimuthDegrees = 203.0;
228 
229     SingleSatCorrectionAidl singleSatCorrection1;
230     singleSatCorrection1.singleSatCorrectionFlags =
231             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_SAT_IS_LOS_PROBABILITY |
232             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_COMBINED_EXCESS_PATH_LENGTH |
233             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_COMBINED_EXCESS_PATH_LENGTH_UNC |
234             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_COMBINED_ATTENUATION;
235     singleSatCorrection1.constellation = android::hardware::gnss::GnssConstellationType::GPS;
236     singleSatCorrection1.svid = 12;
237     singleSatCorrection1.carrierFrequencyHz = 1.59975e+09;
238     singleSatCorrection1.probSatIsLos = 0.50001;
239     singleSatCorrection1.combinedExcessPathLengthMeters = 203.5;
240     singleSatCorrection1.combinedExcessPathLengthUncertaintyMeters = 59.1;
241     singleSatCorrection1.combinedAttenuationDb = -4.3;
242     singleSatCorrection1.excessPathInfos.push_back(
243             createExcessPathInfo(137.4, 25.5, &reflectingPlane, -3.5));
244     singleSatCorrection1.excessPathInfos.push_back(
245             createExcessPathInfo(296.3, 87.2, &reflectingPlane, -5.1));
246 
247     SingleSatCorrectionAidl singleSatCorrection2;
248     singleSatCorrection2.singleSatCorrectionFlags =
249             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_SAT_IS_LOS_PROBABILITY |
250             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_COMBINED_EXCESS_PATH_LENGTH |
251             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_COMBINED_EXCESS_PATH_LENGTH_UNC |
252             SingleSatCorrectionAidl::SINGLE_SAT_CORRECTION_HAS_COMBINED_ATTENUATION;
253     singleSatCorrection2.constellation = GnssConstellationType::GPS;
254     singleSatCorrection2.svid = 9;
255     singleSatCorrection2.carrierFrequencyHz = 1.59975e+09;
256     singleSatCorrection2.probSatIsLos = 0.873;
257     singleSatCorrection2.combinedExcessPathLengthMeters = 26.294;
258     singleSatCorrection2.combinedExcessPathLengthUncertaintyMeters = 10.0;
259     singleSatCorrection2.combinedAttenuationDb = -0.5;
260     singleSatCorrection2.excessPathInfos.push_back(
261             createExcessPathInfo(26.294, 10.0, nullptr, -0.5));
262 
263     std::vector<SingleSatCorrectionAidl> singleSatCorrections = {singleSatCorrection1,
264                                                                  singleSatCorrection2};
265     MeasurementCorrectionsAidl mockCorrections;
266     mockCorrections.latitudeDegrees = 37.4219999;
267     mockCorrections.longitudeDegrees = -122.0840575;
268     mockCorrections.altitudeMeters = 30.60062531;
269     mockCorrections.horizontalPositionUncertaintyMeters = 9.23542;
270     mockCorrections.verticalPositionUncertaintyMeters = 15.02341;
271     mockCorrections.toaGpsNanosecondsOfWeek = 2935633453L;
272     mockCorrections.hasEnvironmentBearing = true;
273     mockCorrections.environmentBearingDegrees = 45.0;
274     mockCorrections.environmentBearingUncertaintyDegrees = 4.0;
275     mockCorrections.satCorrections = singleSatCorrections;
276 
277     return mockCorrections;
278 }
279 
280 /*
281  * MapConstellationType:
282  * Given a GnssConstellationType_2_0 type constellation, maps to its equivalent
283  * GnssConstellationType_1_0 type constellation. For constellations that do not have
284  * an equivalent value, maps to GnssConstellationType_1_0::UNKNOWN
285  */
mapConstellationType(V2_0::GnssConstellationType constellation)286 V1_0::GnssConstellationType Utils::mapConstellationType(V2_0::GnssConstellationType constellation) {
287     switch (constellation) {
288         case V2_0::GnssConstellationType::GPS:
289             return V1_0::GnssConstellationType::GPS;
290         case V2_0::GnssConstellationType::SBAS:
291             return V1_0::GnssConstellationType::SBAS;
292         case V2_0::GnssConstellationType::GLONASS:
293             return V1_0::GnssConstellationType::GLONASS;
294         case V2_0::GnssConstellationType::QZSS:
295             return V1_0::GnssConstellationType::QZSS;
296         case V2_0::GnssConstellationType::BEIDOU:
297             return V1_0::GnssConstellationType::BEIDOU;
298         case V2_0::GnssConstellationType::GALILEO:
299             return V1_0::GnssConstellationType::GALILEO;
300         default:
301             return V1_0::GnssConstellationType::UNKNOWN;
302     }
303 }
304 
isAutomotiveDevice()305 bool Utils::isAutomotiveDevice() {
306     char buffer[PROPERTY_VALUE_MAX] = {0};
307     property_get("ro.hardware.type", buffer, "");
308     return strncmp(buffer, "automotive", PROPERTY_VALUE_MAX) == 0;
309 }
310 
distanceMeters(double lat1,double lon1,double lat2,double lon2)311 double Utils::distanceMeters(double lat1, double lon1, double lat2, double lon2) {
312     double R = 6378.137;  // Radius of earth in KM
313     double dLat = lat2 * M_PI / 180 - lat1 * M_PI / 180;
314     double dLon = lon2 * M_PI / 180 - lon1 * M_PI / 180;
315     double a = sin(dLat / 2) * sin(dLat / 2) +
316                cos(lat1 * M_PI / 180) * cos(lat2 * M_PI / 180) * sin(dLon / 2) * sin(dLon / 2);
317     double c = 2 * atan2(sqrt(a), sqrt(1 - a));
318     double d = R * c;
319     return d * 1000;  // meters
320 }
321 
322 }  // namespace common
323 }  // namespace gnss
324 }  // namespace hardware
325 }  // namespace android
326