1 /*
2  * Copyright (C) 2020 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 <chrono>
18 #include <log/log.h>
19 #include <utils/SystemClock.h>
20 #include "gnss_hw_listener.h"
21 #include "util.h"
22 
23 namespace goldfish {
24 namespace {
testNmeaField(const char * i,const char * end,const char * v,const char sep)25 const char* testNmeaField(const char* i, const char* end,
26                           const char* v,
27                           const char sep) {
28     while (i < end) {
29         if (*v == 0) {
30             return (*i == sep) ? (i + 1) : nullptr;
31         } else if (*v == *i) {
32             ++v;
33             ++i;
34         } else {
35             return nullptr;
36         }
37     }
38 
39     return nullptr;
40 }
41 
skipAfter(const char * i,const char * end,const char c)42 const char* skipAfter(const char* i, const char* end, const char c) {
43     for (; i < end; ++i) {
44         if (*i == c) {
45             return i + 1;
46         }
47     }
48     return nullptr;
49 }
50 
convertDMMF(const int dmm,const int f,int p10)51 double convertDMMF(const int dmm, const int f, int p10) {
52     const int d = dmm / 100;
53     const int m = dmm % 100;
54     int base10 = 1;
55     for (; p10 > 0; --p10) { base10 *= 10; }
56 
57     return double(d) + (m + (f / double(base10))) / 60.0;
58 }
59 
sign(char m,char positive)60 double sign(char m, char positive) { return (m == positive) ? 1.0 : -1; }
61 
62 }  // namespace
63 
GnssHwListener(const DataSink * sink)64 GnssHwListener::GnssHwListener(const DataSink* sink): m_sink(sink) {
65     m_buffer.reserve(256);
66 }
67 
reset()68 void GnssHwListener::reset() {
69     m_buffer.clear();
70 }
71 
consume(char c)72 void GnssHwListener::consume(char c) {
73     if (c == '$' || !m_buffer.empty()) {
74         m_buffer.push_back(c);
75     }
76     if (c == '\n') {
77         const ahg20::ElapsedRealtime ts = util::makeElapsedRealtime(util::nowNanos());
78 
79         if (parse(m_buffer.data() + 1, m_buffer.data() + m_buffer.size() - 2, ts)) {
80             m_sink->gnssNmea(ts.timestampNs / 1000000,
81                              hidl_string(m_buffer.data(), m_buffer.size()));
82         } else {
83             m_buffer.back() = 0;
84             ALOGW("%s:%d: failed to parse an NMEA message, '%s'",
85                   __PRETTY_FUNCTION__, __LINE__, m_buffer.data());
86         }
87         m_buffer.clear();
88     } else if (m_buffer.size() >= 1024) {
89         ALOGW("%s:%d buffer was too long, dropped", __PRETTY_FUNCTION__, __LINE__);
90         m_buffer.clear();
91     }
92 }
93 
parse(const char * begin,const char * end,const ahg20::ElapsedRealtime & ts)94 bool GnssHwListener::parse(const char* begin, const char* end,
95                            const ahg20::ElapsedRealtime& ts) {
96     if (const char* fields = testNmeaField(begin, end, "GPRMC", ',')) {
97         return parseGPRMC(fields, end, ts);
98     } else if (const char* fields = testNmeaField(begin, end, "GPGGA", ',')) {
99         return parseGPGGA(fields, end, ts);
100     } else {
101         return false;
102     }
103 }
104 
105 //        begin                                                          end
106 // $GPRMC,195206,A,1000.0000,N,10000.0000,E,173.8,231.8,010420,004.2,W*47
107 //          1    2    3      4    5       6     7     8      9    10 11 12
108 //      1  195206     Time Stamp
109 //      2  A          validity - A-ok, V-invalid
110 //      3  1000.0000  current Latitude
111 //      4  N          North/South
112 //      5  10000.0000 current Longitude
113 //      6  E          East/West
114 //      7  173.8      Speed in knots
115 //      8  231.8      True course
116 //      9  010420     Date Stamp (13 June 1994)
117 //     10  004.2      Variation
118 //     11  W          East/West
119 //     12  *70        checksum
parseGPRMC(const char * begin,const char *,const ahg20::ElapsedRealtime & ts)120 bool GnssHwListener::parseGPRMC(const char* begin, const char*,
121                                 const ahg20::ElapsedRealtime& ts) {
122     double speedKnots = 0;
123     double course = 0;
124     double variation = 0;
125     int latdmm = 0;
126     int londmm = 0;
127     int latf = 0;
128     int lonf = 0;
129     int latdmmConsumed = 0;
130     int latfConsumed = 0;
131     int londmmConsumed = 0;
132     int lonfConsumed = 0;
133     int hhmmss = -1;
134     int ddmoyy = 0;
135     char validity = 0;
136     char ns = 0;    // north/south
137     char ew = 0;    // east/west
138     char var_ew = 0;
139 
140     if (sscanf(begin, "%06d,%c,%d.%n%d%n,%c,%d.%n%d%n,%c,%lf,%lf,%d,%lf,%c*",
141                &hhmmss, &validity,
142                &latdmm, &latdmmConsumed, &latf, &latfConsumed, &ns,
143                &londmm, &londmmConsumed, &lonf, &lonfConsumed, &ew,
144                &speedKnots, &course,
145                &ddmoyy,
146                &variation, &var_ew) != 13) {
147         return false;
148     }
149     if (validity != 'A') {
150         return false;
151     }
152 
153     const double lat = convertDMMF(latdmm, latf, latfConsumed - latdmmConsumed) * sign(ns, 'N');
154     const double lon = convertDMMF(londmm, lonf, lonfConsumed - londmmConsumed) * sign(ew, 'E');
155     const double speed = speedKnots * 0.514444;
156 
157     ahg20::GnssLocation loc20;
158     loc20.elapsedRealtime = ts;
159 
160     auto& loc10 = loc20.v1_0;
161 
162     loc10.latitudeDegrees = lat;
163     loc10.longitudeDegrees = lon;
164     loc10.speedMetersPerSec = speed;
165     loc10.bearingDegrees = course;
166     loc10.horizontalAccuracyMeters = 5;
167     loc10.speedAccuracyMetersPerSecond = .5;
168     loc10.bearingAccuracyDegrees = 30;
169     loc10.timestamp = ts.timestampNs / 1000000;
170 
171     using ahg10::GnssLocationFlags;
172     loc10.gnssLocationFlags =
173         GnssLocationFlags::HAS_LAT_LONG |
174         GnssLocationFlags::HAS_SPEED |
175         GnssLocationFlags::HAS_BEARING |
176         GnssLocationFlags::HAS_HORIZONTAL_ACCURACY |
177         GnssLocationFlags::HAS_SPEED_ACCURACY |
178         GnssLocationFlags::HAS_BEARING_ACCURACY;
179 
180     if (m_flags & GnssLocationFlags::HAS_ALTITUDE) {
181         loc10.altitudeMeters = m_altitude;
182         loc10.verticalAccuracyMeters = .5;
183         loc10.gnssLocationFlags |= GnssLocationFlags::HAS_ALTITUDE |
184                                    GnssLocationFlags::HAS_VERTICAL_ACCURACY;
185 
186     }
187 
188     m_sink->gnssLocation(loc20);
189     return true;
190 }
191 
192 // $GPGGA,123519,4807.0382,N,12204.9799,W,1,6,,4.2,M,0.,M,,,*47
193 //    time of fix      123519     12:35:19 UTC
194 //    latitude         4807.0382  48 degrees, 07.0382 minutes
195 //    north/south      N or S
196 //    longitude        12204.9799 122 degrees, 04.9799 minutes
197 //    east/west        E or W
198 //    fix quality      1          standard GPS fix
199 //    satellites       1 to 12    number of satellites being tracked
200 //    HDOP             <dontcare> horizontal dilution
201 //    altitude         4.2        altitude above sea-level
202 //    altitude units   M          to indicate meters
203 //    diff             <dontcare> height of sea-level above ellipsoid
204 //    diff units       M          to indicate meters (should be <dontcare>)
205 //    dgps age         <dontcare> time in seconds since last DGPS fix
206 //    dgps sid         <dontcare> DGPS station id
parseGPGGA(const char * begin,const char * end,const ahg20::ElapsedRealtime &)207 bool GnssHwListener::parseGPGGA(const char* begin, const char* end,
208                                 const ahg20::ElapsedRealtime&) {
209     double altitude = 0;
210     int latdmm = 0;
211     int londmm = 0;
212     int latf = 0;
213     int lonf = 0;
214     int latdmmConsumed = 0;
215     int latfConsumed = 0;
216     int londmmConsumed = 0;
217     int lonfConsumed = 0;
218     int hhmmss = 0;
219     int fixQuality = 0;
220     int nSatellites = 0;
221     int consumed = 0;
222     char ns = 0;
223     char ew = 0;
224     char altitudeUnit = 0;
225 
226     if (sscanf(begin, "%06d,%d.%n%d%n,%c,%d.%n%d%n,%c,%d,%d,%n",
227                &hhmmss,
228                &latdmm, &latdmmConsumed, &latf, &latfConsumed, &ns,
229                &londmm, &londmmConsumed, &lonf, &lonfConsumed, &ew,
230                &fixQuality,
231                &nSatellites,
232                &consumed) != 9) {
233         return false;
234     }
235 
236     begin = skipAfter(begin + consumed, end, ',');  // skip HDOP
237     if (!begin) {
238         return false;
239     }
240     if (sscanf(begin, "%lf,%c,", &altitude, &altitudeUnit) != 2) {
241         return false;
242     }
243     if (altitudeUnit != 'M') {
244         return false;
245     }
246 
247     m_altitude = altitude;
248     m_flags |= ahg10::GnssLocationFlags::HAS_ALTITUDE;
249 
250     hidl_vec<ahg20::IGnssCallback::GnssSvInfo> svInfo(nSatellites);
251     for (int i = 0; i < nSatellites; ++i) {
252         auto* info20 = &svInfo[i];
253         auto* info10 = &info20->v1_0;
254 
255         info20->constellation = ahg20::GnssConstellationType::GPS;
256         info10->svid = i + 3;
257         info10->constellation = ahg10::GnssConstellationType::GPS;
258         info10->cN0Dbhz = 30;
259         info10->elevationDegrees = 0;
260         info10->azimuthDegrees = 0;
261         info10->carrierFrequencyHz = 1.59975e+09;
262         info10->svFlag = ahg10::IGnssCallback::GnssSvFlags::HAS_CARRIER_FREQUENCY | 0;
263     }
264 
265     m_sink->gnssSvStatus(svInfo);
266 
267     return true;
268 }
269 
270 }  // namespace goldfish
271