rotor-control-stm32/src/usb.rs

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,
    }
}