poc client, refactor embedded

Cargo.lock

@@ -437,6 +437,7 @@ dependencies = [
  "embedded-hal 1.0.0",
  "heapless",
  "micromath",
+ "portable-atomic",
  "qingke",
  "qingke-rt",
 ]
@@ -512,6 +513,9 @@ name = "portable-atomic"
 version = "1.10.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6"
+dependencies = [
+ "critical-section",
+]
 
 [[package]]
 name = "proc-macro-error"

Cargo.toml

@@ -26,6 +26,7 @@ embassy-futures = "0.1.1"
 heapless = "0.8.0"
 critical-section = "1.2.0"
 micromath = "2.1.0"
+portable-atomic = { version = "1.10.0", features = ["critical-section"] }
 
 [profile.release]
 strip = false

client/Cargo.toml

@@ -0,0 +1,8 @@
+[package]
+name = "client"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+image = "0.25.5"
+serialport = "4.7.0"

client/src/main.rs

@@ -0,0 +1,88 @@
+use image::ImageReader;
+use std::env;
+use std::fs::{self, DirEntry};
+use std::path::{Path, PathBuf};
+use std::thread::sleep;
+use std::time::Duration;
+
+#[repr(u8)]
+#[derive(Eq, PartialEq, Debug)]
+enum Command {
+    Info = 0,
+    Frame = 1,
+    Brightness = 2,
+    Invalid = 255,
+}
+
+fn find_images(path: PathBuf) -> Vec<DirEntry> {
+    let mut res = Vec::new();
+    for f in path.read_dir().unwrap() {
+        if let Ok(entry) = f {
+            if entry.file_name().as_encoded_bytes().ends_with(b".png") {
+                res.push(entry);
+            }
+        }
+    }
+    res
+}
+
+fn main() {
+    let mut args = env::args();
+
+    let mut port = serialport::new(args.nth(1).unwrap(), 1_000_000)
+        .open()
+        .unwrap();
+    let frames = find_images("frames/".into());
+    let src_fps = 50;
+
+    port.write(&[Command::Info as u8, 0, 0]).unwrap();
+    port.set_timeout(Duration::from_secs(1)).unwrap();
+
+    let (width, height, tgt_fps, mut cur_frame) = {
+        let mut info = [0u8; 14];
+        port.read_exact(&mut info).unwrap();
+        let width = u16::from_be_bytes(info[0..2].try_into().unwrap());
+        let height = u16::from_be_bytes(info[2..4].try_into().unwrap());
+        let tgt_fps = u16::from_be_bytes(info[4..6].try_into().unwrap());
+        let cur_frame = u64::from_be_bytes(info[6..14].try_into().unwrap());
+        (width, height, tgt_fps, cur_frame)
+    };
+
+    let src_frametime = 1. / src_fps as f64;
+    let dst_frametime = 1. / tgt_fps as f64;
+    let frames_per_frame = (tgt_fps / src_fps) as u64;
+
+    let frame_pkt_size = width * height + 8;
+
+    let bright = 0;
+    port.write(&[Command::Brightness as u8]).unwrap();
+    port.write(&1u16.to_be_bytes()).unwrap();
+    port.write(&[bright]).unwrap();
+    {
+        let mut buf = [0u8; 1];
+        port.read_exact(&mut buf).unwrap();
+    }
+
+    loop {
+        for frame in &frames {
+            let next_frame = cur_frame + frames_per_frame;
+            let img = ImageReader::open(frame.path()).unwrap().decode().unwrap();
+            let gs = img.as_luma8().unwrap();
+            println!(
+                "writing frame bytes {} for display at {}, cur_frame: {}",
+                gs.len(),
+                next_frame, cur_frame
+            );
+            port.write(&[Command::Frame as u8]).unwrap();
+            port.write(&frame_pkt_size.to_be_bytes()).unwrap();
+            port.write(&next_frame.to_be_bytes()).unwrap();
+            port.write(&gs).unwrap();
+            cur_frame = {
+                let mut buf = [0; 8];
+                port.read_exact(&mut buf).unwrap();
+                u64::from_be_bytes(buf)
+            };
+            sleep(Duration::from_secs_f64(src_frametime));
+        }
+    }
+}

src/main.rs

@@ -23,14 +23,19 @@ use hal::timer::simple_pwm::{PwmPin, SimplePwm};
 use hal::{peripherals, usbd};
 use heapless::{binary_heap::Min, BinaryHeap};
 use micromath::F32Ext;
+use portable_atomic::AtomicU64;
 
 bind_interrupts!(struct Irqs {
     USB_LP_CAN1_RX0 => usbd::InterruptHandler<peripherals::USBD>;
 });
 
 const FPS: usize = 120;
+const WIDTH: usize = 16;
+const HEIGHT: usize = 16;
 const EMPTY_FRAME: Frame = Frame::empty();
-static FRAME_QUEUE: Mutex<RefCell<BinaryHeap<Frame, Min, 30>>> =
+
+static CUR_FRAME: AtomicU64 = AtomicU64::new(0);
+static MBI_QUEUE: Mutex<RefCell<BinaryHeap<MbiCommand, Min, 30>>> =
     Mutex::new(RefCell::new(BinaryHeap::new()));
 
 /// Unfortunately embedded_hal doesn't support atomic writes to multiple bytes, which is required for this implementation, so we must take a platform register :(
@@ -41,7 +46,8 @@ struct MBI5043<SDI, DCLK, LE> {
     port: Gpio,
 }
 
-#[repr(u32)]
+// Should be an enum containing the data itself, for auto generic
+#[repr(u8)]
 enum DataType {
     Data = 1,
     Global = 3,
@@ -132,14 +138,181 @@ impl<'a, SDI: Pin, DCLK: Pin, LE: Pin> MBI5043<SDI, DCLK, LE> {
 
 pub trait MBI {
     fn send_pixels<T: Fn(u8) -> u16>(&self, pixels: &[u8], transform: T);
+    fn set_gain(&self, gain: u8);
 }
 
 impl<SDI: Pin, DCLK: Pin, LE: Pin> MBI for MBI5043<SDI, DCLK, LE> {
     fn send_pixels<T: Fn(u8) -> u16>(&self, pixels: &[u8], transform: T) {
         for pixel in pixels {
-            self.send_word::<1>(&transform(*pixel));
+            self.send_word::<{ DataType::Data as u8 }>(&transform(*pixel));
+        }
+        // dummy word to latch the output registers
+        self.send_word::<{ DataType::Global as u8 }>(&0x0000);
+    }
+    // TODO: Need to store rest of the config and restore it here
+    fn set_gain(&self, gain: u8) {
+        let config = (gain as u16) << 4;
+        self.send_word::<15>(&config);
+        self.send_word::<11>(&config);
+    }
+}
+
+#[derive(Debug)]
+enum ProtocolState {
+    WaitCmd,
+    WaitLengthLsb,
+    WaitLengthMsb,
+    WaitData,
+}
+
+#[repr(u8)]
+#[derive(Eq, PartialEq, Debug)]
+enum Command {
+    Info = 0,
+    Frame = 1,
+    Brightness = 2,
+    Invalid = 255,
+}
+
+impl From<u8> for Command {
+    fn from(value: u8) -> Self {
+        match value {
+            0 => Command::Info,
+            1 => Command::Frame,
+            2 => Command::Brightness,
+            _ => Command::Invalid,
+        }
+    }
+}
+
+struct Protocol<'d, T: usbd::Instance + 'd> {
+    class: &'d mut CdcAcmClass<'d, usbd::Driver<'d, T>>,
+    state: ProtocolState,
+    cmd: Command,
+    to_read: u16,
+    cur_pos: usize,
+    buf: [u8; 1024],
+}
+
+impl<'d, T: usbd::Instance + 'd> Protocol<'d, T> {
+    fn new(class: &'d mut CdcAcmClass<'d, usbd::Driver<'d, T>>) -> Self {
+        Self {
+            class,
+            state: ProtocolState::WaitCmd,
+            cmd: Command::Info,
+            to_read: 0,
+            cur_pos: 0,
+            buf: [0; 1024],
+        }
+    }
+    fn transition(&mut self, new_state: ProtocolState) {
+        // println!("transition {:?} -> {:?}", self.state, new_state);
+        self.state = new_state
+    }
+    async fn receive(&mut self) -> Result<(), Disconnected> {
+        let mut buf = [0; 64];
+        loop {
+            let n = self.class.read_packet(&mut buf).await?;
+            let mut data = &buf[..n];
+            while !data.is_empty() {
+                match self.state {
+                    ProtocolState::WaitCmd => {
+                        let cmd: Command = data[0].into();
+                        if cmd != Command::Invalid {
+                            self.cmd = cmd;
+                            self.transition(ProtocolState::WaitLengthMsb);
+                        }
+                    }
+                    ProtocolState::WaitLengthMsb => {
+                        self.to_read = (data[0] as u16) << 8;
+                        self.transition(ProtocolState::WaitLengthLsb);
+                    }
+                    ProtocolState::WaitLengthLsb => {
+                        self.cur_pos = 0;
+                        self.to_read |= data[0] as u16;
+                        // println!("expecting {} bytes", self.to_read);
+                        if self.to_read != 0 {
+                            self.transition(ProtocolState::WaitData);
+                        } else {
+                            self.handle().await;
+                            self.transition(ProtocolState::WaitCmd);
+                        }
+                    }
+                    ProtocolState::WaitData => {
+                        if self.to_read != 0 {
+                            self.buf[self.cur_pos] = data[0];
+                            self.cur_pos += 1;
+                            self.to_read -= 1;
+                        }
+                        if self.to_read == 0 {
+                            self.handle().await;
+                            self.transition(ProtocolState::WaitCmd);
+                        }
+                    }
+                }
+                data = &data[1..];
+            }
+        }
+    }
+    async fn send_frame_no(&mut self) {
+        let frame_be = CUR_FRAME
+            .load(core::sync::atomic::Ordering::Relaxed)
+            .to_be_bytes();
+        self.class.write_packet(&frame_be).await.unwrap();
+    }
+
+    async fn handle(&mut self) {
+        // println!("handling cmd: {:?}", self.cmd);
+        match self.cmd {
+            Command::Info => self.handle_info().await,
+            Command::Frame => self.handle_frame().await,
+            Command::Brightness => self.handle_brightness().await,
+            Command::Invalid => unimplemented!(),
+        }
+    }
+
+    // response:
+    //   u16 width
+    //   u16 height
+    //   u16 fps
+    //   u64 cur_frame
+    async fn handle_info(&mut self) {
+        let width_be = (WIDTH as u16).to_be_bytes();
+        let height_be = (HEIGHT as u16).to_be_bytes();
+        let fps_be = (FPS as u16).to_be_bytes();
+        self.class.write_packet(&width_be).await.unwrap();
+        self.class.write_packet(&height_be).await.unwrap();
+        self.class.write_packet(&fps_be).await.unwrap();
+        self.send_frame_no().await;
+    }
+
+    // data:
+    //   u64 when
+    //   u8[WIDTH * HEIGHT] pixels
+    // response:
+    //   u64 cur_frame
+    async fn handle_frame(&mut self) {
+        let cmd = MbiCommand::frame_from_packet(self.buf.as_slice());
+        critical_section::with(|cs| MBI_QUEUE.borrow_ref_mut(cs).push(cmd));
+        self.send_frame_no().await;
+    }
+
+    // data:
+    //  u8 gain - 6 bits, 12.5 - 200%
+    // response:
+    //  u8 - 255 if ok, 0 if not
+    async fn handle_brightness(&mut self) {
+        let gain = self.buf[0];
+        if gain & 0b11000000 != 0 {
+            self.class.write_packet(&[0u8]).await.unwrap();
+        } else {
+            critical_section::with(|cs| {
+                MBI_QUEUE
+                    .borrow_ref_mut(cs)
+                    .push(MbiCommand::from_brightness(gain))
+            }).unwrap();
+            self.class.write_packet(&[255u8]).await.unwrap();
         }
-        self.send_word::<3>(&0x0000);
     }
 }
 
@@ -193,14 +366,16 @@ async fn usb(usb: peripherals::USBD, irq: Irqs, dp: peripherals::PA12, dm: perip
     let usb_fut = usb.run();
 
     let echo_fut = async {
+        let mut protocol = Protocol::new(&mut class);
         loop {
-            class.wait_connection().await;
+            protocol.class.wait_connection().await;
-            receive(&mut class).await;
+            protocol.receive().await.unwrap();
         }
     };
     join(usb_fut, echo_fut).await;
 }
 
+#[derive(Debug)]
 struct Disconnected {}
 
 impl From<EndpointError> for Disconnected {
@@ -212,41 +387,66 @@ impl From<EndpointError> for Disconnected {
     }
 }
 
-async fn receive<'d, T: usbd::Instance + 'd>(
+#[derive(Debug)]
-    class: &mut CdcAcmClass<'d, usbd::Driver<'d, T>>,
+struct MbiCommand {
-) -> Result<(), Disconnected> {
+    when: u64,
-    let mut buf = [0; 64];
+    inner: MbiCommandInner,
-    loop {
+}
-        let n = class.read_packet(&mut buf).await?;
+
-        let data = &buf[..n];
+impl MbiCommand {
-        class.write_packet(data).await?;
+    fn frame_from_packet(packet: &[u8]) -> MbiCommand {
-        let mut frame = Frame::empty();
+        Self {
-        frame.when = 512;
+            when: u64::from_be_bytes(packet[0..8].try_into().unwrap()),
-        let mut i = 0;
+            inner: Frame::from(&packet[8..]).into(),
-        for b in data {
+        }
-            for j in 0..8 {
+    }
-                if b & (1 << j) != 0 {
+    fn from_brightness(gain: u8) -> MbiCommand {
-                    frame.pixels[i * 8 + j] = 128
+        Self {
-                };
+            when: 0,
-            }
+            inner: MbiCommandInner::Brightness(gain),
-            i += 1
         }
-        println!("queueing new frame");
-        critical_section::with(|cs| FRAME_QUEUE.borrow_ref_mut(cs).push(frame));
     }
 }
 
-#[derive(Ord, Eq)]
+impl PartialOrd for MbiCommand {
+    fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
+        self.when.partial_cmp(&other.when)
+    }
+}
+
+impl PartialEq for MbiCommand {
+    fn eq(&self, other: &Self) -> bool {
+        self.when.eq(&other.when)
+    }
+}
+impl Eq for MbiCommand {}
+impl Ord for MbiCommand {
+    fn cmp(&self, other: &Self) -> core::cmp::Ordering {
+        self.when.cmp(&other.when)
+    }
+}
+
+#[derive(Debug)]
+enum MbiCommandInner {
+    Frame(Frame),
+    Brightness(u8),
+}
+
+impl From<Frame> for MbiCommandInner {
+    fn from(value: Frame) -> Self {
+        MbiCommandInner::Frame(value)
+    }
+}
+
+#[derive(Clone, Debug)]
 struct Frame {
-    when: u64,
+    pixels: [u8; WIDTH as usize * HEIGHT as usize],
-    pixels: [u8; 256],
 }
 
 impl Frame {
     const fn empty() -> Self {
         Self {
-            when: 0,
+            pixels: [0; WIDTH as usize * HEIGHT as usize],
-            pixels: [0; 256],
         }
     }
 }
@@ -257,15 +457,15 @@ impl Default for Frame {
     }
 }
 
-impl PartialEq for Frame {
+impl From<&[u8]> for Frame {
-    fn eq(&self, other: &Self) -> bool {
+    fn from(value: &[u8]) -> Self {
-        self.when.eq(&other.when)
+        let mut pixels = [0; WIDTH as usize * HEIGHT as usize];
-    }
+        for i in 0..pixels.len() {
-}
+            let y = i / WIDTH as usize;
-
+            let x = WIDTH as usize - (i % WIDTH as usize) - 1;
-impl PartialOrd for Frame {
+            pixels[y * WIDTH as usize + x] = value[i];
-    fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
+        }
-        self.when.partial_cmp(&other.when)
+        Frame { pixels }
     }
 }
 
@@ -303,14 +503,14 @@ impl Lightbar {
         }
     }
     async fn mbi_send_frame(&self, f: &Frame) {
-        for row in 0..16 {
+        for row in 0..HEIGHT as usize {
             pac::GPIOB.outdr().write_value(Outdr(0));
-            self.mbi
+            self.mbi.send_pixels(
-                .send_pixels(&f.pixels[row * 16..(row + 1) * 16], |val| {
+                &f.pixels[row * WIDTH as usize..(row + 1) * WIDTH as usize],
-                    self.gamma_lut.correct(val)
+                |val| self.gamma_lut.correct(val),
-                });
+            );
             pac::GPIOB.outdr().write_value(Outdr(1 << row));
-            Timer::after_micros(1_000_000 / FPS as u64 / 17).await;
+            Timer::after_micros(1_000_000 / FPS as u64 / (HEIGHT as u64 + 1)).await;
         }
     }
     async fn display_task(&self) {
@@ -322,21 +522,33 @@ impl Lightbar {
             .cfghr()
             .write_value(gpio::regs::Cfghr(0x33333333));
 
-        let mut frame: u64 = 0;
+        let mut cmd: Option<MbiCommand> = None;
         let mut draw_frame: Frame = EMPTY_FRAME;
 
         let mut ticker = Ticker::every(Duration::from_micros(1_000_000 / FPS as u64));
+        // TODO: move to interrupt driven
         loop {
+            let frame = CUR_FRAME.fetch_add(1, core::sync::atomic::Ordering::Relaxed);
             critical_section::with(|cs| {
-                let mut frames = FRAME_QUEUE.borrow_ref_mut(cs);
+                let mut commands = MBI_QUEUE.borrow_ref_mut(cs);
-                if frames.peek().is_some_and(|f| frame >= f.when) {
+
-                    draw_frame = frames.pop().unwrap();
+                if commands.peek().is_some_and(|v| frame >= v.when) {
-                    println!("got new frame for {} @ {}", draw_frame.when, frame);
+                    cmd = commands.pop();
+                } else {
+                    cmd = None
                 }
             });
-            self.mbi_send_frame(&draw_frame).await;
+            if let Some(cmd) = &cmd {
-            frame += 1;
+                match &cmd.inner {
-            ticker.next().await
+                    MbiCommandInner::Brightness(gain) => self.mbi.set_gain(*gain),
+                    MbiCommandInner::Frame(f) => {
+                        draw_frame = f.clone();
+                    }
+                }
+            } else {
+                self.mbi_send_frame(&draw_frame).await;
+                ticker.next().await
+            }
         }
     }
 }
@@ -373,14 +585,9 @@ async fn main(spawner: Spawner) -> ! {
     );
     let ch = hal::timer::Channel::Ch1;
 
-    let max_duty = pwm.get_max_duty();
     pwm.set_duty(ch, 9);
     pwm.enable(ch);
 
-    // println!("exti: {:x}", pac::EXTI.intenr().read().0);
-    // pac::EXTI.intenr().modify(|w| w.set_mr(18, true));
-    // println!("exti: {:x}", pac::EXTI.intenr().read().0);
-
     let app: Lightbar = Lightbar::new(spi_mosi.degrade(), spi_sck.degrade(), le.degrade(), 2.8);
 
     spawner.spawn(usb(p.USBD, Irqs, p.PA12, p.PA11)).unwrap();
@@ -388,23 +595,6 @@ async fn main(spawner: Spawner) -> ! {
         .spawn(blink(p.PC14.degrade(), p.PC15.degrade()))
         .unwrap();
 
-    // spawner
-    //     .spawn(mbi_writer(
-    //         spi_mosi.degrade(),
-    //         spi_sck.degrade(),
-    //         le.degrade(),
-    //     ))
-    //     .unwrap();
-
-    // spawner.spawn(busy()).unwrap();
-
-    critical_section::with(|cs| {
-        FRAME_QUEUE.borrow_ref_mut(cs).push(Frame {
-            when: 60,
-            pixels: [128; 256],
-        })
-    });
-
     loop {
         app.display_task().await
     }
@@ -412,8 +602,9 @@ async fn main(spawner: Spawner) -> ! {
 
 #[inline(never)]
 #[panic_handler]
-fn panic(info: &PanicInfo) -> ! {
+fn panic(_info: &PanicInfo) -> ! {
     loop {
+        println!("{:?}", _info);
         core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst);
     }
 }