202 lines
6.1 KiB
Rust
202 lines
6.1 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::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,
|
|
}
|
|
}
|