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 4bcac8dd..09c10b8c 100644 --- a/boards/stm32f767zi/Cargo.toml +++ b/boards/stm32f767zi/Cargo.toml @@ -4,17 +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"] } @@ -23,14 +51,16 @@ 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"] } +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" hyped_core = { path = "../../lib/core" } hyped_sensors = { path = "../../lib/sensors" } +hyped_localisation = { path = "../../lib/localisation" } hyped_state_machine = { path = "../../lib/state_machine" } hyped_communications = { path = "../../lib/communications" } @@ -49,6 +79,10 @@ hyped_spi_derive = { path = "../../lib/io/hyped_spi/hyped_spi_derive" } name = "telemetry" path = "src/bin/boards/telemetry.rs" +[[bin]] +name = "localisation" +path = "src/bin/boards/localisation.rs" + [[bin]] name = "i2cdetect" path = "src/bin/tools/i2cdetect.rs" 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 new file mode 100644 index 00000000..50266488 --- /dev/null +++ b/boards/stm32f767zi/src/bin/boards/localisation.rs @@ -0,0 +1,142 @@ +#![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, Hertz}, +}; +use embassy_sync::{ + blocking_mutex::{ + raw::{CriticalSectionRawMutex, NoopRawMutex}, + Mutex, + }, + watch::Watch, +}; +use embassy_time::{Duration, Timer}; +use heapless::Vec; +use hyped_boards_stm32f767zi::{ + io::{Stm32f767ziGpioOutput, Stm32f767ziSpi}, + 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, 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, 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) -> ! { + // 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 hyped_spi = Stm32f767ziSpi::new(spi); + + let cs = HypedSpiCsPin::new(Stm32f767ziGpioOutput::new(Output::new( + p.PA4, + Level::High, + 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(); + + 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(); + + 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 = ACCELEROMETERS_DATA.receiver().unwrap(); + + let mut localizer = Localizer::new(); + + info!("Starting localizer loop..."); + + loop { + 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 }, + > = accelerometers_receiver.get().await; + + let optical_data = optical_flow_receiver.get().await; + + 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; + } +} 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 09bce116..ddb70352 100644 --- a/boards/stm32f767zi/src/tasks/sensors.rs +++ b/boards/stm32f767zi/src/tasks/sensors.rs @@ -1,6 +1,8 @@ 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; +pub mod read_optical_flow; pub mod read_temperature; 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/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: diff --git a/lib/communications/src/emergency.rs b/lib/communications/src/emergency.rs index ac9347ba..813c7462 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,9 +61,13 @@ 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) + Reason::try_from(9) ); } } diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index efbdef44..5df945a2 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 = "0.8.0" + libm = "0.2.11" defmt = "0.3" +heapless = { version = "0.8", default-features = false, features = ["serde"]} hyped_core = { path = "../core"} diff --git a/lib/localisation/src/control/localizer.rs b/lib/localisation/src/control/localizer.rs index 1c24d027..1e1715f8 100644 --- a/lib/localisation/src/control/localizer.rs +++ b/lib/localisation/src/control/localizer.rs @@ -24,10 +24,10 @@ const DELTA_T: f64 = 0.01; const STRIPE_WIDTH: f64 = 1.0; pub struct Localizer { - displacement: f64, - velocity: f64, - previous_velocity: f64, - acceleration: f64, + pub displacement: f64, + pub velocity: f64, + pub previous_velocity: f64, + pub acceleration: f64, kalman_filter: KalmanFilter, keyence_checker: KeyenceAgrees, keyence_val: f64, @@ -95,7 +95,7 @@ impl Default for Localizer { } } -#[derive(Debug)] +#[derive(Debug, defmt::Format)] pub enum PreprocessorError { KeyenceUnacceptable, AccelerometerUnnaceptable, diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index f18742c3..152affc2 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -2,8 +2,12 @@ use crate::{ config::{NUM_ACCELEROMETERS, NUM_ALLOWED_ACCELEROMETER_OUTLIERS, NUM_AXIS}, types::{AccelerometerData, RawAccelerometerData, SensorChecks}, }; + +/// For tests to work +#[allow(unused_imports)] use heapless::Vec; -use nalgebra::{ComplexField, SimdComplexField}; +#[cfg(test)] +use libm; /// Stores the quartiles of the data and the bounds for outliers /// which are calculated from the quartiles @@ -18,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 { @@ -121,14 +137,8 @@ impl AccelerometerPreprocessor { ) -> Option> { let accelerometer_data: AccelerometerData = data .iter() - .map(|axis| { - axis.iter() - .fold(0.0, |acc, val| acc + val * val) - .try_sqrt() - .expect("Error calculating magnitude of acceleration") - }) + .map(|axis| libm::sqrtf(axis.iter().fold(0.0, |acc, val| acc + val * val))) .collect(); - let clean_accelerometer_data = self.handle_outliers(accelerometer_data)?; if self.check_reliable() == SensorChecks::Unacceptable { @@ -162,23 +172,23 @@ impl AccelerometerPreprocessor { } pub fn get_quartiles(&self, data: &AccelerometerData) -> Quartiles { + // Clone and sort data let mut sorted_data = data.clone(); - sorted_data.sort_unstable_by(|a, b| a.partial_cmp(b).unwrap()); + insertion_sort(sorted_data.as_mut_slice()); - 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 = index_quartile.simd_floor() as usize - 1; - let index_quartile_ceil = index_quartile.simd_ceil() 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], @@ -192,6 +202,7 @@ impl AccelerometerPreprocessor { #[cfg(test)] mod tests { use super::*; + use libm; #[test] pub fn test_process_data() { @@ -209,10 +220,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(); @@ -220,10 +231,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::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] @@ -245,11 +256,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(); @@ -257,10 +267,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::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] 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()) ); }