First end-to-end movement

.gitignore

@@ -1,2 +1,3 @@
 /target
-/*/output.log
+/*/output.log
+*.bin

Cargo.toml

@@ -4,6 +4,7 @@ resolver = "2"
 members = [ 
     "daemon",
     "firmware",
+    "protocol",
 ]
 
 

daemon/Cargo.toml

@@ -11,8 +11,21 @@ humantime = "2.1.0"
 log = "0.4.21"
 nom = "7.1.3"
 serde_json = "1.0.118"
+serialport = "4.5.1"
 tokio = {version = "1.37.0", features = ["full"]}
 tokio-macros = { version = "0.2.0-alpha.6" }
 tower-http = { version = "0.5.2", features = ["fs", "trace"] }
 tracing = "0.1.40"
 tracing-subscriber = "0.3.18"
+radomctl-protocol = { path = "../protocol" }
+postcard = {version = "1.0.10", features = ["use-std"]}
+dfu-libusb = "0.3.0"
+libusb1-sys = "0.6"
+rusb = "0.9"
+clap = { version = "4.5.19", features = ["derive"] }
+indicatif = "0.17.8"
+tokio-serial = {version = "5.4.4", features = ["codec", "rt"] }
+tokio-util = { version = "0.7.13", features = ["codec", "rt"] }
+bytes = "1.9.0"
+futures-util = "0.3.31"
+futures = "0.3.31"

daemon/src/bin/flash-dfu.rs

@@ -0,0 +1,112 @@
+use std::{
+    io::{self, Seek},
+    thread,
+    time::{self, Duration},
+};
+
+use anyhow::{anyhow, Context};
+use clap::Parser;
+use dfu_libusb::{Dfu, DfuLibusb};
+use postcard::{from_bytes_cobs, to_stdvec_cobs};
+use radomctl_protocol::*;
+use radomctld::logger::setup_logger;
+
+#[derive(Parser)]
+struct Cli {
+    /// The usb serial number of the radom-controller
+    #[arg(short, long)]
+    serialnumber: String,
+
+    /// The firmware file to flash
+    #[arg(short, long)]
+    firmware: std::path::PathBuf,
+}
+
+pub fn main() -> anyhow::Result<()> {
+    let args = Cli::parse();
+
+    let ports = serialport::available_ports().unwrap_or(Vec::<serialport::SerialPortInfo>::new());
+
+    let mut radom_port: Option<String> = None;
+    for port in ports {
+        match port.port_type {
+            serialport::SerialPortType::UsbPort(usb_port_info) => {
+                match usb_port_info.serial_number {
+                    Some(serial) => {
+                        if serial == args.serialnumber {
+                            radom_port = Some(port.port_name.to_owned());
+                            println!("Found radom-controller as {}", port.port_name)
+                        }
+                    }
+                    None => continue,
+                }
+            }
+            _ => continue,
+        }
+    }
+
+    let radom_port = match radom_port {
+        Some(port) => port,
+        _ => {
+            return Err(anyhow!("No matching port found."));
+        }
+    };
+
+    println!("Setting radom-controller to dfu boot...");
+
+    let mut port = serialport::new(radom_port, 115_200)
+        .timeout(Duration::from_millis(10))
+        .open()
+        .expect("Failed to open port");
+
+    let host_msg = HostMessage::TriggerDFUBootloader;
+    let msg_bytes = to_stdvec_cobs(&host_msg).unwrap();
+    port.write_all(&msg_bytes).unwrap();
+    drop(port);
+
+    let context = rusb::Context::new()?;
+    let mut file = std::fs::File::open(args.firmware).context("firmware file not found")?;
+
+    thread::sleep(time::Duration::from_millis(2000));
+
+    let file_size =
+        u32::try_from(file.seek(io::SeekFrom::End(0))?).context("the firmware file is too big")?;
+    file.seek(io::SeekFrom::Start(0))?;
+
+    let vid = 0x0483;
+    let pid = 0xdf11;
+    let intf = 0;
+    let alt = 0;
+    let mut device: Dfu<rusb::Context> =
+        DfuLibusb::open(&context, vid, pid, intf, alt).context("could not open device")?;
+
+    println!("Flashing radom-controller ...");
+
+    let bar = indicatif::ProgressBar::new(file_size as u64);
+    bar.set_style(
+        indicatif::ProgressStyle::default_bar()
+            .template(
+                "{spinner:.green} [{elapsed_precise}] [{bar}] \
+                        {bytes}/{total_bytes} ({bytes_per_sec}) ({eta}) {msg:10}",
+            )?
+            .progress_chars("=>-"),
+    );
+
+    device.with_progress({
+        let bar = bar.clone();
+        move |count| {
+            bar.inc(count as u64);
+            if bar.position() == file_size as u64 {
+                bar.finish();
+            }
+        }
+    });
+
+    device
+        .download(file, file_size)
+        .context("could not write firmware to the device")?;
+
+    println!("Done!");
+
+    Ok(())
+}

daemon/src/lib.rs

@@ -0,0 +1,3 @@
+pub mod logger;
+pub mod rotctlprotocol;
+pub mod rotor;

daemon/src/main.rs

@@ -1,11 +1,16 @@
-mod logger;
-mod rotctlprotocol;
-mod rotor;
-
 use anyhow::Result;
+use anyhow::{anyhow, Context};
+use axum::{
+    extract::State,
+    http::StatusCode,
+    routing::{get, post},
+    Json, Router,
+};
+use clap::Parser;
 use fern::colors::{Color, ColoredLevelConfig};
 use log::{debug, error, info, warn, Level};
 use serde_json::{json, Value};
+use std::time::Duration;
 use std::{borrow::Borrow, io};
 use tokio::{
     self,
@@ -14,22 +19,17 @@ use tokio::{
     sync::{mpsc, watch},
     task::JoinSet,
 };
-
-use axum::{
-    extract::State,
-    http::StatusCode,
-    routing::{get, post},
-    Json, Router,
-};
+use tokio_serial;
 use tower_http::{
     services::{ServeDir, ServeFile},
     trace::TraceLayer,
 };
 
-use logger::setup_logger;
-use rotor::control_rotor;
-
-use rotctlprotocol::{parse_command, Command};
+use radomctld::{
+    logger::setup_logger,
+    rotctlprotocol::{parse_command, Command},
+    rotor::control_rotor,
+};
 
 async fn process_socket(
     socket: TcpStream,
@@ -89,16 +89,61 @@ struct AxumAppState {
     pos_rx: watch::Receiver<(f32, f32)>,
 }
 
+#[derive(Parser)]
+struct Cli {
+    /// The usb serial number of the radom-controller
+    #[arg(short, long)]
+    serialnumber: String,
+
+    /// Listen address for the webserver
+    #[arg(short, long, default_value = "0.0.0.0:8000")]
+    web_listen_address: String,
+
+    /// Listen address for rotctl
+    #[arg(short, long, default_value = "0.0.0.0:1337")]
+    rotctl_listen_address: String,
+}
+
 #[tokio::main]
 async fn main() -> Result<()> {
     setup_logger()?;
 
+    let args = Cli::parse();
+
+    let ports = tokio_serial::available_ports().unwrap_or(Vec::<serialport::SerialPortInfo>::new());
+
+    let mut radom_port: Option<String> = None;
+    for port in ports {
+        match port.port_type {
+            serialport::SerialPortType::UsbPort(usb_port_info) => {
+                match usb_port_info.serial_number {
+                    Some(serial) => {
+                        debug!("Found a serial port with: {}", serial);
+                        if serial == args.serialnumber {
+                            radom_port = Some(port.port_name.to_owned());
+                            info!("Found radom-controller as {}", port.port_name)
+                        }
+                    }
+                    None => continue,
+                }
+            }
+            _ => continue,
+        }
+    }
+
+    let radom_port = match radom_port {
+        Some(port) => port,
+        _ => {
+            return Err(anyhow!("No matching port found."));
+        }
+    };
+
     let (cmd_tx, cmd_rx) = mpsc::channel::<Command>(16);
     let (pos_tx, pos_rx) = watch::channel::<(f32, f32)>((0.0, 0.0));
 
     let mut tasks = JoinSet::new();
 
-    tasks.spawn(async move { control_rotor(cmd_rx, pos_tx).await });
+    tasks.spawn(async move { control_rotor(cmd_rx, pos_tx, radom_port).await });
 
     let state = AxumAppState {
         pos_rx: pos_rx.clone(),
@@ -111,14 +156,14 @@ async fn main() -> Result<()> {
             .with_state(state)
             .layer(TraceLayer::new_for_http());
 
-        let listener = tokio::net::TcpListener::bind("0.0.0.0:8000").await?;
+        let listener = tokio::net::TcpListener::bind(args.web_listen_address).await?;
         axum::serve(listener, app).await?;
 
         Ok(())
     });
 
     tasks.spawn(async move {
-        let listener = TcpListener::bind("127.0.0.1:1337").await?;
+        let listener = TcpListener::bind(args.rotctl_listen_address).await?;
 
         loop {
             let (socket, _) = listener.accept().await?;

daemon/src/rotor.rs

@@ -1,5 +1,11 @@
 use anyhow::Result;
+use bytes::{BufMut, BytesMut};
+use futures::{stream::StreamExt, SinkExt};
 use log::{debug, error, info, warn};
+use postcard::{from_bytes_cobs, to_stdvec_cobs};
+use radomctl_protocol::{HostMessage, PositionTarget, RadomMessage};
+use std::{env, io, str, time::Duration};
+use tokio::time::sleep;
 use tokio::{
     self,
     io::{AsyncBufReadExt, AsyncWriteExt, BufStream},
@@ -7,46 +13,82 @@ use tokio::{
     sync::{self, mpsc, watch},
     time,
 };
+use tokio_serial::SerialPortBuilderExt;
+use tokio_util::codec::{Decoder, Encoder};
 
 use crate::rotctlprotocol::{parse_command, Command};
 
+struct ProtocolCodec;
+
+impl Decoder for ProtocolCodec {
+    type Item = RadomMessage;
+    type Error = io::Error;
+
+    fn decode(&mut self, src: &mut BytesMut) -> Result<Option<Self::Item>, Self::Error> {
+        let frame_end = src.as_ref().iter().position(|b| *b == 0);
+        if let Some(n) = frame_end {
+            let mut frame = src.split_to(n + 1);
+            let host_msg = from_bytes_cobs::<RadomMessage>(&mut frame).unwrap();
+            return Ok(Some(host_msg));
+        }
+        Ok(None)
+    }
+}
+
+impl Encoder<HostMessage> for ProtocolCodec {
+    type Error = io::Error;
+
+    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(())
+    }
+}
+
 pub async fn control_rotor(
     mut rx_cmd: mpsc::Receiver<Command>,
     pos_tx: watch::Sender<(f32, f32)>,
+    radom_port: String,
 ) -> Result<()> {
-    let mut actual_az = 0.0;
-    let mut actual_el = 0.0;
+    let port = tokio_serial::new(radom_port, 115_200)
+        .timeout(Duration::from_millis(10))
+        .open_native_async()
+        .expect("Failed to open port");
 
-    let mut target_az = 0.0;
-    let mut target_el = 0.0;
+    let (mut port_writer, mut port_reader) = ProtocolCodec.framed(port).split();
 
     loop {
         tokio::select! {
             Some(command) = rx_cmd.recv() => {
                 match command {
                     Command::SetPos(az, el) => {
-                        info!("Received set pos {} {}", az, el);
-                        target_az = az;
-                        target_el = el;
+                        //info!("Received set pos {} {}", az, el);
+                        port_writer.send(HostMessage::SetTarget(PositionTarget { az, el })).await?;
                     }
                     _ => {}
                 }
             },
+
             _ = time::sleep(time::Duration::from_millis(100)) => {
-                if target_az < actual_az {
-                    actual_az -= 1.0;
-                } else if  target_az > actual_az  {
-                    actual_az += 1.0;
-                }
-
-                if target_el < actual_el {
-                    actual_el -= 1.0;
-                } else if  target_el > actual_el  {
-                    actual_el += 1.0;
-                }
-
-                pos_tx.send((actual_az, actual_el)).unwrap();
+                    //info!("Requesting status");
+                    port_writer.send(HostMessage::RequestStatus).await?;
             },
+
+            msg = port_reader.next() => {
+                match msg {
+                    Some(Ok(msg)) => {
+                        match msg {
+                            RadomMessage::Status(status) => {
+                                //info!("Received status {:?}", status);
+                                pos_tx.send((status.position.az, status.position.el)).unwrap();
+                            }
+                            _ => {}
+                        }
+                    }
+                    _ => {}
+                }
+            }
             else => return Ok(())
         };
     }

firmware/.cargo/config.toml

@@ -1,6 +1,6 @@
 [target.'cfg(all(target_arch = "arm", target_os = "none"))']
 # TODO(2) replace `$CHIP` with your chip's name (see `probe-run --list-chips` output)
-runner = "probe-run --chip STM32F401CCU6"
+runner = "probe-rs run --chip STM32F401CCUx"
 rustflags = [
   "-C", "linker=flip-link",
   "-C", "link-arg=-Tlink.x",
@@ -18,4 +18,45 @@ rb = "run --bin"
 rrb = "run --release --bin"
 
 [env]
-DEFMT_LOG = "debug"
+DEFMT_LOG = "debug"
+
+
+
+# cargo build/run
+[profile.dev]
+codegen-units = 1
+debug = 2
+debug-assertions = true # <-
+incremental = false
+opt-level = 'z'         # <-
+overflow-checks = true  # <-
+
+# cargo test
+[profile.test]
+codegen-units = 1
+debug = 2
+debug-assertions = true # <-
+incremental = false
+opt-level = 3           # <-
+overflow-checks = true  # <-
+
+# cargo build/run --release
+[profile.release]
+codegen-units = 1
+debug = 2
+debug-assertions = false # <-
+incremental = false
+lto = 'fat'
+opt-level = 3            # <-
+overflow-checks = false  # <-
+
+# cargo test --release
+[profile.bench]
+codegen-units = 1
+debug = 2
+debug-assertions = false # <-
+incremental = false
+lto = 'fat'
+opt-level = 3            # <-
+overflow-checks = false  # <-
+

firmware/Cargo.toml

@@ -10,7 +10,7 @@ defmt-brtt = { version = "0.1", default-features = false, features = ["rtt"] }
 panic-probe = { version = "0.3", features = ["print-defmt"] }
 rtic = { version = "2.0.1", features = [ "thumbv7-backend" ] }
 defmt-rtt = "0.4"
-stm32f4xx-hal = { version = "0.21.0", features = ["stm32f401"] }
+stm32f4xx-hal = { version = "0.21.0", features = ["stm32f401", "usb_fs"] }
 embedded-hal = {version = "1.0.0"}
 nb = "1.0.0"
 num-traits = { version = "0.2", default-features = false, features = ["libm"] }
@@ -20,6 +20,12 @@ 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"}
+usb-device = "0.3.2"
+usbd-serial = "0.2.2"
+postcard = {version = "1.0.10", features = ["use-defmt"]} 
+heapless = {version = "0.8.0", features = ["defmt-03"]}
+radomctl-protocol = { path = "../protocol" }
+
 
 [features]
 # set logging levels here
@@ -32,50 +38,3 @@ defmt-debug = []
 defmt-info = []
 defmt-warn = []
 defmt-error = []
-
-# cargo build/run
-[profile.dev]
-codegen-units = 1
-debug = 2
-debug-assertions = true # <-
-incremental = false
-opt-level = 'z'         # <-
-overflow-checks = true  # <-
-
-# cargo test
-[profile.test]
-codegen-units = 1
-debug = 2
-debug-assertions = true # <-
-incremental = false
-opt-level = 3           # <-
-overflow-checks = true  # <-
-
-# cargo build/run --release
-[profile.release]
-codegen-units = 1
-debug = 2
-debug-assertions = false # <-
-incremental = false
-lto = 'fat'
-opt-level = 3            # <-
-overflow-checks = false  # <-
-
-# cargo test --release
-[profile.bench]
-codegen-units = 1
-debug = 2
-debug-assertions = false # <-
-incremental = false
-lto = 'fat'
-opt-level = 3            # <-
-overflow-checks = false  # <-
-
-
-# uncomment this to switch from the crates.io version of defmt to its git version
-# check app-template's README for instructions
-# [patch.crates-io]
-# defmt = { git = "https://github.com/knurling-rs/defmt", rev = "use defmt version supported by probe-rs (see changelog)" }
-# defmt-rtt = { git = "https://github.com/knurling-rs/defmt", rev = "use defmt version supported by probe-rs (see changelog)" }
-# defmt-test = { git = "https://github.com/knurling-rs/defmt", rev = "use defmt version supported by probe-rs (see changelog)" }
-# panic-probe = { git = "https://github.com/knurling-rs/defmt", rev = "use defmt version supported by probe-rs (see changelog)" }

firmware/flash-dfu.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+cargo build --release && \
+arm-none-eabi-objcopy -O binary ../target/thumbv7em-none-eabihf/release/radomctl-firmware radomctl-firmware.bin && \
+dfu-util --alt 0 -s 0x08000000:leave -D radomctl-firmware.bin

firmware/src/bootloader.rs

@@ -0,0 +1,54 @@
+use core::{mem::MaybeUninit, ptr::addr_of_mut};
+
+use stm32f4xx_hal::pac;
+
+fn jump_to_bootloader() {
+    unsafe {
+        cortex_m::interrupt::disable();
+
+        let address: u32 = 0x1FFF0000;
+
+        let device = pac::Peripherals::steal();
+        device.SYSCFG.memrm.modify(|_, w| w.bits(0x01));
+
+        let mut p = cortex_m::Peripherals::steal();
+        p.SCB.invalidate_icache();
+        p.SCB.vtor.write(address as u32);
+
+        cortex_m::interrupt::enable();
+
+        cortex_m::asm::bootload(address as *const u32);
+    }
+}
+
+const BOOTLOADER_REQUESTED: u32 = 0xdecafbad;
+
+#[link_section = ".uninit.DFU_FLAG"]
+static mut DFU_FLAG: MaybeUninit<u32> = MaybeUninit::uninit();
+
+fn get_bootloader_flag() -> u32 {
+    unsafe { DFU_FLAG.assume_init() }
+}
+
+fn set_bootloader_flag() {
+    unsafe { DFU_FLAG.write(BOOTLOADER_REQUESTED) };
+}
+
+fn clear_bootloader_flag() {
+    unsafe { DFU_FLAG.write(0) };
+}
+
+pub fn init() {
+    let requested = get_bootloader_flag() == BOOTLOADER_REQUESTED;
+    if requested {
+        jump_to_bootloader();
+    }
+
+    clear_bootloader_flag();
+}
+
+pub fn reboot_to_bootloader() -> ! {
+    defmt::info!("Rebooting into the bootloader");
+    set_bootloader_flag();
+    cortex_m::peripheral::SCB::sys_reset();
+}

firmware/src/main.rs

@@ -6,6 +6,8 @@ use defmt_brtt as _; // global logger
 use panic_probe as _;
 use stm32f4xx_hal as _;
 
+mod bootloader;
+
 // same panicking *behavior* as `panic-probe` but doesn't print a panic message
 // this prevents the panic message being printed *twice* when `defmt::panic` is invoked
 #[defmt::panic_handler]
@@ -13,39 +15,52 @@ fn panic() -> ! {
     cortex_m::asm::udf()
 }
 
-
 #[rtic::app(
     device = stm32f4xx_hal::pac,
     dispatchers = [SPI3]
 )]
 mod app {
 
-    
-
     use as5048a::AS5048A;
-    
+
+    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, Output, PushPull},
         i2c,
+        otg_fs::{UsbBus, UsbBusType, USB},
         pac::{I2C1, SPI1},
         prelude::*,
-        spi,
+        signature, spi,
     };
-
-    use num_traits::{Float, FloatConst};
+    use usb_device::prelude::*;
+    use usb_device::{class_prelude::UsbBusAllocator, device};
+    use usbd_serial::SerialPort;
 
     use xca9548a::{SlaveAddr, Xca9548a};
 
     use qmc5883l::{self, QMC5883L};
 
+    use radomctl_protocol::{HostMessage, *};
+
+    use crate::bootloader;
     use rtic_monotonics::systick::prelude::*;
 
-    systick_monotonic!(Mono, 1000);
+    systick_monotonic!(Mono, 4000);
+
+    const USB_BUFFER_SIZE: usize = 64;
 
     // Shared resources go here
     #[shared]
     struct Shared {
         az_angle: i32,
+        az_compass: i32,
+        az_target: i32,
+
+        el_angle: i32,
+        el_target: i32,
     }
 
     // Local resources go here
@@ -60,17 +75,23 @@ mod app {
         spi_cs3: gpiob::PB15<Output<PushPull>>,
         spi1: spi::Spi<SPI1>,
 
-        az_enable: gpioa::PA12<Output<PushPull>>,
+        az_enable: gpiob::PB8<Output<PushPull>>,
         az_dir: gpioa::PA15<Output<PushPull>>,
         az_step: gpiob::PB3<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>,
+        usb_buffer: Vec<u8, USB_BUFFER_SIZE>,
     }
 
     #[init]
     fn init(cx: init::Context) -> (Shared, Local) {
+        bootloader::init();
+
         defmt::info!("init");
 
         let rcc = cx.device.RCC.constrain();
@@ -80,19 +101,14 @@ mod app {
         let clocks = rcc
             .cfgr
             .use_hse(25.MHz())
-            .require_pll48clk()
             .sysclk(84.MHz())
-            .hclk(84.MHz())
-            .pclk1(42.MHz())
-            .pclk2(84.MHz())
+            .require_pll48clk()
             .freeze();
 
         Mono::start(cx.core.SYST, clocks.sysclk().to_Hz());
 
         defmt::info!("Clock Setup done");
 
-        defmt::info!("Clock Setup done");
-
         // Acquire the GPIO peripherials
         let gpioa = cx.device.GPIOA.split();
         let gpiob = cx.device.GPIOB.split();
@@ -100,6 +116,58 @@ mod app {
 
         let board_led = gpioc.pc13.into_push_pull_output();
 
+        defmt::info!("Basic gpio setup done");
+
+        static mut EP_MEMORY: [u32; 1024] = [0; 1024];
+        static mut USB_BUS: Option<usb_device::bus::UsbBusAllocator<UsbBusType>> = None;
+
+        let usb = USB::new(
+            (
+                cx.device.OTG_FS_GLOBAL,
+                cx.device.OTG_FS_DEVICE,
+                cx.device.OTG_FS_PWRCLK,
+            ),
+            (gpioa.pa11, gpioa.pa12),
+            &clocks,
+        );
+
+        unsafe {
+            USB_BUS.replace(UsbBus::new(usb, &mut EP_MEMORY));
+        }
+
+        let usb_serial = usbd_serial::SerialPort::new(unsafe { USB_BUS.as_ref().unwrap() });
+
+        let serial = unsafe {
+            let u_id0 = 0x1FFF_7A10 as *const u32;
+            let u_id2 = 0x1FFF_7A18 as *const u32;
+
+            defmt::debug!("UID0: {:x}", u_id0.read());
+            defmt::debug!("UID2: {:x}", u_id2.read());
+
+            // See https://community.st.com/t5/stm32-mcus-products/usb-bootloader-serial-number/td-p/432148
+            (u_id0.read() as u64 + u_id2.read() as u64) << 16
+                | (u_id2.read() as u64 & 0xFF00) >> 8
+                | (u_id2.read() as u64 & 0x00FF) << 8
+        };
+
+        static mut SERIAL: String<16> = String::new();
+        unsafe {
+            write!(SERIAL, "{:X}", serial).unwrap();
+        }
+
+        let usb_dev = unsafe {
+            UsbDeviceBuilder::new(USB_BUS.as_ref().unwrap(), UsbVidPid(0x16c0, 0x27dd))
+                .device_class(usbd_serial::USB_CLASS_CDC)
+                .strings(&[StringDescriptors::default()
+                    .manufacturer("Amteurfunk Forschungs Gruppe")
+                    .product("Radom Controler")
+                    .serial_number(SERIAL.as_ref())])
+                .unwrap()
+                .build()
+        };
+
+        defmt::info!("USB Setup done");
+
         // Todo: Check if internal pullups work here
         let scl = gpiob.pb6.into_alternate_open_drain();
         let sda = gpiob.pb7.into_alternate_open_drain();
@@ -136,7 +204,7 @@ mod app {
             (sck, poci, pico),
             spi::Mode {
                 polarity: spi::Polarity::IdleLow,
-                phase: spi::Phase::CaptureOnFirstTransition,
+                phase: spi::Phase::CaptureOnSecondTransition,
             },
             8.MHz(),
             &clocks,
@@ -144,11 +212,13 @@ mod app {
 
         defmt::info!("SPI Setup done");
 
-        let az_enable = gpioa.pa12.into_push_pull_output();
+        let mut az_enable = gpiob.pb8.into_push_pull_output();
+        az_enable.set_high();
         let az_dir = gpioa.pa15.into_push_pull_output();
         let az_step = gpiob.pb3.into_push_pull_output();
 
-        let el_enable = gpiob.pb4.into_push_pull_output();
+        let mut el_enable = gpiob.pb4.into_push_pull_output();
+        el_enable.set_high();
         let el_dir = gpioa.pa8.into_push_pull_output();
         let el_step = gpioa.pa9.into_push_pull_output();
 
@@ -156,9 +226,17 @@ mod app {
 
         poll_i2c::spawn().ok();
         poll_spi::spawn().ok();
+        move_az::spawn().ok();
+        move_el::spawn().ok();
 
         (
-            Shared { az_angle: 0 },
+            Shared {
+                az_angle: 0,
+                az_target: 0,
+                el_angle: 0,
+                el_target: 0,
+                az_compass: 0,
+            },
             Local {
                 i2cmux,
                 board_led,
@@ -175,12 +253,16 @@ mod app {
                 el_enable,
                 el_dir,
                 el_step,
+
+                usb_dev,
+                usb_serial,
+                usb_buffer: Vec::new(),
             },
         )
     }
 
-    #[task(local = [i2cmux, board_led])]
-    async fn poll_i2c(cx: poll_i2c::Context) {
+    #[task(local = [i2cmux, board_led], shared = [az_compass])]
+    async fn poll_i2c(mut cx: poll_i2c::Context) {
         let i2cmux = cx.local.i2cmux;
         let board_led = cx.local.board_led;
 
@@ -212,6 +294,11 @@ mod app {
                             heading -= 2.0 * f32::PI();
                         }
                         let heading_degrees = heading * 180.0 / f32::PI();
+
+                        cx.shared.az_compass.lock(|az_compass| {
+                            *az_compass = heading_degrees as i32 * 10;
+                        });
+
                         defmt::info!("Heading1 {}", heading_degrees);
                         break;
                     }
@@ -253,16 +340,18 @@ mod app {
         }
     }
 
-    #[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3], shared = [az_angle])]
+    #[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3], shared = [az_angle, el_angle])]
     async fn poll_spi(mut cx: poll_spi::Context) {
         let spi1 = cx.local.spi1;
         let encoder_az = cx.local.encoder_az;
+        let encoder_el = cx.local.encoder_el;
 
         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 i32 * 3600 / 16384;
 
@@ -270,39 +359,173 @@ mod app {
                 *az_angle = angle_deg;
             });
 
-            defmt::info!("angle: {:?}", angle_deg);
+            defmt::info!("az angle: {:?}", angle_deg);
 
-            Mono::delay(50.millis()).await;
+            let raw_angle = encoder_el.angle(spi1).unwrap();
+            let angle_deg = raw_angle as i32 * 3600 / 16384;
+
+            cx.shared.el_angle.lock(|el_angle| {
+                *el_angle = angle_deg;
+            });
+
+            defmt::info!("el angle: {:?}", angle_deg);
+
+            Mono::delay(1.millis()).await;
         }
     }
 
-    #[task(local = [az_enable, az_dir, az_step], shared = [az_angle])]
+    #[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;
 
-        let az_target = 42i32;
-
         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;
-            if diff.abs() > 2 {
-                az_enable.set_high();
 
-                if diff > 0 {
+            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_high();
-                Mono::delay(250.micros()).await;
                 az_step.set_low();
-                Mono::delay(250.micros()).await;
+                Mono::delay(delay / 2).await;
+                az_step.set_high();
+                Mono::delay(delay / 2).await;
             } else {
-                az_enable.set_low();
-                Mono::delay(500.micros()).await;
+                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) {
+        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])]
+    fn usb_fs(mut cx: usb_fs::Context) {
+        let usb_dev = cx.local.usb_dev;
+        let serial = cx.local.usb_serial;
+        let buffer = cx.local.usb_buffer;
+
+        if !usb_dev.poll(&mut [serial]) {
+            return;
+        }
+
+        let mut tmp = [0u8; 16];
+        match serial.read(&mut tmp) {
+            Ok(count) if count > 0 => {
+                if buffer.extend_from_slice(&tmp[0..count]).is_err() {
+                    buffer.clear();
+                    defmt::error!("Buffer overflow while waiting for the end of the packet");
+                }
+            }
+            _ => {}
+        }
+
+        loop {
+            if let Some(idx) = buffer.iter().position(|&x| x == 0) {
+                let (msg, rest) = buffer.split_at(idx + 1);
+
+                let mut message = [0u8; 128];
+                message[0..msg.len()].clone_from_slice(msg);
+                let host_msg = from_bytes_cobs::<HostMessage>(&mut message);
+
+                match host_msg {
+                    Ok(host_msg) => match host_msg {
+                        HostMessage::RequestStatus => {
+                            let status = StatusMessage {
+                                position: Position {
+                                    az: cx.shared.az_angle.lock(|az| (*az / 10) as f32),
+                                    el: cx.shared.el_angle.lock(|el| (*el / 10) as f32),
+                                    az_endcoder: 0.0,
+                                    el_encoder: 0.0,
+                                    az_magnetic: 0.0,
+                                    el_magnetic: 0.0,
+                                },
+                                alarms: Vec::new(),
+                            };
+                            let device_msg = RadomMessage::Status(status);
+                            let bytes =
+                                to_vec_cobs::<RadomMessage, USB_BUFFER_SIZE>(&device_msg).unwrap();
+                            serial.write(bytes.as_slice()).unwrap();
+                        }
+                        HostMessage::SetTarget(pos) => {
+                            cx.shared.az_target.lock(|az| *az = (pos.az * 10.0) as i32);
+                            cx.shared.el_target.lock(|el| *el = (pos.el * 10.0) as i32);
+                        }
+                        HostMessage::TriggerDFUBootloader => {
+                            bootloader::reboot_to_bootloader();
+                        }
+                    },
+                    Err(err) => defmt::error!("Unable to parse host message"),
+                };
+
+                *buffer = Vec::<u8, USB_BUFFER_SIZE>::from_slice(rest).unwrap();
+            } else {
+                break;
             }
         }
     }

memory.x

@@ -1,6 +1,5 @@
 MEMORY
 {
-  /* NOTE K = KiBi = 1024 bytes */
   FLASH : ORIGIN = 0x08000000, LENGTH = 256K 
   RAM : ORIGIN = 0x20000000, LENGTH = 64K
 }

protocol/Cargo.toml

@@ -0,0 +1,10 @@
+[package]
+name = "radomctl-protocol"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+serde = {version = "1.0.193", default-features = false, features = ["derive"]}
+heapless = {version = "0.8.0", features = ["serde"]}

protocol/src/lib.rs

@@ -0,0 +1,44 @@
+#![no_std]
+use heapless::Vec;
+
+use serde::{Deserialize, Serialize};
+
+#[derive(Serialize, Deserialize, Debug, PartialEq)]
+pub enum RadomMessage {
+    Status(StatusMessage),
+}
+
+#[derive(Serialize, Deserialize, Debug, PartialEq)]
+pub enum HostMessage {
+    RequestStatus,
+    SetTarget(PositionTarget),
+    TriggerDFUBootloader,
+}
+
+#[derive(Serialize, Deserialize, Debug, PartialEq, Clone)]
+pub struct StatusMessage {
+    pub position: Position,
+    pub alarms: Vec<Alarm, 64>,
+}
+
+#[derive(Serialize, Deserialize, Debug, PartialEq, Clone)]
+pub struct Position {
+    pub az: f32,
+    pub el: f32,
+    pub az_endcoder: f32,
+    pub el_encoder: f32,
+    pub az_magnetic: f32,
+    pub el_magnetic: f32,
+}
+
+#[derive(Serialize, Deserialize, Debug, PartialEq, Clone)]
+pub enum Alarm {
+    AZEncoderFault,
+    ELEncoderFault,
+}
+
+#[derive(Serialize, Deserialize, Debug, PartialEq)]
+pub struct PositionTarget {
+    pub az: f32,
+    pub el: f32,
+}