/* * Copyright (C) 2020 The Android Open Source Project * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include #include #include #include "GnssHwListener.h" namespace aidl { namespace android { namespace hardware { namespace gnss { namespace implementation { namespace { const char* testNmeaField(const char* i, const char* end, const char* v, const char sep) { while (i < end) { if (*v == 0) { return (*i == sep) ? (i + 1) : nullptr; } else if (*v == *i) { ++v; ++i; } else { return nullptr; } } return nullptr; } const char* skipAfter(const char* i, const char* end, const char c) { for (; i < end; ++i) { if (*i == c) { return i + 1; } } return nullptr; } double convertDMMF(const int dmm, const int f, int p10) { const int d = dmm / 100; const int m = dmm % 100; int base10 = 1; for (; p10 > 0; --p10) { base10 *= 10; } return double(d) + (m + (f / double(base10))) / 60.0; } double sign(char m, char positive) { return (m == positive) ? 1.0 : -1; } ElapsedRealtime makeElapsedRealtime(const int64_t timestampNs) { return { .flags = ElapsedRealtime::HAS_TIMESTAMP_NS | ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS, .timestampNs = timestampNs, .timeUncertaintyNs = 1000000.0 }; } } // namespace GnssHwListener::GnssHwListener(IDataSink& sink): mSink(sink) { mBuffer.reserve(256); mSink.onGnssStatusCb(IGnssCallback::GnssStatusValue::ENGINE_ON); } GnssHwListener::~GnssHwListener() { mSink.onGnssStatusCb(IGnssCallback::GnssStatusValue::ENGINE_OFF); } void GnssHwListener::consume(const char* buf, size_t sz) { ALOGD("%s:%s:%d sz=%zu", "GnssHwListener", __func__, __LINE__, sz); for (; sz > 0; ++buf, --sz) { consume1(*buf); } } void GnssHwListener::consume1(const char c) { if (c == '$' || !mBuffer.empty()) { mBuffer.push_back(c); } if (c == '\n') { using namespace std::chrono; const int64_t timestampMs = time_point_cast( system_clock::now()).time_since_epoch().count(); const ElapsedRealtime ert = makeElapsedRealtime( ::android::elapsedRealtimeNano()); if (parse(mBuffer.data() + 1, mBuffer.data() + mBuffer.size() - 2, timestampMs, ert)) { mSink.onGnssNmeaCb(timestampMs, std::string(mBuffer.data(), mBuffer.size())); } else { mBuffer.back() = 0; ALOGW("%s:%d: failed to parse an NMEA message, '%s'", __func__, __LINE__, mBuffer.data()); } mBuffer.clear(); } else if (mBuffer.size() >= 1024) { ALOGW("%s:%d buffer was too long, dropped", __func__, __LINE__); mBuffer.clear(); } } bool GnssHwListener::parse(const char* begin, const char* end, const int64_t timestampMs, const ElapsedRealtime& ert) { if (const char* fields = testNmeaField(begin, end, "GPRMC", ',')) { return parseGPRMC(fields, end, timestampMs, ert); } else if (const char* fields = testNmeaField(begin, end, "GPGGA", ',')) { return parseGPGGA(fields, end, timestampMs, ert); } else { return false; } } // begin end // $GPRMC,195206,A,1000.0000,N,10000.0000,E,173.8,231.8,010420,004.2,W*47 // 1 2 3 4 5 6 7 8 9 10 11 12 // 1 195206 Time Stamp // 2 A validity - A-ok, V-invalid // 3 1000.0000 current Latitude // 4 N North/South // 5 10000.0000 current Longitude // 6 E East/West // 7 173.8 Speed in knots // 8 231.8 True course // 9 010420 Date Stamp (13 June 1994) // 10 004.2 Variation // 11 W East/West // 12 *70 checksum bool GnssHwListener::parseGPRMC(const char* begin, const char*, const int64_t timestampMs, const ElapsedRealtime& ert) { double speedKnots = 0; double course = 0; double variation = 0; int latdmm = 0; int londmm = 0; int latf = 0; int lonf = 0; int latdmmConsumed = 0; int latfConsumed = 0; int londmmConsumed = 0; int lonfConsumed = 0; int hhmmss = -1; int ddmoyy = 0; char validity = 0; char ns = 0; // north/south char ew = 0; // east/west char var_ew = 0; if (sscanf(begin, "%06d,%c,%d.%n%d%n,%c,%d.%n%d%n,%c,%lf,%lf,%d,%lf,%c*", &hhmmss, &validity, &latdmm, &latdmmConsumed, &latf, &latfConsumed, &ns, &londmm, &londmmConsumed, &lonf, &lonfConsumed, &ew, &speedKnots, &course, &ddmoyy, &variation, &var_ew) != 13) { return false; } if (validity != 'A') { return false; } const double lat = convertDMMF(latdmm, latf, latfConsumed - latdmmConsumed) * sign(ns, 'N'); const double lon = convertDMMF(londmm, lonf, lonfConsumed - londmmConsumed) * sign(ew, 'E'); const double speed = speedKnots * 0.514444; GnssLocation loc; loc.elapsedRealtime = ert; loc.latitudeDegrees = lat; loc.longitudeDegrees = lon; loc.speedMetersPerSec = speed; loc.bearingDegrees = course; loc.horizontalAccuracyMeters = 5; loc.speedAccuracyMetersPerSecond = .5; loc.bearingAccuracyDegrees = 30; loc.timestampMillis = timestampMs; loc.gnssLocationFlags = GnssLocation::HAS_LAT_LONG | GnssLocation::HAS_SPEED | GnssLocation::HAS_BEARING | GnssLocation::HAS_HORIZONTAL_ACCURACY | GnssLocation::HAS_SPEED_ACCURACY | GnssLocation::HAS_BEARING_ACCURACY; if (mAltitude.has_value()) { loc.altitudeMeters = mAltitude.value(); loc.verticalAccuracyMeters = .5; loc.gnssLocationFlags |= GnssLocation::HAS_ALTITUDE | GnssLocation::HAS_VERTICAL_ACCURACY; } mSink.onGnssLocationCb(loc); return true; } // $GPGGA,123519,4807.0382,N,12204.9799,W,1,6,,4.2,M,0.,M,,,*47 // time of fix 123519 12:35:19 UTC // latitude 4807.0382 48 degrees, 07.0382 minutes // north/south N or S // longitude 12204.9799 122 degrees, 04.9799 minutes // east/west E or W // fix quality 1 standard GPS fix // satellites 1 to 12 number of satellites being tracked // HDOP horizontal dilution // altitude 4.2 altitude above sea-level // altitude units M to indicate meters // diff height of sea-level above ellipsoid // diff units M to indicate meters (should be ) // dgps age time in seconds since last DGPS fix // dgps sid DGPS station id bool GnssHwListener::parseGPGGA(const char* begin, const char* end, const int64_t /*timestampMs*/, const ElapsedRealtime& /*ert*/) { double altitude = 0; int latdmm = 0; int londmm = 0; int latf = 0; int lonf = 0; int latdmmConsumed = 0; int latfConsumed = 0; int londmmConsumed = 0; int lonfConsumed = 0; int hhmmss = 0; int fixQuality = 0; int nSatellites = 0; int consumed = 0; char ns = 0; char ew = 0; char altitudeUnit = 0; if (sscanf(begin, "%06d,%d.%n%d%n,%c,%d.%n%d%n,%c,%d,%d,%n", &hhmmss, &latdmm, &latdmmConsumed, &latf, &latfConsumed, &ns, &londmm, &londmmConsumed, &lonf, &lonfConsumed, &ew, &fixQuality, &nSatellites, &consumed) != 9) { return false; } begin = skipAfter(begin + consumed, end, ','); // skip HDOP if (!begin) { return false; } if (sscanf(begin, "%lf,%c,", &altitude, &altitudeUnit) != 2) { return false; } if (altitudeUnit != 'M') { return false; } mAltitude = altitude; std::vector svInfo(nSatellites); for (int i = 0; i < nSatellites; ++i) { auto* info = &svInfo[i]; info->svid = i + 3; info->constellation = GnssConstellationType::GPS; info->cN0Dbhz = 30; info->basebandCN0DbHz = 42; info->elevationDegrees = 0; info->azimuthDegrees = 0; info->carrierFrequencyHz = 1.59975e+09; info->svFlag = static_cast(IGnssCallback::GnssSvFlags::HAS_CARRIER_FREQUENCY); } mSink.onGnssSvStatusCb(std::move(svInfo)); return true; } } // namespace implementation } // namespace gnss } // namespace hardware } // namespace android } // namespace aidl