237 lines
8.6 KiB
Rust
237 lines
8.6 KiB
Rust
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,
|
|
}
|
|
}
|