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