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.primers;
17 
18 import androidx.annotation.NonNull;
19 import androidx.annotation.Nullable;
20 
21 import com.android.server.uwb.correction.math.AoaVector;
22 import com.android.server.uwb.correction.math.SphericalVector;
23 import com.android.server.uwb.correction.math.SphericalVector.Annotated;
24 import com.android.server.uwb.correction.pose.IPoseSource;
25 
26 /**
27  * Converts a PDoA azimuth value to a spherical coordinate azimuth by accounting for elevation.
28  * See {@link AoaVector} for information on the difference.
29  * This primer is needed on hardware that does not support elevation, after the ElevationPrimer,
30  * so that the estimated elevation can be used to perform the PDoA-to-azimuth conversion.
31  * This primer is also needed on hardware that supports elevation, but with firmware that does
32  * not perform the PDoA-to-azimuth conversion.
33  */
34 public class AoaPrimer implements IPrimer {
35     /**
36      * Applies corrections to a raw position.
37      *
38      * @param input      The original UWB reading.
39      * @param prediction The previous filtered UWB result adjusted by the pose change since then.
40      * @param poseSource A pose source that may indicate phone orientation.
41      * @param timeMs When the input occurred, in ms since boot.
42      * @return A replacement value for the UWB input that has been corrected for  the situation.
43      */
44     @Override
prime( @onNull SphericalVector.Annotated input, @Nullable SphericalVector prediction, @Nullable IPoseSource poseSource, long timeMs)45     public SphericalVector.Annotated prime(
46             @NonNull SphericalVector.Annotated input,
47             @Nullable SphericalVector prediction,
48             @Nullable IPoseSource poseSource,
49             long timeMs) {
50         if (input.hasElevation && input.hasAzimuth) {
51             // Reinterpret the SphericalVector as an AoAVector, then convert it to a
52             // SphericalVector.
53             return new Annotated(
54                     AoaVector.fromRadians(
55                             input.azimuth,
56                             input.elevation,
57                             input.distance).toSphericalVector(),
58                     true,
59                     true,
60                     input.hasDistance)
61                     .copyFomFrom(input);
62         }
63         // Elevation is the same in both units. If only elevation is known, no conversion needed.
64         return input;
65     }
66 }
67