1 /*
2  * Copyright (C) 2022 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 package com.android.server.uwb;
18 
19 import android.annotation.NonNull;
20 import android.annotation.Nullable;
21 import android.uwb.AngleMeasurement;
22 import android.uwb.AngleOfArrivalMeasurement;
23 import android.uwb.DistanceMeasurement;
24 import android.uwb.RangingMeasurement;
25 import android.uwb.UwbAddress;
26 
27 import com.android.server.uwb.correction.UwbFilterEngine;
28 import com.android.server.uwb.correction.math.SphericalVector;
29 
30 /**
31  * Represents a remote controlee that is involved in a session.
32  */
33 public class UwbControlee implements AutoCloseable {
34     private static final long SEC_TO_MILLI = 1000;
35     private final UwbAddress mUwbAddress;
36     private final UwbInjector mUwbInjector;
37     private final UwbFilterEngine mEngine;
38     /** Error value to use when the engine produces a result that wasn't in the original reading. */
39     private static final double DEFAULT_ERROR_DISTANCE = 0.0;
40     private long mLastMeasurementInstant;
41     private long mPredictionTimeoutMilli = 3000;
42 
43     /**
44      * Creates a new UwbControlee.
45      *
46      * @param uwbAddress The address of the controlee.
47      */
UwbControlee( @onNull UwbAddress uwbAddress, @Nullable UwbFilterEngine engine, @Nullable UwbInjector uwbInjector)48     public UwbControlee(
49             @NonNull UwbAddress uwbAddress,
50             @Nullable UwbFilterEngine engine,
51             @Nullable UwbInjector uwbInjector) {
52         mUwbAddress = uwbAddress;
53         mEngine = engine;
54         mUwbInjector = uwbInjector;
55         if (mUwbInjector != null
56                 && mUwbInjector.getDeviceConfigFacade() != null) {
57             // Injector or deviceConfigFacade might be null during tests and this is fine.
58             mPredictionTimeoutMilli = mUwbInjector
59                     .getDeviceConfigFacade()
60                     .getPredictionTimeoutSeconds() * SEC_TO_MILLI;
61         }
62     }
63 
64     /**
65      * Gets the address of the controlee.
66      *
67      * @return A UwbAddress of the associated controlee.
68      */
getUwbAddress()69     public UwbAddress getUwbAddress() {
70         return mUwbAddress;
71     }
72 
73     /** Shuts down any controlee-specific work. */
74     @Override
close()75     public void close() {
76         if (mEngine != null) {
77             mEngine.close();
78         }
79     }
80 
81     /**
82      * Updates a RangingMeasurement builder to produce a filtered value. If the filter engine
83      *  is not configured, this will not affect the builder.
84      * @param rmBuilder The {@link RangingMeasurement.Builder} to reconfigure.
85      */
filterMeasurement(RangingMeasurement.Builder rmBuilder)86     public void filterMeasurement(RangingMeasurement.Builder rmBuilder) {
87         if (mEngine == null) {
88             // Engine is disabled. Don't modify the builder.
89             return;
90         }
91         RangingMeasurement rawMeasurement = rmBuilder.build();
92 
93         if (rawMeasurement.getStatus() != RangingMeasurement.RANGING_STATUS_SUCCESS) {
94             if (getTime() - mPredictionTimeoutMilli > mLastMeasurementInstant) {
95                 // It's been some time since we last got a good report. Stop reporting values.
96                 return;
97             }
98         } else {
99             mLastMeasurementInstant = getTime();
100         }
101 
102         // Gather az/el/dist
103         AngleOfArrivalMeasurement aoaMeasurement = rawMeasurement.getAngleOfArrivalMeasurement();
104         DistanceMeasurement distMeasurement = rawMeasurement.getDistanceMeasurement();
105         boolean hasAzimuth = false;
106         boolean hasElevation = false;
107         boolean hasDistance = false;
108         float azimuth = 0;
109         float elevation = 0;
110         float distance = 0;
111         double azimuthFom = 1;
112         double elevationFom = 1;
113         double distanceFom = 1;
114         long nowMs = mUwbInjector.getElapsedSinceBootMillis();
115         if (aoaMeasurement != null) {
116             if (aoaMeasurement.getAzimuth() != null
117                     && aoaMeasurement.getAzimuth().getConfidenceLevel() > 0) {
118                 hasAzimuth = true;
119                 azimuth = (float) aoaMeasurement.getAzimuth().getRadians();
120                 azimuthFom = aoaMeasurement.getAzimuth().getConfidenceLevel();
121             }
122             if (aoaMeasurement.getAltitude() != null
123                     && aoaMeasurement.getAltitude().getConfidenceLevel() > 0) {
124                 hasElevation = true;
125                 elevation = (float) aoaMeasurement.getAltitude().getRadians();
126                 elevationFom = aoaMeasurement.getAltitude().getConfidenceLevel();
127             }
128         }
129         if (distMeasurement != null) {
130             hasDistance = true;
131             distance = (float) distMeasurement.getMeters();
132             distanceFom = distMeasurement.getConfidenceLevel();
133         }
134         SphericalVector.Annotated sv = SphericalVector.fromRadians(azimuth, elevation, distance)
135                 .toAnnotated(hasAzimuth, hasElevation, hasDistance);
136 
137         sv.azimuthFom = azimuthFom;
138         sv.elevationFom = elevationFom;
139         sv.distanceFom = distanceFom;
140 
141         // Give to the engine.
142         mEngine.add(sv, nowMs);
143 
144         SphericalVector.Annotated engineResult = mEngine.compute(nowMs);
145         if (engineResult == null) {
146             // Bail early - the engine didn't compute a result, so just leave the builder alone.
147             return;
148         }
149 
150         // Now re-generate the az/el/dist readings based on engine result.
151         updateBuilder(rmBuilder, rawMeasurement, engineResult);
152     }
153 
getTime()154     private long getTime() {
155         if (mUwbInjector == null) {
156             return 0; // Can happen during testing; no time tracking will be supported.
157         }
158         return mUwbInjector.getElapsedSinceBootMillis();
159     }
160 
161     /**
162      * Replaces az/el/dist values in a RangingMeasurement builder.
163      * @param rmBuilder The RangingMeasurement builder to update.
164      * @param rawMeasurement The original raw measurements. Used for fallback and confidence values.
165      * @param replacement The filter engine's result.
166      */
updateBuilder(RangingMeasurement.Builder rmBuilder, RangingMeasurement rawMeasurement, SphericalVector.Annotated replacement)167     private static void updateBuilder(RangingMeasurement.Builder rmBuilder,
168             RangingMeasurement rawMeasurement,
169             SphericalVector.Annotated replacement) {
170         // This is fairly verbose because of how nested data is, the risk of nulls, and the
171         // fact that that azimuth is required up-front, even in the builder. Refactoring so the
172         // RangingMeasurement can be cloned and changed would be nice, but it would change
173         // (or at least add to) an external API.
174 
175         // Switch to success - error statuses cannot have any values.
176         rmBuilder.setStatus(RangingMeasurement.RANGING_STATUS_SUCCESS);
177 
178         AngleOfArrivalMeasurement aoaMeasurement = rawMeasurement.getAngleOfArrivalMeasurement();
179         DistanceMeasurement distMeasurement = rawMeasurement.getDistanceMeasurement();
180 
181         AngleMeasurement azimuthMeasurement = null;
182         AngleMeasurement elevationMeasurement = null;
183 
184         // Any AoA in the original measurement?
185         if (aoaMeasurement != null) {
186             // Any azimuth in the original measurement?
187             if (aoaMeasurement.getAzimuth() != null) {
188                 // Yes - create a new azimuth based on the filter's output.
189                 azimuthMeasurement = new AngleMeasurement(
190                         replacement.azimuth,
191                         aoaMeasurement.getAzimuth().getErrorRadians(),
192                         replacement.azimuthFom
193                 );
194             }
195             // Any elevation in the original measurement?
196             if (aoaMeasurement.getAltitude() != null) {
197                 // Yes - create a new elevation based on the filter's output.
198                 elevationMeasurement = new AngleMeasurement(
199                         replacement.elevation,
200                         aoaMeasurement.getAltitude().getErrorRadians(),
201                         replacement.elevationFom
202                 );
203             }
204         }
205 
206         AngleOfArrivalMeasurement.Builder aoaBuilder = null;
207         // Only create the aoaBuilder if there was an azimuth in the original measurement.
208         if (azimuthMeasurement != null) {
209             aoaBuilder = new AngleOfArrivalMeasurement.Builder(azimuthMeasurement);
210             if (elevationMeasurement != null) {
211                 aoaBuilder.setAltitude(elevationMeasurement);
212             }
213         }
214 
215         DistanceMeasurement.Builder distanceBuilder = new DistanceMeasurement.Builder();
216         if (distMeasurement == null) {
217             // No distance value. Might have been a one-way AoA.
218 
219             // RangingMeasurement.Build requires that any non-error status has a valid
220             //  DistanceMeasurement, so we will create one.
221             distanceBuilder.setErrorMeters(DEFAULT_ERROR_DISTANCE);
222         } else {
223             distanceBuilder.setErrorMeters(distMeasurement.getErrorMeters());
224         }
225         distanceBuilder.setConfidenceLevel(replacement.distanceFom);
226         distanceBuilder.setMeters(replacement.distance);
227 
228         rmBuilder.setDistanceMeasurement(distanceBuilder.build());
229         if (aoaBuilder != null) {
230             rmBuilder.setAngleOfArrivalMeasurement(aoaBuilder.build());
231         }
232     }
233 }
234