Added encoder readout

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;
+        }
+    }
 }