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

Add FPP Lib

Browse Source
This commit is contained in:
jbabs1
2023-05-10 15:56:45 -07:00
parent bcbf171f59
commit d32ee27d13
8 changed files with 114 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
presence/fpp/fpp/Cargo.lock
Generated
+16
View File
@@ -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"
presence/fpp/fpp/Cargo.toml
+1
View File
@@ -7,4 +7,5 @@ edition = "2021"
[dependencies]
ilog = "1.0.1"
itertools = "0.10.5"
lazy_static = "1.4.0"
presence/fpp/fpp/src/fused_presence_utils.rs
+1 -48
View File
@@ -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<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)]
@@ -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<T> {
pub value: *const T,
presence/fpp/fpp/src/lib.rs
-3
View File
@@ -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;
presence/fpp/fpp/src/presence_detector.rs
+74 -34
View File
@@ -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<u64, ProximityEstimate>,
transition_history: VecDeque<ProximityState>,
}
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<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);
) -> Option<ProximityEstimate> {
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<ProximityEstimate> {
self.proximity_estimator.get_proximity_estimate(device_id)
self.best_proximity_estimate_per_device
.get(&device_id)
.copied()
}
}
presence/fpp/fpp_c_ffi/Cargo.lock
Generated
+16
View File
@@ -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"
presence/fpp/fpp_c_ffi/include/presence_detector.h
+3 -21
View File
@@ -2,6 +2,7 @@
#include <cstdint>
#include <cstdlib>
#include <ostream>
#include <new>
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,
presence/fpp/fpp_c_ffi/src/lib.rs
+3 -13
View File
@@ -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,