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.Pose;
22 import com.android.server.uwb.correction.math.SphericalVector;
23 import com.android.server.uwb.correction.pose.IPoseSource;
24 import com.android.server.uwb.correction.pose.IPoseSource.Capabilities;
25 
26 /**
27  * Applies a default pose-based elevation to a UWB reading. A basic "assumption" about what the
28  * elevation might be helps improve the quality of pose-based azimuth compensations, and may
29  * provide a more understandable UWB location guess to the user.
30  * Recommended for hardware that does not support elevation. This should execute before the
31  * AoAPrimer in the primer execution order.
32  * This will replace any existing elevation value, as it assumes that the hardware's elevation is
33  * invalid or zero.
34  */
35 public class ElevationPrimer implements IPrimer {
36     /**
37      * The FOM to apply to estimated elevations. This should not be 0 because that is equivalent to
38      * a reading that should be discarded. */
39     public static double ELEVATION_FOM = 0.3;
40 
41     /**
42      * Applies a default pose-based elevation to a UWB reading that doesn't have one.
43      *
44      * @param input     The original UWB reading.
45      * @param prediction The previous filtered UWB result adjusted by the pose change since then.
46      * @param poseSource A pose source that may indicate phone orientation.
47      * @param timeMs When the input occurred, in ms since boot.
48      * @return A replacement value for the UWB vector that has been corrected for the situation.
49      */
50     @Override
prime( @onNull SphericalVector.Annotated input, @Nullable SphericalVector prediction, @Nullable IPoseSource poseSource, long timeMs)51     public SphericalVector.Annotated prime(
52             @NonNull SphericalVector.Annotated input,
53             @Nullable SphericalVector prediction,
54             @Nullable IPoseSource poseSource,
55             long timeMs) {
56         SphericalVector.Annotated position = input;
57         if (poseSource != null
58                 && poseSource.getCapabilities().contains(Capabilities.UPRIGHT)
59         ) {
60             Pose pose = poseSource.getPose();
61             if (pose != null) {
62                 // The pose source knows which way is upright, so if we don't have
63                 // an AoA elevation, we'll assume that elevation is level with the phone.
64                 // i.e. If the phone pitches down, the elevation would appear up.
65 
66                 position = new SphericalVector.Annotated(
67                     SphericalVector.fromRadians(
68                         input.azimuth,
69                         -pose.rotation.toYawPitchRoll().y, // -Pitch becomes our assumed elevation
70                         input.distance
71                     ),
72                     input.hasAzimuth,
73                     true,
74                     input.hasDistance
75                 ).copyFomFrom(input);
76 
77                 position.elevationFom *= ELEVATION_FOM;
78             }
79         }
80         return position;
81     }
82 }
83