From f032c146c0cc24528aa4fb89947b5eecb5f3e5a8 Mon Sep 17 00:00:00 2001
From: Niklas Kapelle <niklas@kapelle.org>
Date: Thu, 21 May 2026 14:15:25 +0200
Subject: [PATCH] calculate the ips based on the magnetic field strength

---
 Cargo.lock  |  7 +++++++
 Cargo.toml  |  1 +
 src/main.rs | 32 +++++++++++++++++++++++++++++---
 3 files changed, 37 insertions(+), 3 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index ff4e200..f306307 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -277,6 +277,7 @@ dependencies = [
  "embedded-graphics",
  "embedded-hal-bus",
  "heapless 0.9.2",
+ "libm",
  "log",
  "mipidsi",
  "mmc56x3",
@@ -829,6 +830,12 @@ version = "0.2.180"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "bcc35a38544a891a5f7c865aca548a982ccb3b8650a5b06d0fd33a10283c56fc"
 
+[[package]]
+name = "libm"
+version = "0.2.16"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981"
+
 [[package]]
 name = "litrs"
 version = "1.0.0"
diff --git a/Cargo.toml b/Cargo.toml
index 5efc727..6f553ad 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -27,6 +27,7 @@ embedded-graphics = "0.8.1"
 mmc56x3 = "0.1.0"
 static_cell = { version = "2.1.1" }
 portable-atomic = { version = "1.13.1", features = ["critical-section"] }
+libm = "0.2.16"
 
 [profile.dev]
 codegen-units = 1
diff --git a/src/main.rs b/src/main.rs
index aa5c0c0..1f62b58 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -5,6 +5,7 @@ use embassy_executor::Spawner;
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
 use embassy_time::{Duration, Timer};
 use embedded_graphics::draw_target::DrawTargetExt;
+use heapless::Deque;
 use log::error;
 
 use crate::{
@@ -45,6 +46,7 @@ async fn main(spawner: Spawner) {
 
     spawner.must_spawn(measure_task(mag_sens));
     spawner.must_spawn(display_task(display));
+    spawner.must_spawn(proccess_readings());
 
     Timer::after_secs(30).await;
     init::reboot().await;
@@ -52,7 +54,7 @@ async fn main(spawner: Spawner) {
 
 #[embassy_executor::task]
 async fn measure_task(mut sensor: MagSensor) {
-    let sample_window = Duration::from_millis(500);
+    let sample_window = Duration::from_millis(200);
 
     loop {
         match sensor.read_messurement().await {
@@ -72,8 +74,8 @@ async fn measure_task(mut sensor: MagSensor) {
 async fn display_task(mut display: SPIDisplay) {
     let mut como_display = CoMoDisplay::new(display.color_converted());
     loop {
-        let _reading = DISPLAY_CHANNEL.receive().await;
-        match como_display.draw() {
+        let reading = DISPLAY_CHANNEL.receive().await;
+        match como_display.draw(reading) {
             Ok(_) => {}
             Err(_) => {
                 error!("Failed to draw");
@@ -81,3 +83,27 @@ async fn display_task(mut display: SPIDisplay) {
         }
     }
 }
+
+#[embassy_executor::task]
+async fn proccess_readings() {
+    let mut fifo: Deque<f32, 20> = Deque::new();
+    DISPLAY_CHANNEL.send(15_f32).await;
+
+    loop {
+        let reading = SENSOR_CHANNEL.receive().await;
+        let strength =
+            libm::sqrtf(reading.x * reading.x + reading.y * reading.y + reading.z * reading.z);
+        let ips = strength * 0.15;
+
+        if fifo.is_full()
+            && let Some(delayed_value) = fifo.pop_front()
+        {
+            if DISPLAY_CHANNEL.is_full() {
+                let _ = DISPLAY_CHANNEL.try_receive();
+            }
+            DISPLAY_CHANNEL.send(delayed_value).await;
+        }
+
+        fifo.push_back(ips).ok();
+    }
+}