From f38e652c0f1545120f1815e6d1ba9c7da22dbcd5 Mon Sep 17 00:00:00 2001
From: Sebastian <sebastian@sebastians-site.de>
Date: Sat, 3 Jan 2026 16:28:29 +0100
Subject: [PATCH] Wip squash me

---
 firmware/src/axis.rs | 77 ++++++++++++++++++++++++++++++++++++++++++++
 firmware/src/main.rs | 57 ++++----------------------------
 2 files changed, 84 insertions(+), 50 deletions(-)
 create mode 100644 firmware/src/axis.rs

diff --git a/firmware/src/axis.rs b/firmware/src/axis.rs
new file mode 100644
index 0000000..050c133
--- /dev/null
+++ b/firmware/src/axis.rs
@@ -0,0 +1,77 @@
+use as5048a::AS5048A;
+use embedded_hal::{digital::OutputPin, spi::SpiBus};
+use rtic_monotonics::Monotonic;
+use stm32f4xx_hal::{
+    gpio::{AnyPin, Output, PushPull},
+    prelude::*,
+};
+
+use crate::app::Mono;
+
+pub async fn move_axis<TargetMux, AngleMux>(
+    enable: &mut AnyPin<Output<PushPull>>,
+    dir: &mut AnyPin<Output<PushPull>>,
+    step: &mut AnyPin<Output<PushPull>>,
+    target: &mut TargetMux,
+    angle: &mut AngleMux,
+) where
+    TargetMux: rtic::Mutex<T = i32>,
+    AngleMux: rtic::Mutex<T = i32>,
+{
+    loop {
+        let tmp_target = target.lock(|target| *target);
+        let tmp_angle = angle.lock(|angle| *angle);
+        let diff = tmp_angle - tmp_target;
+
+        defmt::info!(
+            "angle diff/target/actual: {:?}/{:?}/{:?}",
+            diff,
+            tmp_target,
+            tmp_angle
+        );
+
+        let delay = if diff.abs() < 10 {
+            10.millis()
+        } else if diff < 100 {
+            5.millis()
+        } else {
+            1.millis()
+        };
+
+        if diff.abs() > 50 {
+            enable.set_low();
+            if diff < 0 {
+                dir.set_high();
+            } else {
+                dir.set_low();
+            }
+
+            step.set_low();
+            Mono::delay(delay / 2).await;
+            step.set_high();
+            Mono::delay(delay / 2).await;
+        } else {
+            enable.set_high();
+            Mono::delay(delay).await;
+        }
+    }
+}
+
+pub async fn update_encoder_angle<SPI, CS, AngleMux>(
+    encoder: &mut AS5048A<SPI, CS>,
+    spi: &mut SPI,
+    angle: &mut AngleMux,
+) where
+    SPI: SpiBus<u8>,
+    CS: OutputPin,
+    AngleMux: rtic::Mutex<T = i32>,
+{
+    let raw_angle = encoder.angle(spi).unwrap();
+    let angle_deg = raw_angle as i32 * 3600 / 16384;
+
+    angle.lock(|az_angle| {
+        *az_angle = angle_deg;
+    });
+
+    defmt::info!("angle: {:?}", angle_deg);
+}
diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index 8f572af..6c70309 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -6,6 +6,7 @@ use defmt_brtt as _; // global logger
 use panic_probe as _;
 use stm32f4xx_hal as _;
 
+mod axis;
 mod bootloader;
 
 // same panicking *behavior* as `panic-probe` but doesn't print a panic message
@@ -48,7 +49,10 @@ mod app {
 
     use radomctl_protocol::{HostMessage, *};
 
-    use crate::bootloader;
+    use crate::{
+        axis::{move_axis, update_encoder_angle},
+        bootloader,
+    };
     use rtic_monotonics::systick::prelude::*;
 
     systick_monotonic!(Mono, 4000);
@@ -342,6 +346,8 @@ mod app {
         let encoder_az = cx.local.encoder_az;
         let encoder_el = cx.local.encoder_el;
 
+        update_encoder_angle(encoder_az, spi1, &mut cx.shared.el_angle).await;
+
         loop {
             /*
             let (diag, gain) = encoder_az.diag_gain(spi1).unwrap();
@@ -370,55 +376,6 @@ mod app {
         }
     }
 
-    async fn move_axis<TargetMux, AngleMux>(
-        enable: &mut AnyPin<Output<PushPull>>,
-        dir: &mut AnyPin<Output<PushPull>>,
-        step: &mut AnyPin<Output<PushPull>>,
-        target: &mut TargetMux,
-        angle: &mut AngleMux,
-    ) where
-        TargetMux: rtic::Mutex<T = i32>,
-        AngleMux: rtic::Mutex<T = i32>,
-    {
-        loop {
-            let tmp_target = target.lock(|target| *target);
-            let tmp_angle = angle.lock(|angle| *angle);
-            let diff = tmp_angle - tmp_target;
-
-            defmt::info!(
-                "angle diff/target/actual: {:?}/{:?}/{:?}",
-                diff,
-                tmp_target,
-                tmp_angle
-            );
-
-            let delay = if diff.abs() < 10 {
-                10.millis()
-            } else if diff < 100 {
-                5.millis()
-            } else {
-                1.millis()
-            };
-
-            if diff.abs() > 50 {
-                enable.set_low();
-                if diff < 0 {
-                    dir.set_high();
-                } else {
-                    dir.set_low();
-                }
-
-                step.set_low();
-                Mono::delay(delay / 2).await;
-                step.set_high();
-                Mono::delay(delay / 2).await;
-            } else {
-                enable.set_high();
-                Mono::delay(delay).await;
-            }
-        }
-    }
-
     #[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])]
     async fn move_az(mut cx: move_az::Context) {
         move_axis(