1 /*
2  * Copyright (C) 2023 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 package com.android.server.uwb.correction;
17 
18 import android.os.Build;
19 import android.util.Log;
20 
21 import androidx.annotation.NonNull;
22 import androidx.annotation.Nullable;
23 
24 import com.android.server.uwb.correction.filtering.IPositionFilter;
25 import com.android.server.uwb.correction.math.Pose;
26 import com.android.server.uwb.correction.math.SphericalVector;
27 import com.android.server.uwb.correction.pose.IPoseSource;
28 import com.android.server.uwb.correction.pose.PoseEventListener;
29 import com.android.server.uwb.correction.primers.IPrimer;
30 
31 import java.util.ArrayList;
32 import java.util.List;
33 import java.util.Objects;
34 
35 /**
36  * Consumes raw UWB values and outputs filtered UWB values. See the {@link UwbFilterEngine.Builder}
37  * for how it is configured.
38  */
39 public class UwbFilterEngine implements AutoCloseable, PoseEventListener {
40     public static final String BIG_LOG_TAG = "UwbFilterEngine";
41     @NonNull private final List<IPrimer> mPrimers;
42     @Nullable private final IPositionFilter mFilter;
43     @Nullable private final IPoseSource mPoseSource;
44     private static final boolean sDebug;
45 
46     static {
47         sDebug = (Build.TYPE != null && Build.TYPE.equals("userdebug"))
48                 || System.getProperty("DEBUG") != null;
49     }
50 
51     /**
52      * The last UWB reading, after priming or filtering, depending on which facilities
53      * are available.  If computation fails or is not possible (ie - filter or primer is not
54      * configured), the computation function will return this.
55      */
56     @Nullable private SphericalVector.Annotated mLastInputState;
57 
58     private boolean mClosed;
59 
UwbFilterEngine( @onNull List<IPrimer> primers, @Nullable IPoseSource poseSource, @Nullable IPositionFilter filter)60     private UwbFilterEngine(
61             @NonNull List<IPrimer> primers,
62             @Nullable IPoseSource poseSource,
63             @Nullable IPositionFilter filter) {
64         this.mPrimers = primers;
65         this.mPoseSource = poseSource;
66         this.mFilter = filter;
67         if (poseSource != null) {
68             // A listener must be registered in order for the poseSource to start.
69             poseSource.registerListener(this);
70         }
71     }
72 
73     /**
74      * Updates the engine with the latest UWB data.
75      * @param position The raw position produced by the UWB hardware.
76      * @param timeMs The time at which the UWB value was received, in ms since boot.
77      */
add(@onNull SphericalVector.Annotated position, long timeMs)78     public void add(@NonNull SphericalVector.Annotated position, long timeMs) {
79         StringBuilder bigLog = sDebug ? new StringBuilder(position.toString()) : null;
80         Objects.requireNonNull(position);
81 
82         SphericalVector prediction = compute(timeMs);
83 
84         if (sDebug) {
85             bigLog.append("(Prediction ");
86             bigLog.append(prediction);
87             bigLog.append(")");
88         }
89 
90         for (IPrimer primer: mPrimers) {
91             position = primer.prime(position, prediction, mPoseSource, timeMs);
92             if (bigLog != null) {
93                 bigLog.append(" ->")
94                         .append(primer.getClass().getSimpleName()).append("=")
95                         .append(position);
96             }
97         }
98         if (position.isComplete() || prediction == null) {
99             mLastInputState = position;
100         } else {
101             // Primers did not fully prime the position vector. This can happen when elevation is
102             //  missing and there is no primer for an estimate, or if there was a bad UWB reading.
103             // Fill in with predictions.
104             mLastInputState = SphericalVector.fromRadians(
105                     position.hasAzimuth ? position.azimuth : prediction.azimuth,
106                     position.hasElevation ? position.elevation : prediction.elevation,
107                     position.hasDistance ? position.distance : prediction.distance
108             ).toAnnotated().copyFomFrom(position);
109         }
110         if (mFilter != null) {
111             mFilter.updatePose(mPoseSource, timeMs);
112             mFilter.add(mLastInputState, timeMs);
113             if (bigLog != null) {
114                 bigLog.append(" : filtered=")
115                         .append(mFilter.compute(timeMs));
116             }
117         }
118         if (bigLog != null) {
119             Log.d(BIG_LOG_TAG, bigLog.toString());
120         }
121     }
122 
123     /**
124      * Computes the most probable UWB location as of the given time.
125      * @param timeMs The time at which the UWB value was received, in ms since boot.
126      * @return A SphericalVector representing the most likely UWB location.
127      */
128     @Nullable
compute(long timeMs)129     public SphericalVector.Annotated compute(long timeMs) {
130         if (mFilter != null) {
131             mFilter.updatePose(mPoseSource, timeMs);
132             return mFilter.compute(timeMs);
133         }
134         return mLastInputState;
135     }
136 
137     /**
138      * Gets the current device pose.
139      */
140     @NonNull
getPose()141     public Pose getPose() {
142         Pose pose = null;
143         if (mPoseSource != null) {
144             pose = mPoseSource.getPose();
145         }
146         if (pose == null) {
147             pose = Pose.IDENTITY;
148         }
149         return pose;
150     }
151 
152     /**
153      * Frees or closes all resources consumed by this object.
154      */
155     @Override
close()156     public void close() {
157         if (!mClosed) {
158             mClosed = true;
159             if (mPoseSource != null) {
160                 mPoseSource.unregisterListener(this);
161             }
162         }
163     }
164 
165     /**
166      * Called when there is an update to the device's pose. The origin is arbitrary, but
167      * position could be relative to the starting position, and rotation could be relative
168      * to magnetic north and the direction of gravity.
169      *
170      * @param pose The new location and orientation of the device.
171      */
172     @Override
onPoseChanged(@uppressWarnings"unused") @onNull Pose pose)173     public void onPoseChanged(@SuppressWarnings("unused") @NonNull Pose pose) {
174         // We don't use pose change as they happen at this point. If you're implementing UWB
175         // oversampling, this might be a good place to call compute() and produce a result.
176     }
177 
178     /**
179      * Builder for a {@link UwbFilterEngine}.
180      */
181     public static class Builder {
182         @Nullable private IPositionFilter mFilter;
183         @Nullable private IPoseSource mPoseSource;
184         @NonNull private final ArrayList<IPrimer> mPrimers = new ArrayList<>();
185 
186         /**
187          * Sets the filter this UWB filter engine will use. If not provided, no filtering will
188          * occur.
189          * @param filter The position filter to use.
190          * @return This builder.
191          */
setFilter(IPositionFilter filter)192         public Builder setFilter(IPositionFilter filter) {
193             this.mFilter = filter;
194             return this;
195         }
196 
197         /**
198          * Sets the pose source the UWB filter engine will use. If not set, no pose processing
199          * will occur.
200          * @param poseSource Any pose source.
201          * @return This builder.
202          */
setPoseSource(IPoseSource poseSource)203         public Builder setPoseSource(IPoseSource poseSource) {
204             this.mPoseSource = poseSource;
205             return this;
206         }
207 
208         /**
209          * Adds a primer to the list of primers the engine will use. The primers will execute
210          * in the order in which this is called.
211          * @param primer The primer to add.
212          * @return This builder.
213          */
addPrimer(@onNull IPrimer primer)214         public Builder addPrimer(@NonNull IPrimer primer) {
215             Objects.requireNonNull(primer);
216             this.mPrimers.add(primer);
217             return this;
218         }
219 
220         /**
221          * Builds a UWB filter engine based on the calls made to the builder.
222          * @return the constructed UWB filter engine.
223          */
build()224         public UwbFilterEngine build() {
225             return new UwbFilterEngine(mPrimers, mPoseSource, mFilter);
226         }
227     }
228 }
229