Hal layer for gnss replay (48618f60) · Commits · e / os / android_hardware_interfaces

gnss/2.1/default/Gnss.cpp

+66 −11

Original line number	Diff line number	Diff line
		@@ -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<V2_0::IGnssCallback> Gnss::sGnssCallback_2_0 = nullptr;
		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,6 +101,10 @@ Return<bool> Gnss::start() {
		auto svStatus = filterBlacklistedSatellitesV2_1(Utils::getMockSvInfoListV2_1());
		this->reportSvStatus(svStatus);

		auto currentLocation = getLocationFromHW();
		if (currentLocation != nullptr) {
		this->reportLocation(*currentLocation);
		} else {
		if (sGnssCallback_2_1 != nullptr \|\| sGnssCallback_2_0 != nullptr) {
		const auto location = Utils::getMockLocationV2_0();
		this->reportLocation(location);
		@@ -67,8 +113,13 @@ Return<bool> Gnss::start() {
		this->reportLocation(location);
		}

		// 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 @@ Return<bool> Gnss::stop() {
		if (mThread.joinable()) {
		mThread.join();
		}
		if (mGnssFd != -1) {
		close(mGnssFd);
		mGnssFd = -1;
		}
		return true;
		}

gnss/2.1/default/Gnss.h

+10 −0

Original line number	Diff line number	Diff line
		@@ -24,6 +24,7 @@
		#include <thread>
		#include "GnssAntennaInfo.h"
		#include "GnssConfiguration.h"
		#include "NmeaFixInfo.h"

		namespace android {
		namespace hardware {
		@@ -31,9 +32,14 @@ namespace gnss {
		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 @@ struct Gnss : public IGnss {
		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 @@ struct Gnss : public IGnss {
		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);
		};

gnss/common/utils/default/Android.bp

+1 −0

Original line number	Diff line number	Diff line
		@@ -25,6 +25,7 @@ cc_library_static {
		],
		srcs: [
		"Utils.cpp",
		"NmeaFixInfo.cpp",
		],
		export_include_dirs: ["include"],
		shared_libs: [

gnss/common/utils/default/NmeaFixInfo.cpp

0 → 100644

+266 −0

Original line number	Diff line number	Diff line
		/*
		* 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

gnss/common/utils/default/include/NmeaFixInfo.h

0 → 100644

+92 −0

Original line number	Diff line number	Diff line
		/*
		* 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