From 79fc2f4266ac76efabb89581029f598d9982d0f6 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Tue, 15 Oct 2024 22:00:01 +0100 Subject: [PATCH 01/99] Initial commit --- lib/core/src/types.rs | 14 ++++++ .../src/preprocessing/accelerometer.rs | 49 +++++++++++++++++++ 2 files changed, 63 insertions(+) diff --git a/lib/core/src/types.rs b/lib/core/src/types.rs index ea7e87c1..d814b2b8 100644 --- a/lib/core/src/types.rs +++ b/lib/core/src/types.rs @@ -14,6 +14,20 @@ impl DigitalSignal { } } +type AccelerometerData = Vec; +type RawAccelerometerData = Vec; + +struct RawAccelerationData { + x: i32, + y: i32, + z: i32, + timestamp: i32, + is_sensor_active: bool, +} + +static kNumAccelerators: i32 = 3; +static kNumAxis: i32 = 3; + #[cfg(test)] mod tests { use super::*; diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 0ae040b6..250589fd 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1 +1,50 @@ //test +pub core::types + +trait AcclerometerPreprocessor{ + num_reliable_accelerometers_: i32; + reliable_accelerometers_: Vec; + + fn processData(&self, data: core::types::AccelerometerData) -> core::types::AccelerometerData; + + fn(handleOutliers(&self, data: core::types::AccelerometerData) -> core::types::AccelerometerData; +} + +impl AcclerometerPreprocessor for core::types::AccelerometerData{ + fn processData(&self, data: core::types::AccelerometerData) -> core::types::AccelerometerData{ + // do some processing + let accelerometer_data: core::types::AccelerometerData; + + core::types::kNumAccelerometers.iter().for_each(|i| { + data[i] = data[i] * 2; + magnitude: f32 = 0.0; + core::type::kNumAxis.iter().for_each(|j| { + magnitude += data[i][j] * data[i][j]; + }); + accelerometer_data[i] = magnitude.sqrt(); + }); + + const clean_accelerometer_data: core::types::AccelerometerData = handleOutliers(accelerometer_data); + + return data; + } + + fn handleOutliers(&self, data: core::types::AccelerometerData) { + quartiles: Quartiles; + if (num_reliable_accelerometers_ == core::types::kNumAccelerometers) { + quartiles = getQuartiles(data); + } + else if (num_reliable_accelerometers_ == core::types::kNumAccelerometers - 1) { + let filtered_data = Box::new([0.0; core::types::kNumAccelerometers-1]); + core::type::kNumAccelerometers.iter().for_each(|i| { + if (reliable_accelerometers_.contains(i)) { + filtered_data.push(data[i]); + } + }); + quartiles = getQuartiles(data); + } + else { + num_reliable_accelerometers_++; + } + } +} From 8299f392e184716499644b394ce045e3d973222d Mon Sep 17 00:00:00 2001 From: sean-leishman Date: Wed, 16 Oct 2024 00:06:41 +0100 Subject: [PATCH 02/99] Fixup types in use of entry preprocess function --- Cargo.lock | 1 + lib/core/src/types.rs | 12 +- lib/localisation/Cargo.toml | 2 + lib/localisation/src/lib.rs | 4 + .../src/preprocessing/accelerometer.rs | 117 +++++++++++++----- 5 files changed, 100 insertions(+), 36 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index f06f0c9a..21de47c1 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -311,6 +311,7 @@ dependencies = [ name = "hyped_localisation" version = "0.1.0" dependencies = [ + "hyped_core", "nalgebra", ] diff --git a/lib/core/src/types.rs b/lib/core/src/types.rs index d814b2b8..4372824a 100644 --- a/lib/core/src/types.rs +++ b/lib/core/src/types.rs @@ -14,10 +14,7 @@ impl DigitalSignal { } } -type AccelerometerData = Vec; -type RawAccelerometerData = Vec; - -struct RawAccelerationData { +pub struct RawAccelerationData { x: i32, y: i32, z: i32, @@ -25,8 +22,11 @@ struct RawAccelerationData { is_sensor_active: bool, } -static kNumAccelerators: i32 = 3; -static kNumAxis: i32 = 3; +pub const K_NUM_ACCELEROMETERS: i32 = 3; +pub const K_NUM_AXIS: i32 = 3; + +pub type RawAccelerometerData = [[f32; K_NUM_AXIS as usize]; K_NUM_ACCELEROMETERS as usize]; +pub type AccelerometerData = [f32; K_NUM_ACCELEROMETERS as usize]; #[cfg(test)] mod tests { diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index d9f27530..3f79efeb 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -5,3 +5,5 @@ edition = "2021" [dependencies] nalgebra = "0.33.0" + +hyped_core = { path = "../core" } diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index 0c9ac1ac..62d662c9 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1 +1,5 @@ #![no_std] + +mod preprocessing { + pub mod accelerometer; +} diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 250589fd..8c336ffa 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,50 +1,107 @@ -//test -pub core::types +use hyped_core::types::AccelerometerData; +use hyped_core::types::RawAccelerometerData; +use hyped_core::types::K_NUM_ACCELEROMETERS; +use hyped_core::types::K_NUM_AXIS; -trait AcclerometerPreprocessor{ - num_reliable_accelerometers_: i32; - reliable_accelerometers_: Vec; - fn processData(&self, data: core::types::AccelerometerData) -> core::types::AccelerometerData; +pub struct AccelerometerPreprocessor { + num_reliable_accelerometers_: i32, + reliable_accelerometers_: [i32; K_NUM_ACCELEROMETERS as usize], +} - fn(handleOutliers(&self, data: core::types::AccelerometerData) -> core::types::AccelerometerData; +struct Quartiles { + q1: f32, + q2: f32, + q3: f32, } -impl AcclerometerPreprocessor for core::types::AccelerometerData{ - fn processData(&self, data: core::types::AccelerometerData) -> core::types::AccelerometerData{ - // do some processing - let accelerometer_data: core::types::AccelerometerData; - - core::types::kNumAccelerometers.iter().for_each(|i| { - data[i] = data[i] * 2; - magnitude: f32 = 0.0; - core::type::kNumAxis.iter().for_each(|j| { - magnitude += data[i][j] * data[i][j]; - }); - accelerometer_data[i] = magnitude.sqrt(); - }); +trait AcclerometerPreprocessor{ + fn new() -> AccelerometerPreprocessor; - const clean_accelerometer_data: core::types::AccelerometerData = handleOutliers(accelerometer_data); + fn process_data(&self, data: RawAccelerometerData) -> AccelerometerData; + fn handle_outliers(&self, data: AccelerometerData); + fn get_quartiles(&self, data: AccelerometerData) -> Quartiles; +} - return data; +impl AcclerometerPreprocessor for AccelerometerPreprocessor { + fn new() -> AccelerometerPreprocessor { + AccelerometerPreprocessor { + num_reliable_accelerometers_: 0, + reliable_accelerometers_: [0; K_NUM_ACCELEROMETERS as usize], + } } - fn handleOutliers(&self, data: core::types::AccelerometerData) { - quartiles: Quartiles; - if (num_reliable_accelerometers_ == core::types::kNumAccelerometers) { - quartiles = getQuartiles(data); + fn handle_outliers(&self, data: AccelerometerData) { + let quartiles: Quartiles; + if (num_reliable_accelerometers_ == K_NUM_ACCELEROMETERS) { + quartiles = get_quartiles(data); } - else if (num_reliable_accelerometers_ == core::types::kNumAccelerometers - 1) { - let filtered_data = Box::new([0.0; core::types::kNumAccelerometers-1]); + else if (num_reliable_accelerometers_ == K_NUM_ACCELEROMETERS - 1) { + let filtered_data = Box::new([0.0; K_NUM_ACCELEROMETERS-1]); core::type::kNumAccelerometers.iter().for_each(|i| { if (reliable_accelerometers_.contains(i)) { filtered_data.push(data[i]); } }); - quartiles = getQuartiles(data); + quartiles = get_quartiles(data); } else { - num_reliable_accelerometers_++; + self.num_reliable_accelerometers_ += 1; } } + + fn get_quartiles(&self, data: AccelerometerData) -> Quartiles { + let mut sorted_data = data.clone(); + // sorted_data.sort(); + let q1 = sorted_data[K_NUM_ACCELEROMETERS/4]; + let q2 = sorted_data[K_NUM_ACCELEROMETERS/2]; + let q3 = sorted_data[K_NUM_ACCELEROMETERS*3/4]; + return Quartiles{q1, q2, q3}; + } + + fn process_data(&self, data: RawAccelerometerData) -> AccelerometerData{ + // do some processing + let mut accelerometer_data: AccelerometerData; + let magnitude: f32; + + data + .iter() + .enumerate() + .for_each(|(i, axis)| { + magnitude = 0.0; + axis.iter().enumerate().map(|(j, val)| { + magnitude += val * val; + }); + accelerometer_data[i] = magnitude.sqrt(); + }); + + let clean_accelerometer_data: AccelerometerData = self.handle_outliers(accelerometer_data); + + clean_accelerometer_data + } + +} + +mod tests { + use super::*; + + #[test] + fn test_accelerometer_preprocessor() { + let preprocessor = AccelerometerPreprocessor { + num_reliable_accelerometers_: 0, + reliable_accelerometers_: Vec::new(), + }; + + let data = core::types::AccelerometerData { + 0: [1.0, 2.0, 3.0], + 1: [4.0, 5.0, 6.0], + 2: [7.0, 8.0, 9.0], + }; + + let processed_data = preprocessor.processData(data); + + assert_eq!(processed_data[0], [2.0, 4.0, 6.0]); + assert_eq!(processed_data[1], [8.0, 10.0, 12.0]); + assert_eq!(processed_data[2], [14.0, 16.0, 18.0]); + } } From 213fce54865c866e39694174cff269a6681706f8 Mon Sep 17 00:00:00 2001 From: sean-leishman Date: Wed, 23 Oct 2024 22:56:29 +0100 Subject: [PATCH 03/99] Update preprocessor main entry functions --- Cargo.lock | 1 + Cargo.toml | 2 +- lib/core/src/types.rs | 18 +- lib/localisation/Cargo.toml | 1 + .../src/preprocessing/accelerometer.rs | 200 ++++++++++++------ 5 files changed, 148 insertions(+), 74 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 21de47c1..13aa749e 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -311,6 +311,7 @@ dependencies = [ name = "hyped_localisation" version = "0.1.0" dependencies = [ + "heapless", "hyped_core", "nalgebra", ] diff --git a/Cargo.toml b/Cargo.toml index a5608263..ca26b885 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,4 +6,4 @@ members = [ exclude = [ "boards/stm32l476rg", ] -resolver = "2" \ No newline at end of file +resolver = "2" diff --git a/lib/core/src/types.rs b/lib/core/src/types.rs index 4372824a..849a2184 100644 --- a/lib/core/src/types.rs +++ b/lib/core/src/types.rs @@ -1,3 +1,5 @@ +use heapless::Vec; + #[derive(PartialEq, Debug)] pub enum DigitalSignal { High, @@ -22,11 +24,19 @@ pub struct RawAccelerationData { is_sensor_active: bool, } -pub const K_NUM_ACCELEROMETERS: i32 = 3; -pub const K_NUM_AXIS: i32 = 3; +pub const K_NUM_ACCELEROMETERS: usize = 3; +pub const K_NUM_AXIS: usize = 3; +pub const K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS: i32 = 2; + +#[derive(PartialEq)] +pub enum SensorChecks { + Unacceptable, + Acceptable, +} -pub type RawAccelerometerData = [[f32; K_NUM_AXIS as usize]; K_NUM_ACCELEROMETERS as usize]; -pub type AccelerometerData = [f32; K_NUM_ACCELEROMETERS as usize]; +pub type RawAccelerometerData = + Vec, NUM_ACC>; +pub type AccelerometerData = Vec; #[cfg(test)] mod tests { diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index 3f79efeb..91fcae50 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -5,5 +5,6 @@ edition = "2021" [dependencies] nalgebra = "0.33.0" +heapless = "0.8.0" hyped_core = { path = "../core" } diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 8c336ffa..b0523ae1 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,12 +1,10 @@ -use hyped_core::types::AccelerometerData; -use hyped_core::types::RawAccelerometerData; -use hyped_core::types::K_NUM_ACCELEROMETERS; -use hyped_core::types::K_NUM_AXIS; - +use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; +use hyped_core::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; pub struct AccelerometerPreprocessor { num_reliable_accelerometers_: i32, - reliable_accelerometers_: [i32; K_NUM_ACCELEROMETERS as usize], + reliable_accelerometers_: [bool; K_NUM_ACCELEROMETERS as usize], + num_outliers_per_accelerometer_: [i32; K_NUM_ACCELEROMETERS as usize], } struct Quartiles { @@ -15,93 +13,157 @@ struct Quartiles { q3: f32, } -trait AcclerometerPreprocessor{ - fn new() -> AccelerometerPreprocessor; - - fn process_data(&self, data: RawAccelerometerData) -> AccelerometerData; - fn handle_outliers(&self, data: AccelerometerData); - fn get_quartiles(&self, data: AccelerometerData) -> Quartiles; +fn get_quartiles(data: &AccelerometerData) -> Quartiles { + let mut sorted_data = data.clone(); + sorted_data.sort_by(|a, b| a.partial_cmp(b).unwrap()); + let q1 = sorted_data[K_NUM_ACCELEROMETERS / 4]; + let q2 = sorted_data[K_NUM_ACCELEROMETERS / 2]; + let q3 = sorted_data[K_NUM_ACCELEROMETERS * 3 / 4]; + return Quartiles { q1, q2, q3 }; } -impl AcclerometerPreprocessor for AccelerometerPreprocessor { - fn new() -> AccelerometerPreprocessor { - AccelerometerPreprocessor { - num_reliable_accelerometers_: 0, - reliable_accelerometers_: [0; K_NUM_ACCELEROMETERS as usize], +impl AccelerometerPreprocessor { + fn handle_outliers( + &mut self, + data: AccelerometerData, + ) -> Option> { + let quartiles = match self.calculate_quartiles(&data) { + Some(quartiles) => quartiles, + None => return None, + }; + + let iqr = quartiles.q3 - quartiles.q1; + let mut lower_bound = quartiles.q1 - 1.5 * iqr; + let mut upper_bound = quartiles.q3 + 1.5 * iqr; + if self.num_reliable_accelerometers_ < K_NUM_ACCELEROMETERS as i32 { + lower_bound = quartiles.q1 - 1.2 * iqr; + upper_bound = quartiles.q3 + 1.2 * iqr; } - } - fn handle_outliers(&self, data: AccelerometerData) { - let quartiles: Quartiles; - if (num_reliable_accelerometers_ == K_NUM_ACCELEROMETERS) { - quartiles = get_quartiles(data); - } - else if (num_reliable_accelerometers_ == K_NUM_ACCELEROMETERS - 1) { - let filtered_data = Box::new([0.0; K_NUM_ACCELEROMETERS-1]); - core::type::kNumAccelerometers.iter().for_each(|i| { - if (reliable_accelerometers_.contains(i)) { - filtered_data.push(data[i]); - } - }); - quartiles = get_quartiles(data); - } - else { - self.num_reliable_accelerometers_ += 1; - } + let accelerometer_data = data + .iter() + .enumerate() + .map(|(i, val)| { + if !self.reliable_accelerometers_[i] { + return quartiles.q2; + } else if val < &lower_bound || val > &upper_bound { + self.num_outliers_per_accelerometer_[i] += 1; + return quartiles.q2; + } else { + self.num_outliers_per_accelerometer_[i] = 0; + return val.clone(); + } + }) + .collect(); + + Some(accelerometer_data) } - fn get_quartiles(&self, data: AccelerometerData) -> Quartiles { - let mut sorted_data = data.clone(); - // sorted_data.sort(); - let q1 = sorted_data[K_NUM_ACCELEROMETERS/4]; - let q2 = sorted_data[K_NUM_ACCELEROMETERS/2]; - let q3 = sorted_data[K_NUM_ACCELEROMETERS*3/4]; - return Quartiles{q1, q2, q3}; + fn calculate_quartiles( + &self, + data: &AccelerometerData, + ) -> Option { + let quartiles: Quartiles; + if self.num_reliable_accelerometers_ == K_NUM_ACCELEROMETERS as i32 { + quartiles = get_quartiles(data); + } else if self.num_reliable_accelerometers_ == (K_NUM_ACCELEROMETERS as i32 - 1) { + const SIZE: usize = K_NUM_ACCELEROMETERS - 1; + let filtered_data: AccelerometerData = (0..SIZE) + .into_iter() + .map(|i| match self.reliable_accelerometers_[i] { + true => data[i], + false => 0.0, + }) + .collect(); + + quartiles = get_quartiles(&filtered_data); + } else { + return None; + } + + Some(quartiles) } - fn process_data(&self, data: RawAccelerometerData) -> AccelerometerData{ + fn process_data( + &mut self, + data: RawAccelerometerData, + ) -> Option> { // do some processing - let mut accelerometer_data: AccelerometerData; - let magnitude: f32; - - data + let mut accelerometer_data: AccelerometerData = + AccelerometerData::new(); + let mut magnitude: f32 = 0.0; + + data.iter().enumerate().for_each(|(i, axis)| { + magnitude = 0.0; + axis.iter().for_each(|val| { + magnitude += val * val; + }); + accelerometer_data[i] = magnitude.sqrt(); + }); + + let clean_accelerometer_data = match self.handle_outliers(accelerometer_data) { + Some(data) => data, + None => return None, + }; + + if self.check_reliable() == SensorChecks::Unacceptable { + return None; + } + + Some(clean_accelerometer_data) + } + + fn check_reliable(&mut self) -> SensorChecks { + self.num_outliers_per_accelerometer_ .iter() .enumerate() - .for_each(|(i, axis)| { - magnitude = 0.0; - axis.iter().enumerate().map(|(j, val)| { - magnitude += val * val; - }); - accelerometer_data[i] = magnitude.sqrt(); + .for_each(|(i, val)| { + if self.reliable_accelerometers_[i] && val >= &K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS + { + self.reliable_accelerometers_[i] = false; + self.num_reliable_accelerometers_ -= 1; + } }); - let clean_accelerometer_data: AccelerometerData = self.handle_outliers(accelerometer_data); + if self.num_reliable_accelerometers_ < K_NUM_ACCELEROMETERS as i32 - 1 { + return SensorChecks::Unacceptable; + } - clean_accelerometer_data + SensorChecks::Acceptable } - } mod tests { use super::*; + use heapless::Vec; #[test] fn test_accelerometer_preprocessor() { - let preprocessor = AccelerometerPreprocessor { + let mut preprocessor = AccelerometerPreprocessor { num_reliable_accelerometers_: 0, - reliable_accelerometers_: Vec::new(), + reliable_accelerometers_: [true; K_NUM_ACCELEROMETERS as usize], + num_outliers_per_accelerometer_: [0; K_NUM_ACCELEROMETERS as usize], }; - let data = core::types::AccelerometerData { - 0: [1.0, 2.0, 3.0], - 1: [4.0, 5.0, 6.0], - 2: [7.0, 8.0, 9.0], - }; - - let processed_data = preprocessor.processData(data); - - assert_eq!(processed_data[0], [2.0, 4.0, 6.0]); - assert_eq!(processed_data[1], [8.0, 10.0, 12.0]); - assert_eq!(processed_data[2], [14.0, 16.0, 18.0]); + assert_eq!(preprocessor.num_reliable_accelerometers_, 0); + assert_eq!( + preprocessor.reliable_accelerometers_, + [true; K_NUM_ACCELEROMETERS as usize] + ); + assert_eq!( + preprocessor.num_outliers_per_accelerometer_, + [0; K_NUM_ACCELEROMETERS as usize] + ); + + let raw_data: RawAccelerometerData = + RawAccelerometerData::from_slice(&[ + Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), + Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), + Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), + ]) + .unwrap(); + + let processed_data = preprocessor.process_data(raw_data); + assert_eq!(processed_data.is_some(), true); } } From d5be831e64ce1331e60ab037647f386239b7ac72 Mon Sep 17 00:00:00 2001 From: sean-leishman Date: Wed, 23 Oct 2024 23:09:06 +0100 Subject: [PATCH 04/99] Clone getQuartiles implementation --- .../src/preprocessing/accelerometer.rs | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index b0523ae1..40f7565d 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,3 +1,4 @@ +use heapless::Vec; use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; use hyped_core::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; @@ -13,15 +14,6 @@ struct Quartiles { q3: f32, } -fn get_quartiles(data: &AccelerometerData) -> Quartiles { - let mut sorted_data = data.clone(); - sorted_data.sort_by(|a, b| a.partial_cmp(b).unwrap()); - let q1 = sorted_data[K_NUM_ACCELEROMETERS / 4]; - let q2 = sorted_data[K_NUM_ACCELEROMETERS / 2]; - let q3 = sorted_data[K_NUM_ACCELEROMETERS * 3 / 4]; - return Quartiles { q1, q2, q3 }; -} - impl AccelerometerPreprocessor { fn handle_outliers( &mut self, @@ -65,7 +57,7 @@ impl AccelerometerPreprocessor { ) -> Option { let quartiles: Quartiles; if self.num_reliable_accelerometers_ == K_NUM_ACCELEROMETERS as i32 { - quartiles = get_quartiles(data); + quartiles = self.get_quartiles(data); } else if self.num_reliable_accelerometers_ == (K_NUM_ACCELEROMETERS as i32 - 1) { const SIZE: usize = K_NUM_ACCELEROMETERS - 1; let filtered_data: AccelerometerData = (0..SIZE) @@ -76,7 +68,7 @@ impl AccelerometerPreprocessor { }) .collect(); - quartiles = get_quartiles(&filtered_data); + quartiles = self.get_quartiles(&filtered_data); } else { return None; } @@ -131,11 +123,35 @@ impl AccelerometerPreprocessor { SensorChecks::Acceptable } + + fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { + let mut sorted_data = data.clone(); + sorted_data.sort_by(|a, b| a.partial_cmp(b).unwrap()); + + let quartile_keys: Vec = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap(); + let quartiles: Vec = quartile_keys + .iter() + .map(|quartile| { + let index_quartile: f32 = self.num_reliable_accelerometers_ as f32 * quartile; + let index_quartile_floor = index_quartile.floor() as usize; + let index_quartile_ceil = index_quartile.ceil() as usize; + + (data.get(index_quartile_floor).unwrap_or_else(|| &0.0) + + data.get(index_quartile_ceil).unwrap_or_else(|| &0.0)) + / 2.0 + }) + .collect(); + + Quartiles { + q1: quartiles[0], + q2: quartiles[1], + q3: quartiles[2], + } + } } mod tests { use super::*; - use heapless::Vec; #[test] fn test_accelerometer_preprocessor() { From e53f612bb320a103fe01fe2c2c650160a1bc8eff Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Thu, 24 Oct 2024 16:53:02 +0100 Subject: [PATCH 05/99] Ensure all reliable accelerometers are kept in range --- .../src/preprocessing/accelerometer.rs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 40f7565d..e461aea2 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -60,14 +60,14 @@ impl AccelerometerPreprocessor { quartiles = self.get_quartiles(data); } else if self.num_reliable_accelerometers_ == (K_NUM_ACCELEROMETERS as i32 - 1) { const SIZE: usize = K_NUM_ACCELEROMETERS - 1; - let filtered_data: AccelerometerData = (0..SIZE) - .into_iter() - .map(|i| match self.reliable_accelerometers_[i] { - true => data[i], - false => 0.0, - }) - .collect(); - + let mut filtered_data: AccelerometerData = Vec::new(); + let mut filtered_data_idx = 0; + data.iter().enumerate().for_each(|(i, val)| { + if self.reliable_accelerometers_[i] { + filtered_data[filtered_data_idx] = val.clone(); + filtered_data_idx += 1; + } + }); quartiles = self.get_quartiles(&filtered_data); } else { return None; From a09ae8f5b257b6ece1660237cd94c78b7d8d9ea2 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Mon, 28 Oct 2024 19:48:13 +0000 Subject: [PATCH 06/99] Add additional tests for quartile calculation --- lib/core/src/types.rs | 4 +- lib/localisation/src/lib.rs | 4 +- lib/localisation/src/preprocessing.rs | 4 + .../src/preprocessing/accelerometer.rs | 153 ++++++++++++++---- 4 files changed, 130 insertions(+), 35 deletions(-) create mode 100644 lib/localisation/src/preprocessing.rs diff --git a/lib/core/src/types.rs b/lib/core/src/types.rs index 849a2184..c989e452 100644 --- a/lib/core/src/types.rs +++ b/lib/core/src/types.rs @@ -24,7 +24,7 @@ pub struct RawAccelerationData { is_sensor_active: bool, } -pub const K_NUM_ACCELEROMETERS: usize = 3; +pub const K_NUM_ACCELEROMETERS: usize = 4; pub const K_NUM_AXIS: usize = 3; pub const K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS: i32 = 2; @@ -34,7 +34,7 @@ pub enum SensorChecks { Acceptable, } -pub type RawAccelerometerData = +pub type RawAccelerometerData = Vec, NUM_ACC>; pub type AccelerometerData = Vec; diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index 62d662c9..c62f8ed5 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1,5 +1,5 @@ -#![no_std] +// #![no_std] -mod preprocessing { +pub mod preprocessing { pub mod accelerometer; } diff --git a/lib/localisation/src/preprocessing.rs b/lib/localisation/src/preprocessing.rs new file mode 100644 index 00000000..4e853c3d --- /dev/null +++ b/lib/localisation/src/preprocessing.rs @@ -0,0 +1,4 @@ +pub mod accelerometer; +pub mod keyence; +pub mod optical; + diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index e461aea2..28e93bbf 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,6 +2,9 @@ use heapless::Vec; use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; use hyped_core::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; +use std::io::{stdout, Write}; + +#[derive(Debug)] pub struct AccelerometerPreprocessor { num_reliable_accelerometers_: i32, reliable_accelerometers_: [bool; K_NUM_ACCELEROMETERS as usize], @@ -12,6 +15,28 @@ struct Quartiles { q1: f32, q2: f32, q3: f32, + + iqr: f32, + lower_bound: f32, + upper_bound: f32, +} + +impl Quartiles { + fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { + let mut bound_factor = 1.5; + if is_unreliable { + bound_factor = 1.2; + } + + Self { + q1, + q2, + q3, + iqr: q3 - q1, + lower_bound: q1 - bound_factor * (q3 - q1), + upper_bound: q3 + bound_factor * (q3 - q1), + } + } } impl AccelerometerPreprocessor { @@ -24,21 +49,13 @@ impl AccelerometerPreprocessor { None => return None, }; - let iqr = quartiles.q3 - quartiles.q1; - let mut lower_bound = quartiles.q1 - 1.5 * iqr; - let mut upper_bound = quartiles.q3 + 1.5 * iqr; - if self.num_reliable_accelerometers_ < K_NUM_ACCELEROMETERS as i32 { - lower_bound = quartiles.q1 - 1.2 * iqr; - upper_bound = quartiles.q3 + 1.2 * iqr; - } - let accelerometer_data = data .iter() .enumerate() .map(|(i, val)| { if !self.reliable_accelerometers_[i] { return quartiles.q2; - } else if val < &lower_bound || val > &upper_bound { + } else if val < &quartiles.lower_bound || val > &quartiles.upper_bound { self.num_outliers_per_accelerometer_[i] += 1; return quartiles.q2; } else { @@ -60,7 +77,8 @@ impl AccelerometerPreprocessor { quartiles = self.get_quartiles(data); } else if self.num_reliable_accelerometers_ == (K_NUM_ACCELEROMETERS as i32 - 1) { const SIZE: usize = K_NUM_ACCELEROMETERS - 1; - let mut filtered_data: AccelerometerData = Vec::new(); + let mut filtered_data: AccelerometerData = + AccelerometerData::from_slice(&[0.0; SIZE]).unwrap(); let mut filtered_data_idx = 0; data.iter().enumerate().for_each(|(i, val)| { if self.reliable_accelerometers_[i] { @@ -78,11 +96,10 @@ impl AccelerometerPreprocessor { fn process_data( &mut self, - data: RawAccelerometerData, + data: RawAccelerometerData, ) -> Option> { - // do some processing let mut accelerometer_data: AccelerometerData = - AccelerometerData::new(); + AccelerometerData::from_slice(&[0.0; K_NUM_ACCELEROMETERS as usize]).unwrap(); let mut magnitude: f32 = 0.0; data.iter().enumerate().for_each(|(i, axis)| { @@ -132,9 +149,10 @@ impl AccelerometerPreprocessor { let quartiles: Vec = quartile_keys .iter() .map(|quartile| { - let index_quartile: f32 = self.num_reliable_accelerometers_ as f32 * quartile; - let index_quartile_floor = index_quartile.floor() as usize; - let index_quartile_ceil = index_quartile.ceil() as usize; + let index_quartile: f32 = + (1.0 + self.num_reliable_accelerometers_ as f32) * quartile; + let index_quartile_floor = index_quartile.floor() as usize - 1; + let index_quartile_ceil = index_quartile.ceil() as usize - 1; (data.get(index_quartile_floor).unwrap_or_else(|| &0.0) + data.get(index_quartile_ceil).unwrap_or_else(|| &0.0)) @@ -142,26 +160,31 @@ impl AccelerometerPreprocessor { }) .collect(); - Quartiles { - q1: quartiles[0], - q2: quartiles[1], - q3: quartiles[2], - } + Quartiles::new( + quartiles[0], + quartiles[1], + quartiles[2], + self.num_reliable_accelerometers_ < K_NUM_ACCELEROMETERS as i32, + ) } } mod tests { use super::*; - #[test] - fn test_accelerometer_preprocessor() { - let mut preprocessor = AccelerometerPreprocessor { - num_reliable_accelerometers_: 0, + fn default_preprocessor() -> AccelerometerPreprocessor { + AccelerometerPreprocessor { + num_reliable_accelerometers_: K_NUM_ACCELEROMETERS as i32, reliable_accelerometers_: [true; K_NUM_ACCELEROMETERS as usize], num_outliers_per_accelerometer_: [0; K_NUM_ACCELEROMETERS as usize], - }; + } + } + + #[test] + fn test_returns_okay() { + let mut preprocessor = default_preprocessor(); - assert_eq!(preprocessor.num_reliable_accelerometers_, 0); + assert_eq!(preprocessor.num_reliable_accelerometers_, 4); assert_eq!( preprocessor.reliable_accelerometers_, [true; K_NUM_ACCELEROMETERS as usize] @@ -171,15 +194,83 @@ mod tests { [0; K_NUM_ACCELEROMETERS as usize] ); - let raw_data: RawAccelerometerData = + let raw_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ - Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), - Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), - Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), + Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) = 3.74 + Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // sqrt(77) = 8.77 + Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), // sqrt(194) = 13.93 + Vec::from_slice(&[10.0, 11.0, 12.0]).unwrap(), // sqrt(365) = 19.1 ]) .unwrap(); let processed_data = preprocessor.process_data(raw_data); assert_eq!(processed_data.is_some(), true); } + + #[test] + fn test_get_quartiles() { + let mut preprocessor = default_preprocessor(); + + let data: AccelerometerData = + AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); + let processed_data = preprocessor.get_quartiles(&data); + + assert_eq!(processed_data.q1, 1.5); + assert_eq!(processed_data.q2, 2.5); + assert_eq!(processed_data.q3, 3.5); + } + + #[test] + fn test_calculate_quartiles_max_reliable() { + let mut preprocessor = default_preprocessor(); + + let data: AccelerometerData = + AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); + let processed_data = preprocessor.calculate_quartiles(&data); + + assert_eq!(processed_data.is_some(), true); + + let processed_data = processed_data.unwrap(); + assert_eq!(processed_data.q1, 1.5); + assert_eq!(processed_data.q2, 2.5); + assert_eq!(processed_data.q3, 3.5); + assert_eq!(processed_data.iqr, 2.0); + assert_eq!(processed_data.lower_bound, -1.5); + assert_eq!(processed_data.upper_bound, 6.5); + } + + #[test] + fn test_calculate_quartiles_one_unreliable() { + let mut preprocessor = default_preprocessor(); + preprocessor.reliable_accelerometers_ = [true, false, true, true]; + preprocessor.num_reliable_accelerometers_ = 3; + + let data: AccelerometerData = + AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); + let processed_data = preprocessor.calculate_quartiles(&data); + + assert_eq!(processed_data.is_some(), true); + + let processed_data = processed_data.unwrap(); + assert_eq!(processed_data.q1, 1.0); + assert_eq!(processed_data.q2, 3.0); + assert_eq!(processed_data.q3, 4.0); + } + + #[test] + fn test_handle_outliers() { + let mut preprocessor = default_preprocessor(); + + let data: AccelerometerData = + AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); + let processed_data = preprocessor.handle_outliers(data); + + assert_eq!(processed_data.is_some(), true); + + let processed_data: AccelerometerData = processed_data.unwrap(); + assert_eq!(processed_data[0], 1.0); + assert_eq!(processed_data[1], 2.0); + assert_eq!(processed_data[2], 3.0); + assert_eq!(processed_data[3], 10.0); + } } From 9fee8f883f0c706d042374c64f4b4056592e2663 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Thu, 31 Oct 2024 19:24:22 +0000 Subject: [PATCH 07/99] Add handle_outlier tests --- lib/localisation/src/lib.rs | 2 +- .../src/preprocessing/accelerometer.rs | 32 ++++++++++++++----- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index c62f8ed5..51dc6f48 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1,4 +1,4 @@ -// #![no_std] +#![no_std] pub mod preprocessing { pub mod accelerometer; diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 28e93bbf..4fb952ca 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,8 +2,6 @@ use heapless::Vec; use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; use hyped_core::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; -use std::io::{stdout, Write}; - #[derive(Debug)] pub struct AccelerometerPreprocessor { num_reliable_accelerometers_: i32, @@ -40,10 +38,10 @@ impl Quartiles { } impl AccelerometerPreprocessor { - fn handle_outliers( + fn handle_outliers( &mut self, data: AccelerometerData, - ) -> Option> { + ) -> Option> { let quartiles = match self.calculate_quartiles(&data) { Some(quartiles) => quartiles, None => return None, @@ -235,8 +233,8 @@ mod tests { assert_eq!(processed_data.q2, 2.5); assert_eq!(processed_data.q3, 3.5); assert_eq!(processed_data.iqr, 2.0); - assert_eq!(processed_data.lower_bound, -1.5); - assert_eq!(processed_data.upper_bound, 6.5); + assert_eq!((processed_data.lower_bound * 100.0).round(), -150.0); + assert_eq!((processed_data.upper_bound * 100.0).round(), 650.0); } #[test] @@ -255,11 +253,16 @@ mod tests { assert_eq!(processed_data.q1, 1.0); assert_eq!(processed_data.q2, 3.0); assert_eq!(processed_data.q3, 4.0); + assert_eq!(processed_data.iqr, 3.0); + assert_eq!((processed_data.lower_bound * 100.0).round(), -260.0); + assert_eq!((processed_data.upper_bound * 100.0).round(), 760.0); } #[test] - fn test_handle_outliers() { + fn test_handle_outlier_replace_median() { let mut preprocessor = default_preprocessor(); + preprocessor.reliable_accelerometers_ = [true, false, true, true]; + preprocessor.num_reliable_accelerometers_ = 3; let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); @@ -269,8 +272,21 @@ mod tests { let processed_data: AccelerometerData = processed_data.unwrap(); assert_eq!(processed_data[0], 1.0); - assert_eq!(processed_data[1], 2.0); + assert_eq!(processed_data[1], 3.0); // replace unreliable with median assert_eq!(processed_data[2], 3.0); assert_eq!(processed_data[3], 10.0); } + + #[test] + fn test_handle_outliers_no_quartiles() { + let mut preprocessor = default_preprocessor(); + preprocessor.reliable_accelerometers_ = [true, false, false, true]; + preprocessor.num_reliable_accelerometers_ = 2; + + let data: AccelerometerData = + AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); + let processed_data = preprocessor.handle_outliers(data); + + assert_eq!(processed_data.is_some(), false); + } } From 36b29766c8d70515c63f8ece21f0b6995ce51a4b Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Fri, 1 Nov 2024 12:56:53 +0000 Subject: [PATCH 08/99] Use localisation/src/types.rs for constants --- lib/core/src/types.rs | 12 +--- lib/localisation/src/lib.rs | 2 + lib/localisation/src/preprocessing.rs | 1 - .../src/preprocessing/accelerometer.rs | 59 ++++++++++--------- lib/localisation/src/types.rs | 15 +++++ 5 files changed, 49 insertions(+), 40 deletions(-) create mode 100644 lib/localisation/src/types.rs diff --git a/lib/core/src/types.rs b/lib/core/src/types.rs index c989e452..444ede37 100644 --- a/lib/core/src/types.rs +++ b/lib/core/src/types.rs @@ -16,16 +16,8 @@ impl DigitalSignal { } } -pub struct RawAccelerationData { - x: i32, - y: i32, - z: i32, - timestamp: i32, - is_sensor_active: bool, -} - -pub const K_NUM_ACCELEROMETERS: usize = 4; -pub const K_NUM_AXIS: usize = 3; +pub const K_NUM_ACCELEROMETERS: i32 = 4; +pub const K_NUM_AXIS: i32 = 3; pub const K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS: i32 = 2; #[derive(PartialEq)] diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index 51dc6f48..64f01127 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -3,3 +3,5 @@ pub mod preprocessing { pub mod accelerometer; } + +pub mod types; diff --git a/lib/localisation/src/preprocessing.rs b/lib/localisation/src/preprocessing.rs index 4e853c3d..6f4132b3 100644 --- a/lib/localisation/src/preprocessing.rs +++ b/lib/localisation/src/preprocessing.rs @@ -1,4 +1,3 @@ pub mod accelerometer; pub mod keyence; pub mod optical; - diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 4fb952ca..0ea01202 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,12 +1,12 @@ +use super::super::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; use heapless::Vec; use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; -use hyped_core::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; #[derive(Debug)] pub struct AccelerometerPreprocessor { - num_reliable_accelerometers_: i32, - reliable_accelerometers_: [bool; K_NUM_ACCELEROMETERS as usize], - num_outliers_per_accelerometer_: [i32; K_NUM_ACCELEROMETERS as usize], + num_reliable_accelerometers: i32, + reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS as usize], + num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS as usize], } struct Quartiles { @@ -51,13 +51,13 @@ impl AccelerometerPreprocessor { .iter() .enumerate() .map(|(i, val)| { - if !self.reliable_accelerometers_[i] { + if !self.reliable_accelerometers[i] { return quartiles.q2; } else if val < &quartiles.lower_bound || val > &quartiles.upper_bound { - self.num_outliers_per_accelerometer_[i] += 1; + self.num_outliers_per_accelerometer[i] += 1; return quartiles.q2; } else { - self.num_outliers_per_accelerometer_[i] = 0; + self.num_outliers_per_accelerometer[i] = 0; return val.clone(); } }) @@ -71,15 +71,15 @@ impl AccelerometerPreprocessor { data: &AccelerometerData, ) -> Option { let quartiles: Quartiles; - if self.num_reliable_accelerometers_ == K_NUM_ACCELEROMETERS as i32 { + if self.num_reliable_accelerometers == K_NUM_ACCELEROMETERS as i32 { quartiles = self.get_quartiles(data); - } else if self.num_reliable_accelerometers_ == (K_NUM_ACCELEROMETERS as i32 - 1) { + } else if self.num_reliable_accelerometers == (K_NUM_ACCELEROMETERS as i32 - 1) { const SIZE: usize = K_NUM_ACCELEROMETERS - 1; let mut filtered_data: AccelerometerData = AccelerometerData::from_slice(&[0.0; SIZE]).unwrap(); let mut filtered_data_idx = 0; data.iter().enumerate().for_each(|(i, val)| { - if self.reliable_accelerometers_[i] { + if self.reliable_accelerometers[i] { filtered_data[filtered_data_idx] = val.clone(); filtered_data_idx += 1; } @@ -121,18 +121,19 @@ impl AccelerometerPreprocessor { } fn check_reliable(&mut self) -> SensorChecks { - self.num_outliers_per_accelerometer_ + self.num_outliers_per_accelerometer .iter() .enumerate() .for_each(|(i, val)| { - if self.reliable_accelerometers_[i] && val >= &K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS + if self.reliable_accelerometers[i] + && val >= &(K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS as i32) { - self.reliable_accelerometers_[i] = false; - self.num_reliable_accelerometers_ -= 1; + self.reliable_accelerometers[i] = false; + self.num_reliable_accelerometers -= 1; } }); - if self.num_reliable_accelerometers_ < K_NUM_ACCELEROMETERS as i32 - 1 { + if self.num_reliable_accelerometers < K_NUM_ACCELEROMETERS as i32 - 1 { return SensorChecks::Unacceptable; } @@ -148,7 +149,7 @@ impl AccelerometerPreprocessor { .iter() .map(|quartile| { let index_quartile: f32 = - (1.0 + self.num_reliable_accelerometers_ as f32) * quartile; + (1.0 + self.num_reliable_accelerometers as f32) * quartile; let index_quartile_floor = index_quartile.floor() as usize - 1; let index_quartile_ceil = index_quartile.ceil() as usize - 1; @@ -162,7 +163,7 @@ impl AccelerometerPreprocessor { quartiles[0], quartiles[1], quartiles[2], - self.num_reliable_accelerometers_ < K_NUM_ACCELEROMETERS as i32, + self.num_reliable_accelerometers < K_NUM_ACCELEROMETERS as i32, ) } } @@ -172,9 +173,9 @@ mod tests { fn default_preprocessor() -> AccelerometerPreprocessor { AccelerometerPreprocessor { - num_reliable_accelerometers_: K_NUM_ACCELEROMETERS as i32, - reliable_accelerometers_: [true; K_NUM_ACCELEROMETERS as usize], - num_outliers_per_accelerometer_: [0; K_NUM_ACCELEROMETERS as usize], + num_reliable_accelerometers: K_NUM_ACCELEROMETERS as i32, + reliable_accelerometers: [true; K_NUM_ACCELEROMETERS as usize], + num_outliers_per_accelerometer: [0; K_NUM_ACCELEROMETERS as usize], } } @@ -182,13 +183,13 @@ mod tests { fn test_returns_okay() { let mut preprocessor = default_preprocessor(); - assert_eq!(preprocessor.num_reliable_accelerometers_, 4); + assert_eq!(preprocessor.num_reliable_accelerometers, 4); assert_eq!( - preprocessor.reliable_accelerometers_, + preprocessor.reliable_accelerometers, [true; K_NUM_ACCELEROMETERS as usize] ); assert_eq!( - preprocessor.num_outliers_per_accelerometer_, + preprocessor.num_outliers_per_accelerometer, [0; K_NUM_ACCELEROMETERS as usize] ); @@ -240,8 +241,8 @@ mod tests { #[test] fn test_calculate_quartiles_one_unreliable() { let mut preprocessor = default_preprocessor(); - preprocessor.reliable_accelerometers_ = [true, false, true, true]; - preprocessor.num_reliable_accelerometers_ = 3; + preprocessor.reliable_accelerometers = [true, false, true, true]; + preprocessor.num_reliable_accelerometers = 3; let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); @@ -261,8 +262,8 @@ mod tests { #[test] fn test_handle_outlier_replace_median() { let mut preprocessor = default_preprocessor(); - preprocessor.reliable_accelerometers_ = [true, false, true, true]; - preprocessor.num_reliable_accelerometers_ = 3; + preprocessor.reliable_accelerometers = [true, false, true, true]; + preprocessor.num_reliable_accelerometers = 3; let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); @@ -280,8 +281,8 @@ mod tests { #[test] fn test_handle_outliers_no_quartiles() { let mut preprocessor = default_preprocessor(); - preprocessor.reliable_accelerometers_ = [true, false, false, true]; - preprocessor.num_reliable_accelerometers_ = 2; + preprocessor.reliable_accelerometers = [true, false, false, true]; + preprocessor.num_reliable_accelerometers = 2; let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); diff --git a/lib/localisation/src/types.rs b/lib/localisation/src/types.rs new file mode 100644 index 00000000..77042ef8 --- /dev/null +++ b/lib/localisation/src/types.rs @@ -0,0 +1,15 @@ +use heapless::Vec; + +pub const K_NUM_ACCELEROMETERS: usize = 4; +pub const K_NUM_AXIS: usize = 3; +pub const K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS: usize = 2; + +#[derive(PartialEq)] +pub enum SensorChecks { + Unacceptable, + Acceptable, +} + +pub type RawAccelerometerData = + Vec, NUM_ACC>; +pub type AccelerometerData = Vec; From 62a36b93fbd92bf829459623dd169fde17984567 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Fri, 1 Nov 2024 13:03:34 +0000 Subject: [PATCH 09/99] Run clippy --- .../src/preprocessing/accelerometer.rs | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 0ea01202..6f323094 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,11 +2,10 @@ use super::super::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTL use heapless::Vec; use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; -#[derive(Debug)] pub struct AccelerometerPreprocessor { num_reliable_accelerometers: i32, - reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS as usize], - num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS as usize], + reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS], + num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS], } struct Quartiles { @@ -50,15 +49,15 @@ impl AccelerometerPreprocessor { let accelerometer_data = data .iter() .enumerate() - .map(|(i, val)| { + .map(|(i, &val)| { if !self.reliable_accelerometers[i] { - return quartiles.q2; - } else if val < &quartiles.lower_bound || val > &quartiles.upper_bound { + quartiles.q2 + } else if val < quartiles.lower_bound || val > quartiles.upper_bound { self.num_outliers_per_accelerometer[i] += 1; - return quartiles.q2; + quartiles.q2 } else { self.num_outliers_per_accelerometer[i] = 0; - return val.clone(); + val } }) .collect(); @@ -80,7 +79,7 @@ impl AccelerometerPreprocessor { let mut filtered_data_idx = 0; data.iter().enumerate().for_each(|(i, val)| { if self.reliable_accelerometers[i] { - filtered_data[filtered_data_idx] = val.clone(); + filtered_data[filtered_data_idx] = *val; filtered_data_idx += 1; } }); @@ -97,7 +96,7 @@ impl AccelerometerPreprocessor { data: RawAccelerometerData, ) -> Option> { let mut accelerometer_data: AccelerometerData = - AccelerometerData::from_slice(&[0.0; K_NUM_ACCELEROMETERS as usize]).unwrap(); + AccelerometerData::from_slice(&[0.0; K_NUM_ACCELEROMETERS]).unwrap(); let mut magnitude: f32 = 0.0; data.iter().enumerate().for_each(|(i, axis)| { @@ -153,8 +152,8 @@ impl AccelerometerPreprocessor { let index_quartile_floor = index_quartile.floor() as usize - 1; let index_quartile_ceil = index_quartile.ceil() as usize - 1; - (data.get(index_quartile_floor).unwrap_or_else(|| &0.0) - + data.get(index_quartile_ceil).unwrap_or_else(|| &0.0)) + (data.get(index_quartile_floor).unwrap_or(&0.0) + + data.get(index_quartile_ceil).unwrap_or(&0.0)) / 2.0 }) .collect(); @@ -174,8 +173,8 @@ mod tests { fn default_preprocessor() -> AccelerometerPreprocessor { AccelerometerPreprocessor { num_reliable_accelerometers: K_NUM_ACCELEROMETERS as i32, - reliable_accelerometers: [true; K_NUM_ACCELEROMETERS as usize], - num_outliers_per_accelerometer: [0; K_NUM_ACCELEROMETERS as usize], + reliable_accelerometers: [true; K_NUM_ACCELEROMETERS], + num_outliers_per_accelerometer: [0; K_NUM_ACCELEROMETERS], } } @@ -186,11 +185,11 @@ mod tests { assert_eq!(preprocessor.num_reliable_accelerometers, 4); assert_eq!( preprocessor.reliable_accelerometers, - [true; K_NUM_ACCELEROMETERS as usize] + [true; K_NUM_ACCELEROMETERS] ); assert_eq!( preprocessor.num_outliers_per_accelerometer, - [0; K_NUM_ACCELEROMETERS as usize] + [0; K_NUM_ACCELEROMETERS] ); let raw_data: RawAccelerometerData = From d1177d0be7972336220094faffc6a8c7ea12a375 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Fri, 1 Nov 2024 13:25:26 +0000 Subject: [PATCH 10/99] Clean-up linter errors --- lib/localisation/src/lib.rs | 5 +- .../src/preprocessing/accelerometer.rs | 66 +++++++++++-------- lib/localisation/src/preprocessing/lib.rs | 1 + 3 files changed, 39 insertions(+), 33 deletions(-) create mode 100644 lib/localisation/src/preprocessing/lib.rs diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index 64f01127..af2013ce 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1,7 +1,4 @@ #![no_std] -pub mod preprocessing { - pub mod accelerometer; -} - +pub mod preprocessing; pub mod types; diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 6f323094..ace0db46 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,13 +2,8 @@ use super::super::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTL use heapless::Vec; use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; -pub struct AccelerometerPreprocessor { - num_reliable_accelerometers: i32, - reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS], - num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS], -} - -struct Quartiles { +#[allow(dead_code)] +pub struct Quartiles { q1: f32, q2: f32, q3: f32, @@ -19,7 +14,7 @@ struct Quartiles { } impl Quartiles { - fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { + pub fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { let mut bound_factor = 1.5; if is_unreliable { bound_factor = 1.2; @@ -36,8 +31,28 @@ impl Quartiles { } } +pub struct AccelerometerPreprocessor { + num_reliable_accelerometers: i32, + reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS], + num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS], +} + +impl Default for AccelerometerPreprocessor { + fn default() -> Self { + Self::new() + } +} + impl AccelerometerPreprocessor { - fn handle_outliers( + pub fn new() -> Self { + Self { + num_reliable_accelerometers: K_NUM_ACCELEROMETERS as i32, + reliable_accelerometers: [true; K_NUM_ACCELEROMETERS], + num_outliers_per_accelerometer: [0; K_NUM_ACCELEROMETERS], + } + } + + pub fn handle_outliers( &mut self, data: AccelerometerData, ) -> Option> { @@ -65,7 +80,7 @@ impl AccelerometerPreprocessor { Some(accelerometer_data) } - fn calculate_quartiles( + pub fn calculate_quartiles( &self, data: &AccelerometerData, ) -> Option { @@ -91,7 +106,7 @@ impl AccelerometerPreprocessor { Some(quartiles) } - fn process_data( + pub fn process_data( &mut self, data: RawAccelerometerData, ) -> Option> { @@ -119,7 +134,7 @@ impl AccelerometerPreprocessor { Some(clean_accelerometer_data) } - fn check_reliable(&mut self) -> SensorChecks { + pub fn check_reliable(&mut self) -> SensorChecks { self.num_outliers_per_accelerometer .iter() .enumerate() @@ -139,7 +154,7 @@ impl AccelerometerPreprocessor { SensorChecks::Acceptable } - fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { + pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { let mut sorted_data = data.clone(); sorted_data.sort_by(|a, b| a.partial_cmp(b).unwrap()); @@ -167,20 +182,13 @@ impl AccelerometerPreprocessor { } } +#[cfg(test)] mod tests { use super::*; - fn default_preprocessor() -> AccelerometerPreprocessor { - AccelerometerPreprocessor { - num_reliable_accelerometers: K_NUM_ACCELEROMETERS as i32, - reliable_accelerometers: [true; K_NUM_ACCELEROMETERS], - num_outliers_per_accelerometer: [0; K_NUM_ACCELEROMETERS], - } - } - #[test] - fn test_returns_okay() { - let mut preprocessor = default_preprocessor(); + pub fn test_returns_okay() { + let mut preprocessor = AccelerometerPreprocessor::new(); assert_eq!(preprocessor.num_reliable_accelerometers, 4); assert_eq!( @@ -206,8 +214,8 @@ mod tests { } #[test] - fn test_get_quartiles() { - let mut preprocessor = default_preprocessor(); + pub fn test_get_quartiles() { + let preprocessor = AccelerometerPreprocessor::new(); let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); @@ -220,7 +228,7 @@ mod tests { #[test] fn test_calculate_quartiles_max_reliable() { - let mut preprocessor = default_preprocessor(); + let preprocessor = AccelerometerPreprocessor::new(); let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); @@ -239,7 +247,7 @@ mod tests { #[test] fn test_calculate_quartiles_one_unreliable() { - let mut preprocessor = default_preprocessor(); + let mut preprocessor = AccelerometerPreprocessor::new(); preprocessor.reliable_accelerometers = [true, false, true, true]; preprocessor.num_reliable_accelerometers = 3; @@ -260,7 +268,7 @@ mod tests { #[test] fn test_handle_outlier_replace_median() { - let mut preprocessor = default_preprocessor(); + let mut preprocessor = AccelerometerPreprocessor::new(); preprocessor.reliable_accelerometers = [true, false, true, true]; preprocessor.num_reliable_accelerometers = 3; @@ -279,7 +287,7 @@ mod tests { #[test] fn test_handle_outliers_no_quartiles() { - let mut preprocessor = default_preprocessor(); + let mut preprocessor = AccelerometerPreprocessor::new(); preprocessor.reliable_accelerometers = [true, false, false, true]; preprocessor.num_reliable_accelerometers = 2; diff --git a/lib/localisation/src/preprocessing/lib.rs b/lib/localisation/src/preprocessing/lib.rs new file mode 100644 index 00000000..a7d02f83 --- /dev/null +++ b/lib/localisation/src/preprocessing/lib.rs @@ -0,0 +1 @@ +pub mod accelerometer; From b7539535e639e21320a4f66480e1cc31ecbe1f59 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Fri, 1 Nov 2024 13:33:57 +0000 Subject: [PATCH 11/99] Fix testing linter warnings --- lib/localisation/src/preprocessing/accelerometer.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index ace0db46..2c35636a 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -210,7 +210,7 @@ mod tests { .unwrap(); let processed_data = preprocessor.process_data(raw_data); - assert_eq!(processed_data.is_some(), true); + assert!(processed_data.is_some()); } #[test] @@ -234,7 +234,7 @@ mod tests { AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); let processed_data = preprocessor.calculate_quartiles(&data); - assert_eq!(processed_data.is_some(), true); + assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); assert_eq!(processed_data.q1, 1.5); @@ -255,7 +255,7 @@ mod tests { AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); let processed_data = preprocessor.calculate_quartiles(&data); - assert_eq!(processed_data.is_some(), true); + assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); assert_eq!(processed_data.q1, 1.0); @@ -276,7 +276,7 @@ mod tests { AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); let processed_data = preprocessor.handle_outliers(data); - assert_eq!(processed_data.is_some(), true); + assert!(processed_data.is_some()); let processed_data: AccelerometerData = processed_data.unwrap(); assert_eq!(processed_data[0], 1.0); @@ -295,6 +295,6 @@ mod tests { AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); let processed_data = preprocessor.handle_outliers(data); - assert_eq!(processed_data.is_some(), false); + assert!(processed_data.is_none()); } } From aa4b41ca470b7e52035f52b17a9d01b287ea0af4 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Mon, 11 Nov 2024 11:06:46 +0000 Subject: [PATCH 12/99] QoL changes --- Cargo.lock | 66 ------------------- lib/localisation/Cargo.toml | 2 +- .../src/preprocessing/accelerometer.rs | 30 +++------ 3 files changed, 10 insertions(+), 88 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 13aa749e..0d0661f0 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -29,12 +29,6 @@ version = "1.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" -[[package]] -name = "bytemuck" -version = "1.19.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8334215b81e418a0a7bdb8ef0849474f40bb10c8b71f1c4ed315cff49f32494d" - [[package]] name = "byteorder" version = "1.5.0" @@ -363,16 +357,6 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0ca88d725a0a943b096803bd34e73a4437208b6077654cc4ecb2947a5f91618d" -[[package]] -name = "matrixmultiply" -version = "0.3.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9380b911e3e96d10c1f415da0876389aaf1b56759054eeb0de7df940c456ba1a" -dependencies = [ - "autocfg", - "rawpointer", -] - [[package]] name = "memchr" version = "2.7.4" @@ -386,8 +370,6 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3c4b5f057b303842cf3262c27e465f4c303572e7f6b0648f60e16248ac3397f4" dependencies = [ "approx", - "matrixmultiply", - "nalgebra-macros", "num-complex", "num-rational", "num-traits", @@ -395,17 +377,6 @@ dependencies = [ "typenum", ] -[[package]] -name = "nalgebra-macros" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.79", -] - [[package]] name = "nb" version = "0.1.3" @@ -427,16 +398,6 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "43794a0ace135be66a25d3ae77d41b91615fb68ae937f904090203e81f755b65" -[[package]] -name = "num-bigint" -version = "0.4.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9" -dependencies = [ - "num-integer", - "num-traits", -] - [[package]] name = "num-complex" version = "0.4.6" @@ -461,7 +422,6 @@ version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" dependencies = [ - "num-bigint", "num-integer", "num-traits", ] @@ -541,12 +501,6 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" -[[package]] -name = "rawpointer" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" - [[package]] name = "rust-mqtt" version = "0.3.0" @@ -566,15 +520,6 @@ version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f3cb5ba0dc43242ce17de99c180e96db90b235b8a9fdc9543c96d2209116bd9f" -[[package]] -name = "safe_arch" -version = "0.7.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c3460605018fdc9612bce72735cba0d27efbcd9904780d44c7e3a9948f96148a" -dependencies = [ - "bytemuck", -] - [[package]] name = "serde" version = "1.0.210" @@ -620,7 +565,6 @@ dependencies = [ "num-complex", "num-traits", "paste", - "wide", ] [[package]] @@ -707,13 +651,3 @@ name = "void" version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" - -[[package]] -name = "wide" -version = "0.7.28" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b828f995bf1e9622031f8009f8481a85406ce1f4d4588ff746d872043e855690" -dependencies = [ - "bytemuck", - "safe_arch", -] diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index 91fcae50..d1ca4627 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -4,7 +4,7 @@ version = "0.1.0" edition = "2021" [dependencies] -nalgebra = "0.33.0" +nalgebra = { version = "0.33.0", default-features = false } heapless = "0.8.0" hyped_core = { path = "../core" } diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 2c35636a..b1682442 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -15,10 +15,7 @@ pub struct Quartiles { impl Quartiles { pub fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { - let mut bound_factor = 1.5; - if is_unreliable { - bound_factor = 1.2; - } + let bound_factor = if is_unreliable { 1.2 } else { 1.5 }; Self { q1, @@ -56,10 +53,7 @@ impl AccelerometerPreprocessor { &mut self, data: AccelerometerData, ) -> Option> { - let quartiles = match self.calculate_quartiles(&data) { - Some(quartiles) => quartiles, - None => return None, - }; + let quartiles = self.calculate_quartiles(&data)?; let accelerometer_data = data .iter() @@ -89,15 +83,12 @@ impl AccelerometerPreprocessor { quartiles = self.get_quartiles(data); } else if self.num_reliable_accelerometers == (K_NUM_ACCELEROMETERS as i32 - 1) { const SIZE: usize = K_NUM_ACCELEROMETERS - 1; - let mut filtered_data: AccelerometerData = - AccelerometerData::from_slice(&[0.0; SIZE]).unwrap(); - let mut filtered_data_idx = 0; - data.iter().enumerate().for_each(|(i, val)| { - if self.reliable_accelerometers[i] { - filtered_data[filtered_data_idx] = *val; - filtered_data_idx += 1; - } - }); + let filtered_data: AccelerometerData = data + .iter() + .enumerate() + .filter(|(i, _)| self.reliable_accelerometers[*i]) + .map(|(_, val)| *val) + .collect(); quartiles = self.get_quartiles(&filtered_data); } else { return None; @@ -122,10 +113,7 @@ impl AccelerometerPreprocessor { accelerometer_data[i] = magnitude.sqrt(); }); - let clean_accelerometer_data = match self.handle_outliers(accelerometer_data) { - Some(data) => data, - None => return None, - }; + let clean_accelerometer_data = self.handle_outliers(accelerometer_data)?; if self.check_reliable() == SensorChecks::Unacceptable { return None; From 58bcf9e38a49ef9c12fc4437c69a835a1af3a5ad Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Mon, 11 Nov 2024 11:08:49 +0000 Subject: [PATCH 13/99] Remove extra impl --- lib/localisation/src/preprocessing/accelerometer.rs | 6 ------ 1 file changed, 6 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index b1682442..464ad523 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -34,12 +34,6 @@ pub struct AccelerometerPreprocessor { num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS], } -impl Default for AccelerometerPreprocessor { - fn default() -> Self { - Self::new() - } -} - impl AccelerometerPreprocessor { pub fn new() -> Self { Self { From 3cf7fd316099c18066ec06c18ad54ed765924a40 Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Mon, 11 Nov 2024 11:12:35 +0000 Subject: [PATCH 14/99] Default impl for clippy --- lib/localisation/src/preprocessing/accelerometer.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 464ad523..0d62ec2d 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -28,6 +28,7 @@ impl Quartiles { } } +#[derive(Default)] pub struct AccelerometerPreprocessor { num_reliable_accelerometers: i32, reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS], From 6e6a502138095831d8e67a87c692ed9aeebfc7cb Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 11 Nov 2024 19:05:28 +0000 Subject: [PATCH 15/99] initial commit --- lib/localisation/src/control/navigator.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/localisation/src/control/navigator.rs b/lib/localisation/src/control/navigator.rs index e69de29b..c767de88 100644 --- a/lib/localisation/src/control/navigator.rs +++ b/lib/localisation/src/control/navigator.rs @@ -0,0 +1 @@ +//test \ No newline at end of file From 3a9585e72c4f4e27578f4bff2c24189da9bf2756 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 11 Nov 2024 19:13:46 +0000 Subject: [PATCH 16/99] getters & setters --- lib/localisation/src/control/navigator.rs | 49 ++++++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/lib/localisation/src/control/navigator.rs b/lib/localisation/src/control/navigator.rs index c767de88..7c9d618e 100644 --- a/lib/localisation/src/control/navigator.rs +++ b/lib/localisation/src/control/navigator.rs @@ -1 +1,48 @@ -//test \ No newline at end of file +use crate::filtering::kalman_filter::KalmanFilter; + + +pub struct Navigator { + displacement: f64, + velocity: f64, + acceleration: f64, + kalman_filter: KalmanFilter, +} + +impl Navigator { + pub fn new(kalman_filter: KalmanFilter) -> Navigator { + Navigator { + displacement: 0.0, + velocity: 0.0, + acceleration: 0.0, + kalman_filter, + } + } + + //Setters + + pub fn set_displacement(&mut self, displacement: f64) { + self.displacement = displacement; + } + + pub fn set_velocity(&mut self, velocity: f64) { + self.velocity = velocity; + } + + pub fn set_acceleration(&mut self, acceleration: f64) { + self.acceleration = acceleration; + } + + //Getters + + pub fn get_displacement(&self) -> f64 { + self.displacement + } + + pub fn get_velocity(&self) -> f64 { + self.velocity + } + + pub fn get_acceleration(&self) -> f64 { + self.acceleration + } +} From b352a149725658b3ed21790bd1ef1b089e491201 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 11 Nov 2024 19:18:40 +0000 Subject: [PATCH 17/99] new name! --- lib/localisation/src/control/{navigator.rs => localizer.rs} | 1 + 1 file changed, 1 insertion(+) rename lib/localisation/src/control/{navigator.rs => localizer.rs} (99%) diff --git a/lib/localisation/src/control/navigator.rs b/lib/localisation/src/control/localizer.rs similarity index 99% rename from lib/localisation/src/control/navigator.rs rename to lib/localisation/src/control/localizer.rs index 7c9d618e..d58508e8 100644 --- a/lib/localisation/src/control/navigator.rs +++ b/lib/localisation/src/control/localizer.rs @@ -1,6 +1,7 @@ use crate::filtering::kalman_filter::KalmanFilter; + pub struct Navigator { displacement: f64, velocity: f64, From 847c2d9ebe612e94f6e57e5bb62d88862c374d0b Mon Sep 17 00:00:00 2001 From: Sean Leishman Date: Thu, 14 Nov 2024 18:50:43 +0000 Subject: [PATCH 18/99] Add comments and tests --- .../src/preprocessing/accelerometer.rs | 99 +++++++++++++++---- 1 file changed, 80 insertions(+), 19 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 0d62ec2d..6d750035 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,7 +1,11 @@ +use crate::types::AccelerometerData; + use super::super::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; use heapless::Vec; -use hyped_core::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; +use hyped_core::types::{RawAccelerometerData, SensorChecks}; +/// Stores the quartiles of the data and the bounds for outliers +/// which are calculated from the quartiles #[allow(dead_code)] pub struct Quartiles { q1: f32, @@ -13,6 +17,7 @@ pub struct Quartiles { upper_bound: f32, } +/// Implementation of the Quartiles struct which calculates the bounds for outliers impl Quartiles { pub fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { let bound_factor = if is_unreliable { 1.2 } else { 1.5 }; @@ -28,14 +33,20 @@ impl Quartiles { } } +/// Responsible for processing accelerometer data and removing outliers #[derive(Default)] pub struct AccelerometerPreprocessor { + /// number of true values in reliable_accelerometers num_reliable_accelerometers: i32, + /// true if accelerometer at index is reliable reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS], + /// number of outliers detected for each accelerometer num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS], } impl AccelerometerPreprocessor { + /// Creates a new AccelerometerPreprocessor + /// By default, all accelerometers are deemed as reliable pub fn new() -> Self { Self { num_reliable_accelerometers: K_NUM_ACCELEROMETERS as i32, @@ -44,6 +55,10 @@ impl AccelerometerPreprocessor { } } + /// Removes points in data that are deemed as outliers + /// This is based on bounds calculated from the quartiles of the data + /// Any points from unreliable accelerometers or those that are out of bounds + /// are replaced with the median of the data pub fn handle_outliers( &mut self, data: AccelerometerData, @@ -69,13 +84,17 @@ impl AccelerometerPreprocessor { Some(accelerometer_data) } + /// Calculates the quartiles of the data + /// If all accelerometers are reliable, the quartiles are calculated normally + /// If one accelerometer is unreliable, the quartiles are calculated with the unreliable + /// accelerometer removed + /// If more than one accelerometer is unreliable, None is returned pub fn calculate_quartiles( &self, data: &AccelerometerData, ) -> Option { - let quartiles: Quartiles; if self.num_reliable_accelerometers == K_NUM_ACCELEROMETERS as i32 { - quartiles = self.get_quartiles(data); + Some(self.get_quartiles(data)) } else if self.num_reliable_accelerometers == (K_NUM_ACCELEROMETERS as i32 - 1) { const SIZE: usize = K_NUM_ACCELEROMETERS - 1; let filtered_data: AccelerometerData = data @@ -84,29 +103,25 @@ impl AccelerometerPreprocessor { .filter(|(i, _)| self.reliable_accelerometers[*i]) .map(|(_, val)| *val) .collect(); - quartiles = self.get_quartiles(&filtered_data); + Some(self.get_quartiles(&filtered_data)) } else { - return None; + None } - - Some(quartiles) } + /// Main function to process accelerometer data + /// This function calculates the magnitude of the acceleration (across all axes) for each + /// accelerometer + /// It then removes outliers from the data and checks if the data is reliable + /// Unreliable data is deemed unacceptable and the function returns None pub fn process_data( &mut self, data: RawAccelerometerData, ) -> Option> { - let mut accelerometer_data: AccelerometerData = - AccelerometerData::from_slice(&[0.0; K_NUM_ACCELEROMETERS]).unwrap(); - let mut magnitude: f32 = 0.0; - - data.iter().enumerate().for_each(|(i, axis)| { - magnitude = 0.0; - axis.iter().for_each(|val| { - magnitude += val * val; - }); - accelerometer_data[i] = magnitude.sqrt(); - }); + let accelerometer_data: AccelerometerData = data + .iter() + .map(|axis| axis.iter().fold(0.0, |acc, val| acc + val * val).sqrt()) + .collect(); let clean_accelerometer_data = self.handle_outliers(accelerometer_data)?; @@ -117,6 +132,9 @@ impl AccelerometerPreprocessor { Some(clean_accelerometer_data) } + /// Sets accelerometers as unreliable if they have more than + /// K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS outliers detected + /// Deems the data unacceptable if more than 1 accelerometer is unreliable pub fn check_reliable(&mut self) -> SensorChecks { self.num_outliers_per_accelerometer .iter() @@ -170,7 +188,7 @@ mod tests { use super::*; #[test] - pub fn test_returns_okay() { + pub fn test_process_data() { let mut preprocessor = AccelerometerPreprocessor::new(); assert_eq!(preprocessor.num_reliable_accelerometers, 4); @@ -194,6 +212,49 @@ mod tests { let processed_data = preprocessor.process_data(raw_data); assert!(processed_data.is_some()); + + let processed_data = processed_data.unwrap(); + assert_eq!(processed_data[0], (14.0 as f32).sqrt()); + assert_eq!(processed_data[1], (77.0 as f32).sqrt()); + assert_eq!(processed_data[2], (194.0 as f32).sqrt()); + assert_eq!(processed_data[3], (365.0 as f32).sqrt()); + } + + #[test] + pub fn test_process_data_one_unreliable() { + let mut preprocessor = AccelerometerPreprocessor::new(); + + assert_eq!(preprocessor.num_reliable_accelerometers, 4); + assert_eq!( + preprocessor.reliable_accelerometers, + [true; K_NUM_ACCELEROMETERS] + ); + assert_eq!( + preprocessor.num_outliers_per_accelerometer, + [0; K_NUM_ACCELEROMETERS] + ); + + preprocessor.reliable_accelerometers = [true, false, true, true]; + preprocessor.num_reliable_accelerometers = 3; + + let raw_data: RawAccelerometerData = + RawAccelerometerData::from_slice(&[ + Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) = 3.74 + Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // sqrt(Median (3.74, 13.93, + // 19.1)) = 13.93 + Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), // sqrt(194) = 13.93 + Vec::from_slice(&[10.0, 11.0, 12.0]).unwrap(), // sqrt(365) = 19.1 + ]) + .unwrap(); + + let processed_data = preprocessor.process_data(raw_data); + assert!(processed_data.is_some()); + + let processed_data = processed_data.unwrap(); + assert_eq!(processed_data[0], (14.0 as f32).sqrt()); + assert_eq!(processed_data[1], (194.0 as f32).sqrt()); + assert_eq!(processed_data[2], (194.0 as f32).sqrt()); + assert_eq!(processed_data[3], (365.0 as f32).sqrt()); } #[test] From 5bc07042ccdde95221694b6c1e9471f65b5cc0fb Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 10:38:55 +0000 Subject: [PATCH 19/99] clippy --- .../src/preprocessing/accelerometer.rs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 6d750035..740dda81 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -214,10 +214,10 @@ mod tests { assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); - assert_eq!(processed_data[0], (14.0 as f32).sqrt()); - assert_eq!(processed_data[1], (77.0 as f32).sqrt()); - assert_eq!(processed_data[2], (194.0 as f32).sqrt()); - assert_eq!(processed_data[3], (365.0 as f32).sqrt()); + assert_eq!(processed_data[0], (14.0_f32).sqrt()); + assert_eq!(processed_data[1], (77.0_f32).sqrt()); + assert_eq!(processed_data[2], (194.0_f32).sqrt()); + assert_eq!(processed_data[3], (365.0_f32).sqrt()); } #[test] @@ -251,10 +251,10 @@ mod tests { assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); - assert_eq!(processed_data[0], (14.0 as f32).sqrt()); - assert_eq!(processed_data[1], (194.0 as f32).sqrt()); - assert_eq!(processed_data[2], (194.0 as f32).sqrt()); - assert_eq!(processed_data[3], (365.0 as f32).sqrt()); + assert_eq!(processed_data[0], (14.0_f32).sqrt()); + assert_eq!(processed_data[1], (194.0_f32).sqrt()); + assert_eq!(processed_data[2], (194.0_f32).sqrt()); + assert_eq!(processed_data[3], (365.0_f32).sqrt()); } #[test] From 894169cb832b3e36d1ae2fb742f07e22dadbe294 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 11:11:27 +0000 Subject: [PATCH 20/99] small fixes to types --- lib/core/src/types.rs | 15 --------------- lib/localisation/Cargo.toml | 1 - .../src/preprocessing/accelerometer.rs | 3 +-- 3 files changed, 1 insertion(+), 18 deletions(-) diff --git a/lib/core/src/types.rs b/lib/core/src/types.rs index 444ede37..f25b2569 100644 --- a/lib/core/src/types.rs +++ b/lib/core/src/types.rs @@ -1,4 +1,3 @@ -use heapless::Vec; #[derive(PartialEq, Debug)] pub enum DigitalSignal { @@ -16,20 +15,6 @@ impl DigitalSignal { } } -pub const K_NUM_ACCELEROMETERS: i32 = 4; -pub const K_NUM_AXIS: i32 = 3; -pub const K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS: i32 = 2; - -#[derive(PartialEq)] -pub enum SensorChecks { - Unacceptable, - Acceptable, -} - -pub type RawAccelerometerData = - Vec, NUM_ACC>; -pub type AccelerometerData = Vec; - #[cfg(test)] mod tests { use super::*; diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index d1ca4627..c2912448 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -7,4 +7,3 @@ edition = "2021" nalgebra = { version = "0.33.0", default-features = false } heapless = "0.8.0" -hyped_core = { path = "../core" } diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 740dda81..30784f7f 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,8 +1,7 @@ -use crate::types::AccelerometerData; +use crate::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; use super::super::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; use heapless::Vec; -use hyped_core::types::{RawAccelerometerData, SensorChecks}; /// Stores the quartiles of the data and the bounds for outliers /// which are calculated from the quartiles From 0ad0161d5fbb1ddf53163ed7b466eeb108a7e235 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 11:13:13 +0000 Subject: [PATCH 21/99] fmt! --- Cargo.lock | 1 - lib/core/src/types.rs | 1 - 2 files changed, 2 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 30d58bef..f56e3650 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -310,7 +310,6 @@ name = "hyped_localisation" version = "0.1.0" dependencies = [ "heapless", - "hyped_core", "nalgebra", ] diff --git a/lib/core/src/types.rs b/lib/core/src/types.rs index f25b2569..ea7e87c1 100644 --- a/lib/core/src/types.rs +++ b/lib/core/src/types.rs @@ -1,4 +1,3 @@ - #[derive(PartialEq, Debug)] pub enum DigitalSignal { High, From 429ec1792c043920b939e695d49c8e1e2ddb9261 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 14:08:01 +0000 Subject: [PATCH 22/99] small bug fixex --- lib/localisation/src/control.rs | 2 +- lib/localisation/src/control/localizer.rs | 8 ++++---- lib/localisation/src/control/navigator.rs | 1 - 3 files changed, 5 insertions(+), 6 deletions(-) delete mode 100644 lib/localisation/src/control/navigator.rs diff --git a/lib/localisation/src/control.rs b/lib/localisation/src/control.rs index 0f450390..eea187d4 100644 --- a/lib/localisation/src/control.rs +++ b/lib/localisation/src/control.rs @@ -1 +1 @@ -pub mod navigator; +pub mod localizer; diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index d58508e8..c16dcd09 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -2,16 +2,16 @@ use crate::filtering::kalman_filter::KalmanFilter; -pub struct Navigator { +pub struct Localizer { displacement: f64, velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, } -impl Navigator { - pub fn new(kalman_filter: KalmanFilter) -> Navigator { - Navigator { +impl Localizer { + pub fn new(kalman_filter: KalmanFilter) -> Localizer { + Localizer { displacement: 0.0, velocity: 0.0, acceleration: 0.0, diff --git a/lib/localisation/src/control/navigator.rs b/lib/localisation/src/control/navigator.rs deleted file mode 100644 index 8b137891..00000000 --- a/lib/localisation/src/control/navigator.rs +++ /dev/null @@ -1 +0,0 @@ - From 61e6cfb8a338556093739ac407fb8fc7f5c9638c Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 14:16:35 +0000 Subject: [PATCH 23/99] deleted redundant file --- lib/localisation/src/preprocessing/lib.rs | 1 - 1 file changed, 1 deletion(-) delete mode 100644 lib/localisation/src/preprocessing/lib.rs diff --git a/lib/localisation/src/preprocessing/lib.rs b/lib/localisation/src/preprocessing/lib.rs deleted file mode 100644 index a7d02f83..00000000 --- a/lib/localisation/src/preprocessing/lib.rs +++ /dev/null @@ -1 +0,0 @@ -pub mod accelerometer; From 222c2a17df699a89ac1d6dd6a355db16c3f2755d Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 14:18:04 +0000 Subject: [PATCH 24/99] smallfix --- lib/localisation/src/lib.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index 9f6a38ca..fc3e412a 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1,5 +1,3 @@ #![no_std] pub mod control; pub mod filtering; -pub mod preprocessing; -pub mod types; From 24a43c2856f68c8f290b54a5fc31a0be299c57d9 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 14:46:29 +0000 Subject: [PATCH 25/99] initialised kalman filter --- lib/localisation/src/control/localizer.rs | 39 +++++++++++++++++++++++ lib/localisation/src/lib.rs | 1 + 2 files changed, 40 insertions(+) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index c16dcd09..0110e7a8 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -1,6 +1,9 @@ +use nalgebra::{Matrix2, Vector2}; +use libm::pow; use crate::filtering::kalman_filter::KalmanFilter; +const DELTA_T: f64 = 0.01; pub struct Localizer { displacement: f64, @@ -11,6 +14,42 @@ pub struct Localizer { impl Localizer { pub fn new(kalman_filter: KalmanFilter) -> Localizer { + + + + let initial_state = Vector2::new(0.0, 0.0); + let initial_covariance = Matrix2::new(1.0, 0.0, 0.0, 1.0); + let transition_matrix = Matrix2::new(1.0, DELTA_T, 0.0, 1.0); + let control_matrix = Vector2::new(0.5 * DELTA_T * DELTA_T, DELTA_T); + let observation_matrix = Matrix2::new(1.0, 0.0, 0.0, DELTA_T); + + // Assuming frequency of 6400hz for IMU at 120 mu g / sqrt(Hz) + // standard deviation = 120 * sqrt(6400) = 9600 mu g = 0.0096 g + // = 0.0096 * 9.81 = 0.094176 m/s^2 + // variance = 0.094176^2 = 0.0089 m/s^2 + + let process_noise: Matrix2 = Matrix2::new( + 0.25 * pow(DELTA_T,4.0), 0.5 * pow(DELTA_T,3.0), + 0.5 * pow(DELTA_T,3.0), pow(DELTA_T,2.0) * 0.0089); + + // We assume the stripe counter is accurate + // Optical flow expects standard deviation of 0.01% of the measured value + // Assuming top speed 10m/s, + // standard deviation = 0.01 * 10 = 0.1 m/s + // variance = 0.1^2 = 0.01 m/s^2 + + let measurement_noise: Matrix2 = Matrix2::new(0.01, 0.0, 0.0, 0.0); + + let kalman_filter = KalmanFilter::new( + initial_state, + initial_covariance, + transition_matrix, + control_matrix, + observation_matrix, + process_noise, + measurement_noise, + ); + Localizer { displacement: 0.0, velocity: 0.0, diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index fc3e412a..062f0500 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1,3 +1,4 @@ #![no_std] pub mod control; pub mod filtering; + From 2392c5492576f254bdd89e119c078ddf7c6cc15b Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 15:39:40 +0000 Subject: [PATCH 26/99] structure changes --- lib/localisation/src/control/localizer.rs | 28 +++++++++++-------- lib/localisation/src/lib.rs | 3 +- .../src/preprocessing/accelerometer.rs | 7 +++-- 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 0110e7a8..71254a61 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -1,7 +1,13 @@ -use nalgebra::{Matrix2, Vector2}; -use libm::pow; -use crate::filtering::kalman_filter::KalmanFilter; +use crate::{ + filtering::kalman_filter::KalmanFilter, + preprocessing::{ + accelerometer::AccelerometerPreprocessor, keyence::KeyenceAgrees, + optical::process_data as process_optical_data, + }, +}; +use libm::pow; +use nalgebra::{Matrix2, Vector2}; const DELTA_T: f64 = 0.01; @@ -14,24 +20,24 @@ pub struct Localizer { impl Localizer { pub fn new(kalman_filter: KalmanFilter) -> Localizer { - - - let initial_state = Vector2::new(0.0, 0.0); let initial_covariance = Matrix2::new(1.0, 0.0, 0.0, 1.0); let transition_matrix = Matrix2::new(1.0, DELTA_T, 0.0, 1.0); let control_matrix = Vector2::new(0.5 * DELTA_T * DELTA_T, DELTA_T); let observation_matrix = Matrix2::new(1.0, 0.0, 0.0, DELTA_T); - + // Assuming frequency of 6400hz for IMU at 120 mu g / sqrt(Hz) // standard deviation = 120 * sqrt(6400) = 9600 mu g = 0.0096 g // = 0.0096 * 9.81 = 0.094176 m/s^2 // variance = 0.094176^2 = 0.0089 m/s^2 let process_noise: Matrix2 = Matrix2::new( - 0.25 * pow(DELTA_T,4.0), 0.5 * pow(DELTA_T,3.0), - 0.5 * pow(DELTA_T,3.0), pow(DELTA_T,2.0) * 0.0089); - + 0.25 * pow(DELTA_T, 4.0), + 0.5 * pow(DELTA_T, 3.0), + 0.5 * pow(DELTA_T, 3.0), + pow(DELTA_T, 2.0) * 0.0089, + ); + // We assume the stripe counter is accurate // Optical flow expects standard deviation of 0.01% of the measured value // Assuming top speed 10m/s, @@ -39,7 +45,7 @@ impl Localizer { // variance = 0.1^2 = 0.01 m/s^2 let measurement_noise: Matrix2 = Matrix2::new(0.01, 0.0, 0.0, 0.0); - + let kalman_filter = KalmanFilter::new( initial_state, initial_covariance, diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index 062f0500..9f6a38ca 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1,4 +1,5 @@ #![no_std] pub mod control; pub mod filtering; - +pub mod preprocessing; +pub mod types; diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 30784f7f..c455bc18 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,6 +1,7 @@ -use crate::types::{AccelerometerData, RawAccelerometerData, SensorChecks}; - -use super::super::types::{K_NUM_ACCELEROMETERS, K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS}; +use crate::types::{ + AccelerometerData, RawAccelerometerData, SensorChecks, K_NUM_ACCELEROMETERS, + K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS, +}; use heapless::Vec; /// Stores the quartiles of the data and the bounds for outliers From 9199d1f1ddb13be67d0a0240739264af8b915367 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 15:50:18 +0000 Subject: [PATCH 27/99] File name changes --- lib/localisation/src/control/localizer.rs | 13 ++++++++++++- lib/localisation/src/preprocessing/optical.rs | 8 ++++---- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 71254a61..4ad22f0e 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -2,10 +2,11 @@ use crate::{ filtering::kalman_filter::KalmanFilter, preprocessing::{ accelerometer::AccelerometerPreprocessor, keyence::KeyenceAgrees, - optical::process_data as process_optical_data, + optical::process_optical_data, }, }; +use heapless::Vec; use libm::pow; use nalgebra::{Matrix2, Vector2}; @@ -16,6 +17,9 @@ pub struct Localizer { velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, + current_optical_reading: Option, 2>>, + current_keyence_reading: Option>, + current_accelerometer_reading: Option>, } impl Localizer { @@ -91,4 +95,11 @@ impl Localizer { pub fn get_acceleration(&self) -> f64 { self.acceleration } + + //pub fn preproccesor( + // &mut self, + // optical_data: Vec, 2>, + // keyence_data: Vec, + // accelerometer_data: Vec, + //) } diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index 2198f6ec..d5f9d1bd 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -2,7 +2,7 @@ use heapless::Vec; use libm::sqrtf; /// Processes the raw optical data to get the magnitude and added to the optical data for each sensor -pub fn process_data(raw_optical_data: Vec, 2>) -> Vec { +pub fn process_optical_data(raw_optical_data: Vec, 2>) -> Vec { let mut optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); for i in 0..2 { @@ -30,7 +30,7 @@ mod tests { ]) .unwrap(); let desired_outcome: Vec = Vec::from_slice(&[sqrtf(2.0), 5.0]).unwrap(); - let result = process_data(raw_optical_data); + let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); } @@ -42,7 +42,7 @@ mod tests { ]) .unwrap(); let desired_outcome: Vec = Vec::from_slice(&[7.2111025, 3.1622777]).unwrap(); - let result = process_data(raw_optical_data); + let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); } @@ -54,7 +54,7 @@ mod tests { ]) .unwrap(); let desired_outcome: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); - let result = process_data(raw_optical_data); + let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); } } From 8a97f967c23d3a09cf7b736d96b64bbfe38ccbd2 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 16:22:37 +0000 Subject: [PATCH 28/99] working --- lib/localisation/src/control/localizer.rs | 37 +++++++++++++++++------ 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 4ad22f0e..430a6334 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -1,7 +1,7 @@ use crate::{ filtering::kalman_filter::KalmanFilter, preprocessing::{ - accelerometer::AccelerometerPreprocessor, keyence::KeyenceAgrees, + accelerometer::AccelerometerPreprocessor, keyence::{KeyenceAgrees, SensorChecks}, optical::process_optical_data, }, }; @@ -17,9 +17,8 @@ pub struct Localizer { velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, - current_optical_reading: Option, 2>>, - current_keyence_reading: Option>, - current_accelerometer_reading: Option>, + accelerometer_preprocessor: AccelerometerPreprocessor, + keyence_checker: KeyenceAgrees, } impl Localizer { @@ -65,6 +64,8 @@ impl Localizer { velocity: 0.0, acceleration: 0.0, kalman_filter, + accelerometer_preprocessor: AccelerometerPreprocessor::new(), + keyence_checker: KeyenceAgrees::new(), } } @@ -96,10 +97,26 @@ impl Localizer { self.acceleration } - //pub fn preproccesor( - // &mut self, - // optical_data: Vec, 2>, - // keyence_data: Vec, - // accelerometer_data: Vec, - //) + pub fn preprocessor( + &mut self, + optical_data: Vec, 2>, + keyence_data: Vec, + accelerometer_data: Vec, + ) { + + let processed_optical_data = process_optical_data(optical_data); + + let keyence_status = self.keyence_checker.check_keyence_agrees(keyence_data.clone()); + //if keyence_status == SensorChecks::Unacceptable { + //TODOLater: Change state + // return; + //} + + + + + + + } + } From 666ca5639489128463db1a012094cafdc566e1c2 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 16:36:59 +0000 Subject: [PATCH 29/99] spelling --- lib/localisation/src/control/localizer.rs | 6 +++--- lib/localisation/src/preprocessing/keyence.rs | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 430a6334..a739af17 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -107,10 +107,10 @@ impl Localizer { let processed_optical_data = process_optical_data(optical_data); let keyence_status = self.keyence_checker.check_keyence_agrees(keyence_data.clone()); - //if keyence_status == SensorChecks::Unacceptable { + if keyence_status == SensorChecks::Unacceptable { //TODOLater: Change state - // return; - //} + return; + } diff --git a/lib/localisation/src/preprocessing/keyence.rs b/lib/localisation/src/preprocessing/keyence.rs index fd81cbb5..44d6a64c 100644 --- a/lib/localisation/src/preprocessing/keyence.rs +++ b/lib/localisation/src/preprocessing/keyence.rs @@ -3,7 +3,7 @@ use heapless::Vec; #[derive(PartialEq, Debug)] pub enum SensorChecks { Acceptable, - Unnaceptable, + Unacceptable, } /// Checks if the two Keyence sensors are in agreement. @@ -27,7 +27,7 @@ impl KeyenceAgrees { pub fn check_keyence_agrees(&mut self, keyence_data: Vec) -> SensorChecks { if keyence_data[0] != keyence_data[1] && !self.previous_keyence_agreement { - return SensorChecks::Unnaceptable; + return SensorChecks::Unacceptable; } else { self.previous_keyence_agreement = keyence_data[0] == keyence_data[1]; } @@ -88,7 +88,7 @@ mod tests { let second_keyence_data: Vec = Vec::from_slice(&[true, false]).unwrap(); let mut keyence_agrees = KeyenceAgrees::new(); let first_outcome = SensorChecks::Acceptable; - let second_outcome = SensorChecks::Unnaceptable; + let second_outcome = SensorChecks::Unacceptable; let initial_try = keyence_agrees.check_keyence_agrees(first_keyence_data); let result = keyence_agrees.check_keyence_agrees(second_keyence_data); assert_eq!(initial_try, first_outcome); From c635a379f86beb48dbb70d0aa29ce3da7c847f99 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 17:51:50 +0000 Subject: [PATCH 30/99] preprocessor --- lib/localisation/src/control/localizer.rs | 56 +++++++++-------------- 1 file changed, 21 insertions(+), 35 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index a739af17..27ea63f8 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -1,12 +1,15 @@ use crate::{ filtering::kalman_filter::KalmanFilter, preprocessing::{ - accelerometer::AccelerometerPreprocessor, keyence::{KeyenceAgrees, SensorChecks}, + accelerometer::AccelerometerPreprocessor, + keyence::{KeyenceAgrees, SensorChecks}, optical::process_optical_data, }, + types::{RawAccelerometerData, K_NUM_ACCELEROMETERS, K_NUM_AXIS}, }; use heapless::Vec; + use libm::pow; use nalgebra::{Matrix2, Vector2}; @@ -69,54 +72,37 @@ impl Localizer { } } - //Setters - - pub fn set_displacement(&mut self, displacement: f64) { - self.displacement = displacement; - } - - pub fn set_velocity(&mut self, velocity: f64) { - self.velocity = velocity; - } - - pub fn set_acceleration(&mut self, acceleration: f64) { - self.acceleration = acceleration; - } - - //Getters - - pub fn get_displacement(&self) -> f64 { - self.displacement - } - - pub fn get_velocity(&self) -> f64 { - self.velocity - } - - pub fn get_acceleration(&self) -> f64 { - self.acceleration - } - pub fn preprocessor( &mut self, optical_data: Vec, 2>, keyence_data: Vec, - accelerometer_data: Vec, + accelerometer_data: RawAccelerometerData, ) { - let processed_optical_data = process_optical_data(optical_data); - let keyence_status = self.keyence_checker.check_keyence_agrees(keyence_data.clone()); + let keyence_status = self + .keyence_checker + .check_keyence_agrees(keyence_data.clone()); + if keyence_status == SensorChecks::Unacceptable { //TODOLater: Change state - return; + return; } + let mut accelerometer_preprocessor = AccelerometerPreprocessor::new(); + let processed_accelerometer_data = + accelerometer_preprocessor.process_data(accelerometer_data); + if processed_accelerometer_data.is_none() { + // TODOLater: Change state if accelerometer data is unacceptable + return; + } + let processed_accelerometer_data = processed_accelerometer_data.unwrap(); + } - - + pub fn update(&mut self) { + // TODOLater: Implement filtrer + update function } } From cecf0330a4983cd4b609dbd4cc61c13daa2a334e Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sat, 16 Nov 2024 17:52:16 +0000 Subject: [PATCH 31/99] format| --- lib/localisation/src/control/localizer.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 27ea63f8..4f331e30 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -98,11 +98,9 @@ impl Localizer { } let processed_accelerometer_data = processed_accelerometer_data.unwrap(); - } pub fn update(&mut self) { // TODOLater: Implement filtrer + update function } - } From eeda22a0533e35575cfa4c3c5a5e739e81f6a74e Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 18 Nov 2024 18:51:43 +0000 Subject: [PATCH 32/99] Fixed preprocess_keyence: changed input vector from bool to int --- lib/localisation/src/control/localizer.rs | 17 ++++++++++++++++- lib/localisation/src/preprocessing/keyence.rs | 18 +++++++++--------- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 4f331e30..27494092 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -14,6 +14,7 @@ use libm::pow; use nalgebra::{Matrix2, Vector2}; const DELTA_T: f64 = 0.01; +const STRIPE_WIDTH: f64 = 1; pub struct Localizer { displacement: f64, @@ -22,6 +23,10 @@ pub struct Localizer { kalman_filter: KalmanFilter, accelerometer_preprocessor: AccelerometerPreprocessor, keyence_checker: KeyenceAgrees, + keyence_val: f64, + optical_val: f64, + accelerometer_val: f64, + } impl Localizer { @@ -69,13 +74,16 @@ impl Localizer { kalman_filter, accelerometer_preprocessor: AccelerometerPreprocessor::new(), keyence_checker: KeyenceAgrees::new(), + keyence_val: 0.0, + optical_val: 0.0, + accelerometer_val: 0.0, } } pub fn preprocessor( &mut self, optical_data: Vec, 2>, - keyence_data: Vec, + keyence_data: Vec, accelerometer_data: RawAccelerometerData, ) { let processed_optical_data = process_optical_data(optical_data); @@ -88,6 +96,11 @@ impl Localizer { //TODOLater: Change state return; } + else { + self.keyence_val = keyence_data[0] as f64; + } + + let mut accelerometer_preprocessor = AccelerometerPreprocessor::new(); let processed_accelerometer_data = @@ -101,6 +114,8 @@ impl Localizer { } pub fn update(&mut self) { + + // TODOLater: Implement filtrer + update function } } diff --git a/lib/localisation/src/preprocessing/keyence.rs b/lib/localisation/src/preprocessing/keyence.rs index 44d6a64c..fef51a99 100644 --- a/lib/localisation/src/preprocessing/keyence.rs +++ b/lib/localisation/src/preprocessing/keyence.rs @@ -25,7 +25,7 @@ impl KeyenceAgrees { } } - pub fn check_keyence_agrees(&mut self, keyence_data: Vec) -> SensorChecks { + pub fn check_keyence_agrees(&mut self, keyence_data: Vec) -> SensorChecks { if keyence_data[0] != keyence_data[1] && !self.previous_keyence_agreement { return SensorChecks::Unacceptable; } else { @@ -42,7 +42,7 @@ mod tests { #[test] fn test_acceptable_success() { - let keyence_data: Vec = Vec::from_slice(&[true, true]).unwrap(); + let keyence_data: Vec = Vec::from_slice(&[0, 1]).unwrap(); let mut keyence_agrees = KeyenceAgrees::new(); let desired_outcome = SensorChecks::Acceptable; let result = keyence_agrees.check_keyence_agrees(keyence_data); @@ -51,7 +51,7 @@ mod tests { #[test] fn test_acceptable_false_success() { - let keyence_data: Vec = Vec::from_slice(&[true, false]).unwrap(); + let keyence_data: Vec = Vec::from_slice(&[0, 1]).unwrap(); let mut keyence_agrees = KeyenceAgrees::new(); let desired_outcome = SensorChecks::Acceptable; let result = keyence_agrees.check_keyence_agrees(keyence_data); @@ -60,8 +60,8 @@ mod tests { #[test] fn test_acceptable_second_false_success() { - let first_keyence_data: Vec = Vec::from_slice(&[true, true]).unwrap(); - let second_keyence_data: Vec = Vec::from_slice(&[true, false]).unwrap(); + let first_keyence_data: Vec = Vec::from_slice(&[1, 1]).unwrap(); + let second_keyence_data: Vec = Vec::from_slice(&[1, 1]).unwrap(); let mut keyence_agrees = KeyenceAgrees::new(); let desired_outcome = SensorChecks::Acceptable; let initial_try = keyence_agrees.check_keyence_agrees(first_keyence_data); @@ -72,8 +72,8 @@ mod tests { #[test] fn test_acceptable_prev_false_success() { - let first_keyence_data: Vec = Vec::from_slice(&[true, false]).unwrap(); - let second_keyence_data: Vec = Vec::from_slice(&[true, true]).unwrap(); + let first_keyence_data: Vec = Vec::from_slice(&[1, 2]).unwrap(); + let second_keyence_data: Vec = Vec::from_slice(&[1, 1]).unwrap(); let mut keyence_agrees = KeyenceAgrees::new(); let desired_outcome = SensorChecks::Acceptable; let initial_try = keyence_agrees.check_keyence_agrees(first_keyence_data); @@ -84,8 +84,8 @@ mod tests { #[test] fn test_unnacceptable_prev_false_success() { - let first_keyence_data: Vec = Vec::from_slice(&[true, false]).unwrap(); - let second_keyence_data: Vec = Vec::from_slice(&[true, false]).unwrap(); + let first_keyence_data: Vec = Vec::from_slice(&[1, 2]).unwrap(); + let second_keyence_data: Vec = Vec::from_slice(&[2, 3]).unwrap(); let mut keyence_agrees = KeyenceAgrees::new(); let first_outcome = SensorChecks::Acceptable; let second_outcome = SensorChecks::Unacceptable; From d815e2ac1025f7d6c60306f5d14adfa06a03ea99 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 18 Nov 2024 18:53:21 +0000 Subject: [PATCH 33/99] small fixes --- lib/localisation/src/control/localizer.rs | 10 +++------- lib/localisation/src/preprocessing/keyence.rs | 2 +- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 27494092..497db597 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -14,7 +14,7 @@ use libm::pow; use nalgebra::{Matrix2, Vector2}; const DELTA_T: f64 = 0.01; -const STRIPE_WIDTH: f64 = 1; +const STRIPE_WIDTH: f64 = 1.0; pub struct Localizer { displacement: f64, @@ -26,7 +26,6 @@ pub struct Localizer { keyence_val: f64, optical_val: f64, accelerometer_val: f64, - } impl Localizer { @@ -95,13 +94,11 @@ impl Localizer { if keyence_status == SensorChecks::Unacceptable { //TODOLater: Change state return; - } - else { + } else { + //TODOLater: Make sure it doesnt take incorrect value self.keyence_val = keyence_data[0] as f64; } - - let mut accelerometer_preprocessor = AccelerometerPreprocessor::new(); let processed_accelerometer_data = accelerometer_preprocessor.process_data(accelerometer_data); @@ -115,7 +112,6 @@ impl Localizer { pub fn update(&mut self) { - // TODOLater: Implement filtrer + update function } } diff --git a/lib/localisation/src/preprocessing/keyence.rs b/lib/localisation/src/preprocessing/keyence.rs index fef51a99..caafefe2 100644 --- a/lib/localisation/src/preprocessing/keyence.rs +++ b/lib/localisation/src/preprocessing/keyence.rs @@ -85,7 +85,7 @@ mod tests { #[test] fn test_unnacceptable_prev_false_success() { let first_keyence_data: Vec = Vec::from_slice(&[1, 2]).unwrap(); - let second_keyence_data: Vec = Vec::from_slice(&[2, 3]).unwrap(); + let second_keyence_data: Vec = Vec::from_slice(&[2, 3]).unwrap(); let mut keyence_agrees = KeyenceAgrees::new(); let first_outcome = SensorChecks::Acceptable; let second_outcome = SensorChecks::Unacceptable; From 34105ec1e47ad54b688567c093c3b056211d7926 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 21 Nov 2024 18:27:53 +0000 Subject: [PATCH 34/99] Finished... I think? --- lib/localisation/src/control/localizer.rs | 43 ++++++++++++++++++++--- 1 file changed, 39 insertions(+), 4 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 497db597..e48c23a6 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -11,7 +11,7 @@ use crate::{ use heapless::Vec; use libm::pow; -use nalgebra::{Matrix2, Vector2}; +use nalgebra::{Matrix2, Vector1, Vector2}; const DELTA_T: f64 = 0.01; const STRIPE_WIDTH: f64 = 1.0; @@ -87,6 +87,12 @@ impl Localizer { ) { let processed_optical_data = process_optical_data(optical_data); + //TODOLater: Make sure this is correct way get velocity + for i in 0..2 { + self.optical_val += processed_optical_data[i] as f64; + } + self.optical_val /= 2.0; + let keyence_status = self .keyence_checker .check_keyence_agrees(keyence_data.clone()); @@ -103,15 +109,44 @@ impl Localizer { let processed_accelerometer_data = accelerometer_preprocessor.process_data(accelerometer_data); if processed_accelerometer_data.is_none() { - // TODOLater: Change state if accelerometer data is unacceptable + // TODOLater: Change state return; } let processed_accelerometer_data = processed_accelerometer_data.unwrap(); + self.accelerometer_val = 0.0; + for i in 0..K_NUM_ACCELEROMETERS { + for j in 0..K_NUM_AXIS { + self.accelerometer_val += processed_accelerometer_data[i] as f64; + } + } + self.accelerometer_val /= (K_NUM_ACCELEROMETERS * K_NUM_AXIS) as f64; } - pub fn update(&mut self) { + pub fn iteration( + &mut self, + optical_data: Vec, 2>, + keyence_data: Vec, + accelerometer_data: RawAccelerometerData, + ) { + self.preprocessor( + optical_data.clone(), + keyence_data.clone(), + accelerometer_data.clone(), + ); + + let control_input = Vector1::new(self.accelerometer_val); + + self.kalman_filter.predict(&control_input); + + let measurement = Vector2::new(self.keyence_val * STRIPE_WIDTH, self.optical_val); + + self.kalman_filter.update(&measurement); + + let state = self.kalman_filter.get_state(); - // TODOLater: Implement filtrer + update function + self.displacement = state[0]; + self.velocity = state[1]; + //TODOLater: Acceleration! } } From 55c0c2600e2ef4dce83867650a8f4d5f757cf312 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 21 Nov 2024 18:35:21 +0000 Subject: [PATCH 35/99] CLIPPYgit add .! and also half-assed acceleration calculation: --- lib/localisation/src/control/localizer.rs | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index e48c23a6..26170259 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -19,9 +19,9 @@ const STRIPE_WIDTH: f64 = 1.0; pub struct Localizer { displacement: f64, velocity: f64, + previous_velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, - accelerometer_preprocessor: AccelerometerPreprocessor, keyence_checker: KeyenceAgrees, keyence_val: f64, optical_val: f64, @@ -69,9 +69,10 @@ impl Localizer { Localizer { displacement: 0.0, velocity: 0.0, + previous_velocity: 0.0, acceleration: 0.0, kalman_filter, - accelerometer_preprocessor: AccelerometerPreprocessor::new(), + //accelerometer_preprocessor: AccelerometerPreprocessor::new(), keyence_checker: KeyenceAgrees::new(), keyence_val: 0.0, optical_val: 0.0, @@ -116,7 +117,7 @@ impl Localizer { let processed_accelerometer_data = processed_accelerometer_data.unwrap(); self.accelerometer_val = 0.0; for i in 0..K_NUM_ACCELEROMETERS { - for j in 0..K_NUM_AXIS { + for _ in 0..K_NUM_AXIS { self.accelerometer_val += processed_accelerometer_data[i] as f64; } } @@ -147,6 +148,8 @@ impl Localizer { self.displacement = state[0]; self.velocity = state[1]; - //TODOLater: Acceleration! + self.acceleration = (self.velocity - self.previous_velocity) / DELTA_T; //TODOLater: is this good enough? + self.previous_velocity = self.velocity; + } } From 39fc6a59b42ffbf358429647ca18ff5ef5e58889 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 21 Nov 2024 18:38:18 +0000 Subject: [PATCH 36/99] this commit will fail --- lib/localisation/src/control/localizer.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 26170259..275be35a 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -22,6 +22,7 @@ pub struct Localizer { previous_velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, + accelerometer_preprocessor: AccelerometerPreprocessor, keyence_checker: KeyenceAgrees, keyence_val: f64, optical_val: f64, @@ -29,7 +30,7 @@ pub struct Localizer { } impl Localizer { - pub fn new(kalman_filter: KalmanFilter) -> Localizer { + pub fn new(_kalman_filter: KalmanFilter) -> Localizer { let initial_state = Vector2::new(0.0, 0.0); let initial_covariance = Matrix2::new(1.0, 0.0, 0.0, 1.0); let transition_matrix = Matrix2::new(1.0, DELTA_T, 0.0, 1.0); @@ -72,7 +73,7 @@ impl Localizer { previous_velocity: 0.0, acceleration: 0.0, kalman_filter, - //accelerometer_preprocessor: AccelerometerPreprocessor::new(), + accelerometer_preprocessor: AccelerometerPreprocessor::new(), keyence_checker: KeyenceAgrees::new(), keyence_val: 0.0, optical_val: 0.0, From 545b4dc2671785f050f6e784022c03a6cd131619 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 21 Nov 2024 18:41:09 +0000 Subject: [PATCH 37/99] i command you to build --- lib/localisation/src/control/localizer.rs | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 275be35a..dac5a088 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -22,7 +22,6 @@ pub struct Localizer { previous_velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, - accelerometer_preprocessor: AccelerometerPreprocessor, keyence_checker: KeyenceAgrees, keyence_val: f64, optical_val: f64, @@ -30,7 +29,7 @@ pub struct Localizer { } impl Localizer { - pub fn new(_kalman_filter: KalmanFilter) -> Localizer { + pub fn new() -> Localizer { let initial_state = Vector2::new(0.0, 0.0); let initial_covariance = Matrix2::new(1.0, 0.0, 0.0, 1.0); let transition_matrix = Matrix2::new(1.0, DELTA_T, 0.0, 1.0); @@ -73,7 +72,6 @@ impl Localizer { previous_velocity: 0.0, acceleration: 0.0, kalman_filter, - accelerometer_preprocessor: AccelerometerPreprocessor::new(), keyence_checker: KeyenceAgrees::new(), keyence_val: 0.0, optical_val: 0.0, @@ -151,6 +149,5 @@ impl Localizer { self.velocity = state[1]; self.acceleration = (self.velocity - self.previous_velocity) / DELTA_T; //TODOLater: is this good enough? self.previous_velocity = self.velocity; - } } From 6f8d85ef9fcfc964df6a7e319842525abf3f7882 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 21 Nov 2024 18:47:08 +0000 Subject: [PATCH 38/99] CLIPPY! --- lib/localisation/src/control/localizer.rs | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index dac5a088..bf6c83c6 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -78,7 +78,15 @@ impl Localizer { accelerometer_val: 0.0, } } +} +impl Default for Localizer { + fn default() -> Self { + Self::new() + } +} + +impl Localizer { pub fn preprocessor( &mut self, optical_data: Vec, 2>, From eaa32c33f42f0c036987a682337049ecf20e446f Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 21 Nov 2024 19:02:26 +0000 Subject: [PATCH 39/99] simple silly test --- lib/localisation/src/control/localizer.rs | 34 +++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index bf6c83c6..03a4565e 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -159,3 +159,37 @@ impl Localizer { self.previous_velocity = self.velocity; } } + +#[cfg(test)] +mod tests { + use super::*; + use heapless::Vec; + + #[test] + fn test_localizer_with_zeros() { + let mut localizer = Localizer::default(); + + let raw_optical_data: Vec, 2> = Vec::from_slice(&[ + Vec::from_slice(&[0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0]).unwrap(), + ]) + .unwrap(); + + let raw_keyence_data: Vec = Vec::from_slice(&[0, 0]).unwrap(); + + let raw_accelerometer_data: RawAccelerometerData = + RawAccelerometerData::from_slice(&[ + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(14) = 3.74 + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(77) = 8.77 + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(194) = 13.93 + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(365) = 19.1 + ]) + .unwrap(); + + localizer.iteration(raw_optical_data, raw_keyence_data, raw_accelerometer_data); + + assert_eq!(localizer.displacement, 0.0); + assert_eq!(localizer.velocity, 0.0); + assert_eq!(localizer.acceleration, 0.0); + } +} From 7bfab5dd86d324fe3a7e9b6997913898fc1f9dc7 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 13 Jan 2025 18:14:56 +0000 Subject: [PATCH 40/99] removed comment --- lib/localisation/src/control/localizer.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 03a4565e..e8d76d07 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -179,10 +179,10 @@ mod tests { let raw_accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(14) = 3.74 - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(77) = 8.77 - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(194) = 13.93 - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), // sqrt(365) = 19.1 + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), ]) .unwrap(); From 73227cf8fee5bb3f052a97a189a222d09e4848be Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 13 Jan 2025 18:20:37 +0000 Subject: [PATCH 41/99] fmt --- lib/localisation/src/control/localizer.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index e8d76d07..49829634 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -179,10 +179,10 @@ mod tests { let raw_accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), ]) .unwrap(); From 3aa1a45c96869c4fd3e97de69c64e2cdc6cb2a29 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 13 Jan 2025 18:24:29 +0000 Subject: [PATCH 42/99] comment --- lib/localisation/src/control/localizer.rs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 49829634..a5ce9ba1 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -95,7 +95,6 @@ impl Localizer { ) { let processed_optical_data = process_optical_data(optical_data); - //TODOLater: Make sure this is correct way get velocity for i in 0..2 { self.optical_val += processed_optical_data[i] as f64; } @@ -109,7 +108,7 @@ impl Localizer { //TODOLater: Change state return; } else { - //TODOLater: Make sure it doesnt take incorrect value + //TODOLater: Check unit of keyence data self.keyence_val = keyence_data[0] as f64; } @@ -155,7 +154,7 @@ impl Localizer { self.displacement = state[0]; self.velocity = state[1]; - self.acceleration = (self.velocity - self.previous_velocity) / DELTA_T; //TODOLater: is this good enough? + self.acceleration = (self.velocity - self.previous_velocity) / DELTA_T; //TODOLater: is this accurate enough? self.previous_velocity = self.velocity; } } From f38152e400aee97de8c57ec21572da0505f43baa Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 13 Jan 2025 18:28:59 +0000 Subject: [PATCH 43/99] comment --- lib/localisation/src/control/localizer.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index a5ce9ba1..1728649f 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -146,6 +146,7 @@ impl Localizer { self.kalman_filter.predict(&control_input); + //TODOLater: Check unit of keyence data let measurement = Vector2::new(self.keyence_val * STRIPE_WIDTH, self.optical_val); self.kalman_filter.update(&measurement); From 2eb9bd749b2f234ac5346c3e91fa69dd0c1deb08 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 20 Jan 2025 18:51:13 +0000 Subject: [PATCH 44/99] Changed from 2 to 1 Optical Flow Sensor --- lib/localisation/src/control/localizer.rs | 21 +++------- lib/localisation/src/preprocessing/optical.rs | 41 ++++++------------- 2 files changed, 18 insertions(+), 44 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 1728649f..cd6e162d 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -89,16 +89,12 @@ impl Default for Localizer { impl Localizer { pub fn preprocessor( &mut self, - optical_data: Vec, 2>, + optical_data: Vec, keyence_data: Vec, accelerometer_data: RawAccelerometerData, ) { - let processed_optical_data = process_optical_data(optical_data); - - for i in 0..2 { - self.optical_val += processed_optical_data[i] as f64; - } - self.optical_val /= 2.0; + let processed_optical_data = process_optical_data(optical_data.clone()); + self.optical_val = processed_optical_data as f64; let keyence_status = self .keyence_checker @@ -132,7 +128,7 @@ impl Localizer { pub fn iteration( &mut self, - optical_data: Vec, 2>, + optical_data: Vec, keyence_data: Vec, accelerometer_data: RawAccelerometerData, ) { @@ -163,17 +159,12 @@ impl Localizer { #[cfg(test)] mod tests { use super::*; - use heapless::Vec; #[test] fn test_localizer_with_zeros() { let mut localizer = Localizer::default(); - let raw_optical_data: Vec, 2> = Vec::from_slice(&[ - Vec::from_slice(&[0.0, 0.0]).unwrap(), - Vec::from_slice(&[0.0, 0.0]).unwrap(), - ]) - .unwrap(); + let optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); let raw_keyence_data: Vec = Vec::from_slice(&[0, 0]).unwrap(); @@ -186,7 +177,7 @@ mod tests { ]) .unwrap(); - localizer.iteration(raw_optical_data, raw_keyence_data, raw_accelerometer_data); + localizer.iteration(optical_data, raw_keyence_data, raw_accelerometer_data); assert_eq!(localizer.displacement, 0.0); assert_eq!(localizer.velocity, 0.0); diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index d5f9d1bd..855b4414 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -2,20 +2,15 @@ use heapless::Vec; use libm::sqrtf; /// Processes the raw optical data to get the magnitude and added to the optical data for each sensor -pub fn process_optical_data(raw_optical_data: Vec, 2>) -> Vec { - let mut optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); +pub fn process_optical_data(raw_optical_data: Vec) -> f32 { + let mut magnitude: f32 = 0.0; - for i in 0..2 { - let mut magnitude: f32 = 0.0; - - for data in raw_optical_data[i].clone() { - let data: f32 = data as f32; - magnitude += data * data; - } - optical_data[i] = sqrtf(magnitude); + for data in raw_optical_data { + let data: f32 = data as f32; + magnitude += data * data; } - optical_data + sqrtf(magnitude) } #[cfg(test)] @@ -24,36 +19,24 @@ mod tests { #[test] fn test_correct_positive() { - let raw_optical_data: Vec, 2> = Vec::from_slice(&[ - Vec::from_slice(&[1.0, 1.0]).unwrap(), - Vec::from_slice(&[3.0, 4.0]).unwrap(), - ]) - .unwrap(); - let desired_outcome: Vec = Vec::from_slice(&[sqrtf(2.0), 5.0]).unwrap(); + let raw_optical_data: Vec = Vec::from_slice(&[1.0, 1.0]).unwrap(); + let desired_outcome: f32 = sqrtf(2.0); let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); } #[test] fn test_correct_negative() { - let raw_optical_data: Vec, 2> = Vec::from_slice(&[ - Vec::from_slice(&[-4.0, -6.0]).unwrap(), - Vec::from_slice(&[-3.0, -1.0]).unwrap(), - ]) - .unwrap(); - let desired_outcome: Vec = Vec::from_slice(&[7.2111025, 3.1622777]).unwrap(); + let raw_optical_data: Vec = Vec::from_slice(&[-4.0, -6.0]).unwrap(); + let desired_outcome: f32 = sqrtf(52.0); let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); } #[test] fn test_correct_zero() { - let raw_optical_data: Vec, 2> = Vec::from_slice(&[ - Vec::from_slice(&[0.0, 0.0]).unwrap(), - Vec::from_slice(&[0.0, 0.0]).unwrap(), - ]) - .unwrap(); - let desired_outcome: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); + let raw_optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); + let desired_outcome: f32 = 0.0; let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); } From dee30e1125a5558c4b5b08d728920a431a35abfb Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 27 Jan 2025 18:14:12 +0000 Subject: [PATCH 45/99] small name change --- lib/localisation/src/control/localizer.rs | 14 ++-- .../src/preprocessing/accelerometer.rs | 64 +++++++++---------- lib/localisation/src/types.rs | 6 +- 3 files changed, 42 insertions(+), 42 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index cd6e162d..88e23af6 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -5,7 +5,7 @@ use crate::{ keyence::{KeyenceAgrees, SensorChecks}, optical::process_optical_data, }, - types::{RawAccelerometerData, K_NUM_ACCELEROMETERS, K_NUM_AXIS}, + types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, }; use heapless::Vec; @@ -91,7 +91,7 @@ impl Localizer { &mut self, optical_data: Vec, keyence_data: Vec, - accelerometer_data: RawAccelerometerData, + accelerometer_data: RawAccelerometerData, ) { let processed_optical_data = process_optical_data(optical_data.clone()); self.optical_val = processed_optical_data as f64; @@ -118,19 +118,19 @@ impl Localizer { let processed_accelerometer_data = processed_accelerometer_data.unwrap(); self.accelerometer_val = 0.0; - for i in 0..K_NUM_ACCELEROMETERS { - for _ in 0..K_NUM_AXIS { + for i in 0..NUM_ACCELEROMETERS { + for _ in 0..NUM_AXIS { self.accelerometer_val += processed_accelerometer_data[i] as f64; } } - self.accelerometer_val /= (K_NUM_ACCELEROMETERS * K_NUM_AXIS) as f64; + self.accelerometer_val /= (NUM_ACCELEROMETERS * NUM_AXIS) as f64; } pub fn iteration( &mut self, optical_data: Vec, keyence_data: Vec, - accelerometer_data: RawAccelerometerData, + accelerometer_data: RawAccelerometerData, ) { self.preprocessor( optical_data.clone(), @@ -168,7 +168,7 @@ mod tests { let raw_keyence_data: Vec = Vec::from_slice(&[0, 0]).unwrap(); - let raw_accelerometer_data: RawAccelerometerData = + let raw_accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index c455bc18..f7ba3853 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1,6 +1,6 @@ use crate::types::{ - AccelerometerData, RawAccelerometerData, SensorChecks, K_NUM_ACCELEROMETERS, - K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS, K_NUM_AXIS, + AccelerometerData, RawAccelerometerData, SensorChecks, NUM_ACCELEROMETERS, + NUM_ALLOWED_ACCELEROMETER_OUTLIERS, NUM_AXIS, }; use heapless::Vec; @@ -39,9 +39,9 @@ pub struct AccelerometerPreprocessor { /// number of true values in reliable_accelerometers num_reliable_accelerometers: i32, /// true if accelerometer at index is reliable - reliable_accelerometers: [bool; K_NUM_ACCELEROMETERS], + reliable_accelerometers: [bool; NUM_ACCELEROMETERS], /// number of outliers detected for each accelerometer - num_outliers_per_accelerometer: [i32; K_NUM_ACCELEROMETERS], + num_outliers_per_accelerometer: [i32; NUM_ACCELEROMETERS], } impl AccelerometerPreprocessor { @@ -49,9 +49,9 @@ impl AccelerometerPreprocessor { /// By default, all accelerometers are deemed as reliable pub fn new() -> Self { Self { - num_reliable_accelerometers: K_NUM_ACCELEROMETERS as i32, - reliable_accelerometers: [true; K_NUM_ACCELEROMETERS], - num_outliers_per_accelerometer: [0; K_NUM_ACCELEROMETERS], + num_reliable_accelerometers: NUM_ACCELEROMETERS as i32, + reliable_accelerometers: [true; NUM_ACCELEROMETERS], + num_outliers_per_accelerometer: [0; NUM_ACCELEROMETERS], } } @@ -61,8 +61,8 @@ impl AccelerometerPreprocessor { /// are replaced with the median of the data pub fn handle_outliers( &mut self, - data: AccelerometerData, - ) -> Option> { + data: AccelerometerData, + ) -> Option> { let quartiles = self.calculate_quartiles(&data)?; let accelerometer_data = data @@ -91,12 +91,12 @@ impl AccelerometerPreprocessor { /// If more than one accelerometer is unreliable, None is returned pub fn calculate_quartiles( &self, - data: &AccelerometerData, + data: &AccelerometerData, ) -> Option { - if self.num_reliable_accelerometers == K_NUM_ACCELEROMETERS as i32 { + if self.num_reliable_accelerometers == NUM_ACCELEROMETERS as i32 { Some(self.get_quartiles(data)) - } else if self.num_reliable_accelerometers == (K_NUM_ACCELEROMETERS as i32 - 1) { - const SIZE: usize = K_NUM_ACCELEROMETERS - 1; + } else if self.num_reliable_accelerometers == (NUM_ACCELEROMETERS as i32 - 1) { + const SIZE: usize = NUM_ACCELEROMETERS - 1; let filtered_data: AccelerometerData = data .iter() .enumerate() @@ -116,9 +116,9 @@ impl AccelerometerPreprocessor { /// Unreliable data is deemed unacceptable and the function returns None pub fn process_data( &mut self, - data: RawAccelerometerData, - ) -> Option> { - let accelerometer_data: AccelerometerData = data + data: RawAccelerometerData, + ) -> Option> { + let accelerometer_data: AccelerometerData = data .iter() .map(|axis| axis.iter().fold(0.0, |acc, val| acc + val * val).sqrt()) .collect(); @@ -133,7 +133,7 @@ impl AccelerometerPreprocessor { } /// Sets accelerometers as unreliable if they have more than - /// K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS outliers detected + /// NUM_ALLOWED_ACCELEROMETER_OUTLIERS outliers detected /// Deems the data unacceptable if more than 1 accelerometer is unreliable pub fn check_reliable(&mut self) -> SensorChecks { self.num_outliers_per_accelerometer @@ -141,14 +141,14 @@ impl AccelerometerPreprocessor { .enumerate() .for_each(|(i, val)| { if self.reliable_accelerometers[i] - && val >= &(K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS as i32) + && val >= &(NUM_ALLOWED_ACCELEROMETER_OUTLIERS as i32) { self.reliable_accelerometers[i] = false; self.num_reliable_accelerometers -= 1; } }); - if self.num_reliable_accelerometers < K_NUM_ACCELEROMETERS as i32 - 1 { + if self.num_reliable_accelerometers < NUM_ACCELEROMETERS as i32 - 1 { return SensorChecks::Unacceptable; } @@ -178,7 +178,7 @@ impl AccelerometerPreprocessor { quartiles[0], quartiles[1], quartiles[2], - self.num_reliable_accelerometers < K_NUM_ACCELEROMETERS as i32, + self.num_reliable_accelerometers < NUM_ACCELEROMETERS as i32, ) } } @@ -194,14 +194,14 @@ mod tests { assert_eq!(preprocessor.num_reliable_accelerometers, 4); assert_eq!( preprocessor.reliable_accelerometers, - [true; K_NUM_ACCELEROMETERS] + [true; NUM_ACCELEROMETERS] ); assert_eq!( preprocessor.num_outliers_per_accelerometer, - [0; K_NUM_ACCELEROMETERS] + [0; NUM_ACCELEROMETERS] ); - let raw_data: RawAccelerometerData = + let raw_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) = 3.74 Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // sqrt(77) = 8.77 @@ -227,17 +227,17 @@ mod tests { assert_eq!(preprocessor.num_reliable_accelerometers, 4); assert_eq!( preprocessor.reliable_accelerometers, - [true; K_NUM_ACCELEROMETERS] + [true; NUM_ACCELEROMETERS] ); assert_eq!( preprocessor.num_outliers_per_accelerometer, - [0; K_NUM_ACCELEROMETERS] + [0; NUM_ACCELEROMETERS] ); preprocessor.reliable_accelerometers = [true, false, true, true]; preprocessor.num_reliable_accelerometers = 3; - let raw_data: RawAccelerometerData = + let raw_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) = 3.74 Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // sqrt(Median (3.74, 13.93, @@ -261,7 +261,7 @@ mod tests { pub fn test_get_quartiles() { let preprocessor = AccelerometerPreprocessor::new(); - let data: AccelerometerData = + let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); let processed_data = preprocessor.get_quartiles(&data); @@ -274,7 +274,7 @@ mod tests { fn test_calculate_quartiles_max_reliable() { let preprocessor = AccelerometerPreprocessor::new(); - let data: AccelerometerData = + let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); let processed_data = preprocessor.calculate_quartiles(&data); @@ -295,7 +295,7 @@ mod tests { preprocessor.reliable_accelerometers = [true, false, true, true]; preprocessor.num_reliable_accelerometers = 3; - let data: AccelerometerData = + let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 4.0]).unwrap(); let processed_data = preprocessor.calculate_quartiles(&data); @@ -316,13 +316,13 @@ mod tests { preprocessor.reliable_accelerometers = [true, false, true, true]; preprocessor.num_reliable_accelerometers = 3; - let data: AccelerometerData = + let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); let processed_data = preprocessor.handle_outliers(data); assert!(processed_data.is_some()); - let processed_data: AccelerometerData = processed_data.unwrap(); + let processed_data: AccelerometerData = processed_data.unwrap(); assert_eq!(processed_data[0], 1.0); assert_eq!(processed_data[1], 3.0); // replace unreliable with median assert_eq!(processed_data[2], 3.0); @@ -335,7 +335,7 @@ mod tests { preprocessor.reliable_accelerometers = [true, false, false, true]; preprocessor.num_reliable_accelerometers = 2; - let data: AccelerometerData = + let data: AccelerometerData = AccelerometerData::from_slice(&[1.0, 2.0, 3.0, 10.0]).unwrap(); let processed_data = preprocessor.handle_outliers(data); diff --git a/lib/localisation/src/types.rs b/lib/localisation/src/types.rs index 77042ef8..04a14e29 100644 --- a/lib/localisation/src/types.rs +++ b/lib/localisation/src/types.rs @@ -1,8 +1,8 @@ use heapless::Vec; -pub const K_NUM_ACCELEROMETERS: usize = 4; -pub const K_NUM_AXIS: usize = 3; -pub const K_NUM_ALLOWED_ACCELEROMETER_OUTLIERS: usize = 2; +pub const NUM_ACCELEROMETERS: usize = 4; +pub const NUM_AXIS: usize = 3; +pub const NUM_ALLOWED_ACCELEROMETER_OUTLIERS: usize = 2; #[derive(PartialEq)] pub enum SensorChecks { From 5adbe563d78d23e8a5d02f97e5ae995dd7140d42 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 11:29:10 +0000 Subject: [PATCH 46/99] optical flow scales with num of sensors --- lib/localisation/src/control/localizer.rs | 31 ++++++++++++------- lib/localisation/src/preprocessing/optical.rs | 28 +++++++++++------ lib/localisation/src/types.rs | 1 + 3 files changed, 40 insertions(+), 20 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 88e23af6..a98f3839 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -5,7 +5,7 @@ use crate::{ keyence::{KeyenceAgrees, SensorChecks}, optical::process_optical_data, }, - types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, + types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS, NUM_OPTICAL_SENSORS}, }; use heapless::Vec; @@ -13,7 +13,12 @@ use heapless::Vec; use libm::pow; use nalgebra::{Matrix2, Vector1, Vector2}; +//TODOLater: Confirm values are correct + +// Time step (s) const DELTA_T: f64 = 0.01; + +// Stripe width (m) const STRIPE_WIDTH: f64 = 1.0; pub struct Localizer { @@ -86,14 +91,20 @@ impl Default for Localizer { } } +pub enum PreprocessorError { + KeyenceUnacceptable, + AccelerometerUnnaceptable, +} + impl Localizer { pub fn preprocessor( &mut self, optical_data: Vec, keyence_data: Vec, accelerometer_data: RawAccelerometerData, - ) { - let processed_optical_data = process_optical_data(optical_data.clone()); + ) -> Result<(), PreprocessorError> { + let processed_optical_data = + process_optical_data(Vec::from_slice(&[optical_data.clone()]).unwrap()); self.optical_val = processed_optical_data as f64; let keyence_status = self @@ -101,19 +112,16 @@ impl Localizer { .check_keyence_agrees(keyence_data.clone()); if keyence_status == SensorChecks::Unacceptable { - //TODOLater: Change state - return; + return Err(PreprocessorError::KeyenceUnacceptable); } else { - //TODOLater: Check unit of keyence data - self.keyence_val = keyence_data[0] as f64; + self.keyence_val = (keyence_data[0] as f64) * STRIPE_WIDTH; } let mut accelerometer_preprocessor = AccelerometerPreprocessor::new(); let processed_accelerometer_data = accelerometer_preprocessor.process_data(accelerometer_data); if processed_accelerometer_data.is_none() { - // TODOLater: Change state - return; + return Err(PreprocessorError::AccelerometerUnnaceptable); } let processed_accelerometer_data = processed_accelerometer_data.unwrap(); @@ -124,6 +132,8 @@ impl Localizer { } } self.accelerometer_val /= (NUM_ACCELEROMETERS * NUM_AXIS) as f64; + + Ok(()) } pub fn iteration( @@ -142,7 +152,6 @@ impl Localizer { self.kalman_filter.predict(&control_input); - //TODOLater: Check unit of keyence data let measurement = Vector2::new(self.keyence_val * STRIPE_WIDTH, self.optical_val); self.kalman_filter.update(&measurement); @@ -151,7 +160,7 @@ impl Localizer { self.displacement = state[0]; self.velocity = state[1]; - self.acceleration = (self.velocity - self.previous_velocity) / DELTA_T; //TODOLater: is this accurate enough? + self.acceleration = self.accelerometer_val; self.previous_velocity = self.velocity; } } diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index 855b4414..b237c660 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -1,25 +1,33 @@ +use crate::types::NUM_OPTICAL_SENSORS; use heapless::Vec; use libm::sqrtf; /// Processes the raw optical data to get the magnitude and added to the optical data for each sensor -pub fn process_optical_data(raw_optical_data: Vec) -> f32 { - let mut magnitude: f32 = 0.0; +pub fn process_optical_data(raw_optical_data: Vec, NUM_OPTICAL_SENSORS>) -> f32 { + let mut total_magnitude: f32 = 0.0; - for data in raw_optical_data { - let data: f32 = data as f32; - magnitude += data * data; + for sensor_data in raw_optical_data { + let mut magnitude: f32 = 0.0; + + for data in sensor_data { + let data: f32 = data as f32; + magnitude += data * data; + } + total_magnitude += sqrtf(magnitude); } - sqrtf(magnitude) + total_magnitude / NUM_OPTICAL_SENSORS as f32 } #[cfg(test)] mod tests { use super::*; + use crate::types::NUM_OPTICAL_SENSORS; #[test] fn test_correct_positive() { - let raw_optical_data: Vec = Vec::from_slice(&[1.0, 1.0]).unwrap(); + let raw_optical_data: Vec, NUM_OPTICAL_SENSORS> = + Vec::from_slice(&[Vec::from_slice(&[1.0, 1.0]).unwrap()]).unwrap(); let desired_outcome: f32 = sqrtf(2.0); let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); @@ -27,7 +35,8 @@ mod tests { #[test] fn test_correct_negative() { - let raw_optical_data: Vec = Vec::from_slice(&[-4.0, -6.0]).unwrap(); + let raw_optical_data: Vec, NUM_OPTICAL_SENSORS> = + Vec::from_slice(&[Vec::from_slice(&[-4.0, -6.0]).unwrap()]).unwrap(); let desired_outcome: f32 = sqrtf(52.0); let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); @@ -35,7 +44,8 @@ mod tests { #[test] fn test_correct_zero() { - let raw_optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); + let raw_optical_data: Vec, NUM_OPTICAL_SENSORS> = + Vec::from_slice(&[Vec::from_slice(&[0.0, 0.0]).unwrap()]).unwrap(); let desired_outcome: f32 = 0.0; let result = process_optical_data(raw_optical_data); assert_eq!(result, desired_outcome); diff --git a/lib/localisation/src/types.rs b/lib/localisation/src/types.rs index 04a14e29..6a3446fc 100644 --- a/lib/localisation/src/types.rs +++ b/lib/localisation/src/types.rs @@ -3,6 +3,7 @@ use heapless::Vec; pub const NUM_ACCELEROMETERS: usize = 4; pub const NUM_AXIS: usize = 3; pub const NUM_ALLOWED_ACCELEROMETER_OUTLIERS: usize = 2; +pub const NUM_OPTICAL_SENSORS: usize = 1; #[derive(PartialEq)] pub enum SensorChecks { From 7796505d803872b20bfc575a5346e67b1506eb09 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 11:31:42 +0000 Subject: [PATCH 47/99] small fix --- lib/localisation/src/control/localizer.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index a98f3839..156a0493 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -5,7 +5,7 @@ use crate::{ keyence::{KeyenceAgrees, SensorChecks}, optical::process_optical_data, }, - types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS, NUM_OPTICAL_SENSORS}, + types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, }; use heapless::Vec; From 50133f1423b9aa31942b65772d36b1ded86474ac Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 11:36:53 +0000 Subject: [PATCH 48/99] something abour passing on errors --- lib/localisation/src/control/localizer.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 156a0493..15fa6ac9 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -141,12 +141,12 @@ impl Localizer { optical_data: Vec, keyence_data: Vec, accelerometer_data: RawAccelerometerData, - ) { + ) -> Result<(), PreprocessorError> { self.preprocessor( optical_data.clone(), keyence_data.clone(), accelerometer_data.clone(), - ); + )?; let control_input = Vector1::new(self.accelerometer_val); @@ -162,6 +162,8 @@ impl Localizer { self.velocity = state[1]; self.acceleration = self.accelerometer_val; self.previous_velocity = self.velocity; + + Ok(()) } } From 6c17de8ec9f83abf1ab2121e5b0c2a6b10850e10 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 11:51:22 +0000 Subject: [PATCH 49/99] minor test fixes --- lib/localisation/src/control/localizer.rs | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 15fa6ac9..13c52995 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -31,6 +31,7 @@ pub struct Localizer { keyence_val: f64, optical_val: f64, accelerometer_val: f64, + accelerometer_preprocessor: AccelerometerPreprocessor, } impl Localizer { @@ -81,6 +82,7 @@ impl Localizer { keyence_val: 0.0, optical_val: 0.0, accelerometer_val: 0.0, + accelerometer_preprocessor: AccelerometerPreprocessor::new(), } } } @@ -91,12 +93,14 @@ impl Default for Localizer { } } +#[derive(Debug)] pub enum PreprocessorError { KeyenceUnacceptable, AccelerometerUnnaceptable, } impl Localizer { + /// Preprocesses the data from the sensors, checking for errors and outliers pub fn preprocessor( &mut self, optical_data: Vec, @@ -117,9 +121,9 @@ impl Localizer { self.keyence_val = (keyence_data[0] as f64) * STRIPE_WIDTH; } - let mut accelerometer_preprocessor = AccelerometerPreprocessor::new(); - let processed_accelerometer_data = - accelerometer_preprocessor.process_data(accelerometer_data); + let processed_accelerometer_data = self + .accelerometer_preprocessor + .process_data(accelerometer_data); if processed_accelerometer_data.is_none() { return Err(PreprocessorError::AccelerometerUnnaceptable); } @@ -136,6 +140,7 @@ impl Localizer { Ok(()) } + /// Preprocesses the sensor data by calling the preprocessor and then runs the kalman filter pub fn iteration( &mut self, optical_data: Vec, @@ -172,13 +177,11 @@ mod tests { use super::*; #[test] - fn test_localizer_with_zeros() { + fn test_localizer_with_zeros() -> Result<(), PreprocessorError> { let mut localizer = Localizer::default(); let optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); - let raw_keyence_data: Vec = Vec::from_slice(&[0, 0]).unwrap(); - let raw_accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap(), @@ -188,10 +191,12 @@ mod tests { ]) .unwrap(); - localizer.iteration(optical_data, raw_keyence_data, raw_accelerometer_data); + localizer.iteration(optical_data, raw_keyence_data, raw_accelerometer_data)?; assert_eq!(localizer.displacement, 0.0); assert_eq!(localizer.velocity, 0.0); assert_eq!(localizer.acceleration, 0.0); + + Ok(()) } } From 604a43e1be4ee9f285a1e6d01b43aaf0bad4c978 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 11:52:53 +0000 Subject: [PATCH 50/99] RUST FORMAT --- lib/localisation/src/control/localizer.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 13c52995..0b66340c 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -196,7 +196,7 @@ mod tests { assert_eq!(localizer.displacement, 0.0); assert_eq!(localizer.velocity, 0.0); assert_eq!(localizer.acceleration, 0.0); - + Ok(()) } } From 105744163fde2a5d15173485d1ad64dd7fa8f648 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 12:12:30 +0000 Subject: [PATCH 51/99] test commit --- boards/stm32f767zi/src/bin/localizer.rs | 1 + 1 file changed, 1 insertion(+) create mode 100644 boards/stm32f767zi/src/bin/localizer.rs diff --git a/boards/stm32f767zi/src/bin/localizer.rs b/boards/stm32f767zi/src/bin/localizer.rs new file mode 100644 index 00000000..c767de88 --- /dev/null +++ b/boards/stm32f767zi/src/bin/localizer.rs @@ -0,0 +1 @@ +//test \ No newline at end of file From f08d8a733e95d5627d05f739da510a105f3f0afc Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 12:48:38 +0000 Subject: [PATCH 52/99] todo:read the sensors --- boards/stm32f767zi/src/bin/loc.rs | 52 +++++++++++++++++++++++++ boards/stm32f767zi/src/bin/localizer.rs | 1 - 2 files changed, 52 insertions(+), 1 deletion(-) create mode 100644 boards/stm32f767zi/src/bin/loc.rs delete mode 100644 boards/stm32f767zi/src/bin/localizer.rs diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs new file mode 100644 index 00000000..8fc830d8 --- /dev/null +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -0,0 +1,52 @@ +#![no_std] +#![no_main] + + +use defmt::*; +use embassy_executor::Spawner; +use embassy_time::{Duration, Timer}; + +use crate::{ + Localizer, + types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, +}; + +#[embassy_executor::main] +async fn main(_spawner: Spawner) -> ! { + info!("Starting localizer loop..."); + + let mut localizer = Localizer::new(); + + loop { + + //Simulated data + + let optical_data: Vec = Vec::from_slice(&[0.5, 0.5]).unwrap(); + + let keyence_data: Vec = Vec::from_slice(&[1, 1]).unwrap(); + + let accelerometer_data: RawAccelerometerData = + RawAccelerometerData::from_slice(&[ + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + ]) + .unwrap(); + + match localizer.iteration(optical_data, keyence_data, accelerometer_data) { + Ok(()) => { + info!( + "Iteration OK: displacement = {} m, velocity = {} m/s, acceleration = {} m/s²", + localizer.displacement, localizer.velocity, localizer.acceleration + ); + } + Err(e) => { + error!("Iteration error: {:?}", e); + } + } + + // Wait 100ms before the next update. + Timer::after(Duration::from_millis(100)).await; + } +} diff --git a/boards/stm32f767zi/src/bin/localizer.rs b/boards/stm32f767zi/src/bin/localizer.rs deleted file mode 100644 index c767de88..00000000 --- a/boards/stm32f767zi/src/bin/localizer.rs +++ /dev/null @@ -1 +0,0 @@ -//test \ No newline at end of file From 4ac1d795e0100f083b749594ed94aea209cf8bcb Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 13:04:20 +0000 Subject: [PATCH 53/99] now reading stripe counter --- boards/stm32f767zi/src/bin/loc.rs | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 8fc830d8..d666ee3b 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -5,25 +5,47 @@ use defmt::*; use embassy_executor::Spawner; use embassy_time::{Duration, Timer}; +use embassy_stm32::gpio::{Input, Pull}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; +use hyped_boards_stm32f767zi::tasks::read_keyence::read_keyence; use crate::{ Localizer, types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, }; +/// A Watch to hold the latest Keyence stripe count. +static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); + #[embassy_executor::main] async fn main(_spawner: Spawner) -> ! { + + + let p = init(Default::default()); + let gpio_pin = Input::new(p.PC13, Pull::Down); + + // Create a sender and a receiver for the Keyence stripe count. + let sender = KEYENCE_STRIPE_COUNT.sender(); + let mut receiver = KEYENCE_STRIPE_COUNT.receiver().unwrap(); + + spawner.spawn(read_keyence(gpio_pin, sender)).unwrap(); + + info!("Starting localizer loop..."); let mut localizer = Localizer::new(); loop { - //Simulated data + + // Wait for a new Keyence stripe count. + let new_stripe_count = receiver.get().await; + defmt::info!("New Keyence stripe count: {}", new_stripe_count); let optical_data: Vec = Vec::from_slice(&[0.5, 0.5]).unwrap(); - let keyence_data: Vec = Vec::from_slice(&[1, 1]).unwrap(); + // THERE SHOULD BE TWO STRIPE COUNTS ARGH + let keyence_data: Vec = Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); let accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ From 7fe19d4ee5d35d8bca323114af62b9161ea34335 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 13:09:38 +0000 Subject: [PATCH 54/99] fixex? --- boards/stm32f767zi/src/bin/loc.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index d666ee3b..63b1884c 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -1,13 +1,15 @@ #![no_std] #![no_main] - use defmt::*; -use embassy_executor::Spawner; use embassy_time::{Duration, Timer}; +use embassy_executor::Spawner; use embassy_stm32::gpio::{Input, Pull}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; use hyped_boards_stm32f767zi::tasks::read_keyence::read_keyence; +use panic_probe as _; + +use heapless::Vec; use crate::{ Localizer, From 3309d7ce15dfef7754015ee18eba706cd094d4c9 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 13:20:09 +0000 Subject: [PATCH 55/99] fixed some more errors --- boards/stm32f767zi/Cargo.toml | 1 + boards/stm32f767zi/src/bin/loc.rs | 6 ++---- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/boards/stm32f767zi/Cargo.toml b/boards/stm32f767zi/Cargo.toml index 790c4081..f711029d 100644 --- a/boards/stm32f767zi/Cargo.toml +++ b/boards/stm32f767zi/Cargo.toml @@ -13,6 +13,7 @@ embassy-futures = { version = "0.1.0", git = "https://github.com/embassy-rs/emba defmt = "0.3" defmt-rtt = "0.4" +heapless = "0.7" cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] } cortex-m-rt = "0.7.0" diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 63b1884c..e7380258 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -11,10 +11,8 @@ use panic_probe as _; use heapless::Vec; -use crate::{ - Localizer, - types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, -}; +use localisation::control::localizer::Localizer; +use localisation::types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}; /// A Watch to hold the latest Keyence stripe count. static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); From 0117d02c82a6f37a7614baabb85af919e034b376 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 13:29:22 +0000 Subject: [PATCH 56/99] PLEASE WORK I BEG YOU --- boards/stm32f767zi/Cargo.toml | 1 + boards/stm32f767zi/src/bin/loc.rs | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/boards/stm32f767zi/Cargo.toml b/boards/stm32f767zi/Cargo.toml index f711029d..25af4b12 100644 --- a/boards/stm32f767zi/Cargo.toml +++ b/boards/stm32f767zi/Cargo.toml @@ -26,6 +26,7 @@ static_cell = "2" hyped_core = { path = "../../lib/core" } hyped_sensors = { path = "../../lib/sensors" } +hyped_localisation = { path = "../../lib/localisation" } hyped_adc = { path = "../../lib/io/hyped_adc" } hyped_adc_derive = { path = "../../lib/io/hyped_adc/hyped_adc_derive" } diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index e7380258..8d0bfd30 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -11,8 +11,8 @@ use panic_probe as _; use heapless::Vec; -use localisation::control::localizer::Localizer; -use localisation::types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}; +use hyped_localisation::src::control::localizer::Localizer; +use hyped_localisation::src::types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}; /// A Watch to hold the latest Keyence stripe count. static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); From 63482d74ed3442ec682c453467257d780c9c06b3 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 13:39:46 +0000 Subject: [PATCH 57/99] fixed nostd errors --- lib/localisation/src/preprocessing/accelerometer.rs | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index f7ba3853..5f34f258 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -3,6 +3,7 @@ use crate::types::{ NUM_ALLOWED_ACCELEROMETER_OUTLIERS, NUM_AXIS, }; use heapless::Vec; +use libm; /// Stores the quartiles of the data and the bounds for outliers /// which are calculated from the quartiles @@ -120,9 +121,11 @@ impl AccelerometerPreprocessor { ) -> Option> { let accelerometer_data: AccelerometerData = data .iter() - .map(|axis| axis.iter().fold(0.0, |acc, val| acc + val * val).sqrt()) + .map(|axis| { + let sum = axis.iter().fold(0.0, |acc, val| acc + val * val); + libm::sqrtf(sum) + }) .collect(); - let clean_accelerometer_data = self.handle_outliers(accelerometer_data)?; if self.check_reliable() == SensorChecks::Unacceptable { @@ -157,7 +160,7 @@ impl AccelerometerPreprocessor { pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { let mut sorted_data = data.clone(); - sorted_data.sort_by(|a, b| a.partial_cmp(b).unwrap()); + sorted_data.as_mut_slice().sort_by(|a, b| a.partial_cmp(b).unwrap()); let quartile_keys: Vec = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap(); let quartiles: Vec = quartile_keys @@ -165,8 +168,8 @@ impl AccelerometerPreprocessor { .map(|quartile| { let index_quartile: f32 = (1.0 + self.num_reliable_accelerometers as f32) * quartile; - let index_quartile_floor = index_quartile.floor() as usize - 1; - let index_quartile_ceil = index_quartile.ceil() as usize - 1; + let index_quartile_floor = libm::floorf(index_quartile) as usize - 1; + let index_quartile_ceil = libm::ceilf(index_quartile) as usize - 1; (data.get(index_quartile_floor).unwrap_or(&0.0) + data.get(index_quartile_ceil).unwrap_or(&0.0)) From 719e0be21058cd8557d1096991364555c28bd3ac Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 13:40:45 +0000 Subject: [PATCH 58/99] fmt.... --- lib/localisation/src/preprocessing/accelerometer.rs | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 5f34f258..b59f8cca 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -160,7 +160,9 @@ impl AccelerometerPreprocessor { pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { let mut sorted_data = data.clone(); - sorted_data.as_mut_slice().sort_by(|a, b| a.partial_cmp(b).unwrap()); + sorted_data + .as_mut_slice() + .sort_by(|a, b| a.partial_cmp(b).unwrap()); let quartile_keys: Vec = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap(); let quartiles: Vec = quartile_keys @@ -168,8 +170,8 @@ impl AccelerometerPreprocessor { .map(|quartile| { let index_quartile: f32 = (1.0 + self.num_reliable_accelerometers as f32) * quartile; - let index_quartile_floor = libm::floorf(index_quartile) as usize - 1; - let index_quartile_ceil = libm::ceilf(index_quartile) as usize - 1; + let index_quartile_floor = libm::floorf(index_quartile) as usize - 1; + let index_quartile_ceil = libm::ceilf(index_quartile) as usize - 1; (data.get(index_quartile_floor).unwrap_or(&0.0) + data.get(index_quartile_ceil).unwrap_or(&0.0)) From ecf9526531181edc555422fee4d2811ded8b0d0c Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 13:47:56 +0000 Subject: [PATCH 59/99] WORKWORKWORK --- lib/localisation/src/preprocessing/accelerometer.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index b59f8cca..d389a835 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -162,7 +162,7 @@ impl AccelerometerPreprocessor { let mut sorted_data = data.clone(); sorted_data .as_mut_slice() - .sort_by(|a, b| a.partial_cmp(b).unwrap()); + .sort_unstable_by(|a, b| a.partial_cmp(b).unwrap()); let quartile_keys: Vec = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap(); let quartiles: Vec = quartile_keys From ea6d098367626489b317434bd62135b6272daaec Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 15:03:20 +0000 Subject: [PATCH 60/99] changed path --- boards/stm32f767zi/src/bin/loc.rs | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 8d0bfd30..f769fcaf 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -2,42 +2,39 @@ #![no_main] use defmt::*; -use embassy_time::{Duration, Timer}; use embassy_executor::Spawner; use embassy_stm32::gpio::{Input, Pull}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; +use embassy_time::{Duration, Timer}; use hyped_boards_stm32f767zi::tasks::read_keyence::read_keyence; use panic_probe as _; use heapless::Vec; -use hyped_localisation::src::control::localizer::Localizer; -use hyped_localisation::src::types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}; +use hyped_localisation::{ + control::localizer::Localizer, + types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, +}; /// A Watch to hold the latest Keyence stripe count. static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); #[embassy_executor::main] async fn main(_spawner: Spawner) -> ! { - - let p = init(Default::default()); let gpio_pin = Input::new(p.PC13, Pull::Down); - + // Create a sender and a receiver for the Keyence stripe count. let sender = KEYENCE_STRIPE_COUNT.sender(); let mut receiver = KEYENCE_STRIPE_COUNT.receiver().unwrap(); - - spawner.spawn(read_keyence(gpio_pin, sender)).unwrap(); + spawner.spawn(read_keyence(gpio_pin, sender)).unwrap(); info!("Starting localizer loop..."); let mut localizer = Localizer::new(); loop { - - // Wait for a new Keyence stripe count. let new_stripe_count = receiver.get().await; defmt::info!("New Keyence stripe count: {}", new_stripe_count); @@ -45,7 +42,8 @@ async fn main(_spawner: Spawner) -> ! { let optical_data: Vec = Vec::from_slice(&[0.5, 0.5]).unwrap(); // THERE SHOULD BE TWO STRIPE COUNTS ARGH - let keyence_data: Vec = Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); + let keyence_data: Vec = + Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); let accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ From fa747599271db72733c9644ff852135e21b287a2 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 15:18:52 +0000 Subject: [PATCH 61/99] small fixes to imports and such --- boards/stm32f767zi/src/bin/loc.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index f769fcaf..59967fbc 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -3,7 +3,7 @@ use defmt::*; use embassy_executor::Spawner; -use embassy_stm32::gpio::{Input, Pull}; +use embassy_stm32::{init, gpio::{Input, Pull}}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; use embassy_time::{Duration, Timer}; use hyped_boards_stm32f767zi::tasks::read_keyence::read_keyence; @@ -20,7 +20,7 @@ use hyped_localisation::{ static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); #[embassy_executor::main] -async fn main(_spawner: Spawner) -> ! { +async fn main(spawner: Spawner) -> ! { let p = init(Default::default()); let gpio_pin = Input::new(p.PC13, Pull::Down); From 490337ee25ee9f712a391bf6e7e191415cf93d46 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 15:32:47 +0000 Subject: [PATCH 62/99] fixed some issues checking --- boards/stm32f767zi/Cargo.toml | 2 +- boards/stm32f767zi/src/bin/loc.rs | 22 +++++++++++----------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/boards/stm32f767zi/Cargo.toml b/boards/stm32f767zi/Cargo.toml index 25af4b12..b95e5727 100644 --- a/boards/stm32f767zi/Cargo.toml +++ b/boards/stm32f767zi/Cargo.toml @@ -13,7 +13,7 @@ embassy-futures = { version = "0.1.0", git = "https://github.com/embassy-rs/emba defmt = "0.3" defmt-rtt = "0.4" -heapless = "0.7" +heapless = "0.8.0" cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] } cortex-m-rt = "0.7.0" diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 59967fbc..a2dd62b4 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -19,8 +19,10 @@ use hyped_localisation::{ /// A Watch to hold the latest Keyence stripe count. static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); + #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { + // Import `init` so that we can initialize board peripherals. let p = init(Default::default()); let gpio_pin = Input::new(p.PC13, Pull::Down); @@ -39,12 +41,9 @@ async fn main(spawner: Spawner) -> ! { let new_stripe_count = receiver.get().await; defmt::info!("New Keyence stripe count: {}", new_stripe_count); + // Create the sensor data. (Optical and accelerometer data are simulated.) let optical_data: Vec = Vec::from_slice(&[0.5, 0.5]).unwrap(); - - // THERE SHOULD BE TWO STRIPE COUNTS ARGH - let keyence_data: Vec = - Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); - + let keyence_data: Vec = Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); let accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), @@ -56,17 +55,18 @@ async fn main(spawner: Spawner) -> ! { match localizer.iteration(optical_data, keyence_data, accelerometer_data) { Ok(()) => { - info!( - "Iteration OK: displacement = {} m, velocity = {} m/s, acceleration = {} m/s²", - localizer.displacement, localizer.velocity, localizer.acceleration + defmt::info!( + "Iteration OK: displacement = {} m, velocity = {} m/s, acceleration = {} m/s**2", + localizer.displacement(), + localizer.velocity(), + localizer.acceleration() ); } Err(e) => { - error!("Iteration error: {:?}", e); + defmt::error!("Iteration error: {:?}", e); } } - // Wait 100ms before the next update. Timer::after(Duration::from_millis(100)).await; } -} +} \ No newline at end of file From 693adb0b3dbd252cc68efd097c6fc96738179a26 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 15:44:41 +0000 Subject: [PATCH 63/99] added getters --- lib/localisation/src/control/localizer.rs | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 0b66340c..77527ee9 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -172,6 +172,21 @@ impl Localizer { } } +// Getters +impl Localizer { + pub fn displacement(&self) -> f64 { + self.displacement + } + + pub fn velocity(&self) -> f64 { + self.velocity + } + + pub fn acceleration(&self) -> f64 { + self.acceleration + } +} + #[cfg(test)] mod tests { use super::*; @@ -193,9 +208,9 @@ mod tests { localizer.iteration(optical_data, raw_keyence_data, raw_accelerometer_data)?; - assert_eq!(localizer.displacement, 0.0); - assert_eq!(localizer.velocity, 0.0); - assert_eq!(localizer.acceleration, 0.0); + assert_eq!(localizer.displacement(), 0.0); + assert_eq!(localizer.velocity(), 0.0); + assert_eq!(localizer.acceleration(), 0.0); Ok(()) } From 8b3ac47410cf799f612cd0a64d32a29cfad88a20 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 15:51:12 +0000 Subject: [PATCH 64/99] PLEASE WORK PLEASE: added demfr lib --- lib/localisation/Cargo.toml | 3 ++- lib/localisation/src/control/localizer.rs | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index d65a7943..762e6af3 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -6,4 +6,5 @@ edition = "2021" [dependencies] nalgebra = { version = "0.33.0", default-features = false, features = ["libm"] } heapless = "0.8.0" -libm = "0.2.11" \ No newline at end of file +libm = "0.2.11" +defmt = { version = "0.3.10", features = ["derive"] } \ No newline at end of file diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 77527ee9..ee120c4f 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -93,7 +93,7 @@ impl Default for Localizer { } } -#[derive(Debug)] +#[derive(Debug, defmt::Format)] pub enum PreprocessorError { KeyenceUnacceptable, AccelerometerUnnaceptable, From f687d3e321f12352c21d52a78ab29259fe90bd20 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 15:53:34 +0000 Subject: [PATCH 65/99] wrong version of defmt --- Cargo.lock | 1 + lib/localisation/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/Cargo.lock b/Cargo.lock index 40f143f0..9eb26d17 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -362,6 +362,7 @@ dependencies = [ name = "hyped_localisation" version = "0.1.0" dependencies = [ + "defmt", "heapless", "libm", "nalgebra", diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index 762e6af3..4aebd94c 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -7,4 +7,4 @@ edition = "2021" nalgebra = { version = "0.33.0", default-features = false, features = ["libm"] } heapless = "0.8.0" libm = "0.2.11" -defmt = { version = "0.3.10", features = ["derive"] } \ No newline at end of file +defmt = "0.3" \ No newline at end of file From 59d3957ed04bc801fdfa47b232aa4ba542c0ffdc Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 16:06:17 +0000 Subject: [PATCH 66/99] fmt --- boards/stm32f767zi/src/bin/loc.rs | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index a2dd62b4..afaeee5f 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -3,7 +3,10 @@ use defmt::*; use embassy_executor::Spawner; -use embassy_stm32::{init, gpio::{Input, Pull}}; +use embassy_stm32::{ + gpio::{Input, Pull}, + init, +}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; use embassy_time::{Duration, Timer}; use hyped_boards_stm32f767zi::tasks::read_keyence::read_keyence; @@ -19,7 +22,6 @@ use hyped_localisation::{ /// A Watch to hold the latest Keyence stripe count. static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); - #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { // Import `init` so that we can initialize board peripherals. @@ -43,7 +45,8 @@ async fn main(spawner: Spawner) -> ! { // Create the sensor data. (Optical and accelerometer data are simulated.) let optical_data: Vec = Vec::from_slice(&[0.5, 0.5]).unwrap(); - let keyence_data: Vec = Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); + let keyence_data: Vec = + Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); let accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), @@ -69,4 +72,4 @@ async fn main(spawner: Spawner) -> ! { Timer::after(Duration::from_millis(100)).await; } -} \ No newline at end of file +} From 0a67808ff729a245eb5aad87e1bbe4912c4b671a Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Sun, 9 Feb 2025 17:00:07 +0000 Subject: [PATCH 67/99] addded second stripe counter --- boards/stm32f767zi/src/bin/loc.rs | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index afaeee5f..b6310a31 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -20,7 +20,8 @@ use hyped_localisation::{ }; /// A Watch to hold the latest Keyence stripe count. -static KEYENCE_STRIPE_COUNT: Watch = Watch::new(); +static KEYENCE_1_STRIPE_COUNT: Watch = Watch::new(); +static KEYENCE_2_STRIPE_COUNT: Watch = Watch::new(); #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { @@ -29,10 +30,13 @@ async fn main(spawner: Spawner) -> ! { let gpio_pin = Input::new(p.PC13, Pull::Down); // Create a sender and a receiver for the Keyence stripe count. - let sender = KEYENCE_STRIPE_COUNT.sender(); - let mut receiver = KEYENCE_STRIPE_COUNT.receiver().unwrap(); + let sender1 = KEYENCE_1_STRIPE_COUNT.sender(); + let mut receiver1 = KEYENCE_1_STRIPE_COUNT.receiver().unwrap(); + let sender2 = KEYENCE_2_STRIPE_COUNT.sender(); + let mut receiver2 = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); - spawner.spawn(read_keyence(gpio_pin, sender)).unwrap(); + spawner.spawn(read_keyence(gpio_pin, sender1)).unwrap(); + spawner.spawn(read_keyence(gpio_pin, sender2)).unwrap(); info!("Starting localizer loop..."); @@ -40,13 +44,16 @@ async fn main(spawner: Spawner) -> ! { loop { // Wait for a new Keyence stripe count. - let new_stripe_count = receiver.get().await; - defmt::info!("New Keyence stripe count: {}", new_stripe_count); + let (stripe_count1, stripe_count2) = join!(receiver1.get(), receiver2.get()); + defmt::info!( + "New Keyence stripe counts: sensor1 = {}, sensor2 = {}", + stripe_count1, + stripe_count2 + ); // Create the sensor data. (Optical and accelerometer data are simulated.) let optical_data: Vec = Vec::from_slice(&[0.5, 0.5]).unwrap(); - let keyence_data: Vec = - Vec::from_slice(&[new_stripe_count, new_stripe_count]).unwrap(); + let keyence_data: Vec = Vec::from_slice(&[stripe_count1, stripe_count2]).unwrap(); let accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), From 4ef8799af44d66b3ad4113295d120ae56dc6fbe4 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 13 Feb 2025 18:15:21 +0000 Subject: [PATCH 68/99] small fix, only continues when both keyence recieved --- boards/stm32f767zi/src/bin/loc.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index b6310a31..e5ca76a0 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -44,7 +44,9 @@ async fn main(spawner: Spawner) -> ! { loop { // Wait for a new Keyence stripe count. - let (stripe_count1, stripe_count2) = join!(receiver1.get(), receiver2.get()); + let stripe_count1 = receiver1.get().await; + let stripe_count2 = receiver2.get().await; + defmt::info!( "New Keyence stripe counts: sensor1 = {}, sensor2 = {}", stripe_count1, From b397c8429644d77c7618ead00354ea54ffa59380 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 13 Feb 2025 18:19:43 +0000 Subject: [PATCH 69/99] small fix --- boards/stm32f767zi/src/bin/loc.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index e5ca76a0..db2082f7 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -35,8 +35,8 @@ async fn main(spawner: Spawner) -> ! { let sender2 = KEYENCE_2_STRIPE_COUNT.sender(); let mut receiver2 = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); - spawner.spawn(read_keyence(gpio_pin, sender1)).unwrap(); - spawner.spawn(read_keyence(gpio_pin, sender2)).unwrap(); + spawner.spawn(read_keyence(gpio_pin.clone(), sender1)).unwrap(); + spawner.spawn(read_keyence(gpio_pin.clone(), sender2)).unwrap(); info!("Starting localizer loop..."); From 41511847f242d62fd48440c3168d2ed89ca62386 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 13 Feb 2025 18:22:42 +0000 Subject: [PATCH 70/99] fmt --- boards/stm32f767zi/src/bin/loc.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index db2082f7..fe703b82 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -35,8 +35,12 @@ async fn main(spawner: Spawner) -> ! { let sender2 = KEYENCE_2_STRIPE_COUNT.sender(); let mut receiver2 = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); - spawner.spawn(read_keyence(gpio_pin.clone(), sender1)).unwrap(); - spawner.spawn(read_keyence(gpio_pin.clone(), sender2)).unwrap(); + spawner + .spawn(read_keyence(gpio_pin.clone(), sender1)) + .unwrap(); + spawner + .spawn(read_keyence(gpio_pin.clone(), sender2)) + .unwrap(); info!("Starting localizer loop..."); From 51e1ea213827180c6633ac146014807493b3a0ae Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 13 Feb 2025 18:28:45 +0000 Subject: [PATCH 71/99] changed to 2 pins --- boards/stm32f767zi/src/bin/loc.rs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index fe703b82..99ca8d92 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -27,7 +27,8 @@ static KEYENCE_2_STRIPE_COUNT: Watch = Watch::n async fn main(spawner: Spawner) -> ! { // Import `init` so that we can initialize board peripherals. let p = init(Default::default()); - let gpio_pin = Input::new(p.PC13, Pull::Down); + let gpio_pin1 = Input::new(p.PC13, Pull::Down); + let gpio_pin2 = Input::new(p.PC13, Pull::Down); // Create a sender and a receiver for the Keyence stripe count. let sender1 = KEYENCE_1_STRIPE_COUNT.sender(); @@ -36,10 +37,10 @@ async fn main(spawner: Spawner) -> ! { let mut receiver2 = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); spawner - .spawn(read_keyence(gpio_pin.clone(), sender1)) + .spawn(read_keyence(gpio_pin1, sender1)) .unwrap(); spawner - .spawn(read_keyence(gpio_pin.clone(), sender2)) + .spawn(read_keyence(gpio_pin2, sender2)) .unwrap(); info!("Starting localizer loop..."); From f404986f63e38179e23fd120d2fc44e0530919c3 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 13 Feb 2025 18:32:27 +0000 Subject: [PATCH 72/99] pin change --- boards/stm32f767zi/src/bin/loc.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 99ca8d92..eb650331 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -28,7 +28,7 @@ async fn main(spawner: Spawner) -> ! { // Import `init` so that we can initialize board peripherals. let p = init(Default::default()); let gpio_pin1 = Input::new(p.PC13, Pull::Down); - let gpio_pin2 = Input::new(p.PC13, Pull::Down); + let gpio_pin2 = Input::new(p.PC14, Pull::Down); // Create a sender and a receiver for the Keyence stripe count. let sender1 = KEYENCE_1_STRIPE_COUNT.sender(); From 136281bb682e9d3fea599c5e1f91a9047d167423 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:06:05 +0000 Subject: [PATCH 73/99] what? --- lib/localisation/Cargo.toml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index 76bf1e75..dc1ac651 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -7,7 +7,11 @@ edition = "2021" nalgebra = { version = "0.33.0", default-features = false, features = ["libm"] } heapless = "0.8.0" libm = "0.2.11" +<<<<<<< HEAD +defmt = "0.3" +======= defmt = "0.3" [dev-dependencies] defmt = { version = "0.3", features = ["unstable-test"] } +>>>>>>> refs/remotes/origin/main From 1ed26b169afdc8842bf71c99ef33f10fe5771a10 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:08:54 +0000 Subject: [PATCH 74/99] unsure --- lib/localisation/Cargo.toml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index dc1ac651..76bf1e75 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -7,11 +7,7 @@ edition = "2021" nalgebra = { version = "0.33.0", default-features = false, features = ["libm"] } heapless = "0.8.0" libm = "0.2.11" -<<<<<<< HEAD -defmt = "0.3" -======= defmt = "0.3" [dev-dependencies] defmt = { version = "0.3", features = ["unstable-test"] } ->>>>>>> refs/remotes/origin/main From 8d6791cc66302fcb88123b1a85f32d8751bba7c6 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:14:27 +0000 Subject: [PATCH 75/99] libm functions --- .../src/preprocessing/accelerometer.rs | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index dd4fe716..25e4fced 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -121,7 +121,7 @@ impl AccelerometerPreprocessor { ) -> Option> { let accelerometer_data: AccelerometerData = data .iter() - .map(|axis| axis.iter().fold(0.0, |acc, val| acc + val * val).sqrt()) + .map(|axis| libm::sqrtf32(axis.iter().fold(0.0, |acc, val| acc + val * val))) .collect(); let clean_accelerometer_data = self.handle_outliers(accelerometer_data)?; @@ -165,8 +165,8 @@ impl AccelerometerPreprocessor { .map(|quartile| { let index_quartile: f32 = (1.0 + self.num_reliable_accelerometers as f32) * quartile; - let index_quartile_floor = index_quartile.floor() as usize - 1; - let index_quartile_ceil = index_quartile.ceil() as usize - 1; + let index_quartile_floor = libm::floorf32(index_quartile) as usize - 1; + let index_quartile_ceil = libm::ceilf32(index_quartile) as usize - 1; (data.get(index_quartile_floor).unwrap_or(&0.0) + data.get(index_quartile_ceil).unwrap_or(&0.0)) @@ -186,6 +186,7 @@ impl AccelerometerPreprocessor { #[cfg(test)] mod tests { use super::*; + use libm; #[test] pub fn test_process_data() { @@ -203,10 +204,10 @@ mod tests { let raw_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ - Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) = 3.74 - Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // sqrt(77) = 8.77 - Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), // sqrt(194) = 13.93 - Vec::from_slice(&[10.0, 11.0, 12.0]).unwrap(), // sqrt(365) = 19.1 + Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) ≈ 3.74 + Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // sqrt(77) ≈ 8.77 + Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), // sqrt(194) ≈ 13.93 + Vec::from_slice(&[10.0, 11.0, 12.0]).unwrap(), // sqrt(365) ≈ 19.1 ]) .unwrap(); @@ -214,10 +215,10 @@ mod tests { assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); - assert_eq!(processed_data[0], (14.0_f32).sqrt()); - assert_eq!(processed_data[1], (77.0_f32).sqrt()); - assert_eq!(processed_data[2], (194.0_f32).sqrt()); - assert_eq!(processed_data[3], (365.0_f32).sqrt()); + assert_eq!(processed_data[0], libm::sqrtf32(14.0_f32)); + assert_eq!(processed_data[1], libm::sqrtf32(77.0_f32)); + assert_eq!(processed_data[2], libm::sqrtf32(194.0_f32)); + assert_eq!(processed_data[3], libm::sqrtf32(365.0_f32)); } #[test] @@ -239,11 +240,10 @@ mod tests { let raw_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ - Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) = 3.74 - Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // sqrt(Median (3.74, 13.93, - // 19.1)) = 13.93 - Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), // sqrt(194) = 13.93 - Vec::from_slice(&[10.0, 11.0, 12.0]).unwrap(), // sqrt(365) = 19.1 + Vec::from_slice(&[1.0, 2.0, 3.0]).unwrap(), // sqrt(14) ≈ 3.74 + Vec::from_slice(&[4.0, 5.0, 6.0]).unwrap(), // replaced with median (from sqrt(14), sqrt(194), sqrt(365)) + Vec::from_slice(&[7.0, 8.0, 9.0]).unwrap(), // sqrt(194) ≈ 13.93 + Vec::from_slice(&[10.0, 11.0, 12.0]).unwrap(), // sqrt(365) ≈ 19.1 ]) .unwrap(); @@ -251,10 +251,10 @@ mod tests { assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); - assert_eq!(processed_data[0], (14.0_f32).sqrt()); - assert_eq!(processed_data[1], (194.0_f32).sqrt()); - assert_eq!(processed_data[2], (194.0_f32).sqrt()); - assert_eq!(processed_data[3], (365.0_f32).sqrt()); + assert_eq!(processed_data[0], libm::sqrtf32(14.0_f32)); + assert_eq!(processed_data[1], libm::sqrtf32(194.0_f32)); + assert_eq!(processed_data[2], libm::sqrtf32(194.0_f32)); + assert_eq!(processed_data[3], libm::sqrtf32(365.0_f32)); } #[test] From 0d53f1792359b479ad7133eab715c08eeb2a2a1e Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:17:43 +0000 Subject: [PATCH 76/99] libm fixes --- lib/control/src/pneumatics.rs | 3 +-- lib/localisation/src/preprocessing/accelerometer.rs | 8 +++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/lib/control/src/pneumatics.rs b/lib/control/src/pneumatics.rs index 4251b061..b0d2db73 100644 --- a/lib/control/src/pneumatics.rs +++ b/lib/control/src/pneumatics.rs @@ -1,5 +1,4 @@ -use embassy_time::with_timeout; -use embassy_time::Duration; +use embassy_time::{with_timeout, Duration}; use hyped_gpio::HypedGpioOutputPin; use hyped_i2c::HypedI2c; use hyped_sensors::{ diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 25e4fced..06a9b705 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -157,7 +157,9 @@ impl AccelerometerPreprocessor { pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { let mut sorted_data = data.clone(); - sorted_data.sort_by(|a, b| a.partial_cmp(b).unwrap()); + sorted_data + .as_mut_slice() + .sort_by(|a, b| a.partial_cmp(b).unwrap()); let quartile_keys: Vec = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap(); let quartiles: Vec = quartile_keys @@ -165,8 +167,8 @@ impl AccelerometerPreprocessor { .map(|quartile| { let index_quartile: f32 = (1.0 + self.num_reliable_accelerometers as f32) * quartile; - let index_quartile_floor = libm::floorf32(index_quartile) as usize - 1; - let index_quartile_ceil = libm::ceilf32(index_quartile) as usize - 1; + let index_quartile_floor = libm::floorf(index_quartile) as usize - 1; + let index_quartile_ceil = libm::ceilf(index_quartile) as usize - 1; (data.get(index_quartile_floor).unwrap_or(&0.0) + data.get(index_quartile_ceil).unwrap_or(&0.0)) From d9bb13e421743f784cc9afe97febbd1a3fe6ba96 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:26:52 +0000 Subject: [PATCH 77/99] fixed libm and sort in accelerometre --- .../src/preprocessing/accelerometer.rs | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 06a9b705..9e8ef843 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -121,7 +121,7 @@ impl AccelerometerPreprocessor { ) -> Option> { let accelerometer_data: AccelerometerData = data .iter() - .map(|axis| libm::sqrtf32(axis.iter().fold(0.0, |acc, val| acc + val * val))) + .map(|axis| libm::sqrtf(axis.iter().fold(0.0, |acc, val| acc + val * val))) .collect(); let clean_accelerometer_data = self.handle_outliers(accelerometer_data)?; @@ -217,10 +217,10 @@ mod tests { assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); - assert_eq!(processed_data[0], libm::sqrtf32(14.0_f32)); - assert_eq!(processed_data[1], libm::sqrtf32(77.0_f32)); - assert_eq!(processed_data[2], libm::sqrtf32(194.0_f32)); - assert_eq!(processed_data[3], libm::sqrtf32(365.0_f32)); + assert_eq!(processed_data[0], libm::sqrtf(14.0_f32)); + assert_eq!(processed_data[1], libm::sqrtf(77.0_f32)); + assert_eq!(processed_data[2], libm::sqrtf(194.0_f32)); + assert_eq!(processed_data[3], libm::sqrtf(365.0_f32)); } #[test] @@ -253,10 +253,10 @@ mod tests { assert!(processed_data.is_some()); let processed_data = processed_data.unwrap(); - assert_eq!(processed_data[0], libm::sqrtf32(14.0_f32)); - assert_eq!(processed_data[1], libm::sqrtf32(194.0_f32)); - assert_eq!(processed_data[2], libm::sqrtf32(194.0_f32)); - assert_eq!(processed_data[3], libm::sqrtf32(365.0_f32)); + assert_eq!(processed_data[0], libm::sqrtf(14.0_f32)); + assert_eq!(processed_data[1], libm::sqrtf(194.0_f32)); + assert_eq!(processed_data[2], libm::sqrtf(194.0_f32)); + assert_eq!(processed_data[3], libm::sqrtf(365.0_f32)); } #[test] From bfcfc7d0b5813908b1b49263f500d8206b126bfe Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:28:21 +0000 Subject: [PATCH 78/99] PLEAE --- lib/localisation/src/preprocessing/accelerometer.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 9e8ef843..812f0c18 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -159,7 +159,7 @@ impl AccelerometerPreprocessor { let mut sorted_data = data.clone(); sorted_data .as_mut_slice() - .sort_by(|a, b| a.partial_cmp(b).unwrap()); + .is_sorted_by(|a, b| a.partial_cmp(b).unwrap()); let quartile_keys: Vec = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap(); let quartiles: Vec = quartile_keys From a9aee1f32e1c5559eae6ce482450d39b3815eeb4 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:31:02 +0000 Subject: [PATCH 79/99] heapless fix --- lib/localisation/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index 76bf1e75..8f3be879 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -5,7 +5,7 @@ edition = "2021" [dependencies] nalgebra = { version = "0.33.0", default-features = false, features = ["libm"] } -heapless = "0.8.0" +heapless = { version = "0.8", default-features = false, features = ["serde"]} libm = "0.2.11" defmt = "0.3" From 820ae49662dd51b41f7d11c73bfc3529617c1c40 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:38:07 +0000 Subject: [PATCH 80/99] fixed sort --- .../src/preprocessing/accelerometer.rs | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 812f0c18..f99a03da 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -156,25 +156,25 @@ impl AccelerometerPreprocessor { } pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { + // Clone and sort data let mut sorted_data = data.clone(); sorted_data .as_mut_slice() - .is_sorted_by(|a, b| a.partial_cmp(b).unwrap()); + .sort_by(|a, b| a.partial_cmp(b).unwrap()); - let quartile_keys: Vec = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap(); - let quartiles: Vec = quartile_keys - .iter() - .map(|quartile| { - let index_quartile: f32 = - (1.0 + self.num_reliable_accelerometers as f32) * quartile; - let index_quartile_floor = libm::floorf(index_quartile) as usize - 1; - let index_quartile_ceil = libm::ceilf(index_quartile) as usize - 1; - - (data.get(index_quartile_floor).unwrap_or(&0.0) - + data.get(index_quartile_ceil).unwrap_or(&0.0)) - / 2.0 - }) - .collect(); + let quartile_keys: [f32; 3] = [0.25, 0.5, 0.75]; + let mut quartiles: [f32; 3] = [0.0; 3]; + + for (i, &quartile) in quartile_keys.iter().enumerate() { + let index_quartile = (1.0 + self.num_reliable_accelerometers as f32) * quartile; + + let index_quartile_floor = libm::floorf(index_quartile) as usize - 1; + let index_quartile_ceil = libm::ceilf(index_quartile) as usize - 1; + + quartiles[i] = (data.get(index_quartile_floor).unwrap_or(&0.0) + + data.get(index_quartile_ceil).unwrap_or(&0.0)) + / 2.0; + } Quartiles::new( quartiles[0], From d3b6c67a387663757bebeecee14d305b57efcd69 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:49:08 +0000 Subject: [PATCH 81/99] shoddy temprary sorting fix --- .../src/preprocessing/accelerometer.rs | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index f99a03da..a7896158 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -18,6 +18,19 @@ pub struct Quartiles { upper_bound: f32, } + +fn insertion_sort(slice: &mut [f32]) { + for i in 1..slice.len() { + let key = slice[i]; + let mut j = i; + while j > 0 && slice[j - 1] > key { + slice[j] = slice[j - 1]; + j -= 1; + } + slice[j] = key; + } +} + /// Implementation of the Quartiles struct which calculates the bounds for outliers impl Quartiles { pub fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { @@ -158,9 +171,7 @@ impl AccelerometerPreprocessor { pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { // Clone and sort data let mut sorted_data = data.clone(); - sorted_data - .as_mut_slice() - .sort_by(|a, b| a.partial_cmp(b).unwrap()); + insertion_sort(sorted_data.as_mut_slice()); let quartile_keys: [f32; 3] = [0.25, 0.5, 0.75]; let mut quartiles: [f32; 3] = [0.0; 3]; From 404a80000775252ff479b803ce7cc1e7b2f8918a Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:49:27 +0000 Subject: [PATCH 82/99] fmt --- lib/localisation/src/preprocessing/accelerometer.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index a7896158..1e6c798c 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,7 +2,6 @@ use crate::types::{ AccelerometerData, RawAccelerometerData, SensorChecks, NUM_ACCELEROMETERS, NUM_ALLOWED_ACCELEROMETER_OUTLIERS, NUM_AXIS, }; -use heapless::Vec; use libm; /// Stores the quartiles of the data and the bounds for outliers @@ -18,7 +17,6 @@ pub struct Quartiles { upper_bound: f32, } - fn insertion_sort(slice: &mut [f32]) { for i in 1..slice.len() { let key = slice[i]; From 1431e2ca74dbb450268b4036da47bcf4c8511eb4 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Feb 2025 18:49:41 +0000 Subject: [PATCH 83/99] fmt --- lib/localisation/src/preprocessing/accelerometer.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 1e6c798c..a7896158 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,6 +2,7 @@ use crate::types::{ AccelerometerData, RawAccelerometerData, SensorChecks, NUM_ACCELEROMETERS, NUM_ALLOWED_ACCELEROMETER_OUTLIERS, NUM_AXIS, }; +use heapless::Vec; use libm; /// Stores the quartiles of the data and the bounds for outliers @@ -17,6 +18,7 @@ pub struct Quartiles { upper_bound: f32, } + fn insertion_sort(slice: &mut [f32]) { for i in 1..slice.len() { let key = slice[i]; From c4f8ba9832fb548f335fbb34d1b06b7f5ea54d62 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 6 Mar 2025 18:33:03 +0000 Subject: [PATCH 84/99] fmt --- lib/localisation/src/preprocessing/accelerometer.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index a7896158..5e401aaf 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -3,6 +3,7 @@ use crate::types::{ NUM_ALLOWED_ACCELEROMETER_OUTLIERS, NUM_AXIS, }; use heapless::Vec; +#[cfg(test)] use libm; /// Stores the quartiles of the data and the bounds for outliers @@ -18,7 +19,6 @@ pub struct Quartiles { upper_bound: f32, } - fn insertion_sort(slice: &mut [f32]) { for i in 1..slice.len() { let key = slice[i]; From fb4bd934365a8cc15ce784d0a2a80262dcf57229 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 6 Mar 2025 18:35:17 +0000 Subject: [PATCH 85/99] fmt and heapless fix --- boards/stm32f767zi/Cargo.toml | 1 - lib/localisation/Cargo.toml | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/stm32f767zi/Cargo.toml b/boards/stm32f767zi/Cargo.toml index 41655498..3a6c5dc6 100644 --- a/boards/stm32f767zi/Cargo.toml +++ b/boards/stm32f767zi/Cargo.toml @@ -24,7 +24,6 @@ critical-section = "1.1" embedded-storage = "0.3.1" static_cell = "2" -heapless = { version = "0.8", default-features = false, features = ["serde"]} rust-mqtt = { version = "0.3.0", default-features = false, features = ["defmt"] } serde = { version = "1.0", default-features = false, features = ["derive"] } serde-json-core = "0.1.0" diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index 8f3be879..4f6300f1 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -5,9 +5,10 @@ edition = "2021" [dependencies] nalgebra = { version = "0.33.0", default-features = false, features = ["libm"] } -heapless = { version = "0.8", default-features = false, features = ["serde"]} + libm = "0.2.11" defmt = "0.3" +heapless = { version = "0.8", default-features = false, features = ["serde"]} [dev-dependencies] defmt = { version = "0.3", features = ["unstable-test"] } From 6673a158cd6f3dba08705974bd9cd16744330ceb Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 6 Mar 2025 18:41:31 +0000 Subject: [PATCH 86/99] board fmt --- boards/stm32f767zi/src/bin/loc.rs | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index eb650331..1d42163a 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -36,12 +36,8 @@ async fn main(spawner: Spawner) -> ! { let sender2 = KEYENCE_2_STRIPE_COUNT.sender(); let mut receiver2 = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); - spawner - .spawn(read_keyence(gpio_pin1, sender1)) - .unwrap(); - spawner - .spawn(read_keyence(gpio_pin2, sender2)) - .unwrap(); + spawner.spawn(read_keyence(gpio_pin1, sender1)).unwrap(); + spawner.spawn(read_keyence(gpio_pin2, sender2)).unwrap(); info!("Starting localizer loop..."); From 7e10dbb40b176160554529765d4ba4914ed05e43 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 6 Mar 2025 18:46:23 +0000 Subject: [PATCH 87/99] clippy will dislike this --- lib/localisation/src/preprocessing/accelerometer.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 5e401aaf..94301332 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,6 +2,9 @@ use crate::types::{ AccelerometerData, RawAccelerometerData, SensorChecks, NUM_ACCELEROMETERS, NUM_ALLOWED_ACCELEROMETER_OUTLIERS, NUM_AXIS, }; + +/// For tests to work +#[allow(unused_imports)] use heapless::Vec; #[cfg(test)] use libm; From d185e8d484d2b83f525fc63f4099cfa979d0e446 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 6 Mar 2025 18:55:35 +0000 Subject: [PATCH 88/99] fixed sorting issue! --- .../src/preprocessing/accelerometer.rs | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 94301332..f527de99 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -22,18 +22,6 @@ pub struct Quartiles { upper_bound: f32, } -fn insertion_sort(slice: &mut [f32]) { - for i in 1..slice.len() { - let key = slice[i]; - let mut j = i; - while j > 0 && slice[j - 1] > key { - slice[j] = slice[j - 1]; - j -= 1; - } - slice[j] = key; - } -} - /// Implementation of the Quartiles struct which calculates the bounds for outliers impl Quartiles { pub fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { @@ -174,7 +162,9 @@ impl AccelerometerPreprocessor { pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { // Clone and sort data let mut sorted_data = data.clone(); - insertion_sort(sorted_data.as_mut_slice()); + sorted_data + .as_mut_slice() + .sort_by(|a, b| a.partial_cmp(b).unwrap()); let quartile_keys: [f32; 3] = [0.25, 0.5, 0.75]; let mut quartiles: [f32; 3] = [0.0; 3]; From ee48f4c91b2685a1c5c70cde6db55c2d534354ca Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 6 Mar 2025 18:56:57 +0000 Subject: [PATCH 89/99] reverse. --- .../src/preprocessing/accelerometer.rs | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index f527de99..94301332 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -22,6 +22,18 @@ pub struct Quartiles { upper_bound: f32, } +fn insertion_sort(slice: &mut [f32]) { + for i in 1..slice.len() { + let key = slice[i]; + let mut j = i; + while j > 0 && slice[j - 1] > key { + slice[j] = slice[j - 1]; + j -= 1; + } + slice[j] = key; + } +} + /// Implementation of the Quartiles struct which calculates the bounds for outliers impl Quartiles { pub fn new(q1: f32, q2: f32, q3: f32, is_unreliable: bool) -> Self { @@ -162,9 +174,7 @@ impl AccelerometerPreprocessor { pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { // Clone and sort data let mut sorted_data = data.clone(); - sorted_data - .as_mut_slice() - .sort_by(|a, b| a.partial_cmp(b).unwrap()); + insertion_sort(sorted_data.as_mut_slice()); let quartile_keys: [f32; 3] = [0.25, 0.5, 0.75]; let mut quartiles: [f32; 3] = [0.0; 3]; From 5c96dec89163dce2f26dfe59f6cf1b0d86d53048 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 10 Mar 2025 18:34:35 +0000 Subject: [PATCH 90/99] removed getters --- lib/localisation/src/control/localizer.rs | 26 ++++++----------------- 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 42c896d4..e19e7347 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -21,9 +21,9 @@ const DELTA_T: f64 = 0.01; const STRIPE_WIDTH: f64 = 1.0; pub struct Localizer { - displacement: f64, - velocity: f64, - previous_velocity: f64, + pub displacement: f64, + pub velocity: f64, + pub previous_velocity: f64, acceleration: f64, kalman_filter: KalmanFilter, keyence_checker: KeyenceAgrees, @@ -171,20 +171,6 @@ impl Localizer { } } -// Getters -impl Localizer { - pub fn displacement(&self) -> f64 { - self.displacement - } - - pub fn velocity(&self) -> f64 { - self.velocity - } - - pub fn acceleration(&self) -> f64 { - self.acceleration - } -} #[cfg(test)] mod tests { @@ -207,9 +193,9 @@ mod tests { localizer.iteration(optical_data, raw_keyence_data, raw_accelerometer_data)?; - assert_eq!(localizer.displacement(), 0.0); - assert_eq!(localizer.velocity(), 0.0); - assert_eq!(localizer.acceleration(), 0.0); + assert_eq!(localizer.displacement, 0.0); + assert_eq!(localizer.velocity, 0.0); + assert_eq!(localizer.acceleration, 0.0); Ok(()) } From 9e26ccab7b9560be97ba95356fa0967867868139 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 10 Mar 2025 18:46:18 +0000 Subject: [PATCH 91/99] added optical flow sensor --- boards/stm32f767zi/src/bin/loc.rs | 47 +++++++++++++++++++---- lib/localisation/src/control/localizer.rs | 1 - 2 files changed, 39 insertions(+), 9 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 1d42163a..77c622bd 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -4,20 +4,28 @@ use defmt::*; use embassy_executor::Spawner; use embassy_stm32::{ - gpio::{Input, Pull}, + gpio::{Input, Output, Level, Pull, Speed}, init, + spi::{self, BitOrder, Spi}, + time::khz, +}; +use embassy_sync::{ + blocking_mutex::raw::CriticalSectionRawMutex, + watch::Watch, }; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; use embassy_time::{Duration, Timer}; -use hyped_boards_stm32f767zi::tasks::read_keyence::read_keyence; -use panic_probe as _; - use heapless::Vec; - +use hyped_boards_stm32f767zi::{ + io::{Stm32f767ziGpioOutput, Stm32f767ziSpi}, + tasks::read_keyence::read_keyence, +}; use hyped_localisation::{ control::localizer::Localizer, types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, }; +use hyped_sensors::optical_flow::OpticalFlow; +use hyped_spi::HypedSpiCsPin; +use panic_probe as _; /// A Watch to hold the latest Keyence stripe count. static KEYENCE_1_STRIPE_COUNT: Watch = Watch::new(); @@ -27,6 +35,26 @@ static KEYENCE_2_STRIPE_COUNT: Watch = Watch::n async fn main(spawner: Spawner) -> ! { // Import `init` so that we can initialize board peripherals. let p = init(Default::default()); + + let mut spi_config = spi::Config::default(); + spi_config.frequency = khz(400); + spi_config.bit_order = BitOrder::MsbFirst; + + let spi = Spi::new_blocking(p.SPI1, p.PB3, p.PB5, p.PB4, spi_config); + let mut hyped_spi = Stm32f767ziSpi::new(spi); + + let cs = HypedSpiCsPin::new(Stm32f767ziGpioOutput::new(Output::new( + p.PA4, + Level::High, + Speed::VeryHigh, + ))); + + let mut optical_flow = OpticalFlow::new(&mut hyped_spi, cs) + .await + .expect("Failed to initialize optical flow sensor."); + + defmt::info!("Optical flow sensor initialized."); + let gpio_pin1 = Input::new(p.PC13, Pull::Down); let gpio_pin2 = Input::new(p.PC14, Pull::Down); @@ -48,14 +76,17 @@ async fn main(spawner: Spawner) -> ! { let stripe_count1 = receiver1.get().await; let stripe_count2 = receiver2.get().await; + let flow = optical_flow.get_motion().await.unwrap(); + let optical_data: Vec = Vec::from_slice(&[flow.x as f64, flow.y as f64]) + .unwrap(); + defmt::info!( "New Keyence stripe counts: sensor1 = {}, sensor2 = {}", stripe_count1, stripe_count2 ); - // Create the sensor data. (Optical and accelerometer data are simulated.) - let optical_data: Vec = Vec::from_slice(&[0.5, 0.5]).unwrap(); + // Create the sensor data. (accelerometer data is simulated.) let keyence_data: Vec = Vec::from_slice(&[stripe_count1, stripe_count2]).unwrap(); let accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index e19e7347..3b796575 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -171,7 +171,6 @@ impl Localizer { } } - #[cfg(test)] mod tests { use super::*; From 303216fe63ff8deaa1c6a9824654c15af9ac0217 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 20 Mar 2025 18:44:23 +0000 Subject: [PATCH 92/99] test --- boards/stm32f767zi/src/bin/loc.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 77c622bd..a57759a5 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -86,7 +86,7 @@ async fn main(spawner: Spawner) -> ! { stripe_count2 ); - // Create the sensor data. (accelerometer data is simulated.) + // Create the sensor data. (no accelerometer data) let keyence_data: Vec = Vec::from_slice(&[stripe_count1, stripe_count2]).unwrap(); let accelerometer_data: RawAccelerometerData = RawAccelerometerData::from_slice(&[ From bed00d6d4e665ff78f831d9036f621f88d975e62 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 20 Mar 2025 18:51:19 +0000 Subject: [PATCH 93/99] brackets removed, code improved --- boards/stm32f767zi/src/bin/loc.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index a57759a5..265701b7 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -101,9 +101,9 @@ async fn main(spawner: Spawner) -> ! { Ok(()) => { defmt::info!( "Iteration OK: displacement = {} m, velocity = {} m/s, acceleration = {} m/s**2", - localizer.displacement(), - localizer.velocity(), - localizer.acceleration() + localizer.displacement, + localizer.velocity, + localizer.acceleration ); } Err(e) => { From 291cad6d6331e403598e06f9c79bf38d9d7ca8d8 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 20 Mar 2025 18:56:45 +0000 Subject: [PATCH 94/99] forgot to make acceleration public lol --- lib/localisation/src/control/localizer.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 3b796575..9211ce18 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -24,7 +24,7 @@ pub struct Localizer { pub displacement: f64, pub velocity: f64, pub previous_velocity: f64, - acceleration: f64, + pub acceleration: f64, kalman_filter: KalmanFilter, keyence_checker: KeyenceAgrees, keyence_val: f64, From 835c4825f0004566c43f3e9e123745c1144ba011 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Thu, 20 Mar 2025 19:20:55 +0000 Subject: [PATCH 95/99] hmm fail? --- boards/stm32f767zi/src/bin/loc.rs | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 265701b7..4e304da7 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -4,15 +4,12 @@ use defmt::*; use embassy_executor::Spawner; use embassy_stm32::{ - gpio::{Input, Output, Level, Pull, Speed}, + gpio::{Input, Level, Output, Pull, Speed}, init, spi::{self, BitOrder, Spi}, time::khz, }; -use embassy_sync::{ - blocking_mutex::raw::CriticalSectionRawMutex, - watch::Watch, -}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; use embassy_time::{Duration, Timer}; use heapless::Vec; use hyped_boards_stm32f767zi::{ @@ -77,8 +74,7 @@ async fn main(spawner: Spawner) -> ! { let stripe_count2 = receiver2.get().await; let flow = optical_flow.get_motion().await.unwrap(); - let optical_data: Vec = Vec::from_slice(&[flow.x as f64, flow.y as f64]) - .unwrap(); + let optical_data: Vec = Vec::from_slice(&[flow.x as f64, flow.y as f64]).unwrap(); defmt::info!( "New Keyence stripe counts: sensor1 = {}, sensor2 = {}", From e7ae587d02a562c3b76923dc6c9d1b2531b37401 Mon Sep 17 00:00:00 2001 From: Michael Belyaev Date: Mon, 24 Mar 2025 18:20:49 +0000 Subject: [PATCH 96/99] test. --- boards/stm32f767zi/src/bin/loc.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/loc.rs index 4e304da7..da44bb92 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/loc.rs @@ -69,7 +69,7 @@ async fn main(spawner: Spawner) -> ! { let mut localizer = Localizer::new(); loop { - // Wait for a new Keyence stripe count. + // Wait for new Keyence stripe count. let stripe_count1 = receiver1.get().await; let stripe_count2 = receiver2.get().await; From ce9532ce784bf24ef098c80a89b6e2b0c06692c1 Mon Sep 17 00:00:00 2001 From: David Beechey Date: Sat, 21 Jun 2025 10:44:23 +0100 Subject: [PATCH 97/99] start working on finishing localisation --- boards/stm32f767zi/Cargo.lock | 88 +++++++++++++++++++ boards/stm32f767zi/Cargo.toml | 49 +++++++++-- .../bin/{loc.rs => boards/localisation.rs} | 88 +++++++++++-------- boards/stm32f767zi/src/tasks/sensors.rs | 1 + .../src/tasks/sensors/read_optical_flow.rs | 31 +++++++ config/sensors.yaml | 1 + 6 files changed, 213 insertions(+), 45 deletions(-) rename boards/stm32f767zi/src/bin/{loc.rs => boards/localisation.rs} (52%) create mode 100644 boards/stm32f767zi/src/tasks/sensors/read_optical_flow.rs diff --git a/boards/stm32f767zi/Cargo.lock b/boards/stm32f767zi/Cargo.lock index 685faa4f..c64ececc 100644 --- a/boards/stm32f767zi/Cargo.lock +++ b/boards/stm32f767zi/Cargo.lock @@ -20,6 +20,15 @@ version = "0.2.21" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + [[package]] name = "arraydeque" version = "0.5.1" @@ -741,6 +750,7 @@ dependencies = [ "hyped_gpio_derive", "hyped_i2c", "hyped_i2c_derive", + "hyped_localisation", "hyped_sensors", "hyped_spi", "hyped_spi_derive", @@ -833,6 +843,17 @@ dependencies = [ "syn", ] +[[package]] +name = "hyped_localisation" +version = "0.1.0" +dependencies = [ + "defmt", + "heapless 0.8.0", + "hyped_core", + "libm", + "nalgebra", +] + [[package]] name = "hyped_measurement_ids" version = "0.1.0" @@ -893,6 +914,12 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" +[[package]] +name = "libm" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" + [[package]] name = "litrs" version = "0.4.1" @@ -905,6 +932,20 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0ca88d725a0a943b096803bd34e73a4437208b6077654cc4ecb2947a5f91618d" +[[package]] +name = "nalgebra" +version = "0.33.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "26aecdf64b707efd1310e3544d709c5c0ac61c13756046aaaba41be5c4f66a3b" +dependencies = [ + "approx", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + [[package]] name = "nb" version = "0.1.3" @@ -926,6 +967,34 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "43794a0ace135be66a25d3ae77d41b91615fb68ae937f904090203e81f755b65" +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-integer", + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.19" @@ -933,6 +1002,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", + "libm", ] [[package]] @@ -951,6 +1021,12 @@ dependencies = [ "defmt", ] +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "pin-project-lite" version = "0.2.16" @@ -1110,6 +1186,18 @@ dependencies = [ "syn", ] +[[package]] +name = "simba" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b3a386a501cd104797982c15ae17aafe8b9261315b5d07e3ec803f2ea26be0fa" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", +] + [[package]] name = "smoltcp" version = "0.11.0" diff --git a/boards/stm32f767zi/Cargo.toml b/boards/stm32f767zi/Cargo.toml index 99480734..5d4355b1 100644 --- a/boards/stm32f767zi/Cargo.toml +++ b/boards/stm32f767zi/Cargo.toml @@ -4,18 +4,45 @@ version = "0.1.0" edition = "2021" [dependencies] -embassy-stm32 = { version = "0.1.0", features = ["defmt", "stm32f767zi", "memory-x", "unstable-pac", "time-driver-any", "exti"] , git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"} -embassy-sync = { version = "0.6", features = ["defmt"], git = "https://github.com/embassy-rs/embassy", rev = "92326f10b5be1d6fdc6bd414eb0656e3890bd825"} -embassy-executor = { version = "0.6.0", features = ["task-arena-size-32768", "arch-cortex-m", "executor-thread", "defmt", "integrated-timers"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"} -embassy-time = { version = "0.3.1", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"} -embassy-net = { version = "0.4.0", features = ["defmt", "tcp", "dhcpv4", "medium-ethernet"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"} -embassy-futures = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"} +embassy-stm32 = { version = "0.1.0", features = [ + "defmt", + "stm32f767zi", + "memory-x", + "unstable-pac", + "time-driver-any", + "exti", +], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7" } +embassy-sync = { version = "0.6", features = [ + "defmt", +], git = "https://github.com/embassy-rs/embassy", rev = "92326f10b5be1d6fdc6bd414eb0656e3890bd825" } +embassy-executor = { version = "0.6.0", features = [ + "task-arena-size-32768", + "arch-cortex-m", + "executor-thread", + "defmt", + "integrated-timers", +], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7" } +embassy-time = { version = "0.3.1", features = [ + "defmt", + "defmt-timestamp-uptime", + "tick-hz-32_768", +], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7" } +embassy-net = { version = "0.4.0", features = [ + "defmt", + "tcp", + "dhcpv4", + "medium-ethernet", +], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7" } +embassy-futures = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7" } defmt = "0.3" defmt-rtt = "0.4" heapless = "0.8.0" -cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] } +cortex-m = { version = "0.7.6", features = [ + "inline-asm", + "critical-section-single-core", +] } cortex-m-rt = "0.7.0" embedded-hal = "0.2.6" panic-probe = { version = "0.3", features = ["print-defmt"] } @@ -24,7 +51,9 @@ critical-section = "1.1" embedded-storage = "0.3.1" static_cell = "2" -rust-mqtt = { version = "0.3.0", default-features = false, features = ["defmt"] } +rust-mqtt = { version = "0.3.0", default-features = false, features = [ + "defmt", +] } serde = { version = "1.0", default-features = false, features = ["derive"] } serde-json-core = "0.1.0" typenum = "1.17.0" @@ -50,6 +79,10 @@ hyped_spi_derive = { path = "../../lib/io/hyped_spi/hyped_spi_derive" } name = "telemetry" path = "src/bin/boards/telemetry.rs" +[[bin]] +name = "localiastion" +path = "src/bin/boards/localisation.rs" + [[bin]] name = "i2cdetect" path = "src/bin/tools/i2cdetect.rs" diff --git a/boards/stm32f767zi/src/bin/loc.rs b/boards/stm32f767zi/src/bin/boards/localisation.rs similarity index 52% rename from boards/stm32f767zi/src/bin/loc.rs rename to boards/stm32f767zi/src/bin/boards/localisation.rs index da44bb92..ca056894 100644 --- a/boards/stm32f767zi/src/bin/loc.rs +++ b/boards/stm32f767zi/src/bin/boards/localisation.rs @@ -14,20 +14,26 @@ use embassy_time::{Duration, Timer}; use heapless::Vec; use hyped_boards_stm32f767zi::{ io::{Stm32f767ziGpioOutput, Stm32f767ziSpi}, - tasks::read_keyence::read_keyence, + tasks::sensors::{read_keyence::read_keyence, read_optical_flow::read_optical_flow}, }; +use hyped_core::config::{MeasurementId, LOCALISATION_CONFIG}; use hyped_localisation::{ - control::localizer::Localizer, - types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS}, + control::localizer::Localizer, preprocessing::optical, types::RawAccelerometerData, }; -use hyped_sensors::optical_flow::OpticalFlow; use hyped_spi::HypedSpiCsPin; use panic_probe as _; -/// A Watch to hold the latest Keyence stripe count. +/// A Watch to hold the latest Keyence stripe count static KEYENCE_1_STRIPE_COUNT: Watch = Watch::new(); static KEYENCE_2_STRIPE_COUNT: Watch = Watch::new(); +/// A Watch to hold the latest optical flow data +static OPTICAL_FLOW_DATA: Watch< + CriticalSectionRawMutex, + Vec, + 1, +> = Watch::new(); + #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { // Import `init` so that we can initialize board peripherals. @@ -38,7 +44,7 @@ async fn main(spawner: Spawner) -> ! { spi_config.bit_order = BitOrder::MsbFirst; let spi = Spi::new_blocking(p.SPI1, p.PB3, p.PB5, p.PB4, spi_config); - let mut hyped_spi = Stm32f767ziSpi::new(spi); + let hyped_spi = Stm32f767ziSpi::new(spi); let cs = HypedSpiCsPin::new(Stm32f767ziGpioOutput::new(Output::new( p.PA4, @@ -46,35 +52,39 @@ async fn main(spawner: Spawner) -> ! { Speed::VeryHigh, ))); - let mut optical_flow = OpticalFlow::new(&mut hyped_spi, cs) - .await - .expect("Failed to initialize optical flow sensor."); - - defmt::info!("Optical flow sensor initialized."); - - let gpio_pin1 = Input::new(p.PC13, Pull::Down); - let gpio_pin2 = Input::new(p.PC14, Pull::Down); - - // Create a sender and a receiver for the Keyence stripe count. - let sender1 = KEYENCE_1_STRIPE_COUNT.sender(); - let mut receiver1 = KEYENCE_1_STRIPE_COUNT.receiver().unwrap(); - let sender2 = KEYENCE_2_STRIPE_COUNT.sender(); - let mut receiver2 = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); + spawner + .spawn(read_optical_flow(hyped_spi, cs, OPTICAL_FLOW_DATA.sender())) + .unwrap(); + + spawner + .spawn(read_keyence( + Input::new(p.PC13, Pull::Down), + MeasurementId::Keyence1, + KEYENCE_1_STRIPE_COUNT.sender(), + )) + .unwrap(); + spawner + .spawn(read_keyence( + Input::new(p.PC14, Pull::Down), + MeasurementId::Keyence2, + KEYENCE_2_STRIPE_COUNT.sender(), + )) + .unwrap(); + + // Initialise receivers + let mut keyence_1_receiver = KEYENCE_1_STRIPE_COUNT.receiver().unwrap(); + let mut keyence_2_receiver = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); + let mut optical_flow_receiver = OPTICAL_FLOW_DATA.receiver().unwrap(); + let mut accelerometers_receiver = - spawner.spawn(read_keyence(gpio_pin1, sender1)).unwrap(); - spawner.spawn(read_keyence(gpio_pin2, sender2)).unwrap(); + let mut localizer = Localizer::new(); info!("Starting localizer loop..."); - let mut localizer = Localizer::new(); - loop { // Wait for new Keyence stripe count. - let stripe_count1 = receiver1.get().await; - let stripe_count2 = receiver2.get().await; - - let flow = optical_flow.get_motion().await.unwrap(); - let optical_data: Vec = Vec::from_slice(&[flow.x as f64, flow.y as f64]).unwrap(); + let stripe_count1 = keyence_1_receiver.get().await; + let stripe_count2 = keyence_2_receiver.get().await; defmt::info!( "New Keyence stripe counts: sensor1 = {}, sensor2 = {}", @@ -84,14 +94,18 @@ async fn main(spawner: Spawner) -> ! { // Create the sensor data. (no accelerometer data) let keyence_data: Vec = Vec::from_slice(&[stripe_count1, stripe_count2]).unwrap(); - let accelerometer_data: RawAccelerometerData = - RawAccelerometerData::from_slice(&[ - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - ]) - .unwrap(); + let accelerometer_data: RawAccelerometerData< + { LOCALISATION_CONFIG.accelerometers.num_sensors as usize }, + { LOCALISATION_CONFIG.num_axis as usize }, + > = RawAccelerometerData::from_slice(&[ + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), + ]) + .unwrap(); + + let optical_data = optical_flow_receiver.get().await; match localizer.iteration(optical_data, keyence_data, accelerometer_data) { Ok(()) => { diff --git a/boards/stm32f767zi/src/tasks/sensors.rs b/boards/stm32f767zi/src/tasks/sensors.rs index 09bce116..094e637b 100644 --- a/boards/stm32f767zi/src/tasks/sensors.rs +++ b/boards/stm32f767zi/src/tasks/sensors.rs @@ -3,4 +3,5 @@ pub mod read_accelerometers_from_mux; pub mod read_keyence; pub mod read_laser_triangulation; pub mod read_low_pressure; +pub mod read_optical_flow; pub mod read_temperature; diff --git a/boards/stm32f767zi/src/tasks/sensors/read_optical_flow.rs b/boards/stm32f767zi/src/tasks/sensors/read_optical_flow.rs new file mode 100644 index 00000000..74167805 --- /dev/null +++ b/boards/stm32f767zi/src/tasks/sensors/read_optical_flow.rs @@ -0,0 +1,31 @@ +use crate::io::{Stm32f767ziGpioOutput, Stm32f767ziSpi}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Sender}; +use embassy_time::{Duration, Timer}; +use heapless::Vec; +use hyped_core::config::SENSORS_CONFIG; +use hyped_sensors::optical_flow::OpticalFlow; +use hyped_spi::HypedSpiCsPin; + +#[embassy_executor::task] +pub async fn read_optical_flow( + mut hyped_spi: Stm32f767ziSpi, + cs: HypedSpiCsPin, + sender: Sender<'static, CriticalSectionRawMutex, Vec, 1>, +) -> ! { + let mut optical_flow = OpticalFlow::new(&mut hyped_spi, cs) + .await + .expect("Failed to initialize optical flow sensor."); + defmt::info!("Optical flow sensor initialized."); + + loop { + let flow = optical_flow.get_motion().await.unwrap(); + let optical_data: Vec = Vec::from_slice(&[flow.x as f64, flow.y as f64]).unwrap(); + + sender.send(optical_data); + + Timer::after(Duration::from_hz( + SENSORS_CONFIG.sensors.optical_flow.update_frequency as u64, + )) + .await; + } +} diff --git a/config/sensors.yaml b/config/sensors.yaml index 3cd2276e..30762334 100644 --- a/config/sensors.yaml +++ b/config/sensors.yaml @@ -13,6 +13,7 @@ sensors: max_pressure: 10.0 pressure_offset: 0.0 optical_flow: + update_frequency: 100 # Hz timeout_s: 5 retry_duration_ms: 10 temperature: From 2fba78b68a9862ff439f0c3936b6f5b6aeb0b370 Mon Sep 17 00:00:00 2001 From: David Beechey Date: Sat, 21 Jun 2025 15:51:15 +0100 Subject: [PATCH 98/99] in theory localisation including accelerometers --- boards/stm32f767zi/Cargo.toml | 2 +- .../src/bin/accelerometer_mux_test.rs | 55 +++-------- .../stm32f767zi/src/bin/accelerometer_test.rs | 45 ++------- .../src/bin/boards/localisation.rs | 78 +++++++++------ .../stm32f767zi/src/bin/high_pressure_test.rs | 2 +- boards/stm32f767zi/src/tasks.rs | 1 - boards/stm32f767zi/src/tasks/sensors.rs | 1 + .../src/tasks/sensors/read_accelerometer.rs | 13 +-- .../sensors/read_accelerometers_from_mux.rs | 36 +++---- .../tasks/{ => sensors}/read_high_pressure.rs | 0 .../src/tasks/sensors/read_keyence.rs | 2 +- lib/communications/src/emergency.rs | 6 ++ lib/sensors/src/accelerometer.rs | 97 +++---------------- 13 files changed, 113 insertions(+), 225 deletions(-) rename boards/stm32f767zi/src/tasks/{ => sensors}/read_high_pressure.rs (100%) diff --git a/boards/stm32f767zi/Cargo.toml b/boards/stm32f767zi/Cargo.toml index 5d4355b1..09c10b8c 100644 --- a/boards/stm32f767zi/Cargo.toml +++ b/boards/stm32f767zi/Cargo.toml @@ -80,7 +80,7 @@ name = "telemetry" path = "src/bin/boards/telemetry.rs" [[bin]] -name = "localiastion" +name = "localisation" path = "src/bin/boards/localisation.rs" [[bin]] diff --git a/boards/stm32f767zi/src/bin/accelerometer_mux_test.rs b/boards/stm32f767zi/src/bin/accelerometer_mux_test.rs index 3453d3ac..15de624b 100644 --- a/boards/stm32f767zi/src/bin/accelerometer_mux_test.rs +++ b/boards/stm32f767zi/src/bin/accelerometer_mux_test.rs @@ -13,17 +13,21 @@ use embassy_sync::{ }, watch::Watch, }; -use hyped_boards_stm32f767zi::tasks::sensors::read_accelerometers_from_mux::{ - read_accelerometers_from_mux, AccelerometerMuxReadings, +use hyped_boards_stm32f767zi::tasks::sensors::read_accelerometers_from_mux::read_accelerometers_from_mux; +use hyped_localisation::{ + config::{NUM_ACCELEROMETERS, NUM_AXIS}, + types::RawAccelerometerData, }; -use hyped_sensors::SensorValueRange::*; use panic_probe as _; use static_cell::StaticCell; type I2c1Bus = Mutex>>; -static ACCELERATION_MUX_READINGS: Watch = - Watch::new(); +static ACCELERATION_MUX_READINGS: Watch< + CriticalSectionRawMutex, + RawAccelerometerData, + 1, +> = Watch::new(); #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { @@ -49,40 +53,13 @@ async fn main(spawner: Spawner) -> ! { loop { let readings = accelerometer_mux_reading_receiver.changed().await; for (i, reading) in readings.iter().enumerate() { - match reading { - Some(reading) => match reading { - Safe(accelerometer_values) => { - defmt::info!( - "Accelerometer {} reading: x={:?}mg, y={:?}mg, z={:?}mg (safe)", - i, - accelerometer_values.x, - accelerometer_values.y, - accelerometer_values.z - ); - } - Warning(accelerometer_values) => { - defmt::info!( - "Accelerometer {} reading: x={:?}mg, y={:?}mg, z={:?}mg (unsafe)", - i, - accelerometer_values.x, - accelerometer_values.y, - accelerometer_values.z - ); - } - Critical(accelerometer_values) => { - defmt::info!( - "Accelerometer {} reading: x={:?}mg, y={:?}mg, z={:?}mg (critical)", - i, - accelerometer_values.x, - accelerometer_values.y, - accelerometer_values.z - ); - } - }, - None => { - defmt::info!("Accelerometer {} reading: None", i); - } - } + defmt::info!( + "Accelerometer {} reading: x: {}, y: {}, z: {}", + i + 1, + reading[0], + reading[1], + reading[2] + ); } } } diff --git a/boards/stm32f767zi/src/bin/accelerometer_test.rs b/boards/stm32f767zi/src/bin/accelerometer_test.rs index 1ff03716..3149dac6 100644 --- a/boards/stm32f767zi/src/bin/accelerometer_test.rs +++ b/boards/stm32f767zi/src/bin/accelerometer_test.rs @@ -13,22 +13,15 @@ use embassy_sync::{ }, watch::Watch, }; +use heapless::Vec; use hyped_boards_stm32f767zi::tasks::sensors::read_accelerometer::read_accelerometer; -use hyped_sensors::{ - accelerometer::AccelerationValues, - SensorValueRange::{self, *}, -}; use panic_probe as _; use static_cell::StaticCell; type I2c1Bus = Mutex>>; /// Used to keep the latest acceleration values. -static ACCELEROMETER_READING: Watch< - CriticalSectionRawMutex, - Option>, - 1, -> = Watch::new(); +static ACCELEROMETER_READING: Watch>, 1> = Watch::new(); #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { @@ -49,32 +42,14 @@ async fn main(spawner: Spawner) -> ! { loop { let reading = accelerometer_reading_receiver.changed().await; match reading { - Some(accelerometer_values) => match accelerometer_values { - Safe(accelerometer_values) => { - defmt::info!( - "Acceleration: x={:?}mg, y={:?}mg, z={:?}mg (safe)", - accelerometer_values.x, - accelerometer_values.y, - accelerometer_values.z - ); - } - Warning(accelerometer_values) => { - defmt::info!( - "Acceleration: x={:?}mg, y={:?}mg, z={:?}mg (unsafe)", - accelerometer_values.x, - accelerometer_values.y, - accelerometer_values.z - ); - } - Critical(accelerometer_values) => { - defmt::info!( - "Acceleration: x={:?}mg, y={:?}mg, z={:?}mg (critical)", - accelerometer_values.x, - accelerometer_values.y, - accelerometer_values.z - ); - } - }, + Some(accelerometer_values) => { + defmt::info!( + "Accelerometer reading: x: {}, y: {}, z: {}", + accelerometer_values[0], + accelerometer_values[1], + accelerometer_values[2] + ) + } None => { defmt::info!("Failed to read acceleration values.") } diff --git a/boards/stm32f767zi/src/bin/boards/localisation.rs b/boards/stm32f767zi/src/bin/boards/localisation.rs index ca056894..50266488 100644 --- a/boards/stm32f767zi/src/bin/boards/localisation.rs +++ b/boards/stm32f767zi/src/bin/boards/localisation.rs @@ -1,38 +1,52 @@ #![no_std] #![no_main] +use core::cell::RefCell; + use defmt::*; use embassy_executor::Spawner; use embassy_stm32::{ gpio::{Input, Level, Output, Pull, Speed}, + i2c::I2c, init, + mode::Blocking, spi::{self, BitOrder, Spi}, - time::khz, + time::{khz, Hertz}, +}; +use embassy_sync::{ + blocking_mutex::{ + raw::{CriticalSectionRawMutex, NoopRawMutex}, + Mutex, + }, + watch::Watch, }; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; use embassy_time::{Duration, Timer}; use heapless::Vec; use hyped_boards_stm32f767zi::{ io::{Stm32f767ziGpioOutput, Stm32f767ziSpi}, - tasks::sensors::{read_keyence::read_keyence, read_optical_flow::read_optical_flow}, + tasks::sensors::{ + read_accelerometers_from_mux::{read_accelerometers_from_mux, AccelerometerMuxReadings}, + read_keyence::read_keyence, + read_optical_flow::read_optical_flow, + }, }; use hyped_core::config::{MeasurementId, LOCALISATION_CONFIG}; -use hyped_localisation::{ - control::localizer::Localizer, preprocessing::optical, types::RawAccelerometerData, -}; +use hyped_localisation::{control::localizer::Localizer, types::RawAccelerometerData}; use hyped_spi::HypedSpiCsPin; use panic_probe as _; +use static_cell::StaticCell; +type I2c1Bus = Mutex>>; /// A Watch to hold the latest Keyence stripe count static KEYENCE_1_STRIPE_COUNT: Watch = Watch::new(); static KEYENCE_2_STRIPE_COUNT: Watch = Watch::new(); /// A Watch to hold the latest optical flow data -static OPTICAL_FLOW_DATA: Watch< - CriticalSectionRawMutex, - Vec, - 1, -> = Watch::new(); +static OPTICAL_FLOW_DATA: Watch, 1> = Watch::new(); + +/// A Watch to hold the latest accelerometer data +static ACCELEROMETERS_DATA: Watch = + Watch::new(); #[embassy_executor::main] async fn main(spawner: Spawner) -> ! { @@ -52,6 +66,13 @@ async fn main(spawner: Spawner) -> ! { Speed::VeryHigh, ))); + let i2c = I2c::new_blocking(p.I2C1, p.PB8, p.PB9, Hertz(200_000), Default::default()); + + // Initialize the I2C bus and store it in a static cell so that it can be accessed from the task. + static I2C_BUS: StaticCell = StaticCell::new(); + let i2c_bus = I2C_BUS.init(Mutex::new(RefCell::new(i2c))); + defmt::info!("I2C initialized."); + spawner .spawn(read_optical_flow(hyped_spi, cs, OPTICAL_FLOW_DATA.sender())) .unwrap(); @@ -71,39 +92,34 @@ async fn main(spawner: Spawner) -> ! { )) .unwrap(); + spawner + .spawn(read_accelerometers_from_mux( + i2c_bus, + ACCELEROMETERS_DATA.sender(), + )) + .unwrap(); + // Initialise receivers let mut keyence_1_receiver = KEYENCE_1_STRIPE_COUNT.receiver().unwrap(); let mut keyence_2_receiver = KEYENCE_2_STRIPE_COUNT.receiver().unwrap(); let mut optical_flow_receiver = OPTICAL_FLOW_DATA.receiver().unwrap(); - let mut accelerometers_receiver = + let mut accelerometers_receiver = ACCELEROMETERS_DATA.receiver().unwrap(); let mut localizer = Localizer::new(); info!("Starting localizer loop..."); loop { - // Wait for new Keyence stripe count. - let stripe_count1 = keyence_1_receiver.get().await; - let stripe_count2 = keyence_2_receiver.get().await; - - defmt::info!( - "New Keyence stripe counts: sensor1 = {}, sensor2 = {}", - stripe_count1, - stripe_count2 - ); - - // Create the sensor data. (no accelerometer data) - let keyence_data: Vec = Vec::from_slice(&[stripe_count1, stripe_count2]).unwrap(); + let keyence_data: Vec = Vec::from_slice(&[ + keyence_1_receiver.get().await, + keyence_2_receiver.get().await, + ]) + .unwrap(); + let accelerometer_data: RawAccelerometerData< { LOCALISATION_CONFIG.accelerometers.num_sensors as usize }, { LOCALISATION_CONFIG.num_axis as usize }, - > = RawAccelerometerData::from_slice(&[ - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - Vec::from_slice(&[0.0, 0.0, 9.81]).unwrap(), - ]) - .unwrap(); + > = accelerometers_receiver.get().await; let optical_data = optical_flow_receiver.get().await; diff --git a/boards/stm32f767zi/src/bin/high_pressure_test.rs b/boards/stm32f767zi/src/bin/high_pressure_test.rs index ec910fb4..80ba5020 100644 --- a/boards/stm32f767zi/src/bin/high_pressure_test.rs +++ b/boards/stm32f767zi/src/bin/high_pressure_test.rs @@ -5,7 +5,7 @@ use defmt_rtt as _; use embassy_executor::Spawner; use embassy_stm32::gpio::{Input, Pull}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, watch::Watch}; -use hyped_boards_stm32f767zi::tasks::read_high_pressure::read_high_pressure; +use hyped_boards_stm32f767zi::tasks::sensors::read_high_pressure::read_high_pressure; use hyped_sensors::high_pressure::{HighPressureError, State}; use panic_probe as _; diff --git a/boards/stm32f767zi/src/tasks.rs b/boards/stm32f767zi/src/tasks.rs index 79b514fa..77550f4f 100644 --- a/boards/stm32f767zi/src/tasks.rs +++ b/boards/stm32f767zi/src/tasks.rs @@ -2,6 +2,5 @@ pub mod can; pub mod can_to_mqtt; pub mod mqtt; pub mod network; -pub mod read_high_pressure; pub mod sensors; pub mod state_machine; diff --git a/boards/stm32f767zi/src/tasks/sensors.rs b/boards/stm32f767zi/src/tasks/sensors.rs index 094e637b..ddb70352 100644 --- a/boards/stm32f767zi/src/tasks/sensors.rs +++ b/boards/stm32f767zi/src/tasks/sensors.rs @@ -1,5 +1,6 @@ pub mod read_accelerometer; pub mod read_accelerometers_from_mux; +pub mod read_high_pressure; pub mod read_keyence; pub mod read_laser_triangulation; pub mod read_low_pressure; diff --git a/boards/stm32f767zi/src/tasks/sensors/read_accelerometer.rs b/boards/stm32f767zi/src/tasks/sensors/read_accelerometer.rs index fa6ed940..3eb5d5ef 100644 --- a/boards/stm32f767zi/src/tasks/sensors/read_accelerometer.rs +++ b/boards/stm32f767zi/src/tasks/sensors/read_accelerometer.rs @@ -10,11 +10,9 @@ use embassy_sync::{ watch::Sender, }; use embassy_time::{Duration, Timer}; +use heapless::Vec; use hyped_core::config::SENSORS_CONFIG; -use hyped_sensors::{ - accelerometer::{AccelerationValues, Accelerometer, AccelerometerAddresses, Status}, - SensorValueRange, -}; +use hyped_sensors::accelerometer::{Accelerometer, AccelerometerAddresses, Status}; type I2c1Bus = Mutex>>; @@ -22,12 +20,7 @@ type I2c1Bus = Mutex>>; #[embassy_executor::task] pub async fn read_accelerometer( i2c_bus: &'static I2c1Bus, - sender: Sender< - 'static, - CriticalSectionRawMutex, - Option>, - 1, - >, + sender: Sender<'static, CriticalSectionRawMutex, Option>, 1>, ) -> ! { let mut hyped_i2c = Stm32f767ziI2c::new(i2c_bus); diff --git a/boards/stm32f767zi/src/tasks/sensors/read_accelerometers_from_mux.rs b/boards/stm32f767zi/src/tasks/sensors/read_accelerometers_from_mux.rs index cd4d7f51..96416bfa 100644 --- a/boards/stm32f767zi/src/tasks/sensors/read_accelerometers_from_mux.rs +++ b/boards/stm32f767zi/src/tasks/sensors/read_accelerometers_from_mux.rs @@ -11,16 +11,15 @@ use embassy_sync::{ }; use embassy_time::{Duration, Timer}; use heapless::Vec; -use hyped_core::config::{LOCALISATION_CONFIG, SENSORS_CONFIG}; +use hyped_core::config::SENSORS_CONFIG; use hyped_i2c::{i2c_mux::DEFAULT_MUX_ADDRESS, HypedI2c}; -use hyped_sensors::{ - accelerometer::{self, AccelerationValues, Accelerometer, AccelerometerAddresses}, - SensorValueRange::{self}, +use hyped_localisation::{ + config::{NUM_ACCELEROMETERS, NUM_AXIS}, + types::RawAccelerometerData, }; -const NUM_ACCELEROMETERS: usize = LOCALISATION_CONFIG.accelerometers.num_sensors as usize; +use hyped_sensors::accelerometer::{self, Accelerometer, AccelerometerAddresses}; -pub type AccelerometerMuxReadings = - Vec>, NUM_ACCELEROMETERS>; +pub type AccelerometerMuxReadings = RawAccelerometerData; type I2c1Bus = Mutex>>; @@ -77,8 +76,6 @@ pub async fn read_accelerometers_from_mux( defmt::info!("Accelerometer 4 initialized."); loop { - let mut readings: AccelerometerMuxReadings = Vec::new(); - // Read from all accelerometers defmt::info!("Reading accelerometers from mux"); @@ -97,10 +94,7 @@ pub async fn read_accelerometers_from_mux( panic!("Could not get status of accelerometer") } } - - readings - .push(accelerometer_1.read()) - .expect("Failed to add acceleration reading to vector of readings."); + let reading_1 = accelerometer_1.read().unwrap(); // Read the second accelerometer match accelerometer_2.check_status() { @@ -112,10 +106,7 @@ pub async fn read_accelerometers_from_mux( panic!("Could not get status of accelerometer") } } - - readings - .push(accelerometer_2.read()) - .expect("Failed to add acceleration reading to vector of readings."); + let reading_2 = accelerometer_2.read().unwrap(); // Select channel 1 hyped_i2c @@ -132,9 +123,7 @@ pub async fn read_accelerometers_from_mux( panic!("Could not get status of accelerometer") } } - readings - .push(accelerometer_3.read()) - .expect("Failed to add acceleration reading to vector of readings."); + let reading_3 = accelerometer_3.read().unwrap(); // Read the second accelerometer match accelerometer_4.check_status() { @@ -146,9 +135,10 @@ pub async fn read_accelerometers_from_mux( panic!("Could not get status of accelerometer") } } - readings - .push(accelerometer_4.read()) - .expect("Failed to add acceleration reading to vector of readings."); + let reading_4 = accelerometer_4.read().unwrap(); + + let readings: RawAccelerometerData = + Vec::from_slice(&[reading_1, reading_2, reading_3, reading_4]).unwrap(); sender.send(readings); Timer::after(Duration::from_hz( diff --git a/boards/stm32f767zi/src/tasks/read_high_pressure.rs b/boards/stm32f767zi/src/tasks/sensors/read_high_pressure.rs similarity index 100% rename from boards/stm32f767zi/src/tasks/read_high_pressure.rs rename to boards/stm32f767zi/src/tasks/sensors/read_high_pressure.rs diff --git a/boards/stm32f767zi/src/tasks/sensors/read_keyence.rs b/boards/stm32f767zi/src/tasks/sensors/read_keyence.rs index ce2856fb..c847aef1 100644 --- a/boards/stm32f767zi/src/tasks/sensors/read_keyence.rs +++ b/boards/stm32f767zi/src/tasks/sensors/read_keyence.rs @@ -10,7 +10,7 @@ use hyped_core::{ use hyped_sensors::keyence::Keyence; /// Test task that just continually updates the stripe count from the Keyence sensor (or other GPIO pin input) -#[embassy_executor::task] +#[embassy_executor::task(pool_size = 2)] pub async fn read_keyence( gpio_pin: Input<'static>, measurement_id: MeasurementId, diff --git a/lib/communications/src/emergency.rs b/lib/communications/src/emergency.rs index ac9347ba..89e697e3 100644 --- a/lib/communications/src/emergency.rs +++ b/lib/communications/src/emergency.rs @@ -9,6 +9,7 @@ pub enum Reason { MissingHeartbeat = 5, TemperatureUpperLimitFailure = 6, TemperatureLowerLimitFailure = 7, + AccelerometerCriticalLimit = 8, } impl TryFrom for Reason { @@ -23,6 +24,7 @@ impl TryFrom for Reason { 5 => Ok(Reason::MissingHeartbeat), 6 => Ok(Reason::TemperatureUpperLimitFailure), 7 => Ok(Reason::TemperatureLowerLimitFailure), + 8 => Ok(Reason::AccelerometerCriticalLimit), _ => Err("Invalid reason for emergency stop"), } } @@ -59,6 +61,10 @@ mod tests { Reason::TemperatureLowerLimitFailure, Reason::try_from(Reason::TemperatureLowerLimitFailure as u8).unwrap() ); + assert_eq!( + Reason::AccelerometerCriticalLimit, + Reason::try_from(Reason::AccelerometerCriticalLimit as u8).unwrap() + ); assert_eq!( Err("Invalid reason for emergency stop"), Reason::try_from(8) diff --git a/lib/sensors/src/accelerometer.rs b/lib/sensors/src/accelerometer.rs index 2a2199e1..4ae2c872 100644 --- a/lib/sensors/src/accelerometer.rs +++ b/lib/sensors/src/accelerometer.rs @@ -1,8 +1,6 @@ -use defmt::Format; +use heapless::Vec; use hyped_i2c::{i2c_write_or_err, HypedI2c, I2cError}; -use crate::SensorValueRange; - /// Accelerometer implements the logic to read the temperature from the LIS2DS12 accelerometer /// using the peripheral provided by the HypedI2c trait. /// @@ -17,7 +15,6 @@ use crate::SensorValueRange; pub struct Accelerometer<'a, T: HypedI2c + 'a> { i2c: &'a mut T, device_address: u8, - calculate_bounds: fn(AccelerationValues) -> SensorValueRange, } impl<'a, T: HypedI2c> Accelerometer<'a, T> { @@ -25,14 +22,6 @@ impl<'a, T: HypedI2c> Accelerometer<'a, T> { pub fn new( i2c: &'a mut T, device_address: AccelerometerAddresses, - ) -> Result { - Self::new_with_bounds(i2c, device_address, default_calculate_bounds) - } - - pub fn new_with_bounds( - i2c: &'a mut T, - device_address: AccelerometerAddresses, - calculate_bounds: fn(AccelerationValues) -> SensorValueRange, ) -> Result { let device_address = device_address as u8; @@ -62,12 +51,11 @@ impl<'a, T: HypedI2c> Accelerometer<'a, T> { Ok(Self { i2c, device_address, - calculate_bounds, }) } /// Read the acceleration for each axis and return them as floating point values in gs. - pub fn read(&mut self) -> Option> { + pub fn read(&mut self) -> Option> { // Read the low and high bytes of the acceleration and combine them to get the acceleration for each axis let x_low_byte = self.i2c.read_byte(self.device_address, LIS2DS12_OUT_X_L)?; let x_high_byte = self.i2c.read_byte(self.device_address, LIS2DS12_OUT_X_H)?; @@ -90,11 +78,11 @@ impl<'a, T: HypedI2c> Accelerometer<'a, T> { if z_combined >= TWO_POWER_15 { z_combined -= TWO_POWER_16; } - let x = x_combined * LIS2DS12_ACCEL_SCALING_FACTOR; - let y = y_combined * LIS2DS12_ACCEL_SCALING_FACTOR; - let z = z_combined * LIS2DS12_ACCEL_SCALING_FACTOR; + let x = (x_combined * LIS2DS12_ACCEL_SCALING_FACTOR) / 1000.0; + let y = (y_combined * LIS2DS12_ACCEL_SCALING_FACTOR) / 1000.0; + let z = (z_combined * LIS2DS12_ACCEL_SCALING_FACTOR) / 1000.0; - Some((self.calculate_bounds)(AccelerationValues { x, y, z })) + Some(Vec::from_slice(&[x, y, z]).unwrap()) } pub fn check_status(&mut self) -> Status { @@ -105,13 +93,6 @@ impl<'a, T: HypedI2c> Accelerometer<'a, T> { } } -#[derive(Debug, PartialEq, Clone, Format)] -pub struct AccelerationValues { - pub x: f32, - pub y: f32, - pub z: f32, -} - pub enum AccelerometerAddresses { Address1d = 0x1D, Address1e = 0x1E, @@ -138,24 +119,6 @@ impl Status { } } -/// Default calculation of bounds for the accelerometer, if no bounds function is provided. -/// The bounds are set to: -/// Safe: Between -6g and +6g -/// Warning: -8g to -6g and +6g to +8g -/// Critical: Below -8g and above +8g -pub fn default_calculate_bounds( - values: AccelerationValues, -) -> SensorValueRange { - let mut values_iter = [values.x, values.y, values.z].into_iter(); // there's probably a better way of doing this - if values_iter.any(|i| i >= 8000.0 || i <= -8000.0) { - SensorValueRange::Critical(values) - } else if values_iter.any(|i| i >= 6000.0 || i <= -6000.0) { - SensorValueRange::Warning(values) - } else { - SensorValueRange::Safe(values) - } -} - // Registers for the LIS2DS12 accelerometer const LIS2DS12_CTRL1_ADDRESS: u8 = 0x20; const LIS2DS12_CTRL2_ADDRESS: u8 = 0x21; @@ -274,11 +237,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: 0.0, - y: 0.0, - z: 0.0 - })) + Some(Vec::from_slice(&[0.0, 0.0, 0.0]).unwrap()) ); } @@ -335,11 +294,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: 1220.0, - y: 0.0, - z: 0.0 - })) + Some(Vec::from_slice(&[1220.0, 0.0, 0.0]).unwrap()) ); } @@ -396,11 +351,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: -1220.0, - y: 0.0, - z: 0.0 - })) + Some(Vec::from_slice(&[-1220.0, 0.0, 0.0]).unwrap()) ); } @@ -457,11 +408,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: 0.0, - y: 1220.0, - z: 0.0 - })) + Some(Vec::from_slice(&[0.0, 1220.0, 0.0]).unwrap()) ); } @@ -518,11 +465,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: 0.0, - y: -1220.0, - z: 0.0 - })) + Some(Vec::from_slice(&[0.0, -1220.0, 0.0]).unwrap()) ); } @@ -579,11 +522,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: 0.0, - y: 0.0, - z: 1220.0 - })) + Some(Vec::from_slice(&[0.0, 0.0, 1220.0]).unwrap()) ); } @@ -640,11 +579,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: 0.0, - y: 0.0, - z: -1220.0 - })) + Some(Vec::from_slice(&[0.0, 0.0, -1220.0]).unwrap()) ); } @@ -700,11 +635,7 @@ mod tests { Accelerometer::new(&mut i2c, AccelerometerAddresses::Address1d).unwrap(); assert_eq!( accelerometer.read(), - Some(SensorValueRange::Safe(AccelerationValues { - x: 122.0, - y: 244.0, - z: -488.0 - })) + Some(Vec::from_slice(&[122.0, 244.0, -488.0]).unwrap()) ); } From 7adf8960086072f47a65b3d87245e9b9d4abaf1d Mon Sep 17 00:00:00 2001 From: David Beechey Date: Sat, 21 Jun 2025 16:06:04 +0100 Subject: [PATCH 99/99] fix test --- lib/communications/src/emergency.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/communications/src/emergency.rs b/lib/communications/src/emergency.rs index 89e697e3..813c7462 100644 --- a/lib/communications/src/emergency.rs +++ b/lib/communications/src/emergency.rs @@ -67,7 +67,7 @@ mod tests { ); assert_eq!( Err("Invalid reason for emergency stop"), - Reason::try_from(8) + Reason::try_from(9) ); } }