Implemented basic protocol parsing

src/display.rs

@@ -0,0 +1,80 @@
+use core::fmt::Write;
+
+use embassy_stm32::i2c;
+use embassy_stm32::peripherals;
+use embassy_stm32::time::Hertz;
+use embassy_time::{Duration, Timer};
+
+use embedded_graphics::{
+    mono_font::MonoTextStyle,
+    pixelcolor::BinaryColor,
+    prelude::*,
+    primitives::{PrimitiveStyleBuilder, Rectangle},
+    text::Text,
+};
+use profont::{PROFONT_12_POINT, PROFONT_7_POINT};
+use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};
+
+use arrayvec::ArrayString;
+
+#[embassy_executor::task]
+pub async fn display_task(i2c1: peripherals::I2C1, sda: peripherals::PB6, scl: peripherals::PB7) {
+    let i2c = i2c::I2c::new(i2c1, sda, scl, Hertz::hz(100_000), i2c::Config::default());
+
+    let interface = I2CDisplayInterface::new(i2c);
+
+    let mut display = Ssd1306::new(interface, DisplaySize128x32, DisplayRotation::Rotate0)
+        .into_buffered_graphics_mode();
+    display.init().unwrap();
+
+    let text_large = MonoTextStyle::new(&PROFONT_12_POINT, BinaryColor::On);
+    let text_large_inv = MonoTextStyle::new(&PROFONT_12_POINT, BinaryColor::Off);
+    let text_small = MonoTextStyle::new(&PROFONT_7_POINT, BinaryColor::On);
+
+    let style_filled = PrimitiveStyleBuilder::new()
+        .fill_color(BinaryColor::On)
+        .build();
+
+    loop {
+        display.clear();
+
+        Text::new("AFG rotor ctrl v0.1.0", Point::new(0, 6), text_small)
+            .draw(&mut display)
+            .unwrap();
+
+        Text::new("AZ: 23", Point::new(1, 17), text_large)
+            .draw(&mut display)
+            .unwrap();
+
+        Text::new("EL: 42", Point::new(64, 17), text_large)
+            .draw(&mut display)
+            .unwrap();
+
+        Rectangle::new(Point::new(0, 19), Size::new(128, 23))
+            .into_styled(style_filled)
+            .draw(&mut display)
+            .unwrap();
+
+        Text::new("AZ: 23", Point::new(1, 30), text_large_inv)
+            .draw(&mut display)
+            .unwrap();
+
+        Text::new("EL: 42", Point::new(64, 30), text_large_inv)
+            .draw(&mut display)
+            .unwrap();
+
+        /*
+                let now = Instant::now().as_millis();
+
+
+                let mut buf = ArrayString::<20>::new();
+                write!(&mut buf, "{}", now).expect("Can't write");
+                Text::new(&buf, Point::new(0, 20), text_large)
+                    .draw(&mut display)
+                    .unwrap();
+        */
+        display.flush().unwrap();
+
+        Timer::after(Duration::from_millis(500)).await;
+    }
+}

src/main.rs

@@ -11,24 +11,17 @@ use panic_probe as _;
 use embassy_executor::Spawner;
 use embassy_stm32::gpio::{Level, Output, Speed};
 use embassy_stm32::time::Hertz;
-use embassy_stm32::usb::{Driver, Instance};
+use embassy_stm32::Config;
-use embassy_stm32::{i2c, interrupt, Config};
+use embassy_time::{Duration, Timer};
-use embassy_time::{Duration, Instant, Timer};
-use embassy_usb::driver::EndpointError;
-use embassy_usb::Builder;
-use embassy_usb_serial::{CdcAcmClass, State};
-use futures::future::join;
 
-use embedded_graphics::{
+mod display;
-    mono_font::MonoTextStyle, pixelcolor::BinaryColor, prelude::*, text::Text,
+use display::display_task;
-};
-use profont::PROFONT_12_POINT;
-use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};
 
-use arrayvec::ArrayString;
+mod usb;
+use usb::usb_task;
 
 #[embassy_executor::main]
-async fn main(_spawner: Spawner) {
+async fn main(spawner: Spawner) {
     let mut config = Config::default();
     config.rcc.hse = Some(Hertz(8_000_000));
     config.rcc.sys_ck = Some(Hertz(48_000_000));
@@ -46,116 +39,6 @@ async fn main(_spawner: Spawner) {
         Timer::after(Duration::from_millis(10)).await;
     }
 
-    // Create the driver, from the HAL.
+    spawner.spawn(usb_task(p.USB, p.PA12, p.PA11)).unwrap();
-    let irq = interrupt::take!(USB_LP_CAN1_RX0);
+    spawner.spawn(display_task(p.I2C1, p.PB6, p.PB7)).unwrap();
-    let driver = Driver::new(p.USB, irq, p.PA12, p.PA11);
-
-    // Create embassy-usb Config
-    let config = embassy_usb::Config::new(0xc0de, 0xcafe);
-    //config.max_packet_size_0 = 64;
-
-    // Create embassy-usb DeviceBuilder using the driver and config.
-    // It needs some buffers for building the descriptors.
-    let mut device_descriptor = [0; 256];
-    let mut config_descriptor = [0; 256];
-    let mut bos_descriptor = [0; 256];
-    let mut control_buf = [0; 7];
-
-    let mut state = State::new();
-
-    let mut builder = Builder::new(
-        driver,
-        config,
-        &mut device_descriptor,
-        &mut config_descriptor,
-        &mut bos_descriptor,
-        &mut control_buf,
-        None,
-    );
-
-    // Create classes on the builder.
-    let mut class = CdcAcmClass::new(&mut builder, &mut state, 64);
-
-    // Build the builder.
-    let mut usb = builder.build();
-
-    // Run the USB device.
-    let usb_fut = usb.run();
-
-    let i2c = i2c::I2c::new(
-        p.I2C1,
-        p.PB6,
-        p.PB7,
-        Hertz::hz(100_000),
-        i2c::Config::default(),
-    );
-
-    let interface = I2CDisplayInterface::new(i2c);
-
-    let display_fut = async {
-        let mut display = Ssd1306::new(interface, DisplaySize128x32, DisplayRotation::Rotate0)
-            .into_buffered_graphics_mode();
-        display.init().unwrap();
-
-        let text_style = MonoTextStyle::new(&PROFONT_12_POINT, BinaryColor::On);
-
-        loop {
-            display.clear();
-
-            Text::new("Hello world", Point::new(0, 7), text_style)
-                .draw(&mut display)
-                .unwrap();
-
-            let now = Instant::now().as_millis();
-
-            let mut buf = ArrayString::<20>::new();
-            write!(&mut buf, "{}", now).expect("Can't write");
-            Text::new(&buf, Point::new(0, 20), text_style)
-                .draw(&mut display)
-                .unwrap();
-
-            display.flush().unwrap();
-
-            Timer::after(Duration::from_millis(500)).await;
-        }
-    };
-
-    // Do stuff with the class!
-    let echo_fut = async {
-        loop {
-            class.wait_connection().await;
-            defmt::info!("Connected");
-            let _ = echo(&mut class).await;
-            defmt::info!("Disconnected");
-        }
-    };
-
-    // Run everything concurrently.
-    // If we had made everything `'static` above instead, we could do this using separate tasks instead.
-    join(join(usb_fut, display_fut), echo_fut).await;
-}
-
-struct Disconnected {}
-
-impl From<EndpointError> for Disconnected {
-    fn from(val: EndpointError) -> Self {
-        match val {
-            EndpointError::BufferOverflow => panic!("Buffer overflow"),
-            EndpointError::Disabled => Disconnected {},
-        }
-    }
-}
-
-async fn echo<'d, T: Instance + 'd>(
-    class: &mut CdcAcmClass<'d, Driver<'d, T>>,
-) -> Result<(), Disconnected> {
-    let mut buf = [0; 64];
-    loop {
-        let n = class.read_packet(&mut buf).await?;
-        let data = &buf[..n];
-        defmt::info!("data: {:x}", data);
-        class.write_packet(">".as_bytes()).await?;
-        class.write_packet(data).await?;
-        class.write_packet("\n".as_bytes()).await?;
-    }
 }

src/usb.rs

@@ -0,0 +1,155 @@
+use embassy_executor::Spawner;
+use embassy_stm32::gpio::{Level, Output, Speed};
+use embassy_stm32::peripherals;
+use embassy_stm32::time::Hertz;
+use embassy_stm32::usb::Driver;
+use embassy_stm32::{interrupt, Config};
+use embassy_time::{Duration, Timer};
+use embassy_usb::Builder;
+use embassy_usb_serial::{CdcAcmClass, State};
+use futures::future::join;
+
+use arrayvec::{ArrayString, ArrayVec};
+
+#[embassy_executor::task]
+pub async fn usb_task(usb: peripherals::USB, dp_pin: peripherals::PA12, dm_pin: peripherals::PA11) {
+    let irq = interrupt::take!(USB_LP_CAN1_RX0);
+    let driver = Driver::new(usb, irq, dp_pin, dm_pin);
+
+    // Create embassy-usb Config
+    let config = embassy_usb::Config::new(0xc0de, 0xcafe);
+    //config.max_packet_size_0 = 64;
+
+    // Create embassy-usb DeviceBuilder using the driver and config.
+    // It needs some buffers for building the descriptors.
+    let mut device_descriptor = [0; 256];
+    let mut config_descriptor = [0; 256];
+    let mut bos_descriptor = [0; 256];
+    let mut control_buf = [0; 7];
+
+    let mut state = State::new();
+
+    let mut builder = Builder::new(
+        driver,
+        config,
+        &mut device_descriptor,
+        &mut config_descriptor,
+        &mut bos_descriptor,
+        &mut control_buf,
+        None,
+    );
+
+    // Create classes on the builder.
+    let mut class = CdcAcmClass::new(&mut builder, &mut state, 64);
+
+    // Build the builder.
+    let mut usb = builder.build();
+
+    // Do stuff with the class!
+    let usb_handler_fut = async {
+        loop {
+            class.wait_connection().await;
+            defmt::info!("USB connected");
+            let mut packet = [0; 64];
+            let mut buffer = ArrayVec::<u8, 64>::new();
+
+            loop {
+                let n = match class.read_packet(&mut packet).await {
+                    Ok(n) => n,
+                    Err(err) => {
+                        defmt::error!("Unable to read packet: {}", err);
+                        break;
+                    }
+                };
+
+                if buffer.try_extend_from_slice(&packet[..n]).is_err() {
+                    buffer.clear();
+                    buffer.try_extend_from_slice(&packet[..n]).unwrap();
+                }
+
+                let mut line_end = 0;
+                for i in 0..buffer.len() {
+                    if buffer[i] == '\r' as u8 {
+                        line_end = i;
+                        break;
+                    }
+                }
+
+                defmt::info!("Line end: {}", line_end);
+                defmt::info!("buffer: {}", buffer.as_slice());
+
+                if line_end > 0 {
+                    let cmd = parse_command(&buffer.as_slice()[..line_end]);
+                    for _ in 0..line_end + 1 {
+                        buffer.remove(0);
+                    }
+                    defmt::info!("truncated buffer");
+
+                    if cmd != Gs232Cmd::Unkown {
+                        match class.write_packet(b"\r").await {
+                            Ok(_) => {}
+                            Err(err) => {
+                                defmt::error!("Unable to write packet: {}", err);
+                                break;
+                            }
+                        };
+                    }
+                }
+            }
+            defmt::info!("USB disconnected");
+        }
+    };
+
+    join(usb.run(), usb_handler_fut).await;
+}
+
+#[derive(PartialEq)]
+enum Gs232Cmd {
+    Unkown,
+    GetAl,
+    GetEz,
+    GetAlEz,
+    MoveTo(u16, u16),
+    Stop,
+}
+
+fn parse_command(data: &[u8]) -> Gs232Cmd {
+    match data[0] as char {
+        'B' => {
+            if data.len() == 1 {
+                Gs232Cmd::GetAl
+            } else {
+                Gs232Cmd::Unkown
+            }
+        }
+
+        'C' => {
+            if data.len() == 2 && data[1] as char == '2' {
+                Gs232Cmd::GetAlEz
+            } else if data.len() == 1 {
+                Gs232Cmd::GetEz
+            } else {
+                Gs232Cmd::Unkown
+            }
+        }
+
+        'W' => {
+            if data.len() == 8 {
+
+                Gs232Cmd::Unkown
+            } else {
+                Gs232Cmd::Unkown
+            }
+        }
+
+        'S' => {
+            if data.len() == 1 {
+                Gs232Cmd::Stop
+            } else {
+                Gs232Cmd::Unkown
+            }
+        }
+
+        _ => Gs232Cmd::Unkown,
+    }
+}