diff options
Diffstat (limited to 'gnss/common/utils/default/GnssRawMeasurementParser.cpp')
| -rw-r--r-- | gnss/common/utils/default/GnssRawMeasurementParser.cpp | 305 |
1 files changed, 305 insertions, 0 deletions
diff --git a/gnss/common/utils/default/GnssRawMeasurementParser.cpp b/gnss/common/utils/default/GnssRawMeasurementParser.cpp new file mode 100644 index 0000000000..c066229ae9 --- /dev/null +++ b/gnss/common/utils/default/GnssRawMeasurementParser.cpp @@ -0,0 +1,305 @@ +/* + * 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 |