Loading gnss/aidl/default/GnssMeasurementInterface.cpp +22 −3 Original line number Diff line number Diff line Loading @@ -19,11 +19,17 @@ #include "GnssMeasurementInterface.h" #include <aidl/android/hardware/gnss/BnGnss.h> #include <log/log.h> #include "DeviceFileReader.h" #include "GnssRawMeasurementParser.h" #include "GnssReplayUtils.h" #include "Utils.h" namespace aidl::android::hardware::gnss { using Utils = ::android::hardware::gnss::common::Utils; using ReplayUtils = ::android::hardware::gnss::common::ReplayUtils; using GnssRawMeasurementParser = ::android::hardware::gnss::common::GnssRawMeasurementParser; using DeviceFileReader = ::android::hardware::gnss::common::DeviceFileReader; std::shared_ptr<IGnssMeasurementCallback> GnssMeasurementInterface::sCallback = nullptr; Loading Loading @@ -63,9 +69,22 @@ void GnssMeasurementInterface::start(const bool enableCorrVecOutputs) { mIsActive = true; mThread = std::thread([this, enableCorrVecOutputs]() { while (mIsActive == true) { std::string rawMeasurementStr = ""; if (ReplayUtils::hasGnssDeviceFile() && ReplayUtils::isGnssRawMeasurement( rawMeasurementStr = DeviceFileReader::Instance().getGnssRawMeasurementData())) { ALOGD("rawMeasurementStr(size: %zu) from device file: %s", rawMeasurementStr.size(), rawMeasurementStr.c_str()); auto measurement = GnssRawMeasurementParser::getMeasurementFromStrs(rawMeasurementStr); if (measurement != nullptr) { this->reportMeasurement(*measurement); } } else { auto measurement = Utils::getMockMeasurement(enableCorrVecOutputs); this->reportMeasurement(measurement); } std::this_thread::sleep_for(std::chrono::milliseconds(mMinIntervalMillis)); } }); Loading gnss/common/utils/default/Android.bp +6 −1 Original line number Diff line number Diff line Loading @@ -38,9 +38,14 @@ cc_library_static { "v2_1/GnssDebug.cpp", "v2_1/GnssMeasurement.cpp", "v2_1/GnssMeasurementCorrections.cpp", "DeviceFileReader.cpp", "FixLocationParser.cpp", "GnssRawMeasurementParser.cpp", "GnssReplayUtils.cpp", "MockLocation.cpp", "Utils.cpp", "NmeaFixInfo.cpp", "ParseUtils.cpp", "Utils.cpp", ], export_include_dirs: ["include"], shared_libs: [ Loading gnss/common/utils/default/DeviceFileReader.cpp 0 → 100644 +109 −0 Original line number Diff line number Diff line /* * Copyright (C) 2021 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 "DeviceFileReader.h" namespace android { namespace hardware { namespace gnss { namespace common { void DeviceFileReader::getDataFromDeviceFile(const std::string& command, int mMinIntervalMs) { char inputBuffer[INPUT_BUFFER_SIZE]; std::string deviceFilePath = ""; if (command == CMD_GET_LOCATION) { deviceFilePath = ReplayUtils::getFixedLocationPath(); } else if (command == CMD_GET_RAWMEASUREMENT) { deviceFilePath = ReplayUtils::getGnssPath(); } else { // Invalid command return; } int mGnssFd = open(deviceFilePath.c_str(), O_RDWR | O_NONBLOCK); if (mGnssFd == -1) { return; } int bytes_write = write(mGnssFd, command.c_str(), command.size()); if (bytes_write <= 0) { close(mGnssFd); return; } 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); if (epoll_ret == -1) { close(mGnssFd); return; } while (true) { memset(inputBuffer, 0, INPUT_BUFFER_SIZE); bytes_read = read(mGnssFd, &inputBuffer, INPUT_BUFFER_SIZE); if (bytes_read <= 0) { break; } s_buffer_ += std::string(inputBuffer, bytes_read); } close(mGnssFd); // Trim end of file mark(\n\n\n\n). auto pos = s_buffer_.find("\n\n\n\n"); if (pos != std::string::npos) { inputStr = s_buffer_.substr(0, pos); s_buffer_ = s_buffer_.substr(pos + 4); } else { return; } // Cache the injected data. if (command == CMD_GET_LOCATION) { // TODO validate data data_[CMD_GET_LOCATION] = inputStr; } else if (command == CMD_GET_RAWMEASUREMENT) { if (ReplayUtils::isGnssRawMeasurement(inputStr)) { data_[CMD_GET_RAWMEASUREMENT] = inputStr; } } } std::string DeviceFileReader::getLocationData() { std::unique_lock<std::mutex> lock(mMutex); getDataFromDeviceFile(CMD_GET_LOCATION, 20); return data_[CMD_GET_LOCATION]; } std::string DeviceFileReader::getGnssRawMeasurementData() { std::unique_lock<std::mutex> lock(mMutex); getDataFromDeviceFile(CMD_GET_RAWMEASUREMENT, 20); return data_[CMD_GET_RAWMEASUREMENT]; } DeviceFileReader::DeviceFileReader() {} DeviceFileReader::~DeviceFileReader() {} } // namespace common } // namespace gnss } // namespace hardware } // namespace android gnss/common/utils/default/FixLocationParser.cpp 0 → 100644 +76 −0 Original line number Diff line number Diff line /* * Copyright (C) 2022 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 "FixLocationParser.h" #include <android/hardware/gnss/1.0/IGnss.h> namespace android { namespace hardware { namespace gnss { namespace common { std::unique_ptr<V2_0::GnssLocation> FixLocationParser::getLocationFromInputStr( const std::string& locationStr) { /* * Fix,Provider,LatitudeDegrees,LongitudeDegrees,AltitudeMeters,SpeedMps, * AccuracyMeters,BearingDegrees,UnixTimeMillis,SpeedAccuracyMps,BearingAccuracyDegrees, * elapsedRealtimeNanos */ if (locationStr.empty()) { return nullptr; } std::vector<std::string> locationStrRecords; ParseUtils::splitStr(locationStr, LINE_SEPARATOR, locationStrRecords); if (locationStrRecords.empty()) { return nullptr; } std::vector<std::string> locationValues; ParseUtils::splitStr(locationStrRecords[0], COMMA_SEPARATOR, locationValues); if (locationValues.size() < 12) { return nullptr; } V2_0::ElapsedRealtime elapsedRealtime = { .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 = 1020400}; V1_0::GnssLocation locationV1 = { .gnssLocationFlags = 0xFF, .latitudeDegrees = ParseUtils::tryParseDouble(locationValues[2], 0), .longitudeDegrees = ParseUtils::tryParseDouble(locationValues[3], 0), .altitudeMeters = ParseUtils::tryParseDouble(locationValues[4], 0), .speedMetersPerSec = ParseUtils::tryParsefloat(locationValues[5], 0), .bearingDegrees = ParseUtils::tryParsefloat(locationValues[7], 0), .horizontalAccuracyMeters = ParseUtils::tryParsefloat(locationValues[6], 0), .verticalAccuracyMeters = ParseUtils::tryParsefloat(locationValues[6], 0), .speedAccuracyMetersPerSecond = ParseUtils::tryParsefloat(locationValues[9], 0), .bearingAccuracyDegrees = ParseUtils::tryParsefloat(locationValues[10], 0), .timestamp = ParseUtils::tryParseLongLong(locationValues[8], 0)}; V2_0::GnssLocation locationV2 = {.v1_0 = locationV1, .elapsedRealtime = elapsedRealtime}; return std::make_unique<V2_0::GnssLocation>(locationV2); } } // namespace common } // namespace gnss } // namespace hardware } // namespace android gnss/common/utils/default/GnssRawMeasurementParser.cpp 0 → 100644 +305 −0 Original line number Diff line number Diff line /* * Copyright (C) 2021 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 "GnssRawMeasurementParser.h" namespace android { namespace hardware { namespace gnss { namespace common { using aidl::android::hardware::gnss::ElapsedRealtime; using aidl::android::hardware::gnss::GnssClock; using aidl::android::hardware::gnss::GnssConstellationType; using aidl::android::hardware::gnss::GnssData; using aidl::android::hardware::gnss::GnssMeasurement; using aidl::android::hardware::gnss::GnssMultipathIndicator; using aidl::android::hardware::gnss::GnssSignalType; using ParseUtils = ::android::hardware::gnss::common::ParseUtils; std::unordered_map<std::string, int> GnssRawMeasurementParser::getColumnIdNameMappingFromHeader( const std::string& header) { std::vector<std::string> columnNames; std::unordered_map<std::string, int> columnNameIdMapping; std::string s = header; // Trim left spaces s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](unsigned char ch) { return !std::isspace(ch); })); // Trim right spaces s.erase(std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) { return !std::isspace(ch); }) .base(), s.end()); // Remove comment symbol, start from `Raw`. s = s.substr(s.find("Raw")); ParseUtils::splitStr(s, COMMA_SEPARATOR, columnNames); int columnId = 0; for (auto& name : columnNames) { columnNameIdMapping[name] = columnId++; } return columnNameIdMapping; } int GnssRawMeasurementParser::getClockFlags( const std::vector<std::string>& rawMeasurementRecordValues, const std::unordered_map<std::string, int>& columnNameIdMapping) { int clockFlags = 0; if (!rawMeasurementRecordValues[columnNameIdMapping.at("LeapSecond")].empty()) { clockFlags |= GnssClock::HAS_LEAP_SECOND; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("FullBiasNanos")].empty()) { clockFlags |= GnssClock::HAS_FULL_BIAS; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("BiasNanos")].empty()) { clockFlags |= GnssClock::HAS_BIAS; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("BiasUncertaintyNanos")].empty()) { clockFlags |= GnssClock::HAS_BIAS_UNCERTAINTY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("DriftNanosPerSecond")].empty()) { clockFlags |= GnssClock::HAS_DRIFT; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("DriftUncertaintyNanosPerSecond")] .empty()) { clockFlags |= GnssClock::HAS_DRIFT_UNCERTAINTY; } return clockFlags; } int GnssRawMeasurementParser::getElapsedRealtimeFlags( const std::vector<std::string>& rawMeasurementRecordValues, const std::unordered_map<std::string, int>& columnNameIdMapping) { int elapsedRealtimeFlags = ElapsedRealtime::HAS_TIMESTAMP_NS; if (!rawMeasurementRecordValues[columnNameIdMapping.at("TimeUncertaintyNanos")].empty()) { elapsedRealtimeFlags |= ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS; } return elapsedRealtimeFlags; } int GnssRawMeasurementParser::getRawMeasurementFlags( const std::vector<std::string>& rawMeasurementRecordValues, const std::unordered_map<std::string, int>& columnNameIdMapping) { int rawMeasurementFlags = 0; if (!rawMeasurementRecordValues[columnNameIdMapping.at("SnrInDb")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_SNR; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierFrequencyHz")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_FREQUENCY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierCycles")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_CYCLES; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierPhase")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_PHASE; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierPhaseUncertainty")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_PHASE_UNCERTAINTY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("AgcDb")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_AUTOMATIC_GAIN_CONTROL; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("FullInterSignalBiasNanos")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_FULL_ISB; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("FullInterSignalBiasUncertaintyNanos")] .empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_FULL_ISB_UNCERTAINTY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("SatelliteInterSignalBiasNanos")] .empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_SATELLITE_ISB; } if (!rawMeasurementRecordValues[columnNameIdMapping.at( "SatelliteInterSignalBiasUncertaintyNanos")] .empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_SATELLITE_ISB_UNCERTAINTY; } // HAS_SATELLITE_PVT and HAS_CORRELATION_VECTOR fields currently not in rawmeasurement // output, need add them later. return rawMeasurementFlags; } GnssConstellationType GnssRawMeasurementParser::getGnssConstellationType(int constellationType) { GnssConstellationType gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::UNKNOWN; switch (constellationType) { case 1: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::GPS; break; case 2: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::SBAS; break; case 3: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::GLONASS; break; case 4: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::QZSS; break; case 5: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::BEIDOU; break; case 6: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::GALILEO; break; default: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::UNKNOWN; } return gnssConstellationType; } std::unique_ptr<GnssData> GnssRawMeasurementParser::getMeasurementFromStrs( std::string& rawMeasurementStr) { /* * Raw,utcTimeMillis,TimeNanos,LeapSecond,TimeUncertaintyNanos,FullBiasNanos,BiasNanos, * BiasUncertaintyNanos,DriftNanosPerSecond,DriftUncertaintyNanosPerSecond, * HardwareClockDiscontinuityCount,Svid,TimeOffsetNanos,State,ReceivedSvTimeNanos, * ReceivedSvTimeUncertaintyNanos,Cn0DbHz,PseudorangeRateMetersPerSecond, * PseudorangeRateUncertaintyMetersPerSecond,AccumulatedDeltaRangeState, * AccumulatedDeltaRangeMeters,AccumulatedDeltaRangeUncertaintyMeters,CarrierFrequencyHz, * CarrierCycles,CarrierPhase,CarrierPhaseUncertainty,MultipathIndicator,SnrInDb, * ConstellationType,AgcDb,BasebandCn0DbHz,FullInterSignalBiasNanos, * FullInterSignalBiasUncertaintyNanos,SatelliteInterSignalBiasNanos, * SatelliteInterSignalBiasUncertaintyNanos,CodeType,ChipsetElapsedRealtimeNanos */ ALOGD("Parsing %zu bytes rawMeasurementStr.", rawMeasurementStr.size()); if (rawMeasurementStr.empty()) { return nullptr; } std::vector<std::string> rawMeasurementStrRecords; ParseUtils::splitStr(rawMeasurementStr, LINE_SEPARATOR, rawMeasurementStrRecords); if (rawMeasurementStrRecords.size() <= 1) { ALOGE("Raw GNSS Measurements parser failed. (No records) "); return nullptr; } // Get the column name mapping from the header. std::unordered_map<std::string, int> columnNameIdMapping = getColumnIdNameMappingFromHeader(rawMeasurementStrRecords[0]); if (columnNameIdMapping.size() < 37 || !ParseUtils::isValidHeader(columnNameIdMapping)) { ALOGE("Raw GNSS Measurements parser failed. (No header or missing columns.) "); return nullptr; } // Set GnssClock from 1st record. std::size_t pointer = 1; std::vector<std::string> firstRecordValues; ParseUtils::splitStr(rawMeasurementStrRecords[pointer], COMMA_SEPARATOR, firstRecordValues); GnssClock clock = { .gnssClockFlags = getClockFlags(firstRecordValues, columnNameIdMapping), .timeNs = ParseUtils::tryParseLongLong( firstRecordValues[columnNameIdMapping.at("TimeNanos")], 0), .fullBiasNs = ParseUtils::tryParseLongLong( firstRecordValues[columnNameIdMapping.at("FullBiasNanos")], 0), .biasNs = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("BiasNanos")], 0), .biasUncertaintyNs = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("BiasUncertaintyNanos")], 0), .driftNsps = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("DriftNanosPerSecond")], 0), .driftUncertaintyNsps = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("DriftNanosPerSecond")], 0), .hwClockDiscontinuityCount = ParseUtils::tryParseInt( firstRecordValues[columnNameIdMapping.at("HardwareClockDiscontinuityCount")], 0)}; ElapsedRealtime timestamp = { .flags = getElapsedRealtimeFlags(firstRecordValues, columnNameIdMapping), .timestampNs = ParseUtils::tryParseLongLong( firstRecordValues[columnNameIdMapping.at("ChipsetElapsedRealtimeNanos")]), .timeUncertaintyNs = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("TimeUncertaintyNanos")], 0)}; std::vector<GnssMeasurement> measurementsVec; for (pointer = 1; pointer < rawMeasurementStrRecords.size(); pointer++) { std::vector<std::string> rawMeasurementValues; std::string line = rawMeasurementStrRecords[pointer]; ParseUtils::splitStr(line, COMMA_SEPARATOR, rawMeasurementValues); GnssSignalType signalType = { .constellation = getGnssConstellationType(ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("ConstellationType")], 0)), .carrierFrequencyHz = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("CarrierFrequencyHz")], 0), .codeType = rawMeasurementValues[columnNameIdMapping.at("CodeType")], }; GnssMeasurement measurement = { .flags = getRawMeasurementFlags(rawMeasurementValues, columnNameIdMapping), .svid = ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("Svid")], 0), .signalType = signalType, .receivedSvTimeInNs = ParseUtils::tryParseLongLong( rawMeasurementValues[columnNameIdMapping.at("ReceivedSvTimeNanos")], 0), .receivedSvTimeUncertaintyInNs = ParseUtils::tryParseLongLong(rawMeasurementValues[columnNameIdMapping.at( "ReceivedSvTimeUncertaintyNanos")], 0), .antennaCN0DbHz = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("Cn0DbHz")], 0), .basebandCN0DbHz = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("BasebandCn0DbHz")], 0), .agcLevelDb = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("AgcDb")], 0), .pseudorangeRateMps = ParseUtils::tryParseDouble(rawMeasurementValues[columnNameIdMapping.at( "PseudorangeRateMetersPerSecond")], 0), .pseudorangeRateUncertaintyMps = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at( "PseudorangeRateUncertaintyMetersPerSecond")], 0), .accumulatedDeltaRangeState = ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("AccumulatedDeltaRangeState")], 0), .accumulatedDeltaRangeM = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("AccumulatedDeltaRangeMeters")], 0), .accumulatedDeltaRangeUncertaintyM = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at( "AccumulatedDeltaRangeUncertaintyMeters")], 0), .multipathIndicator = GnssMultipathIndicator::UNKNOWN, // Not in GnssLogger yet. .state = ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("State")], 0), .fullInterSignalBiasNs = ParseUtils::tryParseDouble(rawMeasurementValues[31], 0), .fullInterSignalBiasUncertaintyNs = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("FullInterSignalBiasNanos")], 0), .satelliteInterSignalBiasNs = ParseUtils::tryParseDouble(rawMeasurementValues[columnNameIdMapping.at( "SatelliteInterSignalBiasNanos")], 0), .satelliteInterSignalBiasUncertaintyNs = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at( "SatelliteInterSignalBiasUncertaintyNanos")], 0), .satellitePvt = {}, .correlationVectors = {}}; measurementsVec.push_back(measurement); } GnssData gnssData = { .measurements = measurementsVec, .clock = clock, .elapsedRealtime = timestamp}; return std::make_unique<GnssData>(gnssData); } } // namespace common } // namespace gnss } // namespace hardware } // namespace android Loading
gnss/aidl/default/GnssMeasurementInterface.cpp +22 −3 Original line number Diff line number Diff line Loading @@ -19,11 +19,17 @@ #include "GnssMeasurementInterface.h" #include <aidl/android/hardware/gnss/BnGnss.h> #include <log/log.h> #include "DeviceFileReader.h" #include "GnssRawMeasurementParser.h" #include "GnssReplayUtils.h" #include "Utils.h" namespace aidl::android::hardware::gnss { using Utils = ::android::hardware::gnss::common::Utils; using ReplayUtils = ::android::hardware::gnss::common::ReplayUtils; using GnssRawMeasurementParser = ::android::hardware::gnss::common::GnssRawMeasurementParser; using DeviceFileReader = ::android::hardware::gnss::common::DeviceFileReader; std::shared_ptr<IGnssMeasurementCallback> GnssMeasurementInterface::sCallback = nullptr; Loading Loading @@ -63,9 +69,22 @@ void GnssMeasurementInterface::start(const bool enableCorrVecOutputs) { mIsActive = true; mThread = std::thread([this, enableCorrVecOutputs]() { while (mIsActive == true) { std::string rawMeasurementStr = ""; if (ReplayUtils::hasGnssDeviceFile() && ReplayUtils::isGnssRawMeasurement( rawMeasurementStr = DeviceFileReader::Instance().getGnssRawMeasurementData())) { ALOGD("rawMeasurementStr(size: %zu) from device file: %s", rawMeasurementStr.size(), rawMeasurementStr.c_str()); auto measurement = GnssRawMeasurementParser::getMeasurementFromStrs(rawMeasurementStr); if (measurement != nullptr) { this->reportMeasurement(*measurement); } } else { auto measurement = Utils::getMockMeasurement(enableCorrVecOutputs); this->reportMeasurement(measurement); } std::this_thread::sleep_for(std::chrono::milliseconds(mMinIntervalMillis)); } }); Loading
gnss/common/utils/default/Android.bp +6 −1 Original line number Diff line number Diff line Loading @@ -38,9 +38,14 @@ cc_library_static { "v2_1/GnssDebug.cpp", "v2_1/GnssMeasurement.cpp", "v2_1/GnssMeasurementCorrections.cpp", "DeviceFileReader.cpp", "FixLocationParser.cpp", "GnssRawMeasurementParser.cpp", "GnssReplayUtils.cpp", "MockLocation.cpp", "Utils.cpp", "NmeaFixInfo.cpp", "ParseUtils.cpp", "Utils.cpp", ], export_include_dirs: ["include"], shared_libs: [ Loading
gnss/common/utils/default/DeviceFileReader.cpp 0 → 100644 +109 −0 Original line number Diff line number Diff line /* * Copyright (C) 2021 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 "DeviceFileReader.h" namespace android { namespace hardware { namespace gnss { namespace common { void DeviceFileReader::getDataFromDeviceFile(const std::string& command, int mMinIntervalMs) { char inputBuffer[INPUT_BUFFER_SIZE]; std::string deviceFilePath = ""; if (command == CMD_GET_LOCATION) { deviceFilePath = ReplayUtils::getFixedLocationPath(); } else if (command == CMD_GET_RAWMEASUREMENT) { deviceFilePath = ReplayUtils::getGnssPath(); } else { // Invalid command return; } int mGnssFd = open(deviceFilePath.c_str(), O_RDWR | O_NONBLOCK); if (mGnssFd == -1) { return; } int bytes_write = write(mGnssFd, command.c_str(), command.size()); if (bytes_write <= 0) { close(mGnssFd); return; } 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); if (epoll_ret == -1) { close(mGnssFd); return; } while (true) { memset(inputBuffer, 0, INPUT_BUFFER_SIZE); bytes_read = read(mGnssFd, &inputBuffer, INPUT_BUFFER_SIZE); if (bytes_read <= 0) { break; } s_buffer_ += std::string(inputBuffer, bytes_read); } close(mGnssFd); // Trim end of file mark(\n\n\n\n). auto pos = s_buffer_.find("\n\n\n\n"); if (pos != std::string::npos) { inputStr = s_buffer_.substr(0, pos); s_buffer_ = s_buffer_.substr(pos + 4); } else { return; } // Cache the injected data. if (command == CMD_GET_LOCATION) { // TODO validate data data_[CMD_GET_LOCATION] = inputStr; } else if (command == CMD_GET_RAWMEASUREMENT) { if (ReplayUtils::isGnssRawMeasurement(inputStr)) { data_[CMD_GET_RAWMEASUREMENT] = inputStr; } } } std::string DeviceFileReader::getLocationData() { std::unique_lock<std::mutex> lock(mMutex); getDataFromDeviceFile(CMD_GET_LOCATION, 20); return data_[CMD_GET_LOCATION]; } std::string DeviceFileReader::getGnssRawMeasurementData() { std::unique_lock<std::mutex> lock(mMutex); getDataFromDeviceFile(CMD_GET_RAWMEASUREMENT, 20); return data_[CMD_GET_RAWMEASUREMENT]; } DeviceFileReader::DeviceFileReader() {} DeviceFileReader::~DeviceFileReader() {} } // namespace common } // namespace gnss } // namespace hardware } // namespace android
gnss/common/utils/default/FixLocationParser.cpp 0 → 100644 +76 −0 Original line number Diff line number Diff line /* * Copyright (C) 2022 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 "FixLocationParser.h" #include <android/hardware/gnss/1.0/IGnss.h> namespace android { namespace hardware { namespace gnss { namespace common { std::unique_ptr<V2_0::GnssLocation> FixLocationParser::getLocationFromInputStr( const std::string& locationStr) { /* * Fix,Provider,LatitudeDegrees,LongitudeDegrees,AltitudeMeters,SpeedMps, * AccuracyMeters,BearingDegrees,UnixTimeMillis,SpeedAccuracyMps,BearingAccuracyDegrees, * elapsedRealtimeNanos */ if (locationStr.empty()) { return nullptr; } std::vector<std::string> locationStrRecords; ParseUtils::splitStr(locationStr, LINE_SEPARATOR, locationStrRecords); if (locationStrRecords.empty()) { return nullptr; } std::vector<std::string> locationValues; ParseUtils::splitStr(locationStrRecords[0], COMMA_SEPARATOR, locationValues); if (locationValues.size() < 12) { return nullptr; } V2_0::ElapsedRealtime elapsedRealtime = { .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 = 1020400}; V1_0::GnssLocation locationV1 = { .gnssLocationFlags = 0xFF, .latitudeDegrees = ParseUtils::tryParseDouble(locationValues[2], 0), .longitudeDegrees = ParseUtils::tryParseDouble(locationValues[3], 0), .altitudeMeters = ParseUtils::tryParseDouble(locationValues[4], 0), .speedMetersPerSec = ParseUtils::tryParsefloat(locationValues[5], 0), .bearingDegrees = ParseUtils::tryParsefloat(locationValues[7], 0), .horizontalAccuracyMeters = ParseUtils::tryParsefloat(locationValues[6], 0), .verticalAccuracyMeters = ParseUtils::tryParsefloat(locationValues[6], 0), .speedAccuracyMetersPerSecond = ParseUtils::tryParsefloat(locationValues[9], 0), .bearingAccuracyDegrees = ParseUtils::tryParsefloat(locationValues[10], 0), .timestamp = ParseUtils::tryParseLongLong(locationValues[8], 0)}; V2_0::GnssLocation locationV2 = {.v1_0 = locationV1, .elapsedRealtime = elapsedRealtime}; return std::make_unique<V2_0::GnssLocation>(locationV2); } } // namespace common } // namespace gnss } // namespace hardware } // namespace android
gnss/common/utils/default/GnssRawMeasurementParser.cpp 0 → 100644 +305 −0 Original line number Diff line number Diff line /* * Copyright (C) 2021 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 "GnssRawMeasurementParser.h" namespace android { namespace hardware { namespace gnss { namespace common { using aidl::android::hardware::gnss::ElapsedRealtime; using aidl::android::hardware::gnss::GnssClock; using aidl::android::hardware::gnss::GnssConstellationType; using aidl::android::hardware::gnss::GnssData; using aidl::android::hardware::gnss::GnssMeasurement; using aidl::android::hardware::gnss::GnssMultipathIndicator; using aidl::android::hardware::gnss::GnssSignalType; using ParseUtils = ::android::hardware::gnss::common::ParseUtils; std::unordered_map<std::string, int> GnssRawMeasurementParser::getColumnIdNameMappingFromHeader( const std::string& header) { std::vector<std::string> columnNames; std::unordered_map<std::string, int> columnNameIdMapping; std::string s = header; // Trim left spaces s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](unsigned char ch) { return !std::isspace(ch); })); // Trim right spaces s.erase(std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) { return !std::isspace(ch); }) .base(), s.end()); // Remove comment symbol, start from `Raw`. s = s.substr(s.find("Raw")); ParseUtils::splitStr(s, COMMA_SEPARATOR, columnNames); int columnId = 0; for (auto& name : columnNames) { columnNameIdMapping[name] = columnId++; } return columnNameIdMapping; } int GnssRawMeasurementParser::getClockFlags( const std::vector<std::string>& rawMeasurementRecordValues, const std::unordered_map<std::string, int>& columnNameIdMapping) { int clockFlags = 0; if (!rawMeasurementRecordValues[columnNameIdMapping.at("LeapSecond")].empty()) { clockFlags |= GnssClock::HAS_LEAP_SECOND; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("FullBiasNanos")].empty()) { clockFlags |= GnssClock::HAS_FULL_BIAS; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("BiasNanos")].empty()) { clockFlags |= GnssClock::HAS_BIAS; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("BiasUncertaintyNanos")].empty()) { clockFlags |= GnssClock::HAS_BIAS_UNCERTAINTY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("DriftNanosPerSecond")].empty()) { clockFlags |= GnssClock::HAS_DRIFT; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("DriftUncertaintyNanosPerSecond")] .empty()) { clockFlags |= GnssClock::HAS_DRIFT_UNCERTAINTY; } return clockFlags; } int GnssRawMeasurementParser::getElapsedRealtimeFlags( const std::vector<std::string>& rawMeasurementRecordValues, const std::unordered_map<std::string, int>& columnNameIdMapping) { int elapsedRealtimeFlags = ElapsedRealtime::HAS_TIMESTAMP_NS; if (!rawMeasurementRecordValues[columnNameIdMapping.at("TimeUncertaintyNanos")].empty()) { elapsedRealtimeFlags |= ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS; } return elapsedRealtimeFlags; } int GnssRawMeasurementParser::getRawMeasurementFlags( const std::vector<std::string>& rawMeasurementRecordValues, const std::unordered_map<std::string, int>& columnNameIdMapping) { int rawMeasurementFlags = 0; if (!rawMeasurementRecordValues[columnNameIdMapping.at("SnrInDb")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_SNR; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierFrequencyHz")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_FREQUENCY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierCycles")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_CYCLES; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierPhase")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_PHASE; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("CarrierPhaseUncertainty")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_CARRIER_PHASE_UNCERTAINTY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("AgcDb")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_AUTOMATIC_GAIN_CONTROL; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("FullInterSignalBiasNanos")].empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_FULL_ISB; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("FullInterSignalBiasUncertaintyNanos")] .empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_FULL_ISB_UNCERTAINTY; } if (!rawMeasurementRecordValues[columnNameIdMapping.at("SatelliteInterSignalBiasNanos")] .empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_SATELLITE_ISB; } if (!rawMeasurementRecordValues[columnNameIdMapping.at( "SatelliteInterSignalBiasUncertaintyNanos")] .empty()) { rawMeasurementFlags |= GnssMeasurement::HAS_SATELLITE_ISB_UNCERTAINTY; } // HAS_SATELLITE_PVT and HAS_CORRELATION_VECTOR fields currently not in rawmeasurement // output, need add them later. return rawMeasurementFlags; } GnssConstellationType GnssRawMeasurementParser::getGnssConstellationType(int constellationType) { GnssConstellationType gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::UNKNOWN; switch (constellationType) { case 1: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::GPS; break; case 2: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::SBAS; break; case 3: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::GLONASS; break; case 4: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::QZSS; break; case 5: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::BEIDOU; break; case 6: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::GALILEO; break; default: gnssConstellationType = aidl::android::hardware::gnss::GnssConstellationType::UNKNOWN; } return gnssConstellationType; } std::unique_ptr<GnssData> GnssRawMeasurementParser::getMeasurementFromStrs( std::string& rawMeasurementStr) { /* * Raw,utcTimeMillis,TimeNanos,LeapSecond,TimeUncertaintyNanos,FullBiasNanos,BiasNanos, * BiasUncertaintyNanos,DriftNanosPerSecond,DriftUncertaintyNanosPerSecond, * HardwareClockDiscontinuityCount,Svid,TimeOffsetNanos,State,ReceivedSvTimeNanos, * ReceivedSvTimeUncertaintyNanos,Cn0DbHz,PseudorangeRateMetersPerSecond, * PseudorangeRateUncertaintyMetersPerSecond,AccumulatedDeltaRangeState, * AccumulatedDeltaRangeMeters,AccumulatedDeltaRangeUncertaintyMeters,CarrierFrequencyHz, * CarrierCycles,CarrierPhase,CarrierPhaseUncertainty,MultipathIndicator,SnrInDb, * ConstellationType,AgcDb,BasebandCn0DbHz,FullInterSignalBiasNanos, * FullInterSignalBiasUncertaintyNanos,SatelliteInterSignalBiasNanos, * SatelliteInterSignalBiasUncertaintyNanos,CodeType,ChipsetElapsedRealtimeNanos */ ALOGD("Parsing %zu bytes rawMeasurementStr.", rawMeasurementStr.size()); if (rawMeasurementStr.empty()) { return nullptr; } std::vector<std::string> rawMeasurementStrRecords; ParseUtils::splitStr(rawMeasurementStr, LINE_SEPARATOR, rawMeasurementStrRecords); if (rawMeasurementStrRecords.size() <= 1) { ALOGE("Raw GNSS Measurements parser failed. (No records) "); return nullptr; } // Get the column name mapping from the header. std::unordered_map<std::string, int> columnNameIdMapping = getColumnIdNameMappingFromHeader(rawMeasurementStrRecords[0]); if (columnNameIdMapping.size() < 37 || !ParseUtils::isValidHeader(columnNameIdMapping)) { ALOGE("Raw GNSS Measurements parser failed. (No header or missing columns.) "); return nullptr; } // Set GnssClock from 1st record. std::size_t pointer = 1; std::vector<std::string> firstRecordValues; ParseUtils::splitStr(rawMeasurementStrRecords[pointer], COMMA_SEPARATOR, firstRecordValues); GnssClock clock = { .gnssClockFlags = getClockFlags(firstRecordValues, columnNameIdMapping), .timeNs = ParseUtils::tryParseLongLong( firstRecordValues[columnNameIdMapping.at("TimeNanos")], 0), .fullBiasNs = ParseUtils::tryParseLongLong( firstRecordValues[columnNameIdMapping.at("FullBiasNanos")], 0), .biasNs = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("BiasNanos")], 0), .biasUncertaintyNs = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("BiasUncertaintyNanos")], 0), .driftNsps = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("DriftNanosPerSecond")], 0), .driftUncertaintyNsps = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("DriftNanosPerSecond")], 0), .hwClockDiscontinuityCount = ParseUtils::tryParseInt( firstRecordValues[columnNameIdMapping.at("HardwareClockDiscontinuityCount")], 0)}; ElapsedRealtime timestamp = { .flags = getElapsedRealtimeFlags(firstRecordValues, columnNameIdMapping), .timestampNs = ParseUtils::tryParseLongLong( firstRecordValues[columnNameIdMapping.at("ChipsetElapsedRealtimeNanos")]), .timeUncertaintyNs = ParseUtils::tryParseDouble( firstRecordValues[columnNameIdMapping.at("TimeUncertaintyNanos")], 0)}; std::vector<GnssMeasurement> measurementsVec; for (pointer = 1; pointer < rawMeasurementStrRecords.size(); pointer++) { std::vector<std::string> rawMeasurementValues; std::string line = rawMeasurementStrRecords[pointer]; ParseUtils::splitStr(line, COMMA_SEPARATOR, rawMeasurementValues); GnssSignalType signalType = { .constellation = getGnssConstellationType(ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("ConstellationType")], 0)), .carrierFrequencyHz = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("CarrierFrequencyHz")], 0), .codeType = rawMeasurementValues[columnNameIdMapping.at("CodeType")], }; GnssMeasurement measurement = { .flags = getRawMeasurementFlags(rawMeasurementValues, columnNameIdMapping), .svid = ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("Svid")], 0), .signalType = signalType, .receivedSvTimeInNs = ParseUtils::tryParseLongLong( rawMeasurementValues[columnNameIdMapping.at("ReceivedSvTimeNanos")], 0), .receivedSvTimeUncertaintyInNs = ParseUtils::tryParseLongLong(rawMeasurementValues[columnNameIdMapping.at( "ReceivedSvTimeUncertaintyNanos")], 0), .antennaCN0DbHz = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("Cn0DbHz")], 0), .basebandCN0DbHz = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("BasebandCn0DbHz")], 0), .agcLevelDb = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("AgcDb")], 0), .pseudorangeRateMps = ParseUtils::tryParseDouble(rawMeasurementValues[columnNameIdMapping.at( "PseudorangeRateMetersPerSecond")], 0), .pseudorangeRateUncertaintyMps = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at( "PseudorangeRateUncertaintyMetersPerSecond")], 0), .accumulatedDeltaRangeState = ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("AccumulatedDeltaRangeState")], 0), .accumulatedDeltaRangeM = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("AccumulatedDeltaRangeMeters")], 0), .accumulatedDeltaRangeUncertaintyM = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at( "AccumulatedDeltaRangeUncertaintyMeters")], 0), .multipathIndicator = GnssMultipathIndicator::UNKNOWN, // Not in GnssLogger yet. .state = ParseUtils::tryParseInt( rawMeasurementValues[columnNameIdMapping.at("State")], 0), .fullInterSignalBiasNs = ParseUtils::tryParseDouble(rawMeasurementValues[31], 0), .fullInterSignalBiasUncertaintyNs = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at("FullInterSignalBiasNanos")], 0), .satelliteInterSignalBiasNs = ParseUtils::tryParseDouble(rawMeasurementValues[columnNameIdMapping.at( "SatelliteInterSignalBiasNanos")], 0), .satelliteInterSignalBiasUncertaintyNs = ParseUtils::tryParseDouble( rawMeasurementValues[columnNameIdMapping.at( "SatelliteInterSignalBiasUncertaintyNanos")], 0), .satellitePvt = {}, .correlationVectors = {}}; measurementsVec.push_back(measurement); } GnssData gnssData = { .measurements = measurementsVec, .clock = clock, .elapsedRealtime = timestamp}; return std::make_unique<GnssData>(gnssData); } } // namespace common } // namespace gnss } // namespace hardware } // namespace android