1 /*
2  * Copyright (C) 2020 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <chrono>
18 #include <aidl/android/hardware/gnss/IGnss.h>
19 #include <debug.h>
20 #include "GnssMeasurementInterface.h"
21 
22 namespace aidl {
23 namespace android {
24 namespace hardware {
25 namespace gnss {
26 namespace implementation {
27 namespace {
28 using Clock = std::chrono::steady_clock;
29 
initGnssData(GnssData & data,const int64_t elapsedRealtimeNs,const int64_t timeNs,const int64_t fullBiasNs,const double biasUncertaintyNs,const size_t nMeasurements)30 void initGnssData(GnssData& data,
31                   const int64_t elapsedRealtimeNs,
32                   const int64_t timeNs,
33                   const int64_t fullBiasNs,
34                   const double biasUncertaintyNs,
35                   const size_t nMeasurements) {
36     data.elapsedRealtime.flags = ElapsedRealtime::HAS_TIMESTAMP_NS;
37     data.elapsedRealtime.timestampNs = elapsedRealtimeNs;
38     data.clock.gnssClockFlags = GnssClock::HAS_FULL_BIAS;
39     data.clock.timeNs = timeNs;
40     data.clock.fullBiasNs = fullBiasNs;
41     data.clock.biasUncertaintyNs = biasUncertaintyNs;
42     data.measurements.resize(nMeasurements);
43 }
44 
makeGnssMeasurement(const bool enableCorrVecOutputs,const int svid,const int state,const int64_t receivedSvTimeInNs,const int64_t receivedSvTimeUncertaintyInNs,const double cN0DbHz,const double pseudorangeRateMps,const double pseudorangeRateUncertaintyMps,const int accumulatedDeltaRangeState,const double accumulatedDeltaRangeM,const double accumulatedDeltaRangeUncertaintyM,const int multipathIndicator,const int constellation)45 GnssMeasurement makeGnssMeasurement(const bool enableCorrVecOutputs,
46                                     const int svid,
47                                     const int state,
48                                     const int64_t receivedSvTimeInNs,
49                                     const int64_t receivedSvTimeUncertaintyInNs,
50                                     const double cN0DbHz,
51                                     const double pseudorangeRateMps,
52                                     const double pseudorangeRateUncertaintyMps,
53                                     const int accumulatedDeltaRangeState,
54                                     const double accumulatedDeltaRangeM,
55                                     const double accumulatedDeltaRangeUncertaintyM,
56                                     const int multipathIndicator,
57                                     const int constellation) {
58     GnssMeasurement m;
59 
60     m.flags = GnssMeasurement::HAS_CARRIER_FREQUENCY;
61     m.svid = svid;
62 
63     m.signalType.constellation = static_cast<GnssConstellationType>(constellation);
64     m.signalType.carrierFrequencyHz = 1.59975e+09;
65     m.signalType.codeType = "UNKNOWN";
66 
67     m.timeOffsetNs = 0;
68     m.state = GnssMeasurement::STATE_UNKNOWN | state;
69     m.receivedSvTimeInNs = receivedSvTimeInNs;
70     m.receivedSvTimeUncertaintyInNs = receivedSvTimeUncertaintyInNs;
71     m.antennaCN0DbHz = cN0DbHz;
72     m.basebandCN0DbHz = cN0DbHz - 4;
73     m.pseudorangeRateMps = pseudorangeRateMps;
74     m.pseudorangeRateUncertaintyMps = pseudorangeRateUncertaintyMps;
75     m.accumulatedDeltaRangeState = accumulatedDeltaRangeState;
76     m.accumulatedDeltaRangeM = accumulatedDeltaRangeM;
77     m.accumulatedDeltaRangeUncertaintyM = accumulatedDeltaRangeUncertaintyM;
78     m.multipathIndicator = static_cast<GnssMultipathIndicator>(multipathIndicator);
79 
80     if (enableCorrVecOutputs) {
81         const CorrelationVector correlationVector1 = {
82                 .frequencyOffsetMps = 10,
83                 .samplingWidthM = 30,
84                 .samplingStartM = 0,
85                 .magnitude = {0, 5000, 10000, 5000, 0, 0, 3000, 0}};
86 
87         const CorrelationVector correlationVector2 = {
88                 .frequencyOffsetMps = 20,
89                 .samplingWidthM = 30,
90                 .samplingStartM = -10,
91                 .magnitude = {0, 3000, 5000, 3000, 0, 0, 1000, 0}};
92 
93         m.correlationVectors = {correlationVector1, correlationVector2};
94         m.flags = GnssMeasurement::HAS_CORRELATION_VECTOR;
95     }
96 
97     return m;
98 }
99 
100 }  // namsepace
101 
~GnssMeasurementInterface()102 GnssMeasurementInterface::~GnssMeasurementInterface() {
103     closeImpl();
104 }
105 
setCallback(const std::shared_ptr<IGnssMeasurementCallback> & callback,const bool,const bool enableCorrVecOutputs)106 ndk::ScopedAStatus GnssMeasurementInterface::setCallback(
107         const std::shared_ptr<IGnssMeasurementCallback>& callback,
108         const bool /*enableFullTracking*/,
109         const bool enableCorrVecOutputs) {
110     return setCallbackImpl(callback, enableCorrVecOutputs, 1000);
111 }
112 
close()113 ndk::ScopedAStatus GnssMeasurementInterface::close() {
114     closeImpl();
115     return ndk::ScopedAStatus::ok();
116 }
117 
setCallbackWithOptions(const std::shared_ptr<IGnssMeasurementCallback> & callback,const Options & options)118 ndk::ScopedAStatus GnssMeasurementInterface::setCallbackWithOptions(
119         const std::shared_ptr<IGnssMeasurementCallback>& callback,
120         const Options& options) {
121     return setCallbackImpl(callback, options.enableCorrVecOutputs, options.intervalMs);
122 }
123 
closeImpl()124 void GnssMeasurementInterface::closeImpl() {
125     bool needJoin;
126 
127     {
128         std::lock_guard<std::mutex> lock(mMtx);
129         if (mThread.joinable()) {
130             mRunning = false;
131             mThreadNotification.notify_all();
132             needJoin = true;
133         } else {
134             needJoin = false;
135         }
136     }
137 
138     if (needJoin) {
139         mThread.join();
140     }
141 }
142 
setCallbackImpl(const std::shared_ptr<IGnssMeasurementCallback> & callback,const bool enableCorrVecOutputs,const int intervalMs)143 ndk::ScopedAStatus GnssMeasurementInterface::setCallbackImpl(
144         const std::shared_ptr<IGnssMeasurementCallback>& callback,
145         const bool enableCorrVecOutputs,
146         const int intervalMs) {
147     if (!callback) {
148         return ndk::ScopedAStatus::fromExceptionCode(FAILURE(IGnss::ERROR_INVALID_ARGUMENT));
149     }
150 
151     if (intervalMs <= 0) {
152         return ndk::ScopedAStatus::fromExceptionCode(FAILURE(IGnss::ERROR_INVALID_ARGUMENT));
153     }
154 
155     mGnssData.resize(1);
156 
157     initGnssData(mGnssData[0], 139287, 116834000000, -1189181444165780000, 5.26068202130163, 7);
158     mGnssData[0].measurements[0] = makeGnssMeasurement(enableCorrVecOutputs, 22,  47, 3927349114,      29, 29.9917297363281,  245.509362821673,  0.148940800975766, 1,  6620.74237064615,  0.00271145859733223, 0, 1);
159     mGnssData[0].measurements[1] = makeGnssMeasurement(enableCorrVecOutputs, 23,  47, 3920005435,      14, 36.063377380371,  -731.947951627658, 0.0769754027959242, 1,  -23229.096048105,  0.00142954161856323, 0, 1);
160     mGnssData[0].measurements[2] = makeGnssMeasurement(enableCorrVecOutputs, 25,  47, 3923720994,      56, 24.5171585083007, -329.789995021822,  0.277918601850871, 1, -15511.1976492851,  0.00509250536561012, 0, 1);
161     mGnssData[0].measurements[3] = makeGnssMeasurement(enableCorrVecOutputs, 31,  47, 3925772934,      11, 37.9193840026855,  -380.23772244582, 0.0602980729893803, 1, -11325.9094456612,  0.00115450704470276, 0, 1);
162     mGnssData[0].measurements[4] = makeGnssMeasurement(enableCorrVecOutputs, 32,  47, 3919018415,      21, 32.8980560302734,  581.800347848025,  0.109060249597082, 1,  15707.8963147985,  0.00205808319151401, 0, 1);
163     mGnssData[0].measurements[5] = makeGnssMeasurement(enableCorrVecOutputs, 10, 227, 69142929947304, 127, 23.432445526123,    259.17838762857,   0.31591691295607, 4,  8152.78081298147, 3.40282346638528E+38, 0, 3);
164     mGnssData[0].measurements[6] = makeGnssMeasurement(enableCorrVecOutputs, 2,  227, 69142935176327,  41, 33.180908203125,  -53.8773853795901,  0.104984458760586, 1, -1708.08166640048,  0.00196184404194355, 0, 3);
165 
166     const Clock::duration interval = std::chrono::milliseconds(intervalMs);
167 
168     closeImpl();
169 
170     std::lock_guard<std::mutex> lock(mMtx);
171     mRunning = true;
172 
173     mThread = std::thread([this, callback, interval](){
174         Clock::time_point wakeupT = Clock::now() + interval;
175 
176         for (unsigned gnssDataIndex = 0;; gnssDataIndex = (gnssDataIndex + 1) % mGnssData.size(),
177                                           wakeupT += interval) {
178             std::unique_lock<std::mutex> lock(mMtx);
179             if ((mThreadNotification.wait_until(lock, wakeupT) == std::cv_status::no_timeout) &&
180                     !mRunning) {
181                 return;
182             }
183 
184             callback->gnssMeasurementCb(mGnssData[gnssDataIndex]);
185         }
186     });
187 
188     return ndk::ScopedAStatus::ok();
189 }
190 
191 }  // namespace implementation
192 }  // namespace gnss
193 }  // namespace hardware
194 }  // namespace android
195 }  // namespace aidl
196