From c0f876d98f2478ec3383c01eda1ad4b03bb85609 Mon Sep 17 00:00:00 2001 From: lylebuist <102750153+lylebuist@users.noreply.github.com> Date: Mon, 11 Nov 2024 18:52:27 +0000 Subject: [PATCH 1/5] initial commit --- lib/localisation/src/preprocessing/optical.rs | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index 8b137891..142fa8b8 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -1 +1,11 @@ +use heapless::Vec; +pub struct OpticalPreprocessor { + +} + +impl Default for OpticalPreprocessor { + fn default() -> Self { + Self::new() + } +} From 5631d2b11ce9c59c3aa6f16694d5a522682f49a9 Mon Sep 17 00:00:00 2001 From: lylebuist <102750153+lylebuist@users.noreply.github.com> Date: Mon, 11 Nov 2024 19:30:48 +0000 Subject: [PATCH 2/5] need to sort out types --- Cargo.lock | 7 +++++++ lib/localisation/Cargo.toml | 1 + lib/localisation/src/preprocessing.rs | 1 + lib/localisation/src/preprocessing/optical.rs | 21 ++++++++++++------- 4 files changed, 23 insertions(+), 7 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index d78c302a..2e39f261 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -310,6 +310,7 @@ name = "hyped_localisation" version = "0.1.0" dependencies = [ "heapless", + "libm", "nalgebra", ] @@ -338,6 +339,12 @@ version = "1.0.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" +[[package]] +name = "libm" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" + [[package]] name = "libyml" version = "0.0.5" diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index e6fdb7a3..7a7425bd 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -6,3 +6,4 @@ edition = "2021" [dependencies] nalgebra = { version = "0.33.0", default-features = false } heapless = "0.8.0" +libm = "0.2.11" diff --git a/lib/localisation/src/preprocessing.rs b/lib/localisation/src/preprocessing.rs index e1de1ed6..7262ef76 100644 --- a/lib/localisation/src/preprocessing.rs +++ b/lib/localisation/src/preprocessing.rs @@ -1 +1,2 @@ pub mod keyence; +pub mod optical; \ No newline at end of file diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index 142fa8b8..82c6256a 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -1,11 +1,18 @@ use heapless::Vec; +use libm::sqrtf; -pub struct OpticalPreprocessor { - -} +pub fn process_data(raw_optical_data: Vec, 2>) -> Vec{ -impl Default for OpticalPreprocessor { - fn default() -> Self { - Self::new() + let mut optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); + + for i in 0..1 { + let mut magnitude: f32 = 0.0; + + for data in raw_optical_data[i] { + magnitude += data*data; + } + optical_data[i] = sqrtf(magnitude); } -} + + optical_data +} \ No newline at end of file From 352434173ac6e004bedf566602f553f1854a6dff Mon Sep 17 00:00:00 2001 From: lylebuist <102750153+lylebuist@users.noreply.github.com> Date: Tue, 12 Nov 2024 12:57:53 +0000 Subject: [PATCH 3/5] added tests and doc --- lib/localisation/src/preprocessing.rs | 2 +- lib/localisation/src/preprocessing/optical.rs | 54 ++++++++++++++++--- 2 files changed, 49 insertions(+), 7 deletions(-) diff --git a/lib/localisation/src/preprocessing.rs b/lib/localisation/src/preprocessing.rs index 7262ef76..effba0de 100644 --- a/lib/localisation/src/preprocessing.rs +++ b/lib/localisation/src/preprocessing.rs @@ -1,2 +1,2 @@ pub mod keyence; -pub mod optical; \ No newline at end of file +pub mod optical; diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index 82c6256a..552826cb 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -1,18 +1,60 @@ use heapless::Vec; use libm::sqrtf; -pub fn process_data(raw_optical_data: Vec, 2>) -> Vec{ - +/// Processes the raw optical data to get the magnitude of the optical data for each sensor +pub fn process_data(raw_optical_data: Vec, 2>) -> Vec { let mut optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); - for i in 0..1 { + for i in 0..2 { let mut magnitude: f32 = 0.0; - for data in raw_optical_data[i] { - magnitude += data*data; + for data in raw_optical_data[i].clone() { + let data: f32 = data as f32; + magnitude += data * data; } optical_data[i] = sqrtf(magnitude); } optical_data -} \ No newline at end of file +} + +#[cfg(test)] +mod tests { + use super::*; + + #[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(&[1.4142135, 5.0]).unwrap(); + let result = process_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 result = process_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 result = process_data(raw_optical_data); + assert_eq!(result, desired_outcome); + } +} From b4f8dc12da285f664c984a776babdb291e42628d Mon Sep 17 00:00:00 2001 From: lylebuist <102750153+lylebuist@users.noreply.github.com> Date: Tue, 12 Nov 2024 13:00:08 +0000 Subject: [PATCH 4/5] clippy --- lib/localisation/src/preprocessing/optical.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index 552826cb..4e60407f 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -1,7 +1,7 @@ use heapless::Vec; use libm::sqrtf; -/// Processes the raw optical data to get the magnitude of the optical data for each sensor +/// 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 { let mut optical_data: Vec = Vec::from_slice(&[0.0, 0.0]).unwrap(); From 17f197c5b0fd952c3ba3be06efc3a1b0b2f407d9 Mon Sep 17 00:00:00 2001 From: lylebuist <102750153+lylebuist@users.noreply.github.com> Date: Tue, 12 Nov 2024 13:17:08 +0000 Subject: [PATCH 5/5] used sqrtf in test --- lib/localisation/src/preprocessing/optical.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/localisation/src/preprocessing/optical.rs b/lib/localisation/src/preprocessing/optical.rs index 4e60407f..2198f6ec 100644 --- a/lib/localisation/src/preprocessing/optical.rs +++ b/lib/localisation/src/preprocessing/optical.rs @@ -29,7 +29,7 @@ mod tests { Vec::from_slice(&[3.0, 4.0]).unwrap(), ]) .unwrap(); - let desired_outcome: Vec = Vec::from_slice(&[1.4142135, 5.0]).unwrap(); + let desired_outcome: Vec = Vec::from_slice(&[sqrtf(2.0), 5.0]).unwrap(); let result = process_data(raw_optical_data); assert_eq!(result, desired_outcome); }