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 
NmeaFixInfo()37 NmeaFixInfo::NmeaFixInfo() : hasGMCRecord(false), hasGGARecord(false) {}
38 
getAltitudeMeters() const39 float NmeaFixInfo::getAltitudeMeters() const {
40     return altitudeMeters;
41 }
42 
checkAndConvertToFloat(const std::string & sentence)43 float 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 
getBearingAccuracyDegrees() const50 float NmeaFixInfo::getBearingAccuracyDegrees() const {
51     // Current NMEA doesn't contains beaing accuracy inforamtion
52     return kMockBearingAccuracyDegrees;
53 }
getBearingDegrees() const54 float NmeaFixInfo::getBearingDegrees() const {
55     return bearingDegrees;
56 }
57 
getHorizontalAccuracyMeters() const58 float NmeaFixInfo::getHorizontalAccuracyMeters() const {
59     // Current NMEA doesn't contains horizontal accuracy inforamtion
60     return kMockHorizontalAccuracyMeters;
61 }
62 
getLatDeg() const63 float NmeaFixInfo::getLatDeg() const {
64     return latDeg;
65 }
66 
getLngDeg() const67 float NmeaFixInfo::getLngDeg() const {
68     return lngDeg;
69 }
70 
getSpeedAccuracyMetersPerSecond() const71 float NmeaFixInfo::getSpeedAccuracyMetersPerSecond() const {
72     // Current NMEA doesn't contains speed accuracy inforamtion
73     return kMockSpeedAccuracyMetersPerSecond;
74 }
75 
getSpeedMetersPerSec() const76 float NmeaFixInfo::getSpeedMetersPerSec() const {
77     return speedMetersPerSec;
78 }
79 
getTimestamp() const80 int64_t NmeaFixInfo::getTimestamp() const {
81     return timestamp;
82 }
83 
getVerticalAccuracyMeters() const84 float NmeaFixInfo::getVerticalAccuracyMeters() const {
85     // Current NMEA doesn't contains vertical accuracy inforamtion
86     return kMockVerticalAccuracyMeters;
87 }
88 
nmeaPartsToTimestamp(const std::string & timeStr,const std::string & dateStr)89 int64_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 
isValidFix() const109 bool NmeaFixInfo::isValidFix() const {
110     return hasGMCRecord && hasGGARecord;
111 }
112 
parseGGALine(const std::vector<std::string> & sentenceValues)113 void 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 
parseRMCLine(const std::vector<std::string> & sentenceValues)138 void 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 */
reset()149 void 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 
splitStr(const std::string & line,const char & delimiter,std::vector<std::string> & out)164 void 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 
operator =(const NmeaFixInfo & rhs)173 NmeaFixInfo& 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  */
getLocationFromInputStr(const std::string & inputStr)196 std::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         if (line.compare(0, strlen(GPGA_RECORD_TAG), GPGA_RECORD_TAG) != 0 &&
206             line.compare(0, strlen(GPRMC_RECORD_TAG), GPRMC_RECORD_TAG) != 0) {
207             continue;
208         }
209         std::vector<std::string> sentenceValues;
210         splitStr(line, COMMA_SEPARATOR, sentenceValues);
211         if (sentenceValues.size() < MIN_COL_NUM) {
212             continue;
213         }
214         double currentTimeStamp = std::stof(sentenceValues[1]);
215         // If see a new timestamp, report correct location.
216         if ((currentTimeStamp - lastTimeStamp) > TIMESTAMP_EPSILON &&
217             candidateFixInfo.isValidFix()) {
218             nmeaFixInfo = candidateFixInfo;
219             candidateFixInfo.reset();
220             fixId++;
221         }
222         if (line.compare(0, strlen(GPGA_RECORD_TAG), GPGA_RECORD_TAG) == 0) {
223             candidateFixInfo.fixId = fixId;
224             candidateFixInfo.parseGGALine(sentenceValues);
225         } else if (line.compare(0, strlen(GPRMC_RECORD_TAG), GPRMC_RECORD_TAG) == 0) {
226             candidateFixInfo.parseRMCLine(sentenceValues);
227         }
228     }
229     if (candidateFixInfo.isValidFix()) {
230         nmeaFixInfo = candidateFixInfo;
231         candidateFixInfo.reset();
232     }
233     if (!nmeaFixInfo.isValidFix()) {
234         return nullptr;
235     }
236     return nmeaFixInfo.toGnssLocation();
237 }
238 
239 /**
240  * Parses the input string in NMEA format and convert to GnssLocation.
241  */
toGnssLocation() const242 std::unique_ptr<V2_0::GnssLocation> NmeaFixInfo::toGnssLocation() const {
243     const V2_0::ElapsedRealtime currentOsTimestamp = {
244             .flags = V2_0::ElapsedRealtimeFlags::HAS_TIMESTAMP_NS |
245                      V2_0::ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS,
246             .timestampNs = static_cast<uint64_t>(::android::elapsedRealtimeNano()),
247             // This is an hardcoded value indicating a 1ms of uncertainty between the two clocks.
248             // In an actual implementation provide an estimate of the synchronization uncertainty
249             // or don't set the field.
250             .timeUncertaintyNs = 1000000};
251 
252     V1_0::GnssLocation locationV1 = {
253             .gnssLocationFlags = 0xFF,
254             .latitudeDegrees = this->getLatDeg(),
255             .longitudeDegrees = this->getLngDeg(),
256             .altitudeMeters = this->getAltitudeMeters(),
257             .speedMetersPerSec = this->getSpeedMetersPerSec(),
258             .bearingDegrees = this->getBearingDegrees(),
259             .horizontalAccuracyMeters = this->getHorizontalAccuracyMeters(),
260             .verticalAccuracyMeters = this->getVerticalAccuracyMeters(),
261             .speedAccuracyMetersPerSecond = this->getSpeedAccuracyMetersPerSecond(),
262             .bearingAccuracyDegrees = this->getBearingAccuracyDegrees(),
263             .timestamp = this->getTimestamp()};
264 
265     V2_0::GnssLocation locationV2 = {.v1_0 = locationV1, .elapsedRealtime = currentOsTimestamp};
266 
267     return std::make_unique<V2_0::GnssLocation>(locationV2);
268 }
269 
270 }  // namespace common
271 }  // namespace gnss
272 }  // namespace hardware
273 }  // namespace android