From 187b23db82a9a4319fb32ab52c0b701ead348133 Mon Sep 17 00:00:00 2001
From: Sebastian <sebastian@sebastians-site.de>
Date: Sat, 3 Aug 2024 22:18:09 +0200
Subject: [PATCH] Added encoder readout

---
 Cargo.lock           |  9 ++++++
 firmware/Cargo.toml  |  1 +
 firmware/src/main.rs | 70 ++++++++++++++++++++++++++++++++++++++++++--
 3 files changed, 77 insertions(+), 3 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index e65aa56..67e6a78 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -23,6 +23,14 @@ version = "1.0.86"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b3d1d046238990b9cf5bcde22a3fb3584ee5cf65fb2765f454ed428c7a0063da"
 
+[[package]]
+name = "as5048a"
+version = "0.2.1"
+source = "git+https://github.com/LongHairedHacker/as5048a?rev=b15d716bf47ce4975a6cefebf82006c9b09e8fea#b15d716bf47ce4975a6cefebf82006c9b09e8fea"
+dependencies = [
+ "embedded-hal 0.2.7",
+]
+
 [[package]]
 name = "async-trait"
 version = "0.1.81"
@@ -968,6 +976,7 @@ dependencies = [
 name = "radomctl-firmware"
 version = "0.1.0"
 dependencies = [
+ "as5048a",
  "cortex-m",
  "defmt",
  "defmt-brtt",
diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml
index 9a45d24..ff5f101 100644
--- a/firmware/Cargo.toml
+++ b/firmware/Cargo.toml
@@ -19,6 +19,7 @@ ufmt = "0.2.0"
 qmc5883l = "0.0.1"
 rtic-monotonics = {version = "2.0.2", features = ["cortex-m-systick"]}
 xca9548a = "0.2.1"
+as5048a = { git = "https://github.com/LongHairedHacker/as5048a", rev="b15d716bf47ce4975a6cefebf82006c9b09e8fea"}
 
 [features]
 # set logging levels here
diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index 808959d..ee05b29 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -21,12 +21,16 @@ use rtic::app;
 )]
 mod app {
 
+    use core::ops::Deref;
+
+    use as5048a::AS5048A;
     use cortex_m::{asm, singleton};
     use stm32f4xx_hal::{
         gpio::{self, gpioa, gpiob, gpioc, Alternate, Analog, Input, OpenDrain, Output, PushPull},
         i2c,
-        pac::I2C1,
+        pac::{I2C1, SPI1},
         prelude::*,
+        spi,
     };
 
     use num_traits::{Float, FloatConst};
@@ -50,6 +54,12 @@ mod app {
     struct Local {
         i2cmux: Xca9548a<i2c::I2c<I2C1>>,
         board_led: gpioc::PC13<Output<PushPull>>,
+
+        encoder_az: AS5048A<spi::Spi<SPI1>, gpiob::PB12<Output<PushPull>>>,
+        encoder_el: AS5048A<spi::Spi<SPI1>, gpiob::PB13<Output<PushPull>>>,
+        spi_cs2: gpiob::PB14<Output<PushPull>>,
+        spi_cs3: gpiob::PB15<Output<PushPull>>,
+        spi1: spi::Spi<SPI1>,
     }
 
     #[init]
@@ -76,12 +86,14 @@ mod app {
 
         defmt::info!("Clock Setup done");
 
-        // Acquire the GPIOC peripheral
+        // Acquire the GPIO peripherials
+        let mut gpioa = cx.device.GPIOA.split();
         let mut gpiob = cx.device.GPIOB.split();
         let mut gpioc = cx.device.GPIOC.split();
 
         let board_led = gpioc.pc13.into_push_pull_output();
 
+        // Todo: Check if internal pullups work here
         let scl = gpiob.pb6.into_alternate_open_drain();
         let sda = gpiob.pb7.into_alternate_open_drain();
         let mut i2c = i2c::I2c::new(
@@ -100,13 +112,47 @@ mod app {
 
         defmt::info!("I2C MUX Setup done");
 
+        let spi_cs0 = gpiob.pb12.into_push_pull_output();
+        let encoder_az = AS5048A::new(spi_cs0);
+
+        let mut spi_cs1 = gpiob.pb13.into_push_pull_output();
+        let encoder_el = AS5048A::new(spi_cs1);
+
+        let mut spi_cs2 = gpiob.pb14.into_push_pull_output();
+        let mut spi_cs3 = gpiob.pb15.into_push_pull_output();
+
+        let mut sck = gpioa.pa5.into_push_pull_output();
+        let mut poci = gpioa.pa6;
+        let mut pico = gpioa.pa7.into_push_pull_output();
+        let spi1 = spi::Spi::new(
+            cx.device.SPI1,
+            (sck, poci, pico),
+            spi::Mode {
+                polarity: spi::Polarity::IdleLow,
+                phase: spi::Phase::CaptureOnFirstTransition,
+            },
+            8.MHz(),
+            &clocks,
+        );
+
+        defmt::info!("SPI Setup done");
+
         poll_i2c::spawn().ok();
+        poll_spi::spawn().ok();
 
         (
             Shared {
                 // Initialization of shared resources go here
             },
-            Local { i2cmux, board_led },
+            Local {
+                i2cmux,
+                board_led,
+                encoder_az,
+                encoder_el,
+                spi_cs2,
+                spi_cs3,
+                spi1,
+            },
         )
     }
 
@@ -183,4 +229,22 @@ mod app {
             Mono::delay(100.millis()).await;
         }
     }
+
+    #[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3])]
+    async fn poll_spi(cx: poll_spi::Context) {
+        let spi1 = cx.local.spi1;
+        let encoder_az = cx.local.encoder_az;
+
+        loop {
+            let (diag, gain) = encoder_az.diag_gain(spi1).unwrap();
+            defmt::info!("diag: {:08b} gain: {}", diag, gain);
+            defmt::info!("magnitude: {:?}", encoder_az.magnitude(spi1).unwrap());
+
+            let raw_angle = encoder_az.angle(spi1).unwrap();
+            let angle_deg = raw_angle as u32 * 3600 / 16384;
+            defmt::info!("angle: {:?}", angle_deg);
+
+            Mono::delay(100.millis()).await;
+        }
+    }
 }