rotor-control-stm32/src/usb.rs

use defmt::Format;

use embassy_stm32::interrupt;
use embassy_stm32::peripherals;
use embassy_stm32::usb::Driver;
use embassy_usb::Builder;
use embassy_usb_serial::{CdcAcmClass, State};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::{Receiver, Sender};
use embassy_util::{select, Either};

use futures_util::future::join;

use heapless::String;
use ufmt::uwrite;

use crate::AzElPair;

#[embassy_executor::task]
pub async fn usb_task(
    usb: peripherals::USB,
    dp_pin: peripherals::PA12,
    dm_pin: peripherals::PA11,
    cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
    pos_receiver: Receiver<'static, ThreadModeRawMutex, AzElPair, 1>,
) {
    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 {
        let mut current_pos = AzElPair { az: 0, el: 0 };

        loop {
            class.wait_connection().await;
            defmt::info!("USB connected");
            let mut packet = [0; 64];
            let mut buffer: String<64> = String::new();

            loop {
                let n = match select(class.read_packet(&mut packet), pos_receiver.recv()).await {
                    Either::First(res) => match res {
                        Ok(n) => n,
                        Err(err) => {
                            defmt::error!("Unable to read packet: {}", err);
                            break;
                        }
                    },
                    Either::Second(pair) => {
                        current_pos = pair;
                        continue;
                    }
                };

                for byte in &packet[..n] {
                    if buffer.len() == 64 {
                        buffer.clear();
                    }
                    buffer.push(*byte as char).unwrap();
                }

                let line_end = match buffer.rfind('\r') {
                    Some(n) => n,
                    _ => continue,
                };

                defmt::info!("Line buffer: {:x}", buffer.as_bytes());

                if line_end > 0 {
                    let cmd = parse_command(&buffer.as_str()[..line_end]);
                    defmt::info!("Command: {}", cmd);

                    let mut resp: String<16> = String::new();
                    match cmd {
                        Gs232Cmd::GetAl => {
                            uwrite!(&mut resp, "AZ={}\r", current_pos.az).unwrap();
                        }
                        Gs232Cmd::GetEz => {
                            uwrite!(&mut resp, "EL={}\r", current_pos.el).unwrap();
                        }
                        Gs232Cmd::GetAlEz => {
                            uwrite!(&mut resp, "AZ={} EL={}\r", current_pos.az, current_pos.el)
                                .unwrap();
                        }
                        Gs232Cmd::MoveTo(_) => {
                            cmd_sender.send(cmd).await;
                            resp.push_str("\r").unwrap();
                        }
                        Gs232Cmd::Stop => {
                            cmd_sender.send(cmd).await;
                            resp.push_str("\r").unwrap();
                        }
                        _ => {
                            defmt::error!("Uknown command: {}", &buffer.as_str()[..line_end]);
                            resp.push_str("Unkown command!\r").unwrap();
                        }
                    }

                    match class.write_packet(resp.as_bytes()).await {
                        Ok(_) => {}
                        Err(err) => {
                            defmt::error!("Unable to write packet: {}", err);
                            break;
                        }
                    };
                }
                buffer = String::from(&buffer.as_str()[line_end + 1..]);
            }
            defmt::info!("USB disconnected");
        }
    };

    join(usb.run(), usb_handler_fut).await;
}

#[derive(Format, PartialEq)]
pub enum Gs232Cmd {
    Unkown,
    GetAl,
    GetEz,
    GetAlEz,
    MoveTo(AzElPair),
    Stop,
}

fn parse_command(data: &str) -> Gs232Cmd {
    match data.chars().nth(0).unwrap() {
        'B' => {
            if data.len() == 1 {
                Gs232Cmd::GetAl
            } else {
                Gs232Cmd::Unkown
            }
        }

        'C' => {
            if data.len() == 2 && data.chars().nth(1).unwrap() as char == '2' {
                Gs232Cmd::GetAlEz
            } else if data.len() == 1 {
                Gs232Cmd::GetEz
            } else {
                Gs232Cmd::Unkown
            }
        }

        'W' => {
            if data.len() == 8 {
                if let Ok(az) = data[1..4].parse::<u16>() {
                    if let Ok(el) = data[5..].parse::<u16>() {
                        Gs232Cmd::MoveTo(AzElPair { az, el })
                    } else {
                        Gs232Cmd::Unkown
                    }
                } else {
                    Gs232Cmd::Unkown
                }
            } else {
                Gs232Cmd::Unkown
            }
        }

        'S' => {
            if data.len() == 1 {
                Gs232Cmd::Stop
            } else {
                Gs232Cmd::Unkown
            }
        }

        _ => Gs232Cmd::Unkown,
    }
}