mirror of
https://github.com/kidfromjupiter/nearby.git
synced 2026-09-14 22:56:12 -04:00
Merge branch 'fpp-rust' of https://github.com/jbabs1/nearby into fpp-rust
This commit is contained in:
jbabs1
Generated
-14
@@ -12,17 +12,9 @@ checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91"
|
||||
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"
|
||||
@@ -31,9 +23,3 @@ checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473"
|
||||
dependencies = [
|
||||
"either",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "lazy_static"
|
||||
version = "1.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
|
||||
|
||||
@@ -6,6 +6,4 @@ edition = "2021"
|
||||
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
|
||||
|
||||
[dependencies]
|
||||
ilog = "1.0.1"
|
||||
itertools = "0.10.5"
|
||||
lazy_static = "1.4.0"
|
||||
|
||||
@@ -12,45 +12,26 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
use std::collections::HashMap;
|
||||
const ADVERTISE_TX_POWER_HIGH_DB: i32 = 1;
|
||||
|
||||
use lazy_static::lazy_static;
|
||||
const FSPL_AT_1_METER_DB: i32 = 40;
|
||||
|
||||
const ADVERTISE_TX_POWER_ULTRA_LOW: u8 = 0;
|
||||
const ADVERTISE_TX_POWER_LOW: u8 = 1;
|
||||
const ADVERTISE_TX_POWER_MEDIUM: u8 = 2;
|
||||
const ADVERTISE_TX_POWER_HIGH: u8 = 3;
|
||||
const MEASURED_POWER_AT_1_METER_DB_AT_HIGH_TX_POWER: i32 = -60;
|
||||
|
||||
const FSPL_AT_1_METER_DB: i16 = 40;
|
||||
|
||||
const MEASURED_POWER_AT_1_METER_DB_AT_HIGH_TX_POWER: i8 = -60;
|
||||
|
||||
lazy_static! {
|
||||
static ref TX_POWER_SETTING_TO_DB: HashMap<u8, i8> = {
|
||||
let mut m = HashMap::new();
|
||||
// Nominal power requested to antenna when using different Advertising TX Power settings
|
||||
m.insert(ADVERTISE_TX_POWER_HIGH, 1);
|
||||
m.insert(ADVERTISE_TX_POWER_MEDIUM, -7);
|
||||
m.insert(ADVERTISE_TX_POWER_LOW, -15);
|
||||
m.insert(ADVERTISE_TX_POWER_ULTRA_LOW, -21);
|
||||
m
|
||||
};
|
||||
pub fn compute_distance_meters_at_high_tx_power(rssi: i32) -> f64 {
|
||||
let nominal_tx_power = ADVERTISE_TX_POWER_HIGH_DB;
|
||||
let antenna_gain =
|
||||
(nominal_tx_power - FSPL_AT_1_METER_DB) - MEASURED_POWER_AT_1_METER_DB_AT_HIGH_TX_POWER;
|
||||
let tx_power_at_0_meters = nominal_tx_power - antenna_gain;
|
||||
compute_distance_meters(tx_power_at_0_meters, rssi)
|
||||
}
|
||||
|
||||
pub fn compute_distance_meters_at_high_tx_power(rssi: i16) -> f64 {
|
||||
let nominal_tx_power = TX_POWER_SETTING_TO_DB.get(&ADVERTISE_TX_POWER_HIGH);
|
||||
let antenna_gain = (nominal_tx_power.unwrap() - FSPL_AT_1_METER_DB as i8)
|
||||
- MEASURED_POWER_AT_1_METER_DB_AT_HIGH_TX_POWER;
|
||||
let tx_power_at_0_meters = nominal_tx_power.unwrap() - antenna_gain;
|
||||
compute_distance_meters(tx_power_at_0_meters as i16, rssi)
|
||||
}
|
||||
|
||||
pub fn compute_distance_meters(tx_power_at_0_meters: i16, rssi: i16) -> f64 {
|
||||
let fspl: i16 = tx_power_at_0_meters - rssi;
|
||||
pub fn compute_distance_meters(tx_power_at_0_meters: i32, rssi: i32) -> f64 {
|
||||
let fspl = tx_power_at_0_meters - rssi;
|
||||
ble_fspl_to_meters(fspl)
|
||||
}
|
||||
|
||||
fn ble_fspl_to_meters(fspl: i16) -> f64 {
|
||||
fn ble_fspl_to_meters(fspl: i32) -> f64 {
|
||||
let base: f64 = 10.0;
|
||||
base.powi((fspl - FSPL_AT_1_METER_DB) as i32 / 20)
|
||||
base.powi((fspl - FSPL_AT_1_METER_DB) / 20)
|
||||
}
|
||||
|
||||
@@ -12,67 +12,95 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
pub(crate) const DEFAULT_TAP_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 0.02;
|
||||
pub(crate) const DEFAULT_REACH_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 0.5;
|
||||
pub(crate) const DEFAULT_SHORT_RANGE_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 1.2;
|
||||
pub(crate) const DEFAULT_LONG_RANGE_DISTANCE_THRESHOLD_METERS: f64 = AMBIGUITY_METERS + 3.0;
|
||||
pub(crate) const DEFAULT_CONSECUTIVE_SCANS_REQUIRED: u8 = 2;
|
||||
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;
|
||||
|
||||
// Proximity state from device to another in terms of actionability
|
||||
#[derive(Eq, Hash, Copy, Clone, PartialEq)]
|
||||
/// Proximity state from device to another in terms of actionability
|
||||
#[derive(Eq, Hash, Copy, Clone, PartialEq, Debug)]
|
||||
#[repr(C)]
|
||||
pub enum ProximityState {
|
||||
/// Unknown proximity state
|
||||
Unknown,
|
||||
/// The device is within a tap zone (<0.02m)
|
||||
Tap,
|
||||
/// The device is within a reach zone (<0.5m)
|
||||
Reach,
|
||||
/// The device is within a short range zone (<1.2m)
|
||||
ShortRange,
|
||||
/// The device is within a long range zone (<3.0m)
|
||||
LongRange,
|
||||
/// The device is at a far range
|
||||
Far,
|
||||
}
|
||||
|
||||
// Represents the confidence levels for a given measurement
|
||||
#[derive(Copy, Clone, PartialEq)]
|
||||
/// Represents the confidence levels for a given measurement
|
||||
#[derive(Copy, Clone, PartialEq, Debug)]
|
||||
#[repr(C)]
|
||||
pub enum MeasurementConfidence {
|
||||
/// Measurement confidence is low, the default for BLE medium
|
||||
Low,
|
||||
/// Measurement confidence is medium
|
||||
Medium,
|
||||
/// Measurement confidence is High
|
||||
High,
|
||||
/// Measurement confidence is unknown
|
||||
Unknown,
|
||||
}
|
||||
|
||||
// Data sources that are used to track presence
|
||||
#[derive(Copy, Clone, PartialEq)]
|
||||
/// Data sources that are used to track presence
|
||||
#[derive(Copy, Clone, PartialEq, Debug)]
|
||||
#[repr(C)]
|
||||
pub enum PresenceDataSource {
|
||||
/// Data source for proximity estimate is BLE
|
||||
Ble,
|
||||
/// Data source for proximity estimate is UWB
|
||||
Uwb,
|
||||
/// Data source for proximity estimate is NAN
|
||||
Nan,
|
||||
/// Data source for proximity estimate is unknown
|
||||
Unknown,
|
||||
}
|
||||
|
||||
// A PII-stripped subset of Bluetooth scan result
|
||||
/// A PII-stripped subset of Bluetooth scan result
|
||||
#[repr(C)]
|
||||
pub struct BleScanResult {
|
||||
/// Device ID of the nearby device
|
||||
pub device_id: u64,
|
||||
pub tx_power: COption<i16>,
|
||||
pub rssi: i16,
|
||||
pub elapsed_real_time: u64,
|
||||
/// Transmitting power of signal
|
||||
pub tx_power: MaybeTxPower,
|
||||
/// RSSI value
|
||||
pub rssi: i32,
|
||||
/// Time scan result was obtained
|
||||
pub elapsed_real_time_millis: u64,
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone, PartialEq)]
|
||||
/// Enum representing an optional tx power value
|
||||
#[repr(C)]
|
||||
pub enum MaybeTxPower {
|
||||
/// Valid TX power with associated data value
|
||||
Valid(i32),
|
||||
/// Absent Tx Power
|
||||
Invalid,
|
||||
}
|
||||
|
||||
/// Describes the most accurate and recent measurement for a given device
|
||||
#[derive(Copy, Clone, PartialEq, Debug)]
|
||||
#[repr(C)]
|
||||
pub struct ProximityEstimate {
|
||||
/// Device ID of the nearby device
|
||||
pub device_id: u64,
|
||||
pub distance: f64,
|
||||
/// Distance to the nearby device in meters
|
||||
pub distance_meters: f64,
|
||||
/// Measurement confidence of the estimate
|
||||
pub distance_confidence: MeasurementConfidence,
|
||||
/// The time the proximity estimate was obtained
|
||||
pub elapsed_real_time_millis: u128,
|
||||
/// Proximity state zone of the nearby device
|
||||
pub proximity_state: ProximityState,
|
||||
/// Medium through which the proximity estimate was computed
|
||||
pub source: PresenceDataSource,
|
||||
}
|
||||
|
||||
#[repr(C)]
|
||||
pub struct COption<T> {
|
||||
pub value: *const T,
|
||||
pub present: bool,
|
||||
}
|
||||
|
||||
@@ -1,6 +1,37 @@
|
||||
// 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.
|
||||
|
||||
#![deny(
|
||||
missing_docs,
|
||||
clippy::indexing_slicing,
|
||||
clippy::unwrap_used,
|
||||
clippy::panic,
|
||||
clippy::expect_used
|
||||
)]
|
||||
|
||||
//! Processes raw scan results from BLE, UWB and NAN and outputs proximity estimates/zones
|
||||
|
||||
mod fspl_converter;
|
||||
|
||||
/// Fused presence Utils
|
||||
pub mod fused_presence_utils;
|
||||
|
||||
/// Presence detector module
|
||||
pub mod presence_detector;
|
||||
|
||||
#[cfg(test)]
|
||||
mod fspl_converter_test;
|
||||
|
||||
#[cfg(test)]
|
||||
mod presence_detector_test;
|
||||
|
||||
@@ -12,21 +12,23 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
use std::collections::{HashMap, VecDeque};
|
||||
use std::time::Instant;
|
||||
|
||||
use itertools::Itertools;
|
||||
|
||||
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,
|
||||
BleScanResult, MaybeTxPower, 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 MAX_RSSI_FILTER_VALUE: i32 = 10;
|
||||
const DEFAULT_ESTIMATED_DISTANCE_DATA_TTL_MILLIS: u128 = 4000;
|
||||
|
||||
// Static function for getting proximity state from threshold
|
||||
/// 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;
|
||||
@@ -43,16 +45,31 @@ fn get_proximity_state_from_threshold(distance_meters: f64) -> ProximityState {
|
||||
ProximityState::Far
|
||||
}
|
||||
|
||||
/// Tracks and computes proximity/presence state events.
|
||||
pub struct PresenceDetector {
|
||||
last_range_update_time: u128,
|
||||
last_range_update_time: RangingUpdateTime,
|
||||
best_proximity_estimate_per_device: HashMap<u64, ProximityEstimate>,
|
||||
transition_history: VecDeque<ProximityState>,
|
||||
}
|
||||
|
||||
struct RangingUpdateTime(u128);
|
||||
|
||||
impl RangingUpdateTime {
|
||||
pub fn is_expired(&self) -> bool {
|
||||
let elapsed_real_time_millis = Instant::now().elapsed().as_millis();
|
||||
elapsed_real_time_millis - self.0 > DEFAULT_ESTIMATED_DISTANCE_DATA_TTL_MILLIS
|
||||
}
|
||||
|
||||
pub fn update(&mut self) {
|
||||
self.0 = Instant::now().elapsed().as_millis();
|
||||
}
|
||||
}
|
||||
|
||||
impl PresenceDetector {
|
||||
/// Creates a new instance of presence detector
|
||||
pub fn new() -> Self {
|
||||
PresenceDetector {
|
||||
last_range_update_time: 0,
|
||||
last_range_update_time: RangingUpdateTime(0),
|
||||
best_proximity_estimate_per_device: HashMap::new(),
|
||||
transition_history: VecDeque::with_capacity(
|
||||
(DEFAULT_CONSECUTIVE_SCANS_REQUIRED + 1).into(),
|
||||
@@ -60,6 +77,7 @@ impl PresenceDetector {
|
||||
}
|
||||
}
|
||||
|
||||
/// Updates the presence detector with a new scan result and returns the current proximity estimate
|
||||
pub fn on_ble_scan_result(
|
||||
&mut self,
|
||||
ble_scan_result: BleScanResult,
|
||||
@@ -71,42 +89,49 @@ impl PresenceDetector {
|
||||
.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
|
||||
{
|
||||
if self.last_range_update_time.is_expired() {
|
||||
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 mut tx_power: i32 = 0;
|
||||
if let MaybeTxPower::Valid(some_tx_power) = ble_scan_result.tx_power {
|
||||
tx_power = some_tx_power;
|
||||
}
|
||||
let rssi = ble_scan_result.rssi + tx_power as i16;
|
||||
let rssi = ble_scan_result.rssi + tx_power;
|
||||
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,
|
||||
distance_meters,
|
||||
proximity_state: get_proximity_state_from_threshold(distance_meters),
|
||||
elapsed_real_time_millis,
|
||||
elapsed_real_time_millis: Instant::now().elapsed().as_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 {
|
||||
if self.transition_history.iter().unique().count() == 1
|
||||
&& self.transition_history.len() == DEFAULT_CONSECUTIVE_SCANS_REQUIRED.into()
|
||||
{
|
||||
self.best_proximity_estimate_per_device
|
||||
.insert(device_id, new_proximity_estimate);
|
||||
self.last_range_update_time.update();
|
||||
}
|
||||
self.best_proximity_estimate_per_device
|
||||
.get(&device_id)
|
||||
.copied()
|
||||
}
|
||||
|
||||
pub fn get_proximity_estimate(&mut self, device_id: u64) -> Option<ProximityEstimate> {
|
||||
/// Returns the current proximity estimate for a given device
|
||||
pub fn get_proximity_estimate(&self, device_id: u64) -> Option<ProximityEstimate> {
|
||||
self.best_proximity_estimate_per_device
|
||||
.get(&device_id)
|
||||
.copied()
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for PresenceDetector {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,105 @@
|
||||
// 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::*;
|
||||
use crate::presence_detector::*;
|
||||
|
||||
const BLE_SCAN_RESULT_REACH_ZONE: BleScanResult = BleScanResult {
|
||||
device_id: 1234,
|
||||
tx_power: { MaybeTxPower::Invalid },
|
||||
rssi: -40,
|
||||
elapsed_real_time_millis: 123456,
|
||||
};
|
||||
|
||||
const BLE_SCAN_RESULT_BAD_RSSI: BleScanResult = BleScanResult {
|
||||
rssi: 127,
|
||||
..BLE_SCAN_RESULT_REACH_ZONE
|
||||
};
|
||||
|
||||
const BLE_SCAN_RESULT_SHORT_RANGE_ZONE: BleScanResult = BleScanResult {
|
||||
rssi: -60,
|
||||
..BLE_SCAN_RESULT_REACH_ZONE
|
||||
};
|
||||
|
||||
const REACH_PROXIMITY_ESTIMATE: ProximityEstimate = ProximityEstimate {
|
||||
device_id: 1234,
|
||||
distance_meters: 0.1,
|
||||
distance_confidence: MeasurementConfidence::Low,
|
||||
elapsed_real_time_millis: 0,
|
||||
proximity_state: ProximityState::Reach,
|
||||
source: PresenceDataSource::Ble,
|
||||
};
|
||||
|
||||
const SHORT_RANGE_PROXIMITY_ESTIMATE: ProximityEstimate = ProximityEstimate {
|
||||
distance_meters: 1.0,
|
||||
proximity_state: ProximityState::ShortRange,
|
||||
..REACH_PROXIMITY_ESTIMATE
|
||||
};
|
||||
|
||||
#[test]
|
||||
fn test_on_ble_scan_result_success() {
|
||||
// Tests that the proximity state stored for each device is the accurate one after two
|
||||
// consecutive scan results
|
||||
let mut presence_detector = PresenceDetector::new();
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_REACH_ZONE),
|
||||
None
|
||||
);
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_REACH_ZONE),
|
||||
Some(ProximityEstimate {
|
||||
device_id: 1234,
|
||||
distance_meters: 0.1,
|
||||
distance_confidence: MeasurementConfidence::Low,
|
||||
elapsed_real_time_millis: 0,
|
||||
proximity_state: ProximityState::Reach,
|
||||
source: PresenceDataSource::Ble
|
||||
})
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_on_ble_scan_result_bad_rssi() {
|
||||
// Tests that scan results with bad RSSIs are ignored
|
||||
let mut presence_detector = PresenceDetector::new();
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_REACH_ZONE),
|
||||
None
|
||||
);
|
||||
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_BAD_RSSI),
|
||||
None
|
||||
);
|
||||
}
|
||||
#[test]
|
||||
fn test_on_ble_scan_result_transition_to_new_zone() {
|
||||
let mut presence_detector = PresenceDetector::new();
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_REACH_ZONE),
|
||||
None
|
||||
);
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_REACH_ZONE),
|
||||
Some(REACH_PROXIMITY_ESTIMATE)
|
||||
);
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_SHORT_RANGE_ZONE),
|
||||
Some(REACH_PROXIMITY_ESTIMATE)
|
||||
);
|
||||
assert_eq!(
|
||||
presence_detector.on_ble_scan_result(BLE_SCAN_RESULT_SHORT_RANGE_ZONE),
|
||||
Some(SHORT_RANGE_PROXIMITY_ESTIMATE)
|
||||
);
|
||||
}
|
||||
Generated
+64
-5
@@ -2,6 +2,12 @@
|
||||
# It is not intended for manual editing.
|
||||
version = 3
|
||||
|
||||
[[package]]
|
||||
name = "cfg-if"
|
||||
version = "1.0.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
|
||||
|
||||
[[package]]
|
||||
name = "either"
|
||||
version = "1.8.1"
|
||||
@@ -12,9 +18,7 @@ checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91"
|
||||
name = "fpp"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"ilog",
|
||||
"itertools",
|
||||
"lazy_static",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@@ -22,13 +26,20 @@ name = "fpp_c_ffi"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"fpp",
|
||||
"lazy_static",
|
||||
"rand",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "ilog"
|
||||
version = "1.0.1"
|
||||
name = "getrandom"
|
||||
version = "0.2.9"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a9d6896d5a5d605a3b80da159ccc5a6e1fad346b4d2c1ad51046e22536b9a362"
|
||||
checksum = "c85e1d9ab2eadba7e5040d4e09cbd6d072b76a557ad64e797c2cb9d4da21d7e4"
|
||||
dependencies = [
|
||||
"cfg-if",
|
||||
"libc",
|
||||
"wasi",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "itertools"
|
||||
@@ -44,3 +55,51 @@ name = "lazy_static"
|
||||
version = "1.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
|
||||
|
||||
[[package]]
|
||||
name = "libc"
|
||||
version = "0.2.144"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2b00cc1c228a6782d0f076e7b232802e0c5689d41bb5df366f2a6b6621cfdfe1"
|
||||
|
||||
[[package]]
|
||||
name = "ppv-lite86"
|
||||
version = "0.2.17"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de"
|
||||
|
||||
[[package]]
|
||||
name = "rand"
|
||||
version = "0.8.5"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404"
|
||||
dependencies = [
|
||||
"libc",
|
||||
"rand_chacha",
|
||||
"rand_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_chacha"
|
||||
version = "0.3.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88"
|
||||
dependencies = [
|
||||
"ppv-lite86",
|
||||
"rand_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_core"
|
||||
version = "0.6.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c"
|
||||
dependencies = [
|
||||
"getrandom",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "wasi"
|
||||
version = "0.11.0+wasi-snapshot-preview1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423"
|
||||
|
||||
@@ -7,3 +7,5 @@ edition = "2021"
|
||||
|
||||
[dependencies]
|
||||
fpp = {path = "../fpp"}
|
||||
lazy_static = "1.4.0"
|
||||
rand = "0.8.5"
|
||||
|
||||
@@ -1,74 +1,150 @@
|
||||
// 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.
|
||||
|
||||
#ifndef PRESENCE_DETECTOR_H_
|
||||
#define PRESENCE_DETECTOR_H_
|
||||
|
||||
#include <cstdarg>
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <ostream>
|
||||
#include <new>
|
||||
|
||||
// Represents the confidence levels for a given measurement
|
||||
enum class MeasurementConfidence {
|
||||
Low,
|
||||
Medium,
|
||||
High,
|
||||
Unknown,
|
||||
/// Measurement confidence is low, the default for BLE medium
|
||||
Low,
|
||||
/// Measurement confidence is medium
|
||||
Medium,
|
||||
/// Measurement confidence is High
|
||||
High,
|
||||
/// Measurement confidence is unknown
|
||||
Unknown,
|
||||
};
|
||||
|
||||
|
||||
/// Data sources that are used to track presence
|
||||
enum class PresenceDataSource {
|
||||
Ble,
|
||||
Uwb,
|
||||
Nan,
|
||||
Unknown,
|
||||
/// Data source for proximity estimate is BLE
|
||||
Ble,
|
||||
/// Data source for proximity estimate is UWB
|
||||
Uwb,
|
||||
/// Data source for proximity estimate is NAN
|
||||
Nan,
|
||||
/// Data source for proximity estimate is unknown
|
||||
Unknown,
|
||||
};
|
||||
|
||||
|
||||
/// Proximity state from device to another in terms of actionability
|
||||
enum class ProximityState {
|
||||
Unknown,
|
||||
Tap,
|
||||
Reach,
|
||||
ShortRange,
|
||||
LongRange,
|
||||
Far,
|
||||
/// Unknown proximity state
|
||||
Unknown,
|
||||
/// The device is within a tap zone (<0.02m)
|
||||
Tap,
|
||||
/// The device is within a reach zone (<0.5m)
|
||||
Reach,
|
||||
/// The device is within a short range zone (<1.2m)
|
||||
ShortRange,
|
||||
/// The device is within a long range zone (<3.0m)
|
||||
LongRange,
|
||||
/// The device is at a far range
|
||||
Far,
|
||||
};
|
||||
|
||||
|
||||
struct PresenceDetector;
|
||||
|
||||
|
||||
template<typename T>
|
||||
struct COption {
|
||||
const T *value;
|
||||
bool present;
|
||||
/// Wraps the handle ID to an underlying PresenceDetector object
|
||||
struct PresenceDetectorHandle {
|
||||
uint64_t handle;
|
||||
};
|
||||
|
||||
/// Enum representing an optional tx power value
|
||||
struct MaybeTxPower {
|
||||
enum class Tag {
|
||||
/// Valid TX power with associated data value
|
||||
Valid,
|
||||
/// Absent Tx Power
|
||||
Invalid,
|
||||
};
|
||||
|
||||
struct Valid_Body {
|
||||
int32_t _0;
|
||||
};
|
||||
|
||||
Tag tag;
|
||||
union {
|
||||
Valid_Body valid;
|
||||
};
|
||||
};
|
||||
|
||||
/// A PII-stripped subset of Bluetooth scan result
|
||||
struct BleScanResult {
|
||||
uint64_t device_id;
|
||||
COption<int16_t> tx_power;
|
||||
int16_t rssi;
|
||||
uint64_t elapsed_real_time;
|
||||
/// Device ID of the nearby device
|
||||
uint64_t device_id;
|
||||
/// Transmitting power of signal
|
||||
MaybeTxPower tx_power;
|
||||
/// RSSI value
|
||||
int32_t rssi;
|
||||
/// Time scan result was obtained
|
||||
uint64_t elapsed_real_time_millis;
|
||||
};
|
||||
|
||||
|
||||
/// Describes the most accurate and recent measurement for a given device
|
||||
struct ProximityEstimate {
|
||||
uint64_t device_id;
|
||||
double distance;
|
||||
MeasurementConfidence distance_confidence;
|
||||
u128 elapsed_real_time_millis;
|
||||
ProximityState proximity_state;
|
||||
PresenceDataSource source;
|
||||
/// Device ID of the nearby device
|
||||
uint64_t device_id;
|
||||
/// Distance to the nearby device in meters
|
||||
double distance_meters;
|
||||
/// Measurement confidence of the estimate
|
||||
MeasurementConfidence distance_confidence;
|
||||
/// The time the proximity estimate was obtained
|
||||
u128 elapsed_real_time_millis;
|
||||
/// Proximity state zone of the nearby device
|
||||
ProximityState proximity_state;
|
||||
/// Medium through which the proximity estimate was computed
|
||||
PresenceDataSource source;
|
||||
};
|
||||
|
||||
extern "C" {
|
||||
|
||||
PresenceDetector *presence_detector_create();
|
||||
/// Creates a new presence detector object and returns the handle for the new
|
||||
/// object
|
||||
PresenceDetectorHandle presence_detector_create();
|
||||
|
||||
void update_ble_scan_result(PresenceDetector *presence_detector,
|
||||
BleScanResult ble_scan_result,
|
||||
ProximityEstimate *proximity_estimate);
|
||||
/// Updates PresenceDetector with a new scan result and returns an error code
|
||||
/// if unsuccessful
|
||||
///
|
||||
/// # Safety
|
||||
///
|
||||
/// Ensure that the output parameter refers to an initialized instance
|
||||
int32_t update_ble_scan_result(PresenceDetectorHandle presence_detector_handle,
|
||||
BleScanResult ble_scan_result,
|
||||
ProximityEstimate *proximity_estimate);
|
||||
|
||||
void fpp_get_proximity_estimate(PresenceDetector *presence_detector,
|
||||
uint64_t device_id,
|
||||
ProximityEstimate *proximity_estimate);
|
||||
/// Gets the current proximity estimate for a given device ID
|
||||
///
|
||||
/// # Safety
|
||||
///
|
||||
/// Ensure that the output parameter refers to an initialized instance
|
||||
int32_t get_proximity_estimate(PresenceDetectorHandle presence_detector_handle,
|
||||
uint64_t device_id,
|
||||
ProximityEstimate *proximity_estimate);
|
||||
|
||||
int presence_detector_free(PresenceDetector *presence_detector);
|
||||
/// De-allocates memory for a presence detector object
|
||||
int presence_detector_free(PresenceDetectorHandle presence_detector_handle);
|
||||
|
||||
} // extern "C"
|
||||
} // extern "C"
|
||||
|
||||
#ifdef __cplusplus
|
||||
} // extern "C"
|
||||
#endif
|
||||
|
||||
#endif // PRESENCE_DETECTOR_H_
|
||||
|
||||
@@ -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 core::marker::PhantomData;
|
||||
use fpp::presence_detector::PresenceDetector;
|
||||
use lazy_static::lazy_static;
|
||||
use rand::Rng;
|
||||
use std::collections::HashMap;
|
||||
use std::sync::{Mutex, MutexGuard};
|
||||
|
||||
pub(crate) struct HandleMap<T> {
|
||||
_marker: PhantomData<T>,
|
||||
map: HashMap<u64, T>,
|
||||
}
|
||||
|
||||
impl<T> HandleMap<T> {
|
||||
pub(crate) fn init() -> Self {
|
||||
Self {
|
||||
_marker: Default::default(),
|
||||
map: HashMap::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// inserts an entry into the map and returns the randomly generated handle to the entry
|
||||
pub(crate) fn insert(&mut self, data: T) -> u64 {
|
||||
let mut rng = rand::thread_rng();
|
||||
let mut handle: u64 = rng.gen();
|
||||
|
||||
while self.map.contains_key(&handle) {
|
||||
handle = rng.gen();
|
||||
}
|
||||
|
||||
assert!(self.map.insert(handle, data).is_none());
|
||||
handle
|
||||
}
|
||||
|
||||
/// Removes an entry at a given handle returning an Option of the owned value
|
||||
pub(crate) fn remove(&mut self, handle: &u64) -> Option<T> {
|
||||
self.map.remove(handle)
|
||||
}
|
||||
|
||||
/// Gets a reference to the entry stored at the specified handle
|
||||
pub(crate) fn get(&mut self, handle: &u64) -> Option<&mut T> {
|
||||
self.map.get_mut(handle)
|
||||
}
|
||||
}
|
||||
|
||||
// Returns a threadsafe instance of the global static hashmap tracking the PresenceDetector handles
|
||||
pub(crate) fn get_presence_detector_handle_map(
|
||||
) -> MutexGuard<'static, HandleMap<Box<PresenceDetector>>> {
|
||||
PRESENCE_DETECTOR_HANDLE_MAP
|
||||
.lock()
|
||||
.unwrap_or_else(|err_guard| err_guard.into_inner())
|
||||
}
|
||||
|
||||
// Global hashmap to track valid pointers, this is a safety precaution to make sure we are not
|
||||
// reading from unsafe memory address's passed in by the caller
|
||||
lazy_static! {
|
||||
static ref PRESENCE_DETECTOR_HANDLE_MAP: Mutex<HandleMap<Box<PresenceDetector>>> =
|
||||
Mutex::new(HandleMap::init());
|
||||
}
|
||||
@@ -12,53 +12,121 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#![deny(
|
||||
missing_docs,
|
||||
clippy::indexing_slicing,
|
||||
clippy::unwrap_used,
|
||||
clippy::panic,
|
||||
clippy::expect_used
|
||||
)]
|
||||
|
||||
//! Rust FFI wrapper for PresenceDetector. Can be called from C/C++ clients
|
||||
|
||||
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()))
|
||||
use crate::handle_map::get_presence_detector_handle_map;
|
||||
|
||||
mod handle_map;
|
||||
|
||||
/// Wraps the handle ID to an underlying PresenceDetector object
|
||||
#[repr(C)]
|
||||
pub struct PresenceDetectorHandle {
|
||||
handle: u64,
|
||||
}
|
||||
|
||||
/// Error enum class representing possible errors
|
||||
#[repr(C)]
|
||||
pub enum ProximityEstimateError {
|
||||
/// Returned if the handle is invalid
|
||||
InvalidPresenceDetectorHandle,
|
||||
/// Returned if the output parameter is null
|
||||
NullOutputParameter,
|
||||
}
|
||||
|
||||
impl ProximityEstimateError {
|
||||
fn to_error_code(&self) -> i32 {
|
||||
match self {
|
||||
Self::InvalidPresenceDetectorHandle => -1,
|
||||
Self::NullOutputParameter => -2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const SUCCESS: i32 = 0;
|
||||
|
||||
/// Creates a new presence detector object and returns the handle for the new object
|
||||
#[no_mangle]
|
||||
pub extern "C" fn presence_detector_create() -> PresenceDetectorHandle {
|
||||
let handle = get_presence_detector_handle_map().insert(Box::new(PresenceDetector::new()));
|
||||
PresenceDetectorHandle { handle }
|
||||
}
|
||||
|
||||
/// Updates PresenceDetector with a new scan result and returns an error code if unsuccessful
|
||||
///
|
||||
/// # Safety
|
||||
///
|
||||
/// Ensure that the output parameter refers to an initialized instance
|
||||
#[no_mangle]
|
||||
pub unsafe extern "C" fn update_ble_scan_result(
|
||||
presence_detector: *mut PresenceDetector,
|
||||
presence_detector_handle: PresenceDetectorHandle,
|
||||
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;
|
||||
}
|
||||
}
|
||||
) -> i32 {
|
||||
if let Some(presence_detector) =
|
||||
get_presence_detector_handle_map().get(&presence_detector_handle.handle)
|
||||
{
|
||||
presence_detector
|
||||
.on_ble_scan_result(ble_scan_result)
|
||||
.map(|current_proximity_estimate| {
|
||||
proximity_estimate.as_mut().map(|proximity_estimate| {
|
||||
*proximity_estimate = current_proximity_estimate;
|
||||
Some(SUCCESS)
|
||||
});
|
||||
ProximityEstimateError::NullOutputParameter.to_error_code()
|
||||
});
|
||||
}
|
||||
ProximityEstimateError::InvalidPresenceDetectorHandle.to_error_code()
|
||||
}
|
||||
|
||||
/// Gets the current proximity estimate for a given device ID
|
||||
///
|
||||
/// # Safety
|
||||
///
|
||||
/// Ensure that the output parameter refers to an initialized instance
|
||||
#[no_mangle]
|
||||
pub unsafe extern "C" fn fpp_get_proximity_estimate(
|
||||
presence_detector: *mut PresenceDetector,
|
||||
pub unsafe extern "C" fn get_proximity_estimate(
|
||||
presence_detector_handle: PresenceDetectorHandle,
|
||||
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();
|
||||
}
|
||||
}
|
||||
) -> i32 {
|
||||
if let Some(presence_detector) =
|
||||
get_presence_detector_handle_map().get(&presence_detector_handle.handle)
|
||||
{
|
||||
presence_detector
|
||||
.get_proximity_estimate(device_id)
|
||||
.map(|current_proximity_estimate| {
|
||||
if let Some(proximity_estimate) = proximity_estimate.as_mut() {
|
||||
*proximity_estimate = current_proximity_estimate;
|
||||
return SUCCESS;
|
||||
}
|
||||
ProximityEstimateError::NullOutputParameter.to_error_code()
|
||||
});
|
||||
}
|
||||
|
||||
ProximityEstimateError::InvalidPresenceDetectorHandle.to_error_code()
|
||||
}
|
||||
|
||||
/// De-allocates memory for a presence detector object
|
||||
#[no_mangle]
|
||||
pub unsafe extern "C" fn presence_detector_free(
|
||||
presence_detector: *mut PresenceDetector,
|
||||
pub extern "C" fn presence_detector_free(
|
||||
presence_detector_handle: PresenceDetectorHandle,
|
||||
) -> std::os::raw::c_int {
|
||||
if !presence_detector.is_null() {
|
||||
let _ = Box::from_raw(presence_detector);
|
||||
return 0;
|
||||
if let Some(presence_detector) =
|
||||
get_presence_detector_handle_map().remove(&presence_detector_handle.handle)
|
||||
{
|
||||
let _ = *presence_detector;
|
||||
return SUCCESS;
|
||||
}
|
||||
return -1;
|
||||
ProximityEstimateError::InvalidPresenceDetectorHandle.to_error_code()
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user