| /* Copyright (c) 2017-2020, The Linux Foundation. All rights reserved. |
| * |
| * Redistribution and use in source and binary forms, with or without |
| * modification, are permitted provided that the following conditions are |
| * met: |
| * * Redistributions of source code must retain the above copyright |
| * notice, this list of conditions and the following disclaimer. |
| * * Redistributions in binary form must reproduce the above |
| * copyright notice, this list of conditions and the following |
| * disclaimer in the documentation and/or other materials provided |
| * with the distribution. |
| * * Neither the name of The Linux Foundation, nor the names of its |
| * contributors may be used to endorse or promote products derived |
| * from this software without specific prior written permission. |
| * |
| * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED |
| * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF |
| * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT |
| * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS |
| * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR |
| * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, |
| * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE |
| * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN |
| * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| * |
| */ |
| |
| #include <LocationUtil.h> |
| #include <log_util.h> |
| #include <inttypes.h> |
| #include <loc_misc_utils.h> |
| #include <gps_extended_c.h> |
| |
| namespace android { |
| namespace hardware { |
| namespace gnss { |
| namespace V2_1 { |
| namespace implementation { |
| |
| using ::android::hardware::gnss::V2_0::GnssLocation; |
| using ::android::hardware::gnss::V2_0::ElapsedRealtimeFlags; |
| using ::android::hardware::gnss::V2_0::GnssConstellationType; |
| using ::android::hardware::gnss::V1_0::GnssLocationFlags; |
| using ::android::hardware::gnss::measurement_corrections::V1_0::GnssSingleSatCorrectionFlags; |
| |
| void convertGnssLocation(Location& in, V1_0::GnssLocation& out) |
| { |
| memset(&out, 0, sizeof(V1_0::GnssLocation)); |
| if (in.flags & LOCATION_HAS_LAT_LONG_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_LAT_LONG; |
| out.latitudeDegrees = in.latitude; |
| out.longitudeDegrees = in.longitude; |
| } |
| if (in.flags & LOCATION_HAS_ALTITUDE_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_ALTITUDE; |
| out.altitudeMeters = in.altitude; |
| } |
| if (in.flags & LOCATION_HAS_SPEED_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED; |
| out.speedMetersPerSec = in.speed; |
| } |
| if (in.flags & LOCATION_HAS_BEARING_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING; |
| out.bearingDegrees = in.bearing; |
| } |
| if (in.flags & LOCATION_HAS_ACCURACY_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_HORIZONTAL_ACCURACY; |
| out.horizontalAccuracyMeters = in.accuracy; |
| } |
| if (in.flags & LOCATION_HAS_VERTICAL_ACCURACY_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_VERTICAL_ACCURACY; |
| out.verticalAccuracyMeters = in.verticalAccuracy; |
| } |
| if (in.flags & LOCATION_HAS_SPEED_ACCURACY_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_SPEED_ACCURACY; |
| out.speedAccuracyMetersPerSecond = in.speedAccuracy; |
| } |
| if (in.flags & LOCATION_HAS_BEARING_ACCURACY_BIT) { |
| out.gnssLocationFlags |= GnssLocationFlags::HAS_BEARING_ACCURACY; |
| out.bearingAccuracyDegrees = in.bearingAccuracy; |
| } |
| |
| out.timestamp = static_cast<V1_0::GnssUtcTime>(in.timestamp); |
| } |
| |
| bool getCurrentTime(struct timespec& currentTime, int64_t& sinceBootTimeNanos) |
| { |
| struct timespec sinceBootTime; |
| struct timespec sinceBootTimeTest; |
| bool clockGetTimeSuccess = false; |
| const uint32_t MAX_TIME_DELTA_VALUE_NANOS = 10000; |
| const uint32_t MAX_GET_TIME_COUNT = 20; |
| /* Attempt to get CLOCK_REALTIME and CLOCK_BOOTIME in succession without an interruption |
| or context switch (for up to MAX_GET_TIME_COUNT times) to avoid errors in the calculation */ |
| for (uint32_t i = 0; i < MAX_GET_TIME_COUNT; i++) { |
| if (clock_gettime(CLOCK_BOOTTIME, &sinceBootTime) != 0) { |
| break; |
| }; |
| if (clock_gettime(CLOCK_REALTIME, ¤tTime) != 0) { |
| break; |
| } |
| if (clock_gettime(CLOCK_BOOTTIME, &sinceBootTimeTest) != 0) { |
| break; |
| }; |
| sinceBootTimeNanos = sinceBootTime.tv_sec * 1000000000 + sinceBootTime.tv_nsec; |
| int64_t sinceBootTimeTestNanos = |
| sinceBootTimeTest.tv_sec * 1000000000 + sinceBootTimeTest.tv_nsec; |
| int64_t sinceBootTimeDeltaNanos = sinceBootTimeTestNanos - sinceBootTimeNanos; |
| |
| /* sinceBootTime and sinceBootTimeTest should have a close value if there was no |
| interruption or context switch between clock_gettime for CLOCK_BOOTIME and |
| clock_gettime for CLOCK_REALTIME */ |
| if (sinceBootTimeDeltaNanos < MAX_TIME_DELTA_VALUE_NANOS) { |
| clockGetTimeSuccess = true; |
| break; |
| } else { |
| LOC_LOGd("Delta:%" PRIi64 "ns time too large, retry number #%u...", |
| sinceBootTimeDeltaNanos, i + 1); |
| } |
| } |
| return clockGetTimeSuccess; |
| } |
| |
| void convertGnssLocation(Location& in, V2_0::GnssLocation& out) |
| { |
| memset(&out, 0, sizeof(V2_0::GnssLocation)); |
| convertGnssLocation(in, out.v1_0); |
| |
| struct timespec currentTime; |
| int64_t sinceBootTimeNanos; |
| |
| if (getCurrentTime(currentTime, sinceBootTimeNanos)) { |
| if (in.flags & LOCATION_HAS_ELAPSED_REAL_TIME) { |
| uint64_t qtimerDiff = 0; |
| uint64_t qTimerTickCount = getQTimerTickCount(); |
| if (qTimerTickCount >= in.elapsedRealTime) { |
| qtimerDiff = qTimerTickCount - in.elapsedRealTime; |
| } |
| LOC_LOGv("sinceBootTimeNanos:%" PRIi64 " in.elapsedRealTime=%" PRIi64 "" |
| " qTimerTickCount=%" PRIi64 " qtimerDiff=%" PRIi64 "", |
| sinceBootTimeNanos, in.elapsedRealTime, qTimerTickCount, qtimerDiff); |
| uint64_t qTimerDiffNanos = qTimerTicksToNanos(double(qtimerDiff)); |
| if (sinceBootTimeNanos >= qTimerDiffNanos) { |
| out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIMESTAMP_NS; |
| out.elapsedRealtime.timestampNs = sinceBootTimeNanos - qTimerDiffNanos; |
| out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS; |
| out.elapsedRealtime.timeUncertaintyNs = in.elapsedRealTimeUnc; |
| } |
| } else { |
| int64_t currentTimeNanos = currentTime.tv_sec*1000000000 + currentTime.tv_nsec; |
| int64_t locationTimeNanos = in.timestamp*1000000; |
| LOC_LOGv("sinceBootTimeNanos:%" PRIi64 " currentTimeNanos:%" PRIi64 "" |
| " locationTimeNanos:%" PRIi64 "", |
| sinceBootTimeNanos, currentTimeNanos, locationTimeNanos); |
| if (currentTimeNanos >= locationTimeNanos) { |
| int64_t ageTimeNanos = currentTimeNanos - locationTimeNanos; |
| LOC_LOGv("ageTimeNanos:%" PRIi64 ")", ageTimeNanos); |
| // the max trusted propagation time 100ms for ageTimeNanos to avoid user setting |
| // wrong time, it will affect elapsedRealtimeNanos |
| if (ageTimeNanos <= 100000000) { |
| out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIMESTAMP_NS; |
| out.elapsedRealtime.timestampNs = sinceBootTimeNanos - ageTimeNanos; |
| out.elapsedRealtime.flags |= ElapsedRealtimeFlags::HAS_TIME_UNCERTAINTY_NS; |
| // time uncertainty is the max value between abs(AP_UTC - MP_UTC) and 100ms, to |
| // verify if user change the sys time |
| out.elapsedRealtime.timeUncertaintyNs = |
| std::max(ageTimeNanos, (int64_t)100000000); |
| } |
| } |
| } |
| } |
| LOC_LOGv("out.elapsedRealtime.timestampNs=%" PRIi64 "" |
| " out.elapsedRealtime.timeUncertaintyNs=%" PRIi64 "" |
| " out.elapsedRealtime.flags=0x%X", |
| out.elapsedRealtime.timestampNs, |
| out.elapsedRealtime.timeUncertaintyNs, out.elapsedRealtime.flags); |
| } |
| |
| void convertGnssLocation(const V1_0::GnssLocation& in, Location& out) |
| { |
| memset(&out, 0, sizeof(out)); |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_LAT_LONG) { |
| out.flags |= LOCATION_HAS_LAT_LONG_BIT; |
| out.latitude = in.latitudeDegrees; |
| out.longitude = in.longitudeDegrees; |
| } |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_ALTITUDE) { |
| out.flags |= LOCATION_HAS_ALTITUDE_BIT; |
| out.altitude = in.altitudeMeters; |
| } |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED) { |
| out.flags |= LOCATION_HAS_SPEED_BIT; |
| out.speed = in.speedMetersPerSec; |
| } |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING) { |
| out.flags |= LOCATION_HAS_BEARING_BIT; |
| out.bearing = in.bearingDegrees; |
| } |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_HORIZONTAL_ACCURACY) { |
| out.flags |= LOCATION_HAS_ACCURACY_BIT; |
| out.accuracy = in.horizontalAccuracyMeters; |
| } |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_VERTICAL_ACCURACY) { |
| out.flags |= LOCATION_HAS_VERTICAL_ACCURACY_BIT; |
| out.verticalAccuracy = in.verticalAccuracyMeters; |
| } |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_SPEED_ACCURACY) { |
| out.flags |= LOCATION_HAS_SPEED_ACCURACY_BIT; |
| out.speedAccuracy = in.speedAccuracyMetersPerSecond; |
| } |
| if (in.gnssLocationFlags & GnssLocationFlags::HAS_BEARING_ACCURACY) { |
| out.flags |= LOCATION_HAS_BEARING_ACCURACY_BIT; |
| out.bearingAccuracy = in.bearingAccuracyDegrees; |
| } |
| |
| out.timestamp = static_cast<uint64_t>(in.timestamp); |
| } |
| |
| void convertGnssLocation(const V2_0::GnssLocation& in, Location& out) |
| { |
| memset(&out, 0, sizeof(out)); |
| convertGnssLocation(in.v1_0, out); |
| } |
| |
| void convertGnssConstellationType(GnssSvType& in, V1_0::GnssConstellationType& out) |
| { |
| switch(in) { |
| case GNSS_SV_TYPE_GPS: |
| out = V1_0::GnssConstellationType::GPS; |
| break; |
| case GNSS_SV_TYPE_SBAS: |
| out = V1_0::GnssConstellationType::SBAS; |
| break; |
| case GNSS_SV_TYPE_GLONASS: |
| out = V1_0::GnssConstellationType::GLONASS; |
| break; |
| case GNSS_SV_TYPE_QZSS: |
| out = V1_0::GnssConstellationType::QZSS; |
| break; |
| case GNSS_SV_TYPE_BEIDOU: |
| out = V1_0::GnssConstellationType::BEIDOU; |
| break; |
| case GNSS_SV_TYPE_GALILEO: |
| out = V1_0::GnssConstellationType::GALILEO; |
| break; |
| case GNSS_SV_TYPE_UNKNOWN: |
| default: |
| out = V1_0::GnssConstellationType::UNKNOWN; |
| break; |
| } |
| } |
| |
| void convertGnssConstellationType(GnssSvType& in, V2_0::GnssConstellationType& out) |
| { |
| switch(in) { |
| case GNSS_SV_TYPE_GPS: |
| out = V2_0::GnssConstellationType::GPS; |
| break; |
| case GNSS_SV_TYPE_SBAS: |
| out = V2_0::GnssConstellationType::SBAS; |
| break; |
| case GNSS_SV_TYPE_GLONASS: |
| out = V2_0::GnssConstellationType::GLONASS; |
| break; |
| case GNSS_SV_TYPE_QZSS: |
| out = V2_0::GnssConstellationType::QZSS; |
| break; |
| case GNSS_SV_TYPE_BEIDOU: |
| out = V2_0::GnssConstellationType::BEIDOU; |
| break; |
| case GNSS_SV_TYPE_GALILEO: |
| out = V2_0::GnssConstellationType::GALILEO; |
| break; |
| case GNSS_SV_TYPE_NAVIC: |
| out = V2_0::GnssConstellationType::IRNSS; |
| break; |
| case GNSS_SV_TYPE_UNKNOWN: |
| default: |
| out = V2_0::GnssConstellationType::UNKNOWN; |
| break; |
| } |
| } |
| |
| void convertGnssSvid(GnssSv& in, int16_t& out) |
| { |
| switch (in.type) { |
| case GNSS_SV_TYPE_GPS: |
| out = in.svId; |
| break; |
| case GNSS_SV_TYPE_SBAS: |
| out = in.svId; |
| break; |
| case GNSS_SV_TYPE_GLONASS: |
| out = in.svId - GLO_SV_PRN_MIN + 1; |
| break; |
| case GNSS_SV_TYPE_QZSS: |
| out = in.svId; |
| break; |
| case GNSS_SV_TYPE_BEIDOU: |
| out = in.svId - BDS_SV_PRN_MIN + 1; |
| break; |
| case GNSS_SV_TYPE_GALILEO: |
| out = in.svId - GAL_SV_PRN_MIN + 1; |
| break; |
| case GNSS_SV_TYPE_NAVIC: |
| /*Android doesn't define Navic svid range yet, use Naviv svid [1, 14] now |
| will update this once Android give Navic svid definiitons */ |
| out = in.svId - NAVIC_SV_PRN_MIN + 1; |
| break; |
| default: |
| out = in.svId; |
| break; |
| } |
| } |
| |
| void convertGnssSvid(GnssMeasurementsData& in, int16_t& out) |
| { |
| switch (in.svType) { |
| case GNSS_SV_TYPE_GPS: |
| out = in.svId; |
| break; |
| case GNSS_SV_TYPE_SBAS: |
| out = in.svId; |
| break; |
| case GNSS_SV_TYPE_GLONASS: |
| if (in.svId != 255) { // OSN is known |
| out = in.svId - GLO_SV_PRN_MIN + 1; |
| } else { // OSN is not known, report FCN |
| out = in.gloFrequency + 92; |
| } |
| break; |
| case GNSS_SV_TYPE_QZSS: |
| out = in.svId; |
| break; |
| case GNSS_SV_TYPE_BEIDOU: |
| out = in.svId - BDS_SV_PRN_MIN + 1; |
| break; |
| case GNSS_SV_TYPE_GALILEO: |
| out = in.svId - GAL_SV_PRN_MIN + 1; |
| break; |
| case GNSS_SV_TYPE_NAVIC: |
| /*Android doesn't define Navic svid range yet, use Naviv svid [1, 14] now |
| will update this once Android give Navic svid definiitons */ |
| out = in.svId - NAVIC_SV_PRN_MIN + 1; |
| break; |
| default: |
| out = in.svId; |
| break; |
| } |
| } |
| |
| void convertGnssEphemerisType(GnssEphemerisType& in, GnssDebug::SatelliteEphemerisType& out) |
| { |
| switch(in) { |
| case GNSS_EPH_TYPE_EPHEMERIS: |
| out = GnssDebug::SatelliteEphemerisType::EPHEMERIS; |
| break; |
| case GNSS_EPH_TYPE_ALMANAC: |
| out = GnssDebug::SatelliteEphemerisType::ALMANAC_ONLY; |
| break; |
| case GNSS_EPH_TYPE_UNKNOWN: |
| default: |
| out = GnssDebug::SatelliteEphemerisType::NOT_AVAILABLE; |
| break; |
| } |
| } |
| |
| void convertGnssEphemerisSource(GnssEphemerisSource& in, GnssDebug::SatelliteEphemerisSource& out) |
| { |
| switch(in) { |
| case GNSS_EPH_SOURCE_DEMODULATED: |
| out = GnssDebug::SatelliteEphemerisSource::DEMODULATED; |
| break; |
| case GNSS_EPH_SOURCE_SUPL_PROVIDED: |
| out = GnssDebug::SatelliteEphemerisSource::SUPL_PROVIDED; |
| break; |
| case GNSS_EPH_SOURCE_OTHER_SERVER_PROVIDED: |
| out = GnssDebug::SatelliteEphemerisSource::OTHER_SERVER_PROVIDED; |
| break; |
| case GNSS_EPH_SOURCE_LOCAL: |
| case GNSS_EPH_SOURCE_UNKNOWN: |
| default: |
| out = GnssDebug::SatelliteEphemerisSource::OTHER; |
| break; |
| } |
| } |
| |
| void convertGnssEphemerisHealth(GnssEphemerisHealth& in, GnssDebug::SatelliteEphemerisHealth& out) |
| { |
| switch(in) { |
| case GNSS_EPH_HEALTH_GOOD: |
| out = GnssDebug::SatelliteEphemerisHealth::GOOD; |
| break; |
| case GNSS_EPH_HEALTH_BAD: |
| out = GnssDebug::SatelliteEphemerisHealth::BAD; |
| break; |
| case GNSS_EPH_HEALTH_UNKNOWN: |
| default: |
| out = GnssDebug::SatelliteEphemerisHealth::UNKNOWN; |
| break; |
| } |
| } |
| |
| void convertSingleSatCorrections(const SingleSatCorrection& in, GnssSingleSatCorrection& out) |
| { |
| out.flags = GNSS_MEAS_CORR_UNKNOWN_BIT; |
| if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_SAT_IS_LOS_PROBABILITY)) { |
| out.flags |= GNSS_MEAS_CORR_HAS_SAT_IS_LOS_PROBABILITY_BIT; |
| } |
| if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH)) { |
| out.flags |= GNSS_MEAS_CORR_HAS_EXCESS_PATH_LENGTH_BIT; |
| } |
| if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_EXCESS_PATH_LENGTH_UNC)) { |
| out.flags |= GNSS_MEAS_CORR_HAS_EXCESS_PATH_LENGTH_UNC_BIT; |
| } |
| if (in.singleSatCorrectionFlags & (GnssSingleSatCorrectionFlags::HAS_REFLECTING_PLANE)) { |
| out.flags |= GNSS_MEAS_CORR_HAS_REFLECTING_PLANE_BIT; |
| } |
| switch (in.constellation) { |
| case (::android::hardware::gnss::V1_0::GnssConstellationType::GPS): |
| out.svType = GNSS_SV_TYPE_GPS; |
| break; |
| case (::android::hardware::gnss::V1_0::GnssConstellationType::SBAS): |
| out.svType = GNSS_SV_TYPE_SBAS; |
| break; |
| case (::android::hardware::gnss::V1_0::GnssConstellationType::GLONASS): |
| out.svType = GNSS_SV_TYPE_GLONASS; |
| break; |
| case (::android::hardware::gnss::V1_0::GnssConstellationType::QZSS): |
| out.svType = GNSS_SV_TYPE_QZSS; |
| break; |
| case (::android::hardware::gnss::V1_0::GnssConstellationType::BEIDOU): |
| out.svType = GNSS_SV_TYPE_BEIDOU; |
| break; |
| case (::android::hardware::gnss::V1_0::GnssConstellationType::GALILEO): |
| out.svType = GNSS_SV_TYPE_GALILEO; |
| break; |
| case (::android::hardware::gnss::V1_0::GnssConstellationType::UNKNOWN): |
| default: |
| out.svType = GNSS_SV_TYPE_UNKNOWN; |
| break; |
| } |
| out.svId = in.svid; |
| out.carrierFrequencyHz = in.carrierFrequencyHz; |
| out.probSatIsLos = in.probSatIsLos; |
| out.excessPathLengthMeters = in.excessPathLengthMeters; |
| out.excessPathLengthUncertaintyMeters = in.excessPathLengthUncertaintyMeters; |
| |
| out.reflectingPlane.latitudeDegrees = in.reflectingPlane.latitudeDegrees; |
| out.reflectingPlane.longitudeDegrees = in.reflectingPlane.longitudeDegrees; |
| out.reflectingPlane.altitudeMeters = in.reflectingPlane.altitudeMeters; |
| out.reflectingPlane.azimuthDegrees = in.reflectingPlane.azimuthDegrees; |
| } |
| |
| void convertMeasurementCorrections(const MeasurementCorrectionsV1_0& in, |
| GnssMeasurementCorrections& out) |
| { |
| memset(&out, 0, sizeof(GnssMeasurementCorrections)); |
| out.latitudeDegrees = in.latitudeDegrees; |
| out.longitudeDegrees = in.longitudeDegrees; |
| out.altitudeMeters = in.altitudeMeters; |
| out.horizontalPositionUncertaintyMeters = in.horizontalPositionUncertaintyMeters; |
| out.verticalPositionUncertaintyMeters = in.verticalPositionUncertaintyMeters; |
| out.toaGpsNanosecondsOfWeek = in.toaGpsNanosecondsOfWeek; |
| |
| for (int i = 0; i < in.satCorrections.size(); i++) { |
| GnssSingleSatCorrection gnssSingleSatCorrection; |
| |
| convertSingleSatCorrections(in.satCorrections[i], gnssSingleSatCorrection); |
| out.satCorrections.push_back(gnssSingleSatCorrection); |
| } |
| } |
| |
| } // namespace implementation |
| } // namespace V2_1 |
| } // namespace gnss |
| } // namespace hardware |
| } // namespace android |
