1 // Copyright 2023 Google LLC
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // https://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14
15 //! Ranging library
16
17 #![allow(clippy::empty_line_after_doc_comments)]
18
19 use glam::{EulerRot, Quat, Vec3};
20
21 /// The Free Space Path Loss (FSPL) model is considered as the standard
22 /// under the ideal scenario.
23
24 /// (dBm) PATH_LOSS at 1m for isotropic antenna transmitting BLE.
25 const PATH_LOSS_AT_1M: f32 = 40.20;
26
27 /// Convert distance to RSSI using the free space path loss equation.
28 /// See [Free-space_path_loss][1].
29 ///
30 /// [1]: http://en.wikipedia.org/wiki/Free-space_path_loss
31 ///
32 /// # Parameters
33 ///
34 /// * `distance`: distance in meters (m).
35 /// * `tx_power`: transmitted power (dBm) calibrated to 1 meter.
36 ///
37 /// # Returns
38 ///
39 /// The rssi that would be measured at that distance, in the
40 /// range -120..20 dBm,
distance_to_rssi(tx_power: i8, distance: f32) -> i841 pub fn distance_to_rssi(tx_power: i8, distance: f32) -> i8 {
42 // TODO(b/285634913)
43 // Rootcanal reporting tx_power of 0 or 1 during Nearby Share
44 let new_tx_power = match tx_power {
45 0 | 1 => -49,
46 _ => tx_power,
47 };
48 match distance == 0.0 {
49 true => (new_tx_power as f32 + PATH_LOSS_AT_1M).clamp(-120.0, 20.0) as i8,
50 false => (new_tx_power as f32 - 20.0 * distance.log10()).clamp(-120.0, 20.0) as i8,
51 }
52 }
53
54 // helper function for performing division with
55 // zero division check
56 #[allow(unused)]
checked_div(num: f32, den: f32) -> Option<f32>57 fn checked_div(num: f32, den: f32) -> Option<f32> {
58 (den != 0.).then_some(num / den)
59 }
60
61 // helper function for calculating azimuth angle
62 // from a given 3D delta vector.
63 #[allow(unused)]
azimuth(delta: Vec3) -> f3264 fn azimuth(delta: Vec3) -> f32 {
65 checked_div(delta.x, delta.z).map_or(
66 match delta.x == 0. {
67 true => 0.,
68 false => delta.x.signum() * std::f32::consts::FRAC_2_PI,
69 },
70 f32::atan,
71 ) + if delta.z >= 0. { 0. } else { delta.x.signum() * std::f32::consts::PI }
72 }
73
74 // helper function for calculating elevation angle
75 // from a given 3D delta vector.
76 #[allow(unused)]
elevation(delta: Vec3) -> f3277 fn elevation(delta: Vec3) -> f32 {
78 checked_div(delta.y, f32::sqrt(delta.x.powi(2) + delta.z.powi(2)))
79 .map_or(delta.y.signum() * std::f32::consts::FRAC_PI_2, f32::atan)
80 }
81
82 /// Pose struct
83 ///
84 /// This struct allows for a mathematical representation of
85 /// position and orientation values from the protobufs, which
86 /// would enable to compute range, azimuth, and elevation.
87 #[allow(unused)]
88 pub struct Pose {
89 position: Vec3,
90 orientation: Quat,
91 }
92
93 impl Pose {
94 #[allow(unused)]
new(x: f32, y: f32, z: f32, yaw: f32, pitch: f32, roll: f32) -> Self95 pub fn new(x: f32, y: f32, z: f32, yaw: f32, pitch: f32, roll: f32) -> Self {
96 Pose {
97 // Converts x, y, z from meters to centimeters
98 position: Vec3::new(x * 100., y * 100., z * 100.),
99 // Converts roll, pitch, yaw from degrees to radians
100 orientation: Quat::from_euler(
101 EulerRot::ZXY,
102 roll.to_radians(),
103 pitch.to_radians(),
104 yaw.to_radians(),
105 ),
106 }
107 }
108 }
109
110 /// UWB Ranging Model for computing range, azimuth, and elevation
111 /// The raning model brought from https://github.com/google/pica
112 #[allow(unused)]
compute_range_azimuth_elevation(a: &Pose, b: &Pose) -> anyhow::Result<(f32, i16, i8)>113 pub fn compute_range_azimuth_elevation(a: &Pose, b: &Pose) -> anyhow::Result<(f32, i16, i8)> {
114 let delta = b.position - a.position;
115 let distance = delta.length().clamp(0.0, u16::MAX as f32);
116 let direction = a.orientation.mul_vec3(delta);
117 let azimuth = azimuth(direction).to_degrees().round();
118 let elevation = elevation(direction).to_degrees().round();
119
120 if !(-180. ..=180.).contains(&azimuth) {
121 return Err(anyhow::anyhow!("azimuth is not between -180 and 180. value: {azimuth}"));
122 }
123 if !(-90. ..=90.).contains(&elevation) {
124 return Err(anyhow::anyhow!("elevation is not between -90 and 90. value: {elevation}"));
125 }
126 Ok((distance, azimuth as i16, elevation as i8))
127 }
128
129 #[cfg(test)]
130 mod tests {
131 use super::*;
132
133 #[test]
rssi_at_0m()134 fn rssi_at_0m() {
135 let rssi_at_0m = distance_to_rssi(-120, 0.0);
136 assert_eq!(rssi_at_0m, -79);
137 }
138
139 #[test]
rssi_at_1m()140 fn rssi_at_1m() {
141 // With transmit power at 0 dBm verify a reasonable rssi at 1m
142 let rssi_at_1m = distance_to_rssi(0, 1.0);
143 assert!(rssi_at_1m < -35 && rssi_at_1m > -55);
144 }
145
146 #[test]
rssi_saturate_inf()147 fn rssi_saturate_inf() {
148 // Verify that the rssi saturates at -120 for very large distances.
149 let rssi_inf = distance_to_rssi(-120, 1000.0);
150 assert_eq!(rssi_inf, -120);
151 }
152
153 #[test]
rssi_saturate_sup()154 fn rssi_saturate_sup() {
155 // Verify that the rssi saturates at +20 for the largest tx power
156 // and nearest distance.
157 let rssi_sup = distance_to_rssi(20, 0.0);
158 assert_eq!(rssi_sup, 20);
159 }
160
161 #[test]
range()162 fn range() {
163 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
164 {
165 let b_pose = Pose::new(10.0, 0.0, 0.0, 0.0, 0.0, 0.0);
166 let (range, _, _) = compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
167 assert_eq!(range, 1000.);
168 }
169 {
170 let b_pose = Pose::new(-10.0, 0.0, 0.0, 0.0, 0.0, 0.0);
171 let (range, _, _) = compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
172 assert_eq!(range, 1000.);
173 }
174 {
175 let b_pose = Pose::new(10.0, 10.0, 0.0, 0.0, 0.0, 0.0);
176 let (range, _, _) = compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
177 assert_eq!(range, f32::sqrt(2000000.));
178 }
179 {
180 let b_pose = Pose::new(-10.0, -10.0, -10.0, 0.0, 0.0, 0.0);
181 let (range, _, _) = compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
182 assert_eq!(range, f32::sqrt(3000000.));
183 }
184 }
185
186 #[test]
azimuth_without_rotation()187 fn azimuth_without_rotation() {
188 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
189 {
190 let b_pose = Pose::new(10.0, 0.0, 10.0, 0.0, 0.0, 0.0);
191 let (_, azimuth, elevation) =
192 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
193 assert_eq!(azimuth, 45);
194 assert_eq!(elevation, 0);
195 }
196 {
197 let b_pose = Pose::new(-10.0, 0.0, 10.0, 0.0, 0.0, 0.0);
198 let (_, azimuth, elevation) =
199 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
200 assert_eq!(azimuth, -45);
201 assert_eq!(elevation, 0);
202 }
203 {
204 let b_pose = Pose::new(10.0, 0.0, -10.0, 0.0, 0.0, 0.0);
205 let (_, azimuth, elevation) =
206 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
207 assert_eq!(azimuth, 135);
208 assert_eq!(elevation, 0);
209 }
210 {
211 let b_pose = Pose::new(-10.0, 0.0, -10.0, 0.0, 0.0, 0.0);
212 let (_, azimuth, elevation) =
213 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
214 assert_eq!(azimuth, -135);
215 assert_eq!(elevation, 0);
216 }
217 }
218
219 #[test]
elevation_without_rotation()220 fn elevation_without_rotation() {
221 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
222 {
223 let b_pose = Pose::new(0.0, 10.0, 10.0, 0.0, 0.0, 0.0);
224 let (_, azimuth, elevation) =
225 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
226 assert_eq!(azimuth, 0);
227 assert_eq!(elevation, 45);
228 }
229 {
230 let b_pose = Pose::new(0.0, -10.0, 10.0, 0.0, 0.0, 0.0);
231 let (_, azimuth, elevation) =
232 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
233 assert_eq!(azimuth, 0);
234 assert_eq!(elevation, -45);
235 }
236 {
237 let b_pose = Pose::new(0.0, 10.0, -10.0, 0.0, 0.0, 0.0);
238 let (_, azimuth, elevation) =
239 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
240 assert!(azimuth == 180 || azimuth == -180);
241 assert_eq!(elevation, 45);
242 }
243 {
244 let b_pose = Pose::new(0.0, -10.0, -10.0, 0.0, 0.0, 0.0);
245 let (_, azimuth, elevation) =
246 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
247 assert!(azimuth == 180 || azimuth == -180);
248 assert_eq!(elevation, -45);
249 }
250 }
251
252 #[test]
rotation_only()253 fn rotation_only() {
254 let b_pose = Pose::new(0.0, 0.0, 10.0, 0.0, 0.0, 0.0);
255 {
256 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
257 let (_, azimuth, elevation) =
258 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
259 assert_eq!(azimuth, 0);
260 assert_eq!(elevation, 0);
261 }
262 {
263 let a_pose = Pose::new(0.0, 0.0, 0.0, 45.0, 0.0, 0.0); // <=> azimuth = -45deg
264 let (_, azimuth, elevation) =
265 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
266 assert_eq!(azimuth, 45);
267 assert_eq!(elevation, 0);
268 }
269 {
270 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 45.0, 0.0);
271 let (_, azimuth, elevation) =
272 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
273 assert_eq!(azimuth, 0);
274 assert_eq!(elevation, -45);
275 }
276 {
277 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 0.0, 45.0);
278 let (_, azimuth, elevation) =
279 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
280 assert_eq!(azimuth, 0);
281 assert_eq!(elevation, 0);
282 }
283 }
284
285 #[test]
rotation_only_complex_position()286 fn rotation_only_complex_position() {
287 let b_pose = Pose::new(10.0, 10.0, 10.0, 0.0, 0.0, 0.0);
288 {
289 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
290 let (_, azimuth, elevation) =
291 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
292 assert_eq!(azimuth, 45);
293 assert_eq!(elevation, 35);
294 }
295 {
296 let a_pose = Pose::new(0.0, 0.0, 0.0, 90.0, 0.0, 0.0);
297 let (_, azimuth, elevation) =
298 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
299 assert_eq!(azimuth, 135);
300 assert_eq!(elevation, 35);
301 }
302 {
303 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 90.0, 0.0);
304 let (_, azimuth, elevation) =
305 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
306 assert_eq!(azimuth, 45);
307 assert_eq!(elevation, -35);
308 }
309 {
310 let a_pose = Pose::new(0.0, 0.0, 0.0, 0.0, 0.0, 90.0);
311 let (_, azimuth, elevation) =
312 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
313 assert_eq!(azimuth, -45);
314 assert_eq!(elevation, 35);
315 }
316 {
317 let a_pose = Pose::new(0.0, 0.0, 0.0, -45.0, 35.0, 42.0);
318 let (_, azimuth, elevation) =
319 compute_range_azimuth_elevation(&a_pose, &b_pose).unwrap();
320 assert_eq!(azimuth, 0);
321 assert_eq!(elevation, 0);
322 }
323 }
324 }
325