Wip squash me

daemon/src/rotor.rs

@@ -38,7 +38,6 @@ impl Encoder<HostMessage> for ProtocolCodec {
     fn encode(&mut self, item: HostMessage, dst: &mut BytesMut) -> Result<(), Self::Error> {
         let msg_bytes = to_stdvec_cobs(&item).unwrap();
         dst.put(msg_bytes.as_slice());
-        dst.put_u8(0);
         Ok(())
     }
 }

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

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
@@ -22,13 +23,15 @@ fn panic() -> ! {
 mod app {
 
     use as5048a::AS5048A;
+    use embedded_hal::digital::OutputPin;
+    use rtic::Mutex;
 
-    use core::fmt::Write;
+    use core::{convert::Infallible, fmt::Write};
     use heapless::{String, Vec};
     use num_traits::{Float, FloatConst};
     use postcard::{from_bytes_cobs, to_vec_cobs};
     use stm32f4xx_hal::{
-        gpio::{gpioa, gpiob, gpioc, Output, PushPull},
+        gpio::{gpioa, gpiob, gpioc, AnyPin, Output, PushPull},
         i2c,
         otg_fs::{UsbBus, UsbBusType, USB},
         pac::{I2C1, SPI1},
@@ -46,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);
@@ -76,13 +82,13 @@ mod app {
         spi_cs3: gpiob::PB15<Output<PushPull>>,
         spi1: spi::Spi<SPI1>,
 
-        az_enable: gpiob::PB8<Output<PushPull>>,
-        az_dir: gpioa::PA15<Output<PushPull>>,
-        az_step: gpiob::PB3<Output<PushPull>>,
+        az_enable: AnyPin<Output<PushPull>>,
+        az_dir: AnyPin<Output<PushPull>>,
+        az_step: AnyPin<Output<PushPull>>,
 
-        el_enable: gpiob::PB4<Output<PushPull>>,
-        el_dir: gpioa::PA8<Output<PushPull>>,
-        el_step: gpioa::PA9<Output<PushPull>>,
+        el_enable: AnyPin<Output<PushPull>>,
+        el_dir: AnyPin<Output<PushPull>>,
+        el_step: AnyPin<Output<PushPull>>,
 
         usb_dev: UsbDevice<'static, UsbBusType>,
         usb_serial: SerialPort<'static, UsbBusType>,
@@ -206,15 +212,15 @@ mod app {
 
         defmt::info!("SPI Setup done");
 
-        let mut az_enable = gpiob.pb8.into_push_pull_output();
+        let mut az_enable = gpiob.pb8.into_push_pull_output().erase();
         az_enable.set_high();
-        let az_dir = gpioa.pa15.into_push_pull_output();
-        let az_step = gpiob.pb3.into_push_pull_output();
+        let az_dir = gpioa.pa15.into_push_pull_output().erase();
+        let az_step = gpiob.pb3.into_push_pull_output().erase();
 
-        let mut el_enable = gpiob.pb4.into_push_pull_output();
+        let mut el_enable = gpiob.pb4.into_push_pull_output().erase();
         el_enable.set_high();
-        let el_dir = gpioa.pa8.into_push_pull_output();
-        let el_step = gpioa.pa9.into_push_pull_output();
+        let el_dir = gpioa.pa8.into_push_pull_output().erase();
+        let el_step = gpioa.pa9.into_push_pull_output().erase();
 
         defmt::info!("Motor Setup done");
 
@@ -340,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,92 +378,26 @@ mod app {
 
     #[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])]
     async fn move_az(mut cx: move_az::Context) {
-        let az_enable = cx.local.az_enable;
-        let az_dir = cx.local.az_dir;
-        let az_step = cx.local.az_step;
-
-        loop {
-            let az_target = cx.shared.az_target.lock(|az_target| *az_target);
-            let az_angle = cx.shared.az_angle.lock(|az_angle| *az_angle);
-            let diff = az_angle - az_target;
-
-            defmt::info!(
-                "angle diff/target/actual: {:?}/{:?}/{:?}",
-                diff,
-                az_target,
-                az_angle
-            );
-
-            let delay = if diff.abs() < 10 {
-                10.millis()
-            } else if diff < 100 {
-                5.millis()
-            } else {
-                1.millis()
-            };
-
-            if diff.abs() > 50 {
-                az_enable.set_low();
-                if diff < 0 {
-                    az_dir.set_high();
-                } else {
-                    az_dir.set_low();
-                }
-
-                az_step.set_low();
-                Mono::delay(delay / 2).await;
-                az_step.set_high();
-                Mono::delay(delay / 2).await;
-            } else {
-                az_enable.set_high();
-                Mono::delay(delay).await;
-            }
-        }
+        move_axis(
+            cx.local.az_enable,
+            cx.local.az_dir,
+            cx.local.az_step,
+            &mut cx.shared.az_angle,
+            &mut cx.shared.az_target,
+        )
+        .await;
     }
 
     #[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])]
     async fn move_el(mut cx: move_el::Context) {
-        let el_enable = cx.local.el_enable;
-        let el_dir = cx.local.el_dir;
-        let el_step = cx.local.el_step;
-
-        loop {
-            let el_target = cx.shared.el_target.lock(|el_target| *el_target);
-            let el_angle = cx.shared.el_angle.lock(|el_angle| *el_angle);
-            let diff = el_angle - el_target;
-
-            defmt::info!(
-                "angle diff/target/actual: {:?}/{:?}/{:?}",
-                diff,
-                el_target,
-                el_angle
-            );
-
-            let delay = if diff.abs() < 10 {
-                10.millis()
-            } else if diff < 100 {
-                5.millis()
-            } else {
-                1.millis()
-            };
-
-            if diff.abs() > 50 {
-                el_enable.set_low();
-                if diff < 0 {
-                    el_dir.set_high();
-                } else {
-                    el_dir.set_low();
-                }
-
-                el_step.set_low();
-                Mono::delay(delay / 2).await;
-                el_step.set_high();
-                Mono::delay(delay / 2).await;
-            } else {
-                el_enable.set_high();
-                Mono::delay(delay).await;
-            }
-        }
+        move_axis(
+            cx.local.el_enable,
+            cx.local.el_dir,
+            cx.local.el_step,
+            &mut cx.shared.el_angle,
+            &mut cx.shared.el_target,
+        )
+        .await;
     }
 
     #[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer], shared=[az_target, el_target, az_angle, el_angle])]
@@ -514,7 +456,7 @@ mod app {
                             bootloader::reboot_to_bootloader();
                         }
                     },
-                    Err(err) => defmt::error!("Unable to parse host message"),
+                    Err(err) => defmt::error!("Unable to parse host message: {}", err),
                 };
 
                 *buffer = Vec::<u8, USB_BUFFER_SIZE>::from_slice(rest).unwrap();