daemon/src/rotor.rs

@@ -38,6 +38,7 @@ 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

@@ -1,77 +0,0 @@
-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,7 +6,6 @@ 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
@@ -23,15 +22,13 @@ fn panic() -> ! {
 mod app {
 
     use as5048a::AS5048A;
-    use embedded_hal::digital::OutputPin;
-    use rtic::Mutex;
 
-    use core::{convert::Infallible, fmt::Write};
+    use core::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, AnyPin, Output, PushPull},
+        gpio::{gpioa, gpiob, gpioc, Output, PushPull},
         i2c,
         otg_fs::{UsbBus, UsbBusType, USB},
         pac::{I2C1, SPI1},
@@ -49,10 +46,7 @@ mod app {
 
     use radomctl_protocol::{HostMessage, *};
 
-    use crate::{
-        axis::{move_axis, update_encoder_angle},
-        bootloader,
-    };
+    use crate::bootloader;
     use rtic_monotonics::systick::prelude::*;
 
     systick_monotonic!(Mono, 4000);
@@ -82,13 +76,13 @@ mod app {
         spi_cs3: gpiob::PB15<Output<PushPull>>,
         spi1: spi::Spi<SPI1>,
 
-        az_enable: AnyPin<Output<PushPull>>,
-        az_dir: AnyPin<Output<PushPull>>,
-        az_step: AnyPin<Output<PushPull>>,
+        az_enable: gpiob::PB8<Output<PushPull>>,
+        az_dir: gpioa::PA15<Output<PushPull>>,
+        az_step: gpiob::PB3<Output<PushPull>>,
 
-        el_enable: AnyPin<Output<PushPull>>,
-        el_dir: AnyPin<Output<PushPull>>,
-        el_step: AnyPin<Output<PushPull>>,
+        el_enable: gpiob::PB4<Output<PushPull>>,
+        el_dir: gpioa::PA8<Output<PushPull>>,
+        el_step: gpioa::PA9<Output<PushPull>>,
 
         usb_dev: UsbDevice<'static, UsbBusType>,
         usb_serial: SerialPort<'static, UsbBusType>,
@@ -212,15 +206,15 @@ mod app {
 
         defmt::info!("SPI Setup done");
 
-        let mut az_enable = gpiob.pb8.into_push_pull_output().erase();
+        let mut az_enable = gpiob.pb8.into_push_pull_output();
         az_enable.set_high();
-        let az_dir = gpioa.pa15.into_push_pull_output().erase();
-        let az_step = gpiob.pb3.into_push_pull_output().erase();
+        let az_dir = gpioa.pa15.into_push_pull_output();
+        let az_step = gpiob.pb3.into_push_pull_output();
 
-        let mut el_enable = gpiob.pb4.into_push_pull_output().erase();
+        let mut el_enable = gpiob.pb4.into_push_pull_output();
         el_enable.set_high();
-        let el_dir = gpioa.pa8.into_push_pull_output().erase();
-        let el_step = gpioa.pa9.into_push_pull_output().erase();
+        let el_dir = gpioa.pa8.into_push_pull_output();
+        let el_step = gpioa.pa9.into_push_pull_output();
 
         defmt::info!("Motor Setup done");
 
@@ -346,8 +340,6 @@ 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();
@@ -378,26 +370,92 @@ mod app {
 
     #[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])]
     async fn move_az(mut cx: move_az::Context) {
-        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;
+        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;
+            }
+        }
     }
 
     #[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])]
     async fn move_el(mut cx: move_el::Context) {
-        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;
+        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;
+            }
+        }
     }
 
     #[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer], shared=[az_target, el_target, az_angle, el_angle])]
@@ -456,7 +514,7 @@ mod app {
                             bootloader::reboot_to_bootloader();
                         }
                     },
-                    Err(err) => defmt::error!("Unable to parse host message: {}", err),
+                    Err(err) => defmt::error!("Unable to parse host message"),
                 };
 
                 *buffer = Vec::<u8, USB_BUFFER_SIZE>::from_slice(rest).unwrap();