diff --git a/mission_rust/src/dh.rs b/mission_rust/src/dh.rs new file mode 100644 index 0000000..97940a1 --- /dev/null +++ b/mission_rust/src/dh.rs @@ -0,0 +1,35 @@ +use crate::fsrc::dh::DeviceCom; + +pub struct EchoHandler { + pub buffer: [u8; 10], +} + +#[derive(Debug, Copy, Clone)] +pub struct Command(pub u8); + +#[derive(Debug, Copy, Clone)] +pub struct Reply(pub u8); + +impl DeviceCom for EchoHandler { + type DeviceCommand = Command; + type DeviceReply = Reply; + + fn get_rx_buffer(&mut self) -> &mut [u8] { + &mut self.buffer + } + + fn build_command<'a>(command: Self::DeviceCommand, tx_buffer: &'a mut [u8]) -> &'a [u8] { + tx_buffer[0] = command.0; + &tx_buffer[0..1] + } + + fn parse_device_reply_and_return_remainder<'a>( + data: &'a [u8], + ) -> (Option, &'a [u8]) { + if data.len() == 0 { + return (None, data); + } + + (Some(Reply(data[0])), &data[1..]) + } +} diff --git a/mission_rust/src/fsrc/dh/debug.rs b/mission_rust/src/fsrc/dh/debug.rs new file mode 100644 index 0000000..8af36e0 --- /dev/null +++ b/mission_rust/src/fsrc/dh/debug.rs @@ -0,0 +1,94 @@ +use core::time::Duration; +use core::fmt::Write; + + +use crate::{fsrc::{ + dh::DeviceCom, + osal::{ + self, + io::{HardwareInterface, Read, Write as DeviceWrite}, + }, +}, sifln}; + +pub enum Error { + IoError(osal::io::Error), +} + +impl From for Error { + fn from(value: osal::io::Error) -> Self { + Self::IoError(value) + } +} + +type Result = core::result::Result; + +pub struct DeviceHandlerDebugger<'a, T: DeviceCom> { + implementation: T, + init_commands: &'a [::DeviceCommand], + periodic_commands: &'a [::DeviceCommand], + wait_until_read: Duration, + wait_after_read: Duration, + interface: HardwareInterface, +} + +impl<'a, T: DeviceCom> DeviceHandlerDebugger<'a, T> { + pub fn new( + handler: T, + interface: HardwareInterface, + init_commands: &'a [::DeviceCommand], + periodic_commands: &'a [::DeviceCommand], + period: Duration, + mut read_percentage: f32, + ) -> Self { + if read_percentage > 1.0 { + read_percentage = 1.0; + } + + let period_ms = period.as_millis() as f32; + let wait_until_read_ms = period_ms * read_percentage; + let wait_until_read_ms = wait_until_read_ms as u32; + let period_ms = period_ms as u32; + let wait_after_read_ms = period_ms - wait_until_read_ms; + + let wait_until_read = Duration::from_millis(wait_until_read_ms as u64); + let wait_after_read = Duration::from_millis(wait_after_read_ms as u64); + + Self { + implementation: handler, + interface, + init_commands, + periodic_commands, + wait_until_read, + wait_after_read, + } + } + + fn run_one_iteration(&mut self, command: T::DeviceCommand) -> Result<()> { + let tx_buffer = self.implementation.get_tx_buffer(); + let binary_command = T::build_command(command, tx_buffer); + self.interface.write(binary_command)?; + sifln!("sent {binary_command:?}"); + osal::thread::current().delay(self.wait_until_read); + let rx_buffer = self.implementation.get_rx_buffer(); + let rx_buffer = self.interface.read(rx_buffer)?; + let (reply,remainder) = T::parse_device_reply_and_return_remainder(rx_buffer); + sifln!("got reply: {reply:?}, remainder {remainder:?}"); + osal::thread::current().delay(self.wait_after_read); + Ok(()) + } + + pub fn run(&mut self) -> Result<()> { + for command in self.init_commands.iter() { + self.run_one_iteration(*command)?; + } + + if self.periodic_commands.len() == 0 { + return Ok(()); + } + loop { + for command in self.periodic_commands.iter() { + self.run_one_iteration(*command)?; + } + } + } +} diff --git a/mission_rust/src/fsrc/dh/mod.rs b/mission_rust/src/fsrc/dh/mod.rs new file mode 100644 index 0000000..4f94d1a --- /dev/null +++ b/mission_rust/src/fsrc/dh/mod.rs @@ -0,0 +1,25 @@ +pub mod debug; + +use core::fmt::Debug; + + +pub trait DeviceCom{ + type DeviceCommand: Copy + Debug; + type DeviceReply: Copy + Debug; + + fn build_command<'a>(command: Self::DeviceCommand, tx_buffer: &'a mut [u8]) -> &'a [u8]; + + fn get_rx_buffer(&mut self) -> & mut [u8]; + + fn get_tx_buffer(&mut self) -> &mut [u8] { + self.get_rx_buffer() + } + + fn parse_device_reply_and_return_remainder<'a>( + data: &'a [u8], + ) -> (Option, &'a [u8]); + + // Advance State Machine? + // Write into Datapool + // fn handle_reply(&mut self, reply: Self::DeviceReply); +} \ No newline at end of file diff --git a/mission_rust/src/fsrc/mod.rs b/mission_rust/src/fsrc/mod.rs index 4c8f4fd..105141b 100644 --- a/mission_rust/src/fsrc/mod.rs +++ b/mission_rust/src/fsrc/mod.rs @@ -8,4 +8,5 @@ pub mod osal; //pub mod datasets; //pub mod store; //pub mod mutex; -pub mod introspection; \ No newline at end of file +pub mod introspection; +pub mod dh; \ No newline at end of file diff --git a/mission_rust/src/lib.rs b/mission_rust/src/lib.rs index f4cd634..32e9aec 100644 --- a/mission_rust/src/lib.rs +++ b/mission_rust/src/lib.rs @@ -2,23 +2,27 @@ #![feature(never_type)] //TODO os errors in API calls +mod dh; pub mod fsrc; mod panic; - -use core::time::Duration; use core::fmt::Write; +use core::time::Duration; use fsrc::*; use osal::{ - sync::{Mutex, MutexClone, StaticInit, StaticReadOnceLock}, queue::{MessageQueue, MessageQueueSender}, + sync::{Mutex, MutexClone, StaticInit, StaticReadOnceLock}, thread, }; -use crate::fsrc::osal::io::{self, HardwareInterface, Read, Write as IoWrite}; - - +use crate::{ + dh::EchoHandler, + fsrc::{ + dh::debug::DeviceHandlerDebugger, + osal::io::{self, HardwareInterface, Read, Write as IoWrite}, + }, +}; static THREAD_INIT: StaticReadOnceLock< thread::StaticThread<{ thread::Sizes::MINIMAL_STACK_SIZE + 2024 }>, @@ -184,6 +188,18 @@ static THREAD_3: StaticReadOnceLock< fn init_task() -> ! { sifln!("Mission enter"); + let commands = [dh::Command(0), dh::Command(1), dh::Command(2)]; + let mut debugger = DeviceHandlerDebugger::new( + EchoHandler { buffer: [0; 10] }, + HardwareInterface::new("ps/uart_mtg").unwrap(), + &commands, + &[], + Duration::from_secs(2), + 0.5, + ); + + debugger.run(); + let test1 = TEST1.take().unwrap(); let test2 = TEST2.take().unwrap(); let receiver = RECEIVER.take().unwrap(); @@ -198,8 +214,6 @@ fn init_task() -> ! { test2.run(mutex_copy, a); }); - - THREAD_1.take_no_init().unwrap().spawn(move || { test1.run(); sender.run(clone);