TySS-Dev/nearby
1
0
Fork 0
mirror of https://github.com/kidfromjupiter/nearby.git synced 2026-09-16 07:36:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

Add FPP Lib

Browse Source
This commit is contained in:
jbabs1
2023-05-11 12:40:06 -07:00
parent c91e1d1111
commit e68ce4a836
11 changed files with 333 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
presence/fpp/fpp/Cargo.lock
Generated
+39
View File
@@ -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"
presence/fpp/Cargo.toml → presence/fpp/fpp/Cargo.toml
+1 -2
View File
@@ -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"
presence/fpp/src/fspl_converter.rs → presence/fpp/fpp/src/fspl_converter.rs
View File
presence/fpp/src/fspl_converter_test.rs → presence/fpp/fpp/src/fspl_converter_test.rs
+1 -3
View File
@@ -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]
presence/fpp/src/fused_presence_utils.rs → presence/fpp/fpp/src/fused_presence_utils.rs
+11 -39
View File
@@ -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<ProximityState, ProximityState> = {
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<u8>,
pub tx_power: COption<i16>,
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<T> {
pub value: *const T,
pub present: bool,
}
presence/fpp/src/lib.rs → presence/fpp/fpp/src/lib.rs
+2 -1
View File
@@ -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;
presence/fpp/fpp/src/presence_detector.rs
+112
View File
@@ -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<u64, ProximityEstimate>,
transition_history: VecDeque<ProximityState>,
}
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<ProximityEstimate> {
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<ProximityEstimate> {
self.best_proximity_estimate_per_device
.get(&device_id)
.copied()
}
}
presence/fpp/Cargo.lock → presence/fpp/fpp_c_ffi/Cargo.lock
Generated
+20 -20
View File
@@ -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",
]
presence/fpp/fpp_c_ffi/Cargo.toml
+9
View File
@@ -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"}
presence/fpp/fpp_c_ffi/include/presence_detector.h
+74
View File
@@ -0,0 +1,74 @@
#include <cstdarg>
#include <cstdint>
#include <cstdlib>
#include <ostream>
#include <new>
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<typename T>
struct COption {
const T *value;
bool present;
};
struct BleScanResult {
uint64_t device_id;
COption<int16_t> 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"
presence/fpp/fpp_c_ffi/src/lib.rs
+64
View File
@@ -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;
}