LOC - Localizer Board (HYPE-90)

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.8.0"
 
 cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] }
 cortex-m-rt = "0.7.0"
@@ -25,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" }

boards/stm32f767zi/src/bin/loc.rs

@@ -0,0 +1,82 @@
+#![no_std]
+#![no_main]
+
+use defmt::*;
+use embassy_executor::Spawner;
+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;
+use panic_probe as _;
+
+use heapless::Vec;
+
+use hyped_localisation::{
+    control::localizer::Localizer,
+    types::{RawAccelerometerData, NUM_ACCELEROMETERS, NUM_AXIS},
+};
+
+/// A Watch to hold the latest Keyence stripe count.
+static KEYENCE_1_STRIPE_COUNT: Watch<CriticalSectionRawMutex, u32, 1> = Watch::new();
+static KEYENCE_2_STRIPE_COUNT: Watch<CriticalSectionRawMutex, u32, 1> = 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);
+
+    // 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_keyence(gpio_pin, sender1)).unwrap();
+    spawner.spawn(read_keyence(gpio_pin, sender2)).unwrap();
+
+    info!("Starting localizer loop...");
+
+    let mut localizer = Localizer::new();
+
+    loop {
+        // Wait for a new Keyence 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<f64, 2> = Vec::from_slice(&[0.5, 0.5]).unwrap();
+        let keyence_data: Vec<u32, 2> = Vec::from_slice(&[stripe_count1, stripe_count2]).unwrap();
+        let accelerometer_data: RawAccelerometerData<NUM_ACCELEROMETERS, NUM_AXIS> =
+            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(()) => {
+                defmt::info!(
+                    "Iteration OK: displacement = {} m, velocity = {} m/s, acceleration = {} m/s**2",
+                    localizer.displacement(),
+                    localizer.velocity(),
+                    localizer.acceleration()
+                );
+            }
+            Err(e) => {
+                defmt::error!("Iteration error: {:?}", e);
+            }
+        }
+
+        Timer::after(Duration::from_millis(100)).await;
+    }
+}

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"
+libm = "0.2.11"
+defmt = "0.3"

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,
@@ -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(())
     }

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<AccelerometerData<NUM_ACCELEROMETERS>> {
         let accelerometer_data: AccelerometerData<NUM_ACCELEROMETERS> = 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,16 +160,18 @@ impl AccelerometerPreprocessor {
 
     pub fn get_quartiles<const SIZE: usize>(&self, data: &AccelerometerData<SIZE>) -> Quartiles {
         let mut sorted_data = data.clone();
-        sorted_data.sort_by(|a, b| a.partial_cmp(b).unwrap());
+        sorted_data
+            .as_mut_slice()
+            .sort_unstable_by(|a, b| a.partial_cmp(b).unwrap());
 
         let quartile_keys: Vec<f32, 3> = Vec::from_slice(&[0.25, 0.5, 0.75]).unwrap();
         let quartiles: Vec<f32, 3> = quartile_keys
             .iter()
             .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))