Yipeng Cao | 48618f6 | 2020-03-19 18:11:16 -0700 | [diff] [blame^] | 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 | NmeaFixInfo::NmeaFixInfo() : hasGMCRecord(false), hasGGARecord(false) {} |
| 38 | |
| 39 | float NmeaFixInfo::getAltitudeMeters() const { |
| 40 | return altitudeMeters; |
| 41 | } |
| 42 | |
| 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 | |
| 50 | float NmeaFixInfo::getBearingAccuracyDegrees() const { |
| 51 | // Current NMEA doesn't contains beaing accuracy inforamtion |
| 52 | return kMockBearingAccuracyDegrees; |
| 53 | } |
| 54 | float NmeaFixInfo::getBearingDegrees() const { |
| 55 | return bearingDegrees; |
| 56 | } |
| 57 | |
| 58 | float NmeaFixInfo::getHorizontalAccuracyMeters() const { |
| 59 | // Current NMEA doesn't contains horizontal accuracy inforamtion |
| 60 | return kMockHorizontalAccuracyMeters; |
| 61 | } |
| 62 | |
| 63 | float NmeaFixInfo::getLatDeg() const { |
| 64 | return latDeg; |
| 65 | } |
| 66 | |
| 67 | float NmeaFixInfo::getLngDeg() const { |
| 68 | return lngDeg; |
| 69 | } |
| 70 | |
| 71 | float NmeaFixInfo::getSpeedAccuracyMetersPerSecond() const { |
| 72 | // Current NMEA doesn't contains speed accuracy inforamtion |
| 73 | return kMockSpeedAccuracyMetersPerSecond; |
| 74 | } |
| 75 | |
| 76 | float NmeaFixInfo::getSpeedMetersPerSec() const { |
| 77 | return speedMetersPerSec; |
| 78 | } |
| 79 | |
| 80 | int64_t NmeaFixInfo::getTimestamp() const { |
| 81 | return timestamp; |
| 82 | } |
| 83 | |
| 84 | float NmeaFixInfo::getVerticalAccuracyMeters() const { |
| 85 | // Current NMEA doesn't contains vertical accuracy inforamtion |
| 86 | return kMockVerticalAccuracyMeters; |
| 87 | } |
| 88 | |
| 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 | |
| 109 | bool NmeaFixInfo::isValidFix() const { |
| 110 | return hasGMCRecord && hasGGARecord; |
| 111 | } |
| 112 | |
| 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 | |
| 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 */ |
| 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 | |
| 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 | |
| 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 | */ |
| 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 | 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 | */ |
| 235 | std::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 |