Hal layer for gnss replay
Test: Added following kernel cmdline and tested on local CF
gnss_cmdline.serdev=serial8250/serial0/serial0-0
gnss_cmdline.type=0
serdev_ttyport.pdev_tty_port=ttyS1
Change-Id: I3766c31672aa91341403105759b2fd997e7f8879
diff --git a/gnss/2.1/default/Gnss.cpp b/gnss/2.1/default/Gnss.cpp
index 2b327a9..16f2bf3 100644
--- a/gnss/2.1/default/Gnss.cpp
+++ b/gnss/2.1/default/Gnss.cpp
@@ -17,14 +17,15 @@
#define LOG_TAG "Gnss"
#include "Gnss.h"
+#include <log/log.h>
+#include <sys/epoll.h>
+#include <string>
#include "GnssAntennaInfo.h"
#include "GnssDebug.h"
#include "GnssMeasurement.h"
#include "GnssMeasurementCorrections.h"
#include "Utils.h"
-#include <log/log.h>
-
using ::android::hardware::gnss::common::Utils;
using ::android::hardware::gnss::measurement_corrections::V1_1::implementation::
GnssMeasurementCorrections;
@@ -40,14 +41,55 @@
sp<V1_1::IGnssCallback> Gnss::sGnssCallback_1_1 = nullptr;
sp<V1_0::IGnssCallback> Gnss::sGnssCallback_1_0 = nullptr;
-Gnss::Gnss() : mMinIntervalMs(1000), mGnssConfiguration{new GnssConfiguration()} {}
+Gnss::Gnss()
+ : mMinIntervalMs(1000),
+ mGnssConfiguration{new GnssConfiguration()},
+ mHardwareModeOn(false),
+ mGnssFd(-1) {}
Gnss::~Gnss() {
stop();
}
+std::unique_ptr<V2_0::GnssLocation> Gnss::getLocationFromHW() {
+ char inputBuffer[INPUT_BUFFER_SIZE];
+ mHardwareModeOn = false;
+ if (mGnssFd == -1) {
+ mGnssFd = open(GNSS_PATH, O_RDWR | O_NONBLOCK);
+ }
+ if (mGnssFd == -1) {
+ return nullptr;
+ }
+ // Send control message to device
+ int bytes_write = write(mGnssFd, CMD_GET_LOCATION, strlen(CMD_GET_LOCATION));
+ if (bytes_write <= 0) {
+ return nullptr;
+ }
+
+ struct epoll_event ev, events[1];
+ ev.data.fd = mGnssFd;
+ ev.events = EPOLLIN;
+ int epoll_fd = epoll_create1(0);
+ epoll_ctl(epoll_fd, EPOLL_CTL_ADD, mGnssFd, &ev);
+ int bytes_read = -1;
+ std::string inputStr = "";
+ int epoll_ret = epoll_wait(epoll_fd, events, 1, mMinIntervalMs);
+ // Indicates it is a hardwareMode, don't need to wait outside.
+ mHardwareModeOn = true;
+ if (epoll_ret == -1) {
+ return nullptr;
+ }
+ while (true) {
+ bytes_read = read(mGnssFd, &inputBuffer, INPUT_BUFFER_SIZE);
+ if (bytes_read <= 0) {
+ break;
+ }
+ inputStr += std::string(inputBuffer, bytes_read);
+ }
+ return NmeaFixInfo::getLocationFromInputStr(inputStr);
+}
+
Return<bool> Gnss::start() {
- ALOGD("start");
if (mIsActive) {
ALOGW("Gnss has started. Restarting...");
stop();
@@ -59,15 +101,24 @@
auto svStatus = filterBlacklistedSatellitesV2_1(Utils::getMockSvInfoListV2_1());
this->reportSvStatus(svStatus);
- if (sGnssCallback_2_1 != nullptr || sGnssCallback_2_0 != nullptr) {
- const auto location = Utils::getMockLocationV2_0();
- this->reportLocation(location);
+ auto currentLocation = getLocationFromHW();
+ if (currentLocation != nullptr) {
+ this->reportLocation(*currentLocation);
} else {
- const auto location = Utils::getMockLocationV1_0();
- this->reportLocation(location);
- }
+ if (sGnssCallback_2_1 != nullptr || sGnssCallback_2_0 != nullptr) {
+ const auto location = Utils::getMockLocationV2_0();
+ this->reportLocation(location);
+ } else {
+ const auto location = Utils::getMockLocationV1_0();
+ this->reportLocation(location);
+ }
- std::this_thread::sleep_for(std::chrono::milliseconds(mMinIntervalMs));
+ // Only need do the sleep in the static location mode, which mocks the "wait
+ // for" hardware behavior.
+ if (!mHardwareModeOn) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(mMinIntervalMs));
+ }
+ }
}
});
return true;
@@ -89,6 +140,10 @@
if (mThread.joinable()) {
mThread.join();
}
+ if (mGnssFd != -1) {
+ close(mGnssFd);
+ mGnssFd = -1;
+ }
return true;
}
diff --git a/gnss/2.1/default/Gnss.h b/gnss/2.1/default/Gnss.h
index bd5e6e8..9af0d46 100644
--- a/gnss/2.1/default/Gnss.h
+++ b/gnss/2.1/default/Gnss.h
@@ -24,6 +24,7 @@
#include <thread>
#include "GnssAntennaInfo.h"
#include "GnssConfiguration.h"
+#include "NmeaFixInfo.h"
namespace android {
namespace hardware {
@@ -31,9 +32,14 @@
namespace V2_1 {
using GnssSvInfo = IGnssCallback::GnssSvInfo;
+using ::android::hardware::gnss::common::NmeaFixInfo;
namespace implementation {
+constexpr int INPUT_BUFFER_SIZE = 128;
+constexpr char CMD_GET_LOCATION[] = "CMD_GET_LOCATION";
+constexpr char GNSS_PATH[] = "/dev/gnss0";
+
struct Gnss : public IGnss {
Gnss();
~Gnss();
@@ -95,6 +101,7 @@
Return<sp<V2_1::IGnssAntennaInfo>> getExtensionGnssAntennaInfo() override;
private:
+ std::unique_ptr<V2_0::GnssLocation> getLocationFromHW();
void reportLocation(const V2_0::GnssLocation&) const;
void reportLocation(const V1_0::GnssLocation&) const;
void reportSvStatus(const hidl_vec<GnssSvInfo>&) const;
@@ -106,7 +113,10 @@
std::atomic<long> mMinIntervalMs;
sp<GnssConfiguration> mGnssConfiguration;
std::atomic<bool> mIsActive;
+ std::atomic<bool> mHardwareModeOn;
+ std::atomic<int> mGnssFd;
std::thread mThread;
+
mutable std::mutex mMutex;
hidl_vec<GnssSvInfo> filterBlacklistedSatellitesV2_1(hidl_vec<GnssSvInfo> gnssSvInfoList);
};
diff --git a/gnss/common/utils/default/Android.bp b/gnss/common/utils/default/Android.bp
index 577f6ae..2712e77 100644
--- a/gnss/common/utils/default/Android.bp
+++ b/gnss/common/utils/default/Android.bp
@@ -25,6 +25,7 @@
],
srcs: [
"Utils.cpp",
+ "NmeaFixInfo.cpp",
],
export_include_dirs: ["include"],
shared_libs: [
diff --git a/gnss/common/utils/default/NmeaFixInfo.cpp b/gnss/common/utils/default/NmeaFixInfo.cpp
new file mode 100644
index 0000000..43e008b
--- /dev/null
+++ b/gnss/common/utils/default/NmeaFixInfo.cpp
@@ -0,0 +1,266 @@
+/*
+ * 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.
+ */
+
+#define LOG_TAG "NmeaFixInfo"
+
+#include <Constants.h>
+#include <NmeaFixInfo.h>
+#include <Utils.h>
+#include <log/log.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <utils/SystemClock.h>
+#include <limits>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace android {
+namespace hardware {
+namespace gnss {
+namespace common {
+
+NmeaFixInfo::NmeaFixInfo() : hasGMCRecord(false), hasGGARecord(false) {}
+
+float NmeaFixInfo::getAltitudeMeters() const {
+ return altitudeMeters;
+}
+
+float NmeaFixInfo::checkAndConvertToFloat(const std::string& sentence) {
+ if (sentence.empty()) {
+ return std::numeric_limits<float>::quiet_NaN();
+ }
+ return std::stof(sentence);
+}
+
+float NmeaFixInfo::getBearingAccuracyDegrees() const {
+ // Current NMEA doesn't contains beaing accuracy inforamtion
+ return kMockBearingAccuracyDegrees;
+}
+float NmeaFixInfo::getBearingDegrees() const {
+ return bearingDegrees;
+}
+
+float NmeaFixInfo::getHorizontalAccuracyMeters() const {
+ // Current NMEA doesn't contains horizontal accuracy inforamtion
+ return kMockHorizontalAccuracyMeters;
+}
+
+float NmeaFixInfo::getLatDeg() const {
+ return latDeg;
+}
+
+float NmeaFixInfo::getLngDeg() const {
+ return lngDeg;
+}
+
+float NmeaFixInfo::getSpeedAccuracyMetersPerSecond() const {
+ // Current NMEA doesn't contains speed accuracy inforamtion
+ return kMockSpeedAccuracyMetersPerSecond;
+}
+
+float NmeaFixInfo::getSpeedMetersPerSec() const {
+ return speedMetersPerSec;
+}
+
+int64_t NmeaFixInfo::getTimestamp() const {
+ return timestamp;
+}
+
+float NmeaFixInfo::getVerticalAccuracyMeters() const {
+ // Current NMEA doesn't contains vertical accuracy inforamtion
+ return kMockVerticalAccuracyMeters;
+}
+
+int64_t NmeaFixInfo::nmeaPartsToTimestamp(const std::string& timeStr, const std::string& dateStr) {
+ /**
+ * In NMEA format, the full time can only get from the $GPRMC record, see
+ * the following example:
+ * $GPRMC,213204.00,A,3725.371240,N,12205.589239,W,000.0,000.0,290819,,,A*49
+ * the datetime is stored in two parts, 213204 and 290819, which means
+ * 2019/08/29 21:32:04, however for in unix the year starts from 1900, we
+ * need to add the offset.
+ */
+ struct tm tm;
+ const int32_t unixYearOffset = 100;
+ tm.tm_mday = std::stoi(dateStr.substr(0, 2).c_str());
+ tm.tm_mon = std::stoi(dateStr.substr(2, 2).c_str()) - 1;
+ tm.tm_year = std::stoi(dateStr.substr(4, 2).c_str()) + unixYearOffset;
+ tm.tm_hour = std::stoi(timeStr.substr(0, 2).c_str());
+ tm.tm_min = std::stoi(timeStr.substr(2, 2).c_str());
+ tm.tm_sec = std::stoi(timeStr.substr(4, 2).c_str());
+ return static_cast<int64_t>(mktime(&tm) - timezone);
+}
+
+bool NmeaFixInfo::isValidFix() const {
+ return hasGMCRecord && hasGGARecord;
+}
+
+void NmeaFixInfo::parseGGALine(const std::vector<std::string>& sentenceValues) {
+ if (sentenceValues.size() == 0 || sentenceValues[0].compare(GPGA_RECORD_TAG) != 0) {
+ return;
+ }
+ // LatDeg, need covert to degree, if it is 'N', should be negative value
+ this->latDeg = std::stof(sentenceValues[2].substr(0, 2)) +
+ (std::stof(sentenceValues[2].substr(2)) / 60.0);
+ if (sentenceValues[3].compare("N") != 0) {
+ this->latDeg *= -1;
+ }
+
+ // LngDeg, need covert to degree, if it is 'E', should be negative value
+ this->lngDeg = std::stof(sentenceValues[4].substr(0, 3)) +
+ std::stof(sentenceValues[4].substr(3)) / 60.0;
+ if (sentenceValues[5].compare("E") != 0) {
+ this->lngDeg *= -1;
+ }
+
+ this->altitudeMeters = std::stof(sentenceValues[9]);
+
+ this->hDop = sentenceValues[8].empty() ? std::numeric_limits<float>::quiet_NaN()
+ : std::stof(sentenceValues[8]);
+ this->hasGGARecord = true;
+}
+
+void NmeaFixInfo::parseRMCLine(const std::vector<std::string>& sentenceValues) {
+ if (sentenceValues.size() == 0 || sentenceValues[0].compare(GPRMC_RECORD_TAG) != 0) {
+ return;
+ }
+ this->speedMetersPerSec = checkAndConvertToFloat(sentenceValues[7]);
+ this->bearingDegrees = checkAndConvertToFloat(sentenceValues[8]);
+ this->timestamp = nmeaPartsToTimestamp(sentenceValues[1], sentenceValues[9]);
+ this->hasGMCRecord = true;
+}
+
+/** invalid the current NmeaFixInfo */
+void NmeaFixInfo::reset() {
+ this->altitudeMeters = 0;
+ this->bearingDegrees = 0;
+ this->fixId = 0;
+ this->hasGMCRecord = false;
+ this->hasGGARecord = false;
+ this->latDeg = 0;
+ this->lngDeg = 0;
+ this->hDop = 0;
+ this->vDop = 0;
+ this->satelliteCount = 0;
+ this->speedMetersPerSec = 0;
+ this->timestamp = 0;
+}
+
+void NmeaFixInfo::splitStr(const std::string& line, const char& delimiter,
+ std::vector<std::string>& out) {
+ std::istringstream iss(line);
+ std::string item;
+ while (std::getline(iss, item, delimiter)) {
+ out.push_back(item);
+ }
+}
+
+NmeaFixInfo& NmeaFixInfo::operator=(const NmeaFixInfo& rhs) {
+ if (this == &rhs) return *this;
+ this->altitudeMeters = rhs.altitudeMeters;
+ this->bearingDegrees = rhs.bearingDegrees;
+ this->fixId = rhs.fixId;
+ this->hasGMCRecord = rhs.hasGMCRecord;
+ this->hasGGARecord = rhs.hasGGARecord;
+ this->hDop = rhs.hDop;
+ this->vDop = rhs.vDop;
+ this->latDeg = rhs.latDeg;
+ this->lngDeg = rhs.lngDeg;
+ this->satelliteCount = rhs.satelliteCount;
+ this->speedMetersPerSec = rhs.speedMetersPerSec;
+ this->timestamp = rhs.timestamp;
+
+ return *this;
+}
+
+/**
+ * Parses the input string in NMEA format and convert to GnssLocation.
+ * Currently version only cares about $GPGGA and $GPRMC records. but we
+ * can easily extend to other types supported by NMEA if needed.
+ */
+std::unique_ptr<V2_0::GnssLocation> NmeaFixInfo::getLocationFromInputStr(
+ const std::string& inputStr) {
+ std::vector<std::string> nmeaRecords;
+ splitStr(inputStr, LINE_SEPARATOR, nmeaRecords);
+ NmeaFixInfo nmeaFixInfo;
+ NmeaFixInfo candidateFixInfo;
+ uint32_t fixId = 0;
+ double lastTimeStamp = 0;
+ for (const auto& line : nmeaRecords) {
+ std::vector<std::string> sentenceValues;
+ splitStr(line, COMMA_SEPARATOR, sentenceValues);
+ double currentTimeStamp = std::stof(sentenceValues[1]);
+ // If see a new timestamp, report correct location.
+ if ((currentTimeStamp - lastTimeStamp) > TIMESTAMP_EPSILON &&
+ candidateFixInfo.isValidFix()) {
+ nmeaFixInfo = candidateFixInfo;
+ candidateFixInfo.reset();
+ fixId++;
+ }
+ if (line.compare(0, strlen(GPGA_RECORD_TAG), GPGA_RECORD_TAG) == 0) {
+ candidateFixInfo.fixId = fixId;
+ candidateFixInfo.parseGGALine(sentenceValues);
+ } else if (line.compare(0, strlen(GPRMC_RECORD_TAG), GPRMC_RECORD_TAG) == 0) {
+ candidateFixInfo.parseRMCLine(sentenceValues);
+ }
+ }
+ if (candidateFixInfo.isValidFix()) {
+ nmeaFixInfo = candidateFixInfo;
+ candidateFixInfo.reset();
+ }
+ if (!nmeaFixInfo.isValidFix()) {
+ return nullptr;
+ }
+ return nmeaFixInfo.toGnssLocation();
+}
+
+/**
+ * Parses the input string in NMEA format and convert to GnssLocation.
+ */
+std::unique_ptr<V2_0::GnssLocation> NmeaFixInfo::toGnssLocation() const {
+ const V2_0::ElapsedRealtime currentOsTimestamp = {
+ .flags = V2_0::ElapsedRealtimeFlags::HAS_TIMESTAMP_NS |
+ V2_0::ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS,
+ .timestampNs = static_cast<uint64_t>(::android::elapsedRealtimeNano()),
+ // This is an hardcoded value indicating a 1ms of uncertainty between the two clocks.
+ // In an actual implementation provide an estimate of the synchronization uncertainty
+ // or don't set the field.
+ .timeUncertaintyNs = 1000000};
+
+ V1_0::GnssLocation locationV1 = {
+ .gnssLocationFlags = 0xFF,
+ .latitudeDegrees = this->getLatDeg(),
+ .longitudeDegrees = this->getLngDeg(),
+ .altitudeMeters = this->getAltitudeMeters(),
+ .speedMetersPerSec = this->getSpeedMetersPerSec(),
+ .bearingDegrees = this->getBearingDegrees(),
+ .horizontalAccuracyMeters = this->getHorizontalAccuracyMeters(),
+ .verticalAccuracyMeters = this->getVerticalAccuracyMeters(),
+ .speedAccuracyMetersPerSecond = this->getSpeedAccuracyMetersPerSecond(),
+ .bearingAccuracyDegrees = this->getBearingAccuracyDegrees(),
+ .timestamp = this->getTimestamp()};
+
+ V2_0::GnssLocation locationV2 = {.v1_0 = locationV1, .elapsedRealtime = currentOsTimestamp};
+
+ return std::make_unique<V2_0::GnssLocation>(locationV2);
+}
+
+} // namespace common
+} // namespace gnss
+} // namespace hardware
+} // namespace android
\ No newline at end of file
diff --git a/gnss/common/utils/default/include/NmeaFixInfo.h b/gnss/common/utils/default/include/NmeaFixInfo.h
new file mode 100644
index 0000000..fb2c1a4
--- /dev/null
+++ b/gnss/common/utils/default/include/NmeaFixInfo.h
@@ -0,0 +1,92 @@
+/*
+ * 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.
+ */
+
+#pragma once
+
+#include <Constants.h>
+#include <android/hardware/gnss/1.0/IGnss.h>
+#include <android/hardware/gnss/2.0/IGnss.h>
+#include <hidl/Status.h>
+#include <ctime>
+#include <string>
+namespace android {
+namespace hardware {
+namespace gnss {
+namespace common {
+using ::android::sp;
+using ::android::hardware::hidl_string;
+using ::android::hardware::hidl_vec;
+using ::android::hardware::Return;
+using ::android::hardware::Void;
+
+constexpr char GPGA_RECORD_TAG[] = "$GPGGA";
+constexpr char GPRMC_RECORD_TAG[] = "$GPRMC";
+constexpr char LINE_SEPARATOR = '\n';
+constexpr char COMMA_SEPARATOR = ',';
+constexpr double TIMESTAMP_EPSILON = 0.001;
+
+/** Helper class to parse and store the GNSS fix details information. */
+class NmeaFixInfo {
+ private:
+ float altitudeMeters;
+ float bearingDegrees;
+ uint32_t fixId;
+ bool hasGMCRecord;
+ bool hasGGARecord;
+ float hDop;
+ float vDop;
+ float latDeg;
+ float lngDeg;
+ uint32_t satelliteCount;
+ float speedMetersPerSec;
+ int64_t timestamp;
+
+ public:
+ static std::unique_ptr<V2_0::GnssLocation> getLocationFromInputStr(const std::string& inputStr);
+
+ private:
+ static void splitStr(const std::string& line, const char& delimiter,
+ std::vector<std::string>& out);
+ static float checkAndConvertToFloat(const std::string& sentence);
+ static int64_t nmeaPartsToTimestamp(const std::string& timeStr, const std::string& dateStr);
+
+ NmeaFixInfo();
+ void parseGGALine(const std::vector<std::string>& sentenceValues);
+ void parseRMCLine(const std::vector<std::string>& sentenceValues);
+ std::unique_ptr<V2_0::GnssLocation> toGnssLocation() const;
+
+ // Getters
+ float getAltitudeMeters() const;
+ float getBearingAccuracyDegrees() const;
+ float getBearingDegrees() const;
+ uint32_t getFixId() const;
+ float getHorizontalAccuracyMeters() const;
+ float getLatDeg() const;
+ float getLngDeg() const;
+ float getSpeedAccuracyMetersPerSecond() const;
+ float getSpeedMetersPerSec() const;
+ int64_t getTimestamp() const;
+ float getVerticalAccuracyMeters() const;
+
+ bool isValidFix() const;
+ void reset();
+ NmeaFixInfo& operator=(const NmeaFixInfo& rhs);
+};
+
+} // namespace common
+} // namespace gnss
+} // namespace hardware
+} // namespace android
\ No newline at end of file