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