diff --git a/presence/fpp/fpp/Cargo.lock b/presence/fpp/fpp/Cargo.lock new file mode 100644 index 00000000..bc5c9ce5 --- /dev/null +++ b/presence/fpp/fpp/Cargo.lock @@ -0,0 +1,39 @@ +# This file is automatically @generated by Cargo. +# 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", +] + +[[package]] +name = "ilog" +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" +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..8652b4a7 100644 --- a/presence/fpp/Cargo.toml +++ b/presence/fpp/fpp/Cargo.toml @@ -6,7 +6,6 @@ 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" +itertools = "0.10.5" lazy_static = "1.4.0" -num-traits = "0.2.15" \ No newline at end of file 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 52% rename from presence/fpp/src/fused_presence_utils.rs rename to presence/fpp/fpp/src/fused_presence_utils.rs index f0e31984..a58ac799 100644 --- a/presence/fpp/src/fused_presence_utils.rs +++ b/presence/fpp/fpp/src/fused_presence_utils.rs @@ -12,44 +12,16 @@ // 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)] +#[repr(C)] pub enum ProximityState { Unknown, Tap, @@ -61,6 +33,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 +43,7 @@ pub enum MeasurementConfidence { // Data sources that are used to track presence #[derive(Copy, Clone, PartialEq)] +#[repr(C)] pub enum PresenceDataSource { Ble, Uwb, @@ -78,29 +52,27 @@ 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, pub distance_confidence: MeasurementConfidence, pub elapsed_real_time_millis: u128, + pub proximity_state: ProximityState, pub source: PresenceDataSource, } -#[derive(Copy, Clone, PartialEq)] -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 COption { + pub value: *const T, + pub present: bool, } diff --git a/presence/fpp/src/lib.rs b/presence/fpp/fpp/src/lib.rs similarity index 50% rename from presence/fpp/src/lib.rs rename to presence/fpp/fpp/src/lib.rs index 6d49f6a9..e744ed59 100644 --- a/presence/fpp/src/lib.rs +++ b/presence/fpp/fpp/src/lib.rs @@ -1,5 +1,6 @@ -mod fused_presence_utils; mod fspl_converter; +pub mod fused_presence_utils; +pub mod presence_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..f38a207b --- /dev/null +++ b/presence/fpp/fpp/src/presence_detector.rs @@ -0,0 +1,112 @@ +// 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::fspl_converter::compute_distance_meters_at_high_tx_power; +use crate::fused_presence_utils::{ + 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 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 { + last_range_update_time: u128, + best_proximity_estimate_per_device: HashMap, + transition_history: VecDeque, +} + +impl PresenceDetector { + pub fn new() -> Self { + PresenceDetector { + 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 { + let device_id = ble_scan_result.device_id; + if ble_scan_result.rssi > MAX_RSSI_FILTER_VALUE { + return self + .best_proximity_estimate_per_device + .get(&device_id) + .copied(); + } + 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.best_proximity_estimate_per_device + .get(&device_id) + .copied() + } +} diff --git a/presence/fpp/Cargo.lock b/presence/fpp/fpp_c_ffi/Cargo.lock similarity index 63% rename from presence/fpp/Cargo.lock rename to presence/fpp/fpp_c_ffi/Cargo.lock index dcec71f2..de6e6e32 100644 --- a/presence/fpp/Cargo.lock +++ b/presence/fpp/fpp_c_ffi/Cargo.lock @@ -3,25 +3,25 @@ version = 3 [[package]] -name = "autocfg" -version = "1.1.0" +name = "either" +version = "1.8.1" 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" +checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91" [[package]] name = "fpp" version = "0.1.0" dependencies = [ - "clock", "ilog", + "itertools", "lazy_static", - "num-traits", +] + +[[package]] +name = "fpp_c_ffi" +version = "0.1.0" +dependencies = [ + "fpp", ] [[package]] @@ -30,17 +30,17 @@ 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" 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..171ddf2a --- /dev/null +++ b/presence/fpp/fpp_c_ffi/include/presence_detector.h @@ -0,0 +1,74 @@ +#include +#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; + ProximityState proximity_state; + PresenceDataSource source; +}; + +extern "C" { + +PresenceDetector *presence_detector_create(); + +void update_ble_scan_result(PresenceDetector *presence_detector, + BleScanResult ble_scan_result, + ProximityEstimate *proximity_estimate); + +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..725b3ded --- /dev/null +++ b/presence/fpp/fpp_c_ffi/src/lib.rs @@ -0,0 +1,64 @@ +// 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_estimate: *mut ProximityEstimate, +) { + if let Some(presence_detector) = presence_detector.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_estimate = result; + } + } + } +} + +#[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; +}