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);

    // 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 usb_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 usb_state, 64);

    // Build the builder.
    let mut usb = builder.build();

    // Create a future to handle incomming usb packets
    let usb_handler_fut = async {
        // Initialize the current position in case we get a B or C command,
        // before we get the first the update via pos_receiver
        let mut current_pos = AzElPair { az: 0, el: 0 };

        loop {
            // No much used doing anything until we have a usb connection
            class.wait_connection().await;
            defmt::info!("USB connected");
            // Allocate a space for incomming usb data packets
            let mut packet = [0; 64];
            // Allocate a string to act as buffer to pares the packets linewise
            let mut buffer: String<64> = String::new();

            loop {
                let n = match select(class.read_packet(&mut packet), pos_receiver.recv()).await {
                    // The read_packet furture returned either usb data or an error.
                    Either::First(res) => match res {
                        // In case of an error break the loop and treat it like an usb disconnect
                        Ok(n) => n,
                        // In case of an error break the loop and treat it like an usb disconnect
                        Err(err) => {
                            defmt::error!("Unable to read packet: {}", err);
                            break;
                        }
                    },
                    // The pos_receiver future returned a position update from moment task.
                    // Just update position and restart loop.
                    Either::Second(pair) => {
                        current_pos = pair;
                        continue;
                    }
                };

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

                // Check if the buffer string contains a '\r'
                let line_end = match buffer.rfind('\r') {
                    // Carriage return found, keep the index
                    Some(n) => n,
                    // No carriage return, wait for the next package
                    _ => continue,
                };

                // The is a non-zero amount of characters before the carriage return
                if line_end > 0 {
                    // Try the parse the slice leading up to linend as a GS323 command
                    let cmd = parse_command(&buffer.as_str()[..line_end]);
                    defmt::info!("Command: {}", cmd);

                    // Reverse some space for a respose to the command
                    let mut resp: String<16> = String::new();
                    match cmd {
                        // Get Azimuth command. Respond with last known azimuth
                        Gs232Cmd::GetAz => {
                            uwrite!(&mut resp, "AZ={}\r", current_pos.az).unwrap();
                        }
                        // Get Elevation comman. Respond with last known elevation
                        Gs232Cmd::GetEl => {
                            uwrite!(&mut resp, "EL={}\r", current_pos.el).unwrap();
                        }
                        // Get Azimuth and Elevation. Respond with last known pair
                        Gs232Cmd::GetAzEl => {
                            uwrite!(&mut resp, "AZ={} EL={}\r", current_pos.az, current_pos.el)
                                .unwrap();
                        }
                        // Move to command. Send to movement task. Respond with empty line.
                        Gs232Cmd::MoveTo(_) => {
                            cmd_sender.send(cmd).await;
                            resp.push_str("\r").unwrap();
                        }
                        // Stop command. Send to movement task. Respond with empty line.
                        Gs232Cmd::Stop => {
                            cmd_sender.send(cmd).await;
                            resp.push_str("\r").unwrap();
                        }
                        // Unknown command or parser error. Complain and do nothing.
                        _ => {
                            defmt::error!("Uknown command: {}", &buffer.as_str()[..line_end]);
                            resp.push_str("Unkown command!\r").unwrap();
                        }
                    }

                    // Write the response back via USB
                    match class.write_packet(resp.as_bytes()).await {
                        Ok(_) => {}
                        // Error treat like broken usb connection
                        Err(err) => {
                            defmt::error!("Unable to write packet: {}", err);
                            break;
                        }
                    };
                }
                // Drop the processed line from the buffer
                buffer = String::from(&buffer.as_str()[line_end + 1..]);
            }
            defmt::info!("USB disconnected");
            // USB connection is broken, so better stop the rotor.
            cmd_sender.send(Gs232Cmd::Stop);
        }
    };

    // Run the ubs and handler future both to completion.
    // None of the ever completes, but they will still be polled continously.
    join(usb.run(), usb_handler_fut).await;
}

// Enum for the GS232 commands
#[derive(Format, PartialEq)]
pub enum Gs232Cmd {
    Unkown,
    GetAz,
    GetEl,
    GetAzEl,
    MoveTo(AzElPair),
    Stop,
}

// Parse a GS232 commmand from a string slice
fn parse_command(data: &str) -> Gs232Cmd {
    match data.chars().nth(0).unwrap() {
        'B' => {
            // Get Az command. Format 'B\r'
            if data.len() == 1 {
                Gs232Cmd::GetAz
            } else {
                Gs232Cmd::Unkown
            }
        }

        'C' => {
            // Get AZ and EL. Format 'C2\r'
            if data.len() == 2 && data.chars().nth(1).unwrap() as char == '2' {
                Gs232Cmd::GetAzEl
            // Get EL only 'C\r'
            } else if data.len() == 1 {
                Gs232Cmd::GetEl
            } else {
                Gs232Cmd::Unkown
            }
        }

        'W' => {
            // Set position 'Waaa eee\r' with azimuth aaa and elevation eee.
            // Fortunately rotcld will prepend zeros, so there will always be 3 digits per number.
            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' => {
            // Stop command. Format 'S\r'
            if data.len() == 1 {
                Gs232Cmd::Stop
            } else {
                Gs232Cmd::Unkown
            }
        }

        _ => Gs232Cmd::Unkown,
    }
}