From bcbf171f59f1b3df6485a2f1ffe166d671daf05e Mon Sep 17 00:00:00 2001 From: jbabs1 Date: Thu, 20 Apr 2023 16:00:20 -0700 Subject: [PATCH 1/2] Add FPP Lib --- presence/fpp/fpp/Cargo.lock | 23 +++ presence/fpp/{ => fpp}/Cargo.toml | 2 - presence/fpp/fpp/src/device_proximity_data.rs | 184 ++++++++++++++++++ presence/fpp/{ => fpp}/src/fspl_converter.rs | 0 .../fpp/{ => fpp}/src/fspl_converter_test.rs | 4 +- .../fpp/{ => fpp}/src/fused_presence_utils.rs | 21 +- presence/fpp/fpp/src/lib.rs | 9 + presence/fpp/fpp/src/presence_detector.rs | 72 +++++++ presence/fpp/fpp/src/proximity_estimator.rs | 100 ++++++++++ .../fpp/fpp/src/proximity_state_detector.rs | 77 ++++++++ presence/fpp/{ => fpp_c_ffi}/Cargo.lock | 30 +-- presence/fpp/fpp_c_ffi/Cargo.toml | 9 + .../fpp/fpp_c_ffi/include/presence_detector.h | 92 +++++++++ presence/fpp/fpp_c_ffi/src/lib.rs | 74 +++++++ presence/fpp/src/lib.rs | 5 - 15 files changed, 668 insertions(+), 34 deletions(-) create mode 100644 presence/fpp/fpp/Cargo.lock rename presence/fpp/{ => fpp}/Cargo.toml (84%) create mode 100644 presence/fpp/fpp/src/device_proximity_data.rs rename presence/fpp/{ => fpp}/src/fspl_converter.rs (100%) rename presence/fpp/{ => fpp}/src/fspl_converter_test.rs (92%) rename presence/fpp/{ => fpp}/src/fused_presence_utils.rs (91%) create mode 100644 presence/fpp/fpp/src/lib.rs create mode 100644 presence/fpp/fpp/src/presence_detector.rs create mode 100644 presence/fpp/fpp/src/proximity_estimator.rs create mode 100644 presence/fpp/fpp/src/proximity_state_detector.rs rename presence/fpp/{ => fpp_c_ffi}/Cargo.lock (50%) create mode 100644 presence/fpp/fpp_c_ffi/Cargo.toml create mode 100644 presence/fpp/fpp_c_ffi/include/presence_detector.h create mode 100644 presence/fpp/fpp_c_ffi/src/lib.rs delete mode 100644 presence/fpp/src/lib.rs diff --git a/presence/fpp/fpp/Cargo.lock b/presence/fpp/fpp/Cargo.lock new file mode 100644 index 00000000..c95e5124 --- /dev/null +++ b/presence/fpp/fpp/Cargo.lock @@ -0,0 +1,23 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "fpp" +version = "0.1.0" +dependencies = [ + "ilog", + "lazy_static", +] + +[[package]] +name = "ilog" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a9d6896d5a5d605a3b80da159ccc5a6e1fad346b4d2c1ad51046e22536b9a362" + +[[package]] +name = "lazy_static" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" diff --git a/presence/fpp/Cargo.toml b/presence/fpp/fpp/Cargo.toml similarity index 84% rename from presence/fpp/Cargo.toml rename to presence/fpp/fpp/Cargo.toml index 20030249..132a9604 100644 --- a/presence/fpp/Cargo.toml +++ b/presence/fpp/fpp/Cargo.toml @@ -6,7 +6,5 @@ edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -clock = "0.3.2" ilog = "1.0.1" lazy_static = "1.4.0" -num-traits = "0.2.15" \ No newline at end of file diff --git a/presence/fpp/fpp/src/device_proximity_data.rs b/presence/fpp/fpp/src/device_proximity_data.rs new file mode 100644 index 00000000..15ad5da9 --- /dev/null +++ b/presence/fpp/fpp/src/device_proximity_data.rs @@ -0,0 +1,184 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use std::collections::vec_deque::VecDeque; + +use crate::fused_presence_utils::*; + +// Static function for biasing the prediction to the current state to reduce +// state changes due to noisy signal +fn calculate_proximity_state_hysteresis( + distance_meters: f64, + last_reported_proximity_state: Option, + proximity_state_options: ProximityStateOptions, +) -> ProximityState { + let enlarge_multiplier = 1.0 + proximity_state_options.hysteresis_percentage; + let shorten_multiplier = 1.0 - proximity_state_options.hysteresis_percentage; + let mut tap_threshold = proximity_state_options.tap_threshold_meters; + let mut reach_threshold = proximity_state_options.reach_threshold_meters; + let mut short_range_threshold = proximity_state_options.short_range_threshold_meters; + let mut long_range_threshold = proximity_state_options.long_range_threshold_meters; + + match last_reported_proximity_state { + Some(ref _proximity_state) => match last_reported_proximity_state.unwrap() { + ProximityState::Tap => { + tap_threshold = tap_threshold * enlarge_multiplier; + tap_threshold = + tap_threshold + proximity_state_options.hysteresis_tap_extra_debounce_meters; + } + ProximityState::Reach => { + tap_threshold = tap_threshold * shorten_multiplier; + reach_threshold = reach_threshold * enlarge_multiplier; + } + ProximityState::ShortRange => { + reach_threshold = reach_threshold * shorten_multiplier; + short_range_threshold = short_range_threshold * enlarge_multiplier; + } + ProximityState::LongRange => { + short_range_threshold = short_range_threshold * shorten_multiplier; + long_range_threshold = long_range_threshold * enlarge_multiplier; + } + ProximityState::Far => {} + ProximityState::Unknown => {} + }, + None => {} + } + get_proximity_state_from_threshold( + distance_meters, + tap_threshold, + reach_threshold, + short_range_threshold, + long_range_threshold, + ) +} + +// Static function for getting proximity state from threshold +fn get_proximity_state_from_threshold( + distance_meters: f64, + tap_threshold: f64, + reach_threshold: f64, + short_range_threshold: f64, + long_range_threshold: f64, +) -> ProximityState { + if distance_meters <= tap_threshold { + return ProximityState::Tap; + } + if distance_meters <= reach_threshold { + return ProximityState::Reach; + } + if distance_meters <= short_range_threshold { + return ProximityState::ShortRange; + } + if distance_meters <= long_range_threshold { + return ProximityState::LongRange; + } + ProximityState::Far +} + +#[derive(PartialEq)] +pub struct DeviceProximityData { + device_distance_meters_history: VecDeque, + proximity_state_options: ProximityStateOptions, + transition_history: VecDeque, + current_proximity_state: ProximityState, + source: PresenceDataSource, +} + +impl DeviceProximityData { + pub fn new(proximity_state_options: Option) -> Self { + let mut options = ProximityStateOptions { + tap_threshold_meters: DEFAULT_TAP_DISTANCE_THRESHOLD_METERS, + reach_threshold_meters: DEFAULT_REACH_DISTANCE_THRESHOLD_METERS, + short_range_threshold_meters: DEFAULT_SHORT_RANGE_DISTANCE_THRESHOLD_METERS, + long_range_threshold_meters: DEFAULT_LONG_RANGE_DISTANCE_THRESHOLD_METERS, + hysteresis_percentage: DEFAULT_HYSTERESIS_PERCENTAGE, + hysteresis_tap_extra_debounce_meters: DEFAULT_HYSTERESIS_TAP_EXTRA_DEBOUNCE_METERS, + consecutive_scans_required: DEFAULT_CONSECUTIVE_SCANS_REQUIRED, + }; + if proximity_state_options != None { + options = proximity_state_options.unwrap(); + } + let storage_length = options.consecutive_scans_required + 1; + DeviceProximityData { + device_distance_meters_history: VecDeque::with_capacity(storage_length.into()), + proximity_state_options: options, + transition_history: VecDeque::with_capacity(storage_length.into()), + current_proximity_state: ProximityState::Unknown, + source: PresenceDataSource::Unknown, + } + } + + pub fn get_source(&self) -> &PresenceDataSource { + &self.source + } + + pub fn get_current_proximity_state(&self) -> &ProximityState { + &self.current_proximity_state + } + + pub fn configure_options(&mut self, proximity_state_options: ProximityStateOptions) { + self.proximity_state_options = proximity_state_options; + let old_transition_history = &self.transition_history; + let old_device_distance_meters_history = &self.device_distance_meters_history; + let storage_length = proximity_state_options.consecutive_scans_required + &1; + let mut new_transition_history: VecDeque = + VecDeque::with_capacity(storage_length.into()); + let mut new_device_distance_meters_history: VecDeque = + VecDeque::with_capacity(storage_length.into()); + for history in old_transition_history { + new_transition_history.push_front(*history); + new_transition_history.truncate( + self.proximity_state_options + .consecutive_scans_required + .into(), + ); + } + for distance_meters_history in old_device_distance_meters_history { + new_device_distance_meters_history.push_front(*distance_meters_history); + new_device_distance_meters_history.truncate( + self.proximity_state_options + .consecutive_scans_required + .into(), + ); + } + } + + pub fn update_current_proximity_state( + &mut self, + proximity_estimate: ProximityEstimate, + ) -> &ProximityState { + let last_reported_proximity_state = self.current_proximity_state; + let new_proximity_state = calculate_proximity_state_hysteresis( + proximity_estimate.distance, + Some(last_reported_proximity_state), + self.proximity_state_options, + ); + self.transition_history.push_front(new_proximity_state); + self.transition_history.truncate( + self.proximity_state_options + .consecutive_scans_required + .into(), + ); + if self.transition_history.len() + == self + .proximity_state_options + .consecutive_scans_required + .into() + { + self.current_proximity_state = new_proximity_state; + self.source = proximity_estimate.source; + } + &self.current_proximity_state + } +} diff --git a/presence/fpp/src/fspl_converter.rs b/presence/fpp/fpp/src/fspl_converter.rs similarity index 100% rename from presence/fpp/src/fspl_converter.rs rename to presence/fpp/fpp/src/fspl_converter.rs diff --git a/presence/fpp/src/fspl_converter_test.rs b/presence/fpp/fpp/src/fspl_converter_test.rs similarity index 92% rename from presence/fpp/src/fspl_converter_test.rs rename to presence/fpp/fpp/src/fspl_converter_test.rs index 5e846fb8..efa273c8 100644 --- a/presence/fpp/src/fspl_converter_test.rs +++ b/presence/fpp/fpp/src/fspl_converter_test.rs @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - - use crate::fspl_converter::compute_distance_meters_at_high_tx_power; #[test] @@ -23,7 +21,7 @@ fn test_short_distance() { #[test] fn test_medium_distance() { - assert_eq!(compute_distance_meters_at_high_tx_power( -60), 1.0); + assert_eq!(compute_distance_meters_at_high_tx_power(-60), 1.0); } #[test] diff --git a/presence/fpp/src/fused_presence_utils.rs b/presence/fpp/fpp/src/fused_presence_utils.rs similarity index 91% rename from presence/fpp/src/fused_presence_utils.rs rename to presence/fpp/fpp/src/fused_presence_utils.rs index f0e31984..ecd5a2e7 100644 --- a/presence/fpp/src/fused_presence_utils.rs +++ b/presence/fpp/fpp/src/fused_presence_utils.rs @@ -50,6 +50,7 @@ lazy_static! { // Proximity state from device to another in terms of actionability #[derive(Eq, Hash, Copy, Clone, PartialEq)] +#[repr(C)] pub enum ProximityState { Unknown, Tap, @@ -61,6 +62,7 @@ pub enum ProximityState { // Represents the confidence levels for a given measurement #[derive(Copy, Clone, PartialEq)] +#[repr(C)] pub enum MeasurementConfidence { Low, Medium, @@ -70,6 +72,7 @@ pub enum MeasurementConfidence { // Data sources that are used to track presence #[derive(Copy, Clone, PartialEq)] +#[repr(C)] pub enum PresenceDataSource { Ble, Uwb, @@ -78,14 +81,16 @@ pub enum PresenceDataSource { } // A PII-stripped subset of Bluetooth scan result +#[repr(C)] pub struct BleScanResult { pub device_id: u64, - pub tx_power: Option, + pub tx_power: COption, pub rssi: i16, pub elapsed_real_time: u64, } #[derive(Copy, Clone, PartialEq)] +#[repr(C)] pub struct ProximityEstimate { pub device_id: u64, pub distance: f64, @@ -95,6 +100,7 @@ pub struct ProximityEstimate { } #[derive(Copy, Clone, PartialEq)] +#[repr(C)] pub struct ProximityStateOptions { pub tap_threshold_meters: f64, pub reach_threshold_meters: f64, @@ -104,3 +110,16 @@ pub struct ProximityStateOptions { pub hysteresis_tap_extra_debounce_meters: f64, pub consecutive_scans_required: u8, } + +#[repr(C)] +pub struct ProximityStateResult { + pub device_id: u64, + pub current_proximity_state: ProximityState, + pub current_proximity_estimate: ProximityEstimate, +} + +#[repr(C)] +pub struct COption { + pub value: *const T, + pub present: bool, +} diff --git a/presence/fpp/fpp/src/lib.rs b/presence/fpp/fpp/src/lib.rs new file mode 100644 index 00000000..bb7af9a4 --- /dev/null +++ b/presence/fpp/fpp/src/lib.rs @@ -0,0 +1,9 @@ +mod device_proximity_data; +mod fspl_converter; +pub mod fused_presence_utils; +pub mod presence_detector; +mod proximity_estimator; +mod proximity_state_detector; + +#[cfg(test)] +mod fspl_converter_test; diff --git a/presence/fpp/fpp/src/presence_detector.rs b/presence/fpp/fpp/src/presence_detector.rs new file mode 100644 index 00000000..a84c1c1c --- /dev/null +++ b/presence/fpp/fpp/src/presence_detector.rs @@ -0,0 +1,72 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use crate::fused_presence_utils::{ + BleScanResult, ProximityEstimate, ProximityStateOptions, ProximityStateResult, +}; +use crate::proximity_estimator::*; +use crate::proximity_state_detector::*; + +const MAX_RSSI_FILTER_VALUE: i16 = 10; + +pub struct PresenceDetector { + proximity_estimator: ProximityEstimator, + proximity_state_detector: ProximityStateDetector, +} + +impl PresenceDetector { + pub fn new() -> Self { + PresenceDetector { + proximity_estimator: ProximityEstimator::new(), + proximity_state_detector: ProximityStateDetector::new(), + } + } + + pub fn on_ble_scan_result( + &mut self, + ble_scan_result: BleScanResult, + ) -> Option { + if &ble_scan_result.rssi > &MAX_RSSI_FILTER_VALUE { + // faulty scan result received + return None; + } + + // Update proximity estimator with the new scan result + self.proximity_estimator + .on_ble_scan_result(&ble_scan_result); + let device_id = ble_scan_result.device_id; + let estimated_distance = self + .proximity_estimator + .get_proximity_estimate(device_id as u64); + if estimated_distance != None { + return Some( + self.proximity_state_detector + .on_ranging_results_update(estimated_distance.unwrap()), + ); + } + None + } + + pub fn configure_proximity_state_options( + &mut self, + proximity_state_options: ProximityStateOptions, + ) { + self.proximity_state_detector + .configure_options(proximity_state_options); + } + + pub fn get_proximity_estimate(&mut self, device_id: u64) -> Option { + self.proximity_estimator.get_proximity_estimate(device_id) + } +} diff --git a/presence/fpp/fpp/src/proximity_estimator.rs b/presence/fpp/fpp/src/proximity_estimator.rs new file mode 100644 index 00000000..648df7b3 --- /dev/null +++ b/presence/fpp/fpp/src/proximity_estimator.rs @@ -0,0 +1,100 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use std::collections::HashMap; +use std::time::SystemTime; + +use crate::fspl_converter::*; +use crate::fused_presence_utils::{ + BleScanResult, MeasurementConfidence, PresenceDataSource, ProximityEstimate, +}; + +pub const DEFAULT_ESTIMATED_DISTANCE_DATA_TTL_MILLIS: u128 = 4000; + +pub struct ProximityEstimator { + estimated_distance_data_ttl_millis: u128, + best_proximity_estimate_per_device: HashMap, +} + +impl ProximityEstimator { + pub fn new() -> Self { + ProximityEstimator { + estimated_distance_data_ttl_millis: DEFAULT_ESTIMATED_DISTANCE_DATA_TTL_MILLIS, + best_proximity_estimate_per_device: HashMap::new(), + } + } + + pub fn on_ble_scan_result(&mut self, ble_scan_result: &BleScanResult) { + let device_id = ble_scan_result.device_id; + let elapsed_real_time_millis = SystemTime::now().elapsed().unwrap().as_millis(); + let last_estimated_proximity_estimate = + self.best_proximity_estimate_per_device.get(&device_id); + if last_estimated_proximity_estimate != None + && last_estimated_proximity_estimate.unwrap().source != PresenceDataSource::Ble + && (elapsed_real_time_millis + - last_estimated_proximity_estimate + .unwrap() + .elapsed_real_time_millis as u128) + < self.estimated_distance_data_ttl_millis as u128 + { + // Skip if last estimation is still fresh and is from a more precise data source (uwb/nan) + return; + } + let mut tx_power: i16 = 0; + if ble_scan_result.tx_power.present == true { + tx_power = ble_scan_result.tx_power.value as i16; + } + let rssi = ble_scan_result.rssi + tx_power as i16; + let best_proximity_estimate: ProximityEstimate = ProximityEstimate { + device_id, + distance_confidence: MeasurementConfidence::Low, + distance: compute_distance_meters_at_high_tx_power(rssi), + elapsed_real_time_millis, + source: PresenceDataSource::Ble, + }; + self.best_proximity_estimate_per_device + .insert(device_id, best_proximity_estimate); + } + + pub fn set_estimated_distances_ttl_millis(&mut self, ttl_millis: u128) { + self.estimated_distance_data_ttl_millis = ttl_millis; + } + + pub fn get_proximity_estimate(&mut self, device_id: u64) -> Option { + let elapsed_real_time_millis = SystemTime::now().elapsed().unwrap().as_millis(); + + // Exit early if no recorded proximity estimates + if self.best_proximity_estimate_per_device.get(&device_id) == None { + return None; + } + + let elapsed_time_since_range_result = elapsed_real_time_millis + - self + .best_proximity_estimate_per_device + .get(&device_id) + .unwrap() + .elapsed_real_time_millis; + + // If stale, remove entry + return nothing + if elapsed_time_since_range_result >= self.estimated_distance_data_ttl_millis { + self.best_proximity_estimate_per_device.remove(&device_id); + return None; + } + + return self + .best_proximity_estimate_per_device + .get(&device_id) + .copied(); + } +} diff --git a/presence/fpp/fpp/src/proximity_state_detector.rs b/presence/fpp/fpp/src/proximity_state_detector.rs new file mode 100644 index 00000000..c99b6b81 --- /dev/null +++ b/presence/fpp/fpp/src/proximity_state_detector.rs @@ -0,0 +1,77 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use std::collections::HashMap; +use std::ops::Deref; +use std::time::SystemTime; + +use crate::device_proximity_data::DeviceProximityData; +use crate::fused_presence_utils::*; +use crate::proximity_estimator::*; + +pub struct ProximityStateDetector { + device_proximity_data_map: HashMap, + last_range_update_time: u128, + proximity_state_options: ProximityStateOptions, +} + +impl ProximityStateDetector { + pub fn new() -> Self { + ProximityStateDetector { + device_proximity_data_map: HashMap::new(), + last_range_update_time: 0, + proximity_state_options: DEFAULT_PROXIMITY_STATE_OPTIONS, + } + } + + pub fn configure_options(&mut self, proximity_state_options: ProximityStateOptions) { + self.proximity_state_options = proximity_state_options; + for data in self.device_proximity_data_map.values_mut() { + data.configure_options(proximity_state_options); + } + } + + pub fn on_ranging_results_update( + &mut self, + proximity_estimate: ProximityEstimate, + ) -> ProximityStateResult { + let current_time_millis = SystemTime::now().elapsed().unwrap().as_millis(); + if current_time_millis - &self.last_range_update_time + > DEFAULT_ESTIMATED_DISTANCE_DATA_TTL_MILLIS as u128 + { + self.device_proximity_data_map.clear(); + } + + self.last_range_update_time = current_time_millis; + if self + .device_proximity_data_map + .get(&proximity_estimate.device_id) + == None + { + let new_device_proximity_data = + DeviceProximityData::new(Some(self.proximity_state_options)); + let device_id = &proximity_estimate.device_id; + self.device_proximity_data_map + .insert(*device_id, new_device_proximity_data); + } + + // Fix this code and remove hardcoded current proximity state. Error: Cannot borrow data in a & reference as mutable + // let proximity_state_of_current_range = self.device_proximity_data_map.get(&proximity_estimate.device_id).unwrap().update_current_proximity_state(proximity_estimate); + ProximityStateResult { + device_id: proximity_estimate.device_id, + current_proximity_state: ProximityState::Tap, + current_proximity_estimate: proximity_estimate, + } + } +} diff --git a/presence/fpp/Cargo.lock b/presence/fpp/fpp_c_ffi/Cargo.lock similarity index 50% rename from presence/fpp/Cargo.lock rename to presence/fpp/fpp_c_ffi/Cargo.lock index dcec71f2..68ad1946 100644 --- a/presence/fpp/Cargo.lock +++ b/presence/fpp/fpp_c_ffi/Cargo.lock @@ -2,26 +2,19 @@ # It is not intended for manual editing. version = 3 -[[package]] -name = "autocfg" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" - -[[package]] -name = "clock" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f04e35d66b91b3e7f7c9f9a7e72733f36d8cf7cde681880896aa32e59642111f" - [[package]] name = "fpp" version = "0.1.0" dependencies = [ - "clock", "ilog", "lazy_static", - "num-traits", +] + +[[package]] +name = "fpp_c_ffi" +version = "0.1.0" +dependencies = [ + "fpp", ] [[package]] @@ -35,12 +28,3 @@ name = "lazy_static" version = "1.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" - -[[package]] -name = "num-traits" -version = "0.2.15" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd" -dependencies = [ - "autocfg", -] diff --git a/presence/fpp/fpp_c_ffi/Cargo.toml b/presence/fpp/fpp_c_ffi/Cargo.toml new file mode 100644 index 00000000..0408e584 --- /dev/null +++ b/presence/fpp/fpp_c_ffi/Cargo.toml @@ -0,0 +1,9 @@ +[package] +name = "fpp_c_ffi" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +fpp = {path = "../fpp"} diff --git a/presence/fpp/fpp_c_ffi/include/presence_detector.h b/presence/fpp/fpp_c_ffi/include/presence_detector.h new file mode 100644 index 00000000..93b4d6a2 --- /dev/null +++ b/presence/fpp/fpp_c_ffi/include/presence_detector.h @@ -0,0 +1,92 @@ +#include +#include +#include +#include + +enum class MeasurementConfidence { + Low, + Medium, + High, + Unknown, +}; + + +enum class PresenceDataSource { + Ble, + Uwb, + Nan, + Unknown, +}; + + +enum class ProximityState { + Unknown, + Tap, + Reach, + ShortRange, + LongRange, + Far, +}; + + +struct PresenceDetector; + + +template +struct COption { + const T *value; + bool present; +}; + + +struct BleScanResult { + uint64_t device_id; + COption tx_power; + int16_t rssi; + uint64_t elapsed_real_time; +}; + + +struct ProximityEstimate { + uint64_t device_id; + double distance; + MeasurementConfidence distance_confidence; + u128 elapsed_real_time_millis; + PresenceDataSource source; +}; + +struct ProximityStateResult { + uint64_t device_id; + ProximityState current_proximity_state; + ProximityEstimate current_proximity_estimate; +}; + + +struct ProximityStateOptions { + double tap_threshold_meters; + double reach_threshold_meters; + double short_range_threshold_meters; + double long_range_threshold_meters; + double hysteresis_percentage; + double hysteresis_tap_extra_debounce_meters; + uint8_t consecutive_scans_required; +}; + +extern "C" { + +PresenceDetector *presence_detector_create(); + +void update_ble_scan_result(PresenceDetector *presence_detector, + BleScanResult ble_scan_result, + ProximityStateResult *proximity_state_result); + +void fpp_configure_options(PresenceDetector *presence_detector, + ProximityStateOptions proximity_state_options); + +void fpp_get_proximity_estimate(PresenceDetector *presence_detector, + uint64_t device_id, + ProximityEstimate *proximity_estimate); + +int presence_detector_free(PresenceDetector *presence_detector); + +} // extern "C" diff --git a/presence/fpp/fpp_c_ffi/src/lib.rs b/presence/fpp/fpp_c_ffi/src/lib.rs new file mode 100644 index 00000000..742924b3 --- /dev/null +++ b/presence/fpp/fpp_c_ffi/src/lib.rs @@ -0,0 +1,74 @@ +// Copyright 2023 Google LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use fpp::fused_presence_utils::*; +use fpp::presence_detector::*; + +// C-compatible API +#[no_mangle] +pub unsafe extern "C" fn presence_detector_create() -> *mut PresenceDetector { + Box::into_raw(Box::new(PresenceDetector::new())) +} + +#[no_mangle] +pub unsafe extern "C" fn update_ble_scan_result( + presence_detector: *mut PresenceDetector, + ble_scan_result: BleScanResult, + proximity_state_result: *mut ProximityStateResult, +) { + if let Some(presence_detector) = presence_detector.as_mut() { + if let Some(proximity_state_result) = proximity_state_result.as_mut() { + let result = presence_detector.on_ble_scan_result(ble_scan_result); + if let Some(result) = result { + *proximity_state_result = result; + } + } + } +} + +#[no_mangle] +pub unsafe extern "C" fn fpp_configure_options( + presence_detector: *mut PresenceDetector, + proximity_state_options: ProximityStateOptions, +) { + if let Some(presence_detector) = presence_detector.as_mut() { + presence_detector.configure_proximity_state_options(proximity_state_options); + } +} + +#[no_mangle] +pub unsafe extern "C" fn fpp_get_proximity_estimate( + presence_detector: *mut PresenceDetector, + device_id: u64, + proximity_estimate: *mut ProximityEstimate, +) { + if let Some(presence_detector) = presence_detector.as_mut() { + if presence_detector.get_proximity_estimate(device_id) != None { + if let Some(proximity_estimate) = proximity_estimate.as_mut() { + *proximity_estimate = presence_detector.get_proximity_estimate(device_id).unwrap(); + } + } + } +} + +#[no_mangle] +pub unsafe extern "C" fn presence_detector_free( + presence_detector: *mut PresenceDetector, +) -> std::os::raw::c_int { + if !presence_detector.is_null() { + let _ = Box::from_raw(presence_detector); + return 0; + } + return -1; +} diff --git a/presence/fpp/src/lib.rs b/presence/fpp/src/lib.rs deleted file mode 100644 index 6d49f6a9..00000000 --- a/presence/fpp/src/lib.rs +++ /dev/null @@ -1,5 +0,0 @@ -mod fused_presence_utils; -mod fspl_converter; - -#[cfg(test)] -mod fspl_converter_test; From d32ee27d13c9d8185ee1dae10c7ef6548b495217 Mon Sep 17 00:00:00 2001 From: jbabs1 Date: Thu, 20 Apr 2023 16:00:20 -0700 Subject: [PATCH 2/2] Add FPP Lib --- presence/fpp/fpp/Cargo.lock | 16 +++ presence/fpp/fpp/Cargo.toml | 1 + presence/fpp/fpp/src/fused_presence_utils.rs | 49 +------- presence/fpp/fpp/src/lib.rs | 3 - presence/fpp/fpp/src/presence_detector.rs | 108 ++++++++++++------ presence/fpp/fpp_c_ffi/Cargo.lock | 16 +++ .../fpp/fpp_c_ffi/include/presence_detector.h | 24 +--- presence/fpp/fpp_c_ffi/src/lib.rs | 16 +-- 8 files changed, 114 insertions(+), 119 deletions(-) diff --git a/presence/fpp/fpp/Cargo.lock b/presence/fpp/fpp/Cargo.lock index c95e5124..bc5c9ce5 100644 --- a/presence/fpp/fpp/Cargo.lock +++ b/presence/fpp/fpp/Cargo.lock @@ -2,11 +2,18 @@ # It is not intended for manual editing. version = 3 +[[package]] +name = "either" +version = "1.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91" + [[package]] name = "fpp" version = "0.1.0" dependencies = [ "ilog", + "itertools", "lazy_static", ] @@ -16,6 +23,15 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a9d6896d5a5d605a3b80da159ccc5a6e1fad346b4d2c1ad51046e22536b9a362" +[[package]] +name = "itertools" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473" +dependencies = [ + "either", +] + [[package]] name = "lazy_static" version = "1.4.0" diff --git a/presence/fpp/fpp/Cargo.toml b/presence/fpp/fpp/Cargo.toml index 132a9604..8652b4a7 100644 --- a/presence/fpp/fpp/Cargo.toml +++ b/presence/fpp/fpp/Cargo.toml @@ -7,4 +7,5 @@ edition = "2021" [dependencies] ilog = "1.0.1" +itertools = "0.10.5" lazy_static = "1.4.0" diff --git a/presence/fpp/fpp/src/fused_presence_utils.rs b/presence/fpp/fpp/src/fused_presence_utils.rs index ecd5a2e7..a58ac799 100644 --- a/presence/fpp/fpp/src/fused_presence_utils.rs +++ b/presence/fpp/fpp/src/fused_presence_utils.rs @@ -12,41 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -use lazy_static::lazy_static; -use std::collections::HashMap; - const AMBIGUITY_METERS: f64 = 0.06; pub const DEFAULT_TAP_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 0.02; pub const DEFAULT_REACH_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 0.5; pub const DEFAULT_SHORT_RANGE_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 1.2; pub const DEFAULT_LONG_RANGE_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 3.0; pub const DEFAULT_CONSECUTIVE_SCANS_REQUIRED: u8 = 2; -pub const DEFAULT_HYSTERESIS_PERCENTAGE: f64 = 0.2; -pub const DEFAULT_HYSTERESIS_TAP_EXTRA_DEBOUNCE_METERS: f64 = 0.2; - -pub const DEFAULT_PROXIMITY_STATE_OPTIONS: ProximityStateOptions = ProximityStateOptions { - tap_threshold_meters: DEFAULT_TAP_DISTANCE_THRESHOLD_METERS, - reach_threshold_meters: DEFAULT_REACH_DISTANCE_THRESHOLD_METERS, - short_range_threshold_meters: DEFAULT_SHORT_RANGE_DISTANCE_THRESHOLD_METERS, - long_range_threshold_meters: DEFAULT_LONG_RANGE_DISTANCE_THRESHOLD_METERS, - hysteresis_percentage: DEFAULT_HYSTERESIS_PERCENTAGE, - hysteresis_tap_extra_debounce_meters: DEFAULT_HYSTERESIS_TAP_EXTRA_DEBOUNCE_METERS, - consecutive_scans_required: DEFAULT_CONSECUTIVE_SCANS_REQUIRED, -}; - -lazy_static! { - pub static ref MAP_BLE_ADJUSTED_PROXIMITY_STATES: HashMap = { - let mut m = HashMap::new(); - // For BLE medium, reported granularity is reduced and all ranges are either TAP or LONG_RANGE - m.insert(ProximityState::Tap, ProximityState::Tap); - m.insert(ProximityState::Reach, ProximityState::Reach); - m.insert(ProximityState::ShortRange, ProximityState::Far); - m.insert(ProximityState::LongRange, ProximityState::Far); - m.insert(ProximityState::Far, ProximityState::Far); - m.insert(ProximityState::Unknown, ProximityState::Unknown); - m - }; -} // Proximity state from device to another in terms of actionability #[derive(Eq, Hash, Copy, Clone, PartialEq)] @@ -96,28 +67,10 @@ pub struct ProximityEstimate { pub distance: f64, pub distance_confidence: MeasurementConfidence, pub elapsed_real_time_millis: u128, + pub proximity_state: ProximityState, pub source: PresenceDataSource, } -#[derive(Copy, Clone, PartialEq)] -#[repr(C)] -pub struct ProximityStateOptions { - pub tap_threshold_meters: f64, - pub reach_threshold_meters: f64, - pub short_range_threshold_meters: f64, - pub long_range_threshold_meters: f64, - pub hysteresis_percentage: f64, - pub hysteresis_tap_extra_debounce_meters: f64, - pub consecutive_scans_required: u8, -} - -#[repr(C)] -pub struct ProximityStateResult { - pub device_id: u64, - pub current_proximity_state: ProximityState, - pub current_proximity_estimate: ProximityEstimate, -} - #[repr(C)] pub struct COption { pub value: *const T, diff --git a/presence/fpp/fpp/src/lib.rs b/presence/fpp/fpp/src/lib.rs index bb7af9a4..e744ed59 100644 --- a/presence/fpp/fpp/src/lib.rs +++ b/presence/fpp/fpp/src/lib.rs @@ -1,9 +1,6 @@ -mod device_proximity_data; mod fspl_converter; pub mod fused_presence_utils; pub mod presence_detector; -mod proximity_estimator; -mod proximity_state_detector; #[cfg(test)] mod fspl_converter_test; diff --git a/presence/fpp/fpp/src/presence_detector.rs b/presence/fpp/fpp/src/presence_detector.rs index a84c1c1c..f38a207b 100644 --- a/presence/fpp/fpp/src/presence_detector.rs +++ b/presence/fpp/fpp/src/presence_detector.rs @@ -12,61 +12,101 @@ // See the License for the specific language governing permissions and // limitations under the License. +use crate::fspl_converter::compute_distance_meters_at_high_tx_power; use crate::fused_presence_utils::{ - BleScanResult, ProximityEstimate, ProximityStateOptions, ProximityStateResult, + BleScanResult, MeasurementConfidence, PresenceDataSource, ProximityEstimate, ProximityState, + DEFAULT_CONSECUTIVE_SCANS_REQUIRED, DEFAULT_LONG_RANGE_DISTANCE_THRESHOLD_METERS, + DEFAULT_REACH_DISTANCE_THRESHOLD_METERS, DEFAULT_SHORT_RANGE_DISTANCE_THRESHOLD_METERS, + DEFAULT_TAP_DISTANCE_THRESHOLD_METERS, }; -use crate::proximity_estimator::*; -use crate::proximity_state_detector::*; +use itertools::Itertools; +use std::collections::{HashMap, VecDeque}; +use std::time::SystemTime; const MAX_RSSI_FILTER_VALUE: i16 = 10; +const DEFAULT_ESTIMATED_DISTANCE_DATA_TTL_MILLIS: u128 = 4000; + +// Static function for getting proximity state from threshold +fn get_proximity_state_from_threshold(distance_meters: f64) -> ProximityState { + if distance_meters <= DEFAULT_TAP_DISTANCE_THRESHOLD_METERS { + return ProximityState::Tap; + } + if distance_meters <= DEFAULT_REACH_DISTANCE_THRESHOLD_METERS { + return ProximityState::Reach; + } + if distance_meters <= DEFAULT_SHORT_RANGE_DISTANCE_THRESHOLD_METERS { + return ProximityState::ShortRange; + } + if distance_meters <= DEFAULT_LONG_RANGE_DISTANCE_THRESHOLD_METERS { + return ProximityState::LongRange; + } + ProximityState::Far +} pub struct PresenceDetector { - proximity_estimator: ProximityEstimator, - proximity_state_detector: ProximityStateDetector, + last_range_update_time: u128, + best_proximity_estimate_per_device: HashMap, + transition_history: VecDeque, } impl PresenceDetector { pub fn new() -> Self { PresenceDetector { - proximity_estimator: ProximityEstimator::new(), - proximity_state_detector: ProximityStateDetector::new(), + last_range_update_time: 0, + best_proximity_estimate_per_device: HashMap::new(), + transition_history: VecDeque::with_capacity( + (DEFAULT_CONSECUTIVE_SCANS_REQUIRED + 1).into(), + ), } } pub fn on_ble_scan_result( &mut self, ble_scan_result: BleScanResult, - ) -> Option { - if &ble_scan_result.rssi > &MAX_RSSI_FILTER_VALUE { - // faulty scan result received - return None; - } - - // Update proximity estimator with the new scan result - self.proximity_estimator - .on_ble_scan_result(&ble_scan_result); + ) -> Option { let device_id = ble_scan_result.device_id; - let estimated_distance = self - .proximity_estimator - .get_proximity_estimate(device_id as u64); - if estimated_distance != None { - return Some( - self.proximity_state_detector - .on_ranging_results_update(estimated_distance.unwrap()), - ); + if ble_scan_result.rssi > MAX_RSSI_FILTER_VALUE { + return self + .best_proximity_estimate_per_device + .get(&device_id) + .copied(); } - None - } - - pub fn configure_proximity_state_options( - &mut self, - proximity_state_options: ProximityStateOptions, - ) { - self.proximity_state_detector - .configure_options(proximity_state_options); + let elapsed_real_time_millis = SystemTime::now().elapsed().unwrap().as_millis(); + if elapsed_real_time_millis - &self.last_range_update_time + > DEFAULT_ESTIMATED_DISTANCE_DATA_TTL_MILLIS + { + self.transition_history.clear(); + } + let mut tx_power: i16 = 0; + if ble_scan_result.tx_power.present == true { + tx_power = ble_scan_result.tx_power.value as i16; + } + let rssi = ble_scan_result.rssi + tx_power as i16; + let distance_meters = compute_distance_meters_at_high_tx_power(rssi); + let new_proximity_estimate = ProximityEstimate { + device_id, + distance_confidence: MeasurementConfidence::Low, + distance: distance_meters, + proximity_state: get_proximity_state_from_threshold(distance_meters), + elapsed_real_time_millis, + source: PresenceDataSource::Ble, + }; + self.transition_history + .push_front(new_proximity_estimate.proximity_state); + self.transition_history + .truncate(DEFAULT_CONSECUTIVE_SCANS_REQUIRED.into()); + if self.transition_history.iter().unique().count() == 1 { + self.best_proximity_estimate_per_device + .insert(device_id, new_proximity_estimate); + } + self.best_proximity_estimate_per_device + .get(&device_id) + .copied() } pub fn get_proximity_estimate(&mut self, device_id: u64) -> Option { - self.proximity_estimator.get_proximity_estimate(device_id) + self.best_proximity_estimate_per_device + .get(&device_id) + .copied() } } diff --git a/presence/fpp/fpp_c_ffi/Cargo.lock b/presence/fpp/fpp_c_ffi/Cargo.lock index 68ad1946..de6e6e32 100644 --- a/presence/fpp/fpp_c_ffi/Cargo.lock +++ b/presence/fpp/fpp_c_ffi/Cargo.lock @@ -2,11 +2,18 @@ # It is not intended for manual editing. version = 3 +[[package]] +name = "either" +version = "1.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91" + [[package]] name = "fpp" version = "0.1.0" dependencies = [ "ilog", + "itertools", "lazy_static", ] @@ -23,6 +30,15 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a9d6896d5a5d605a3b80da159ccc5a6e1fad346b4d2c1ad51046e22536b9a362" +[[package]] +name = "itertools" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473" +dependencies = [ + "either", +] + [[package]] name = "lazy_static" version = "1.4.0" diff --git a/presence/fpp/fpp_c_ffi/include/presence_detector.h b/presence/fpp/fpp_c_ffi/include/presence_detector.h index 93b4d6a2..171ddf2a 100644 --- a/presence/fpp/fpp_c_ffi/include/presence_detector.h +++ b/presence/fpp/fpp_c_ffi/include/presence_detector.h @@ -2,6 +2,7 @@ #include #include #include +#include enum class MeasurementConfidence { Low, @@ -52,36 +53,17 @@ struct ProximityEstimate { double distance; MeasurementConfidence distance_confidence; u128 elapsed_real_time_millis; + ProximityState proximity_state; PresenceDataSource source; }; -struct ProximityStateResult { - uint64_t device_id; - ProximityState current_proximity_state; - ProximityEstimate current_proximity_estimate; -}; - - -struct ProximityStateOptions { - double tap_threshold_meters; - double reach_threshold_meters; - double short_range_threshold_meters; - double long_range_threshold_meters; - double hysteresis_percentage; - double hysteresis_tap_extra_debounce_meters; - uint8_t consecutive_scans_required; -}; - extern "C" { PresenceDetector *presence_detector_create(); void update_ble_scan_result(PresenceDetector *presence_detector, BleScanResult ble_scan_result, - ProximityStateResult *proximity_state_result); - -void fpp_configure_options(PresenceDetector *presence_detector, - ProximityStateOptions proximity_state_options); + ProximityEstimate *proximity_estimate); void fpp_get_proximity_estimate(PresenceDetector *presence_detector, uint64_t device_id, diff --git a/presence/fpp/fpp_c_ffi/src/lib.rs b/presence/fpp/fpp_c_ffi/src/lib.rs index 742924b3..725b3ded 100644 --- a/presence/fpp/fpp_c_ffi/src/lib.rs +++ b/presence/fpp/fpp_c_ffi/src/lib.rs @@ -25,28 +25,18 @@ pub unsafe extern "C" fn presence_detector_create() -> *mut PresenceDetector { pub unsafe extern "C" fn update_ble_scan_result( presence_detector: *mut PresenceDetector, ble_scan_result: BleScanResult, - proximity_state_result: *mut ProximityStateResult, + proximity_estimate: *mut ProximityEstimate, ) { if let Some(presence_detector) = presence_detector.as_mut() { - if let Some(proximity_state_result) = proximity_state_result.as_mut() { + if let Some(proximity_estimate) = proximity_estimate.as_mut() { let result = presence_detector.on_ble_scan_result(ble_scan_result); if let Some(result) = result { - *proximity_state_result = result; + *proximity_estimate = result; } } } } -#[no_mangle] -pub unsafe extern "C" fn fpp_configure_options( - presence_detector: *mut PresenceDetector, - proximity_state_options: ProximityStateOptions, -) { - if let Some(presence_detector) = presence_detector.as_mut() { - presence_detector.configure_proximity_state_options(proximity_state_options); - } -} - #[no_mangle] pub unsafe extern "C" fn fpp_get_proximity_estimate( presence_detector: *mut PresenceDetector,