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