Add FPP Lib

This commit is contained in:
jbabs1
2023-05-09 11:16:28 -07:00
parent c91e1d1111
commit bcbf171f59
15 changed files with 668 additions and 34 deletions
+23
View File
@@ -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"
@@ -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"
@@ -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<ProximityState>,
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<f64>,
proximity_state_options: ProximityStateOptions,
transition_history: VecDeque<ProximityState>,
current_proximity_state: ProximityState,
source: PresenceDataSource,
}
impl DeviceProximityData {
pub fn new(proximity_state_options: Option<ProximityStateOptions>) -> 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<ProximityState> =
VecDeque::with_capacity(storage_length.into());
let mut new_device_distance_meters_history: VecDeque<f64> =
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
}
}
@@ -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]
@@ -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<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,
@@ -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<T> {
pub value: *const T,
pub present: bool,
}
+9
View File
@@ -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;
+72
View File
@@ -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<ProximityStateResult> {
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<ProximityEstimate> {
self.proximity_estimator.get_proximity_estimate(device_id)
}
}
+100
View File
@@ -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<u64, ProximityEstimate>,
}
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<ProximityEstimate> {
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();
}
}
@@ -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<u64, DeviceProximityData>,
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,
}
}
}
+7 -23
View File
@@ -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",
]
+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"}
@@ -0,0 +1,92 @@
#include <cstdarg>
#include <cstdint>
#include <cstdlib>
#include <ostream>
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;
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"
+74
View File
@@ -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;
}
-5
View File
@@ -1,5 +0,0 @@
mod fused_presence_utils;
mod fspl_converter;
#[cfg(test)]
mod fspl_converter_test;