diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml index 83e2d931..9b7ac861 100644 --- a/firmware/Cargo.toml +++ b/firmware/Cargo.toml @@ -167,6 +167,7 @@ esp-wifi = { git = "https://github.com/esp-rs/esp-wifi.git", rev = "76ba312", fe "embedded-svc", "wifi", "embassy-net", + "ps-min-modem", ], optional = true } smoltcp = { version = "0.9", default-features = false, features = [ ], optional = true } @@ -213,6 +214,7 @@ nalgebra = { version = "0.31", default-features = false, features = [ "macros", "libm", ] } +approx = "0.5.1" fugit = "0.3" firmware_protocol = { path = "../networking/firmware_protocol", features = [ "nalgebra031", diff --git a/firmware/src/imu/mod.rs b/firmware/src/imu/mod.rs index 775e0b58..9b2f50bd 100644 --- a/firmware/src/imu/mod.rs +++ b/firmware/src/imu/mod.rs @@ -1,6 +1,7 @@ mod drivers; mod fusion; +use approx::AbsDiffEq; use defmt::{debug, info, trace, warn}; use embassy_executor::task; use firmware_protocol::ImuType; @@ -46,6 +47,7 @@ pub async fn imu_task( info!("Initialized IMU!"); let mut i = 0; + let mut prev_q = Quat::identity(); loop { let q = match imu.next_data().await { Ok(q) => q.q, @@ -64,7 +66,10 @@ pub async fn imu_task( ); } i += 1; - quat_signal.signal(q); + if !q.abs_diff_eq(&prev_q, 0.0001) { + prev_q = q; + quat_signal.signal(q); + } } }