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 #define LOG_TAG "NmeaFixInfo"
18
19 #include <Constants.h>
20 #include <NmeaFixInfo.h>
21 #include <Utils.h>
22 #include <log/log.h>
23 #include <sys/stat.h>
24 #include <sys/types.h>
25 #include <unistd.h>
26 #include <utils/SystemClock.h>
27 #include <limits>
28 #include <sstream>
29 #include <string>
30 #include <vector>
31
32 namespace android {
33 namespace hardware {
34 namespace gnss {
35 namespace common {
36
37 using aidl::android::hardware::gnss::ElapsedRealtime;
38 using aidl::android::hardware::gnss::GnssLocation;
39
NmeaFixInfo()40 NmeaFixInfo::NmeaFixInfo() : hasGMCRecord(false), hasGGARecord(false) {}
41
getAltitudeMeters() const42 float NmeaFixInfo::getAltitudeMeters() const {
43 return altitudeMeters;
44 }
45
checkAndConvertToFloat(const std::string & sentence)46 float NmeaFixInfo::checkAndConvertToFloat(const std::string& sentence) {
47 if (sentence.empty()) {
48 return std::numeric_limits<float>::quiet_NaN();
49 }
50 return std::stof(sentence);
51 }
52
getBearingAccuracyDegrees() const53 float NmeaFixInfo::getBearingAccuracyDegrees() const {
54 // Current NMEA doesn't contains beaing accuracy inforamtion
55 return kMockBearingAccuracyDegrees;
56 }
getBearingDegrees() const57 float NmeaFixInfo::getBearingDegrees() const {
58 return bearingDegrees;
59 }
60
getHorizontalAccuracyMeters() const61 float NmeaFixInfo::getHorizontalAccuracyMeters() const {
62 // Current NMEA doesn't contains horizontal accuracy inforamtion
63 return kMockHorizontalAccuracyMeters;
64 }
65
getLatDeg() const66 float NmeaFixInfo::getLatDeg() const {
67 return latDeg;
68 }
69
getLngDeg() const70 float NmeaFixInfo::getLngDeg() const {
71 return lngDeg;
72 }
73
getSpeedAccuracyMetersPerSecond() const74 float NmeaFixInfo::getSpeedAccuracyMetersPerSecond() const {
75 // Current NMEA doesn't contains speed accuracy inforamtion
76 return kMockSpeedAccuracyMetersPerSecond;
77 }
78
getSpeedMetersPerSec() const79 float NmeaFixInfo::getSpeedMetersPerSec() const {
80 return speedMetersPerSec;
81 }
82
getTimestamp() const83 int64_t NmeaFixInfo::getTimestamp() const {
84 return timestamp;
85 }
86
getVerticalAccuracyMeters() const87 float NmeaFixInfo::getVerticalAccuracyMeters() const {
88 // Current NMEA doesn't contains vertical accuracy inforamtion
89 return kMockVerticalAccuracyMeters;
90 }
91
nmeaPartsToTimestamp(const std::string & timeStr,const std::string & dateStr)92 int64_t NmeaFixInfo::nmeaPartsToTimestamp(const std::string& timeStr, const std::string& dateStr) {
93 /**
94 * In NMEA format, the full time can only get from the $GPRMC record, see
95 * the following example:
96 * $GPRMC,213204.00,A,3725.371240,N,12205.589239,W,000.0,000.0,290819,,,A*49
97 * the datetime is stored in two parts, 213204 and 290819, which means
98 * 2019/08/29 21:32:04, however for in unix the year starts from 1900, we
99 * need to add the offset.
100 */
101 struct tm tm;
102 const int32_t unixYearOffset = 100;
103 tm.tm_mday = std::stoi(dateStr.substr(0, 2).c_str());
104 tm.tm_mon = std::stoi(dateStr.substr(2, 2).c_str()) - 1;
105 tm.tm_year = std::stoi(dateStr.substr(4, 2).c_str()) + unixYearOffset;
106 tm.tm_hour = std::stoi(timeStr.substr(0, 2).c_str());
107 tm.tm_min = std::stoi(timeStr.substr(2, 2).c_str());
108 tm.tm_sec = std::stoi(timeStr.substr(4, 2).c_str());
109 return static_cast<int64_t>(mktime(&tm) - timezone);
110 }
111
isValidFix() const112 bool NmeaFixInfo::isValidFix() const {
113 return hasGMCRecord && hasGGARecord;
114 }
115
parseGGALine(const std::vector<std::string> & sentenceValues)116 void NmeaFixInfo::parseGGALine(const std::vector<std::string>& sentenceValues) {
117 if (sentenceValues.size() == 0 || sentenceValues[0].compare(GPGA_RECORD_TAG) != 0) {
118 return;
119 }
120 // LatDeg, need covert to degree, if it is 'N', should be negative value
121 this->latDeg = std::stof(sentenceValues[2].substr(0, 2)) +
122 (std::stof(sentenceValues[2].substr(2)) / 60.0);
123 if (sentenceValues[3].compare("N") != 0) {
124 this->latDeg *= -1;
125 }
126
127 // LngDeg, need covert to degree, if it is 'E', should be negative value
128 this->lngDeg = std::stof(sentenceValues[4].substr(0, 3)) +
129 std::stof(sentenceValues[4].substr(3)) / 60.0;
130 if (sentenceValues[5].compare("E") != 0) {
131 this->lngDeg *= -1;
132 }
133
134 this->altitudeMeters = std::stof(sentenceValues[9]);
135
136 this->hDop = sentenceValues[8].empty() ? std::numeric_limits<float>::quiet_NaN()
137 : std::stof(sentenceValues[8]);
138 this->hasGGARecord = true;
139 }
140
parseRMCLine(const std::vector<std::string> & sentenceValues)141 void NmeaFixInfo::parseRMCLine(const std::vector<std::string>& sentenceValues) {
142 if (sentenceValues.size() == 0 || sentenceValues[0].compare(GPRMC_RECORD_TAG) != 0) {
143 return;
144 }
145 this->speedMetersPerSec = checkAndConvertToFloat(sentenceValues[7]);
146 this->bearingDegrees = checkAndConvertToFloat(sentenceValues[8]);
147 this->timestamp = nmeaPartsToTimestamp(sentenceValues[1], sentenceValues[9]);
148 this->hasGMCRecord = true;
149 }
150
151 /** invalid the current NmeaFixInfo */
reset()152 void NmeaFixInfo::reset() {
153 this->altitudeMeters = 0;
154 this->bearingDegrees = 0;
155 this->fixId = 0;
156 this->hasGMCRecord = false;
157 this->hasGGARecord = false;
158 this->latDeg = 0;
159 this->lngDeg = 0;
160 this->hDop = 0;
161 this->vDop = 0;
162 this->satelliteCount = 0;
163 this->speedMetersPerSec = 0;
164 this->timestamp = 0;
165 }
166
splitStr(const std::string & line,const char & delimiter,std::vector<std::string> & out)167 void NmeaFixInfo::splitStr(const std::string& line, const char& delimiter,
168 std::vector<std::string>& out) {
169 std::istringstream iss(line);
170 std::string item;
171 while (std::getline(iss, item, delimiter)) {
172 out.push_back(item);
173 }
174 }
175
operator =(const NmeaFixInfo & rhs)176 NmeaFixInfo& NmeaFixInfo::operator=(const NmeaFixInfo& rhs) {
177 if (this == &rhs) return *this;
178 this->altitudeMeters = rhs.altitudeMeters;
179 this->bearingDegrees = rhs.bearingDegrees;
180 this->fixId = rhs.fixId;
181 this->hasGMCRecord = rhs.hasGMCRecord;
182 this->hasGGARecord = rhs.hasGGARecord;
183 this->hDop = rhs.hDop;
184 this->vDop = rhs.vDop;
185 this->latDeg = rhs.latDeg;
186 this->lngDeg = rhs.lngDeg;
187 this->satelliteCount = rhs.satelliteCount;
188 this->speedMetersPerSec = rhs.speedMetersPerSec;
189 this->timestamp = rhs.timestamp;
190
191 return *this;
192 }
193
194 /**
195 * Parses the input string in NMEA format and convert to GnssLocation.
196 * Currently version only cares about $GPGGA and $GPRMC records. but we
197 * can easily extend to other types supported by NMEA if needed.
198 */
getLocationFromInputStr(const std::string & inputStr)199 std::unique_ptr<V2_0::GnssLocation> NmeaFixInfo::getLocationFromInputStr(
200 const std::string& inputStr) {
201 std::vector<std::string> nmeaRecords;
202 splitStr(inputStr, LINE_SEPARATOR, nmeaRecords);
203 NmeaFixInfo nmeaFixInfo;
204 NmeaFixInfo candidateFixInfo;
205 uint32_t fixId = 0;
206 double lastTimeStamp = 0;
207 for (const auto& line : nmeaRecords) {
208 if (line.compare(0, strlen(GPGA_RECORD_TAG), GPGA_RECORD_TAG) != 0 &&
209 line.compare(0, strlen(GPRMC_RECORD_TAG), GPRMC_RECORD_TAG) != 0) {
210 continue;
211 }
212 std::vector<std::string> sentenceValues;
213 splitStr(line, COMMA_SEPARATOR, sentenceValues);
214 if (sentenceValues.size() < MIN_COL_NUM) {
215 continue;
216 }
217 double currentTimeStamp = std::stof(sentenceValues[1]);
218 // If see a new timestamp, report correct location.
219 if ((currentTimeStamp - lastTimeStamp) > TIMESTAMP_EPSILON &&
220 candidateFixInfo.isValidFix()) {
221 nmeaFixInfo = candidateFixInfo;
222 candidateFixInfo.reset();
223 fixId++;
224 }
225 if (line.compare(0, strlen(GPGA_RECORD_TAG), GPGA_RECORD_TAG) == 0) {
226 candidateFixInfo.fixId = fixId;
227 candidateFixInfo.parseGGALine(sentenceValues);
228 } else if (line.compare(0, strlen(GPRMC_RECORD_TAG), GPRMC_RECORD_TAG) == 0) {
229 candidateFixInfo.parseRMCLine(sentenceValues);
230 }
231 }
232 if (candidateFixInfo.isValidFix()) {
233 nmeaFixInfo = candidateFixInfo;
234 candidateFixInfo.reset();
235 }
236 if (!nmeaFixInfo.isValidFix()) {
237 return nullptr;
238 }
239 return nmeaFixInfo.toGnssLocation();
240 }
241
242 /**
243 * Convert V2_0::GnssLocation to aidl::GnssLocation.
244 */
getAidlLocationFromInputStr(const std::string & inputStr)245 std::unique_ptr<GnssLocation> NmeaFixInfo::getAidlLocationFromInputStr(
246 const std::string& inputStr) {
247 std::unique_ptr<V2_0::GnssLocation> locationV2 = getLocationFromInputStr(inputStr);
248 if (locationV2 == nullptr) {
249 return nullptr;
250 }
251
252 ElapsedRealtime elapsedRealtime = {
253 .flags = ElapsedRealtime::HAS_TIMESTAMP_NS | ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS,
254 .timestampNs = ::android::elapsedRealtimeNano(),
255 // This is an hardcoded value indicating a 1ms of uncertainty between the two clocks.
256 // In an actual implementation provide an estimate of the synchronization uncertainty
257 // or don't set the field.
258 .timeUncertaintyNs = 1020400};
259
260 GnssLocation location = {
261 .gnssLocationFlags = locationV2->v1_0.gnssLocationFlags,
262 .latitudeDegrees = locationV2->v1_0.latitudeDegrees,
263 .longitudeDegrees = locationV2->v1_0.longitudeDegrees,
264 .altitudeMeters = locationV2->v1_0.altitudeMeters,
265 .speedMetersPerSec = locationV2->v1_0.speedMetersPerSec,
266 .bearingDegrees = locationV2->v1_0.bearingDegrees,
267 .horizontalAccuracyMeters = locationV2->v1_0.horizontalAccuracyMeters,
268 .verticalAccuracyMeters = locationV2->v1_0.verticalAccuracyMeters,
269 .speedAccuracyMetersPerSecond = locationV2->v1_0.speedAccuracyMetersPerSecond,
270 .bearingAccuracyDegrees = locationV2->v1_0.bearingAccuracyDegrees,
271 .timestampMillis = locationV2->v1_0.timestamp,
272 .elapsedRealtime = elapsedRealtime};
273 return std::make_unique<GnssLocation>(location);
274 }
275
276 /**
277 * Parses the input string in NMEA format and convert to GnssLocation.
278 */
toGnssLocation() const279 std::unique_ptr<V2_0::GnssLocation> NmeaFixInfo::toGnssLocation() const {
280 const V2_0::ElapsedRealtime currentOsTimestamp = {
281 .flags = V2_0::ElapsedRealtimeFlags::HAS_TIMESTAMP_NS |
282 V2_0::ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS,
283 .timestampNs = static_cast<uint64_t>(::android::elapsedRealtimeNano()),
284 // This is an hardcoded value indicating a 1ms of uncertainty between the two clocks.
285 // In an actual implementation provide an estimate of the synchronization uncertainty
286 // or don't set the field.
287 .timeUncertaintyNs = 1000000};
288
289 V1_0::GnssLocation locationV1 = {
290 .gnssLocationFlags = 0xFF,
291 .latitudeDegrees = this->getLatDeg(),
292 .longitudeDegrees = this->getLngDeg(),
293 .altitudeMeters = this->getAltitudeMeters(),
294 .speedMetersPerSec = this->getSpeedMetersPerSec(),
295 .bearingDegrees = this->getBearingDegrees(),
296 .horizontalAccuracyMeters = this->getHorizontalAccuracyMeters(),
297 .verticalAccuracyMeters = this->getVerticalAccuracyMeters(),
298 .speedAccuracyMetersPerSecond = this->getSpeedAccuracyMetersPerSecond(),
299 .bearingAccuracyDegrees = this->getBearingAccuracyDegrees(),
300 .timestamp = this->getTimestamp()};
301
302 V2_0::GnssLocation locationV2 = {.v1_0 = locationV1, .elapsedRealtime = currentOsTimestamp};
303
304 return std::make_unique<V2_0::GnssLocation>(locationV2);
305 }
306
307 } // namespace common
308 } // namespace gnss
309 } // namespace hardware
310 } // namespace android