platform_hardware_interfaces/gnss/common/utils/default/NmeaFixInfo.cpp

/*
 * 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 {

using aidl::android::hardware::gnss::ElapsedRealtime;
using aidl::android::hardware::gnss::GnssLocation;

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) {
        if (line.compare(0, strlen(GPGA_RECORD_TAG), GPGA_RECORD_TAG) != 0 &&
            line.compare(0, strlen(GPRMC_RECORD_TAG), GPRMC_RECORD_TAG) != 0) {
            continue;
        }
        std::vector<std::string> sentenceValues;
        splitStr(line, COMMA_SEPARATOR, sentenceValues);
        if (sentenceValues.size() < MIN_COL_NUM) {
            continue;
        }
        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();
}

/**
 * Convert V2_0::GnssLocation to aidl::GnssLocation.
 */
std::unique_ptr<GnssLocation> NmeaFixInfo::getAidlLocationFromInputStr(
        const std::string& inputStr) {
    std::unique_ptr<V2_0::GnssLocation> locationV2 = getLocationFromInputStr(inputStr);
    if (locationV2 == nullptr) {
        return nullptr;
    }

    ElapsedRealtime elapsedRealtime = {
            .flags = ElapsedRealtime::HAS_TIMESTAMP_NS | ElapsedRealtime::HAS_TIME_UNCERTAINTY_NS,
            .timestampNs = ::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};

    GnssLocation location = {
            .gnssLocationFlags = locationV2->v1_0.gnssLocationFlags,
            .latitudeDegrees = locationV2->v1_0.latitudeDegrees,
            .longitudeDegrees = locationV2->v1_0.longitudeDegrees,
            .altitudeMeters = locationV2->v1_0.altitudeMeters,
            .speedMetersPerSec = locationV2->v1_0.speedMetersPerSec,
            .bearingDegrees = locationV2->v1_0.bearingDegrees,
            .horizontalAccuracyMeters = locationV2->v1_0.horizontalAccuracyMeters,
            .verticalAccuracyMeters = locationV2->v1_0.verticalAccuracyMeters,
            .speedAccuracyMetersPerSecond = locationV2->v1_0.speedAccuracyMetersPerSecond,
            .bearingAccuracyDegrees = locationV2->v1_0.bearingAccuracyDegrees,
            .timestampMillis = locationV2->v1_0.timestamp,
            .elapsedRealtime = elapsedRealtime};
    return std::make_unique<GnssLocation>(location);
}

/**
 * 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