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