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::() { if let Ok(el) = data[5..].parse::() { 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, } }