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