1 // Copyright 2024 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 use log::{debug, error};
16 use netsim_proto::model::Device as ProtoDevice;
17 use pica::{Handle, RangingEstimator, RangingMeasurement};
18
19 use crate::devices::{chip::ChipIdentifier, devices_handler::get_device};
20 use crate::ranging::{compute_range_azimuth_elevation, Pose};
21 use crate::uwb::ranging_data::RangingDataSet;
22
23 use std::collections::HashMap;
24 use std::sync::{Arc, Mutex, MutexGuard};
25
26 // Initially a single HashMap might grow into a Struct to allow
27 // for more fields.
28 type State = HashMap<Handle, ChipIdentifier>;
29
30 #[derive(Clone)]
31 pub struct SharedState(Arc<Mutex<State>>);
32
33 impl SharedState {
new() -> Self34 pub fn new() -> Self {
35 SharedState(Arc::new(Mutex::new(State::new())))
36 }
37
lock(&self) -> MutexGuard<State>38 fn lock(&self) -> MutexGuard<State> {
39 self.0.lock().expect("Poisoned SharedState lock")
40 }
41
get_chip_id(&self, pica_id: &Handle) -> anyhow::Result<ChipIdentifier>42 pub fn get_chip_id(&self, pica_id: &Handle) -> anyhow::Result<ChipIdentifier> {
43 self.lock().get(pica_id).ok_or(anyhow::anyhow!("pica_id: {pica_id} not in State")).cloned()
44 }
45
insert(&self, pica_id: Handle, chip_id: ChipIdentifier)46 pub fn insert(&self, pica_id: Handle, chip_id: ChipIdentifier) {
47 self.lock().insert(pica_id, chip_id);
48 }
49
remove(&self, pica_id: &Handle)50 pub fn remove(&self, pica_id: &Handle) {
51 self.lock().remove(pica_id);
52 }
53 }
54
55 // Netsim's UwbRangingEstimator
56 pub struct UwbRangingEstimator {
57 shared_state: SharedState,
58 data_set: RangingDataSet,
59 }
60
61 impl UwbRangingEstimator {
new(shared_state: SharedState) -> Self62 pub fn new(shared_state: SharedState) -> Self {
63 UwbRangingEstimator { shared_state, data_set: RangingDataSet::new(None) }
64 }
65
66 // Utility to convert the UWB Chip handle into the device and chip_id.
handle_to_netsim_model(&self, handle: &Handle) -> Option<(ChipIdentifier, ProtoDevice)>67 fn handle_to_netsim_model(&self, handle: &Handle) -> Option<(ChipIdentifier, ProtoDevice)> {
68 let chip_id = self.shared_state.get_chip_id(handle).map_err(|e| debug!("{e:?}")).ok()?;
69 let device = get_device(&chip_id).map_err(|e| debug!("{e:?}")).ok()?;
70 Some((chip_id, device))
71 }
72 }
73
74 impl RangingEstimator for UwbRangingEstimator {
estimate(&self, a: &Handle, b: &Handle) -> Option<RangingMeasurement>75 fn estimate(&self, a: &Handle, b: &Handle) -> Option<RangingMeasurement> {
76 // Use the Handle to obtain the positions and orientation information in netsim
77 // and perform compute_range_azimuth_elevation
78 let (a_chip_id, a_device) = self.handle_to_netsim_model(a)?;
79 let (b_chip_id, b_device) = self.handle_to_netsim_model(b)?;
80 // Chips are invisible at the PHY layer when uwb is disabled so test here.
81 // Note, chips must always process Host-Controller messages even when
82 // state-off to avoid protocol stack timeouts and other glitches.
83 if !is_uwb_state_on(&a_device, &a_chip_id) || !is_uwb_state_on(&b_device, &b_chip_id) {
84 return None;
85 }
86 let (a_p, a_o) = (a_device.position, a_device.orientation);
87 let (b_p, b_o) = (b_device.position, b_device.orientation);
88 let a_pose = Pose::new(a_p.x, a_p.y, a_p.z, a_o.yaw, a_o.pitch, a_o.roll);
89 let b_pose = Pose::new(b_p.x, b_p.y, b_p.z, b_o.yaw, b_o.pitch, b_o.roll);
90 compute_range_azimuth_elevation(&a_pose, &b_pose)
91 .map(|(range, azimuth, elevation)| RangingMeasurement {
92 range: self.data_set.sample(range, None).round() as u16,
93 azimuth,
94 elevation,
95 })
96 .map_err(|e| error!("{e:?}"))
97 .ok()
98 }
99 }
100
101 /// With given ProtoDevice and ChipIdentifier, return true if state of a UWB chip
102 /// with ChipIdentifier inside ProtoDevice is ON. Otherwise, return false.
is_uwb_state_on(device: &ProtoDevice, chip_id: &ChipIdentifier) -> bool103 fn is_uwb_state_on(device: &ProtoDevice, chip_id: &ChipIdentifier) -> bool {
104 for chip in &device.chips {
105 if chip.has_uwb() && chip.id == chip_id.0 {
106 return chip.uwb().state.unwrap_or(false);
107 }
108 }
109 false
110 }
111
112 #[cfg(test)]
113 mod tests {
114 use super::*;
115
116 use netsim_proto::model::chip::Radio as ProtoRadio;
117 use netsim_proto::model::Chip as ProtoChip;
118
create_proto_device_with_uwb_chip(uwb_state: bool) -> (ProtoDevice, ChipIdentifier)119 fn create_proto_device_with_uwb_chip(uwb_state: bool) -> (ProtoDevice, ChipIdentifier) {
120 // Setting Radio State to true
121 let mut proto_radio = ProtoRadio::new();
122 proto_radio.state = Some(uwb_state);
123
124 // Setting UWB ProtoChip
125 let mut proto_chip = ProtoChip::new();
126 proto_chip.set_uwb(proto_radio);
127 let chip_id = proto_chip.id;
128
129 // Adding chip to proto_device
130 let mut proto_device = ProtoDevice::new();
131 proto_device.chips.push(proto_chip);
132
133 // Return proto_device
134 (proto_device, ChipIdentifier(chip_id))
135 }
136
137 #[test]
test_is_uwb_state_on()138 fn test_is_uwb_state_on() {
139 // False when no uwb chip is present in ProtoDevice
140 assert!(!is_uwb_state_on(&ProtoDevice::new(), &ChipIdentifier(0)));
141
142 // Check if UWB State is on
143 let (proto_device, chip_id) = create_proto_device_with_uwb_chip(true);
144 assert!(is_uwb_state_on(&proto_device, &chip_id));
145
146 // Check if UWB State is off
147 let (proto_device, chip_id) = create_proto_device_with_uwb_chip(false);
148 assert!(!is_uwb_state_on(&proto_device, &chip_id));
149 }
150 }
151